JP2021505979A - Method and device for simultaneous self-position estimation and environment map creation - Google Patents

Abstract

The present invention relates to methods, devices, and non-transitory computer-readable media for simultaneous positioning and mapping. A step of acquiring a wide-field image by a wide-field camera, a step of acquiring a distortion-corrected image corresponding to the wide-field image based on a multi-virtual pinhole camera model, and the wide-field based on the distortion-corrected image. The multi-virtual pinhole camera model includes at least two virtual pinhole cameras in different orientations, including the steps of determining the position and orientation of the camera and creating a map, and said at least two different orientations of virtual pinholes. A method of simultaneously performing self-position estimation and environment map creation, characterized in that the camera center of the camera overlaps the camera center of the wide-field camera. [Selection diagram] Fig. 2

Description

The present invention relates to a field in which self-position estimation and environment map creation are performed at the same time, and particularly to a field in which self-position estimation and environment map creation are performed simultaneously based on a wide-field camera.

Simultaneous self-position estimation and environment map creation (Simultaneous Localization And Mapping, abbreviated as SLAM) is a technology that creates a surrounding environment map while tracking the movement of a robot in real time and realizes goals such as position estimation and navigation. Is.

The camera used in conventional SLAM is a perspective camera, or is called a pinhole camera. Due to the limited angle of view (Field-of-View) of the camera, the common features present in the acquired image are inadequate and tracking by the SLAM algorithm may be invalid. Wide-field cameras have a larger angle of view than the pinhole cameras used in conventional SLAMs, and are therefore widely studied and attracted attention.
There are mainly two types of SLAM technical proposals based on conventional wide-field images.

On the other hand, first, the wide-field image acquired by the wide-field camera is subjected to distortion correction processing using a conventional distortion correction method, and then the image after distortion correction is used as a normal image and self-used using the conventional SLAM technology. Position estimation and environment map creation are performed at the same time. Although such a technical proposal is simple, the conventional distortion correction method causes a lot of loss of the angle of view, and the wide angle of the wide-field camera cannot be fully utilized.

The other is to perform SLAM processing directly on the undistorted wide-field image based on the wide-field camera imaging model. That is, features are directly extracted and processed from a wide-field image that has not been distortion-corrected. Such a proposal impairs system performance because the extracted features can be affected by image distortion and the optimization is very complicated due to the complex wide-field camera imaging model. It ends up.

Therefore, a new SLAM technology that can maintain the entire field of view of a wide-field camera while avoiding the effects of image distortion, and can simultaneously perform depth of field detection, self-position estimation, and environment map creation is desired. ing.

The purpose of this application is to provide a way to perform positioning and mapping at the same time. This method allows you to remove widefield images captured by a widefield camera based on a multi-virtual pinhole camera model and perform simultaneous positioning and mapping based on a distortion-free image.

This application, on the other hand, provides a method of performing self-position estimation and environment mapping at the same time, based on the steps of acquiring a wide-field image with a wide-field camera and the multi-virtual pinhole camera model. The multi-virtual pinhole camera model includes a step of acquiring a distortion-corrected image corresponding to an image, a step of determining the position and orientation of the wide-field camera based on the distortion-corrected image, and a step of creating a map. Self-position estimation and environment mapping, characterized in that the camera centers of the virtual pinhole cameras of at least two different orientations include at least two different orientations and overlap the camera centers of the wide-field camera. How to do at the same time.

In some embodiments, the wide-field camera is a monocular wide-field camera, which determines the position and orientation of the wide-field camera and creates a map based on the distortion-corrected image, which is an initialization step. Including, the initialization step corresponds to a step of acquiring a distortion correction image corresponding to the first time point and a distortion correction image corresponding to the second time point, a distortion correction image corresponding to the first time point, and the second time point. A step of determining feature points matching each other in the distortion-corrected image to be performed, and a step of creating an initial map based on the feature points matching each other.

In some embodiments, creating an initial map based on the matching feature points of the feature points in the distortion-corrected image corresponding to the first time point and the wide-field camera at the first time point. The step of determining the direction vector corresponding to the first feature point based on the camera center, the matching feature point in the distortion-corrected image corresponding to the second time point, and the camera center of the wide-field camera at the second time point. Based on the above, the step of determining the direction vector corresponding to the second feature point, the direction vector corresponding to the first feature point, and the direction vector corresponding to the second feature point are subjected to triangulation, and the feature point is determined. A step of determining the corresponding map point and a step of creating an initial map based on the map point.

In some embodiments, the wide-field camera is a monocular wide-field camera, which determines the position and orientation of the wide-field camera and creates a map based on the distortion-corrected image, which is a global bundle optimization. The global bundle optimization step comprises projecting each map point associated with the key wide field frame onto a multi-virtual pinhole camera model for each key wide field frame in the map and said map. The reprojection points of the points in the multi-virtual pinhole camera model are acquired, and the map points are re-projected based on the re-projection points of the map points in the multi-virtual pinhole camera model and the feature points corresponding to the map points. The step of determining the projection error and determining the reprojection error based on the reprojection error of the map points associated with all the key wide field frames and the position of the key wide field frame based on the reprojection error. With steps to update the orientation and the position of all map points associated with the key wide field frame.

In some embodiments, the wide-field camera is a monocular wide-field camera, which determines the position and orientation of the wide-field camera and creates a map based on the distortion-corrected image, which includes tracking steps. The tracking step projects the map points onto the multi-virtual pinhole camera model and reprojects the map points in the multi-virtual pinhole camera model for each map point associated with the current wide-field frame. The points are acquired, the reprojection error of the map points is determined based on the reprojection points of the map points in the multi-virtual pinhole camera model and the feature points corresponding to the map points, and all the current wides of the map points are determined. A step of determining the reprojection error based on the reprojection error of the map point associated with the field of view frame, and a step of updating the position and orientation of the current wide field of view frame based on the reprojection error.

In some embodiments, the wide-field camera is a monocular wide-field camera, which determines the position and orientation of the wide-field camera and creates a map based on the distortion-corrected image. Including, the map creation step is based on the step of determining the feature points of the current wide-field frame and its reference frame that match each other, the feature points of the current wide-field frame, and the camera center of the current wide-field camera. The second feature point corresponds to the step of determining the direction vector corresponding to the first feature point, the matching feature point of the reference frame, and the camera center of the wide-field camera corresponding to the reference frame. A step of determining a direction vector, a step of performing triangulation on a direction vector corresponding to the first feature point and a direction vector corresponding to the second feature point, and a step of determining a map point corresponding to the feature point, and the above-mentioned With steps to create a map based on map points.

In some embodiments, the mapping step further comprises a local bundle optimization step, which is associated with the key wide field frame for each key wide field frame in the local map. Each map point is projected onto the multi-virtual pinhole camera model, the re-projection point of the map point in the multi-virtual pinhole camera model is acquired, and the re-projection point of the map point in the multi-virtual pinhole camera model and The step of determining the reprojection error of the map point based on the feature points corresponding to the map point and determining the reprojection error based on the reprojection error of the map points associated with all the key wide field frames. And the step of updating the position and orientation of the key wide-field frame and the positions of all the map points associated with the key wide-field frame based on the reprojection error.

In some embodiments, the wide-field camera is a binocular wide-field camera, which is based on the steps of acquiring left-field and right-field images by the binocular wide-field camera and a first multi-virtual pinhole camera model. A step of acquiring a left distortion correction image corresponding to the left field image, a step of acquiring a right distortion correction image corresponding to the right field image based on the second multi-virtual pinhole camera model, and the left distortion correction. The first multi-virtual pinhole camera model comprises at least two different orientations of virtual, including the step of determining the position and orientation of the binocular wide-field camera and creating a map based on the image and the right distortion corrected image. The camera center of the virtual pinhole camera including the pinhole camera and at least two different orientations overlaps the camera center of the left camera of the binocular wide-field camera, and the second multi-virtual pinhole camera model has at least two different. The camera center of the virtual pinhole camera including the orientation virtual pinhole camera and the at least two different orientations is the camera center of the right side camera of the binocular wide-field camera.

In some embodiments, the position and orientation of the binocular wide-field camera and creating a map based on the left distortion corrected image and the right distortion corrected image include an initialization step, said initial. The conversion step includes a step of determining feature points matching each other of the left distortion correction image and the right distortion correction image, and a step of creating an initial map based on the feature points matching each other.

In some embodiments, determining feature points that match the left distortion corrected image and the right distortion corrected image with each other is a pole corresponding to the feature points in the left distortion corrected image in the right distortion corrected image. It includes a step of determining a line and a step of searching for a feature point that matches the feature point in the left distortion correction image on the polar line.

In some embodiments, creating an initial map based on said mutually matching feature points is based on the feature points in the left distortion corrected image and the camera center of the left camera of the binocular wide-field camera. The direction corresponding to the second feature point based on the step of determining the direction vector corresponding to the first feature point, the matching feature point in the right distortion corrected image, and the camera center of the right camera of the binocular wide-field camera. Based on the step of determining the vector and the baseline of the binocular wide-field camera, the direction vector corresponding to the first feature point and the direction vector corresponding to the second feature point are subjected to triangulation to correspond to the feature points. A step of determining a map point to be used and a step of creating an initial map based on the map point.

In some embodiments, the position and orientation of the binocular wide-field camera and creating a map based on the left distortion corrected image and the right distortion corrected image include the global bundle optimization step. The global bundle optimization step projects the map points associated with the key binocular image frames onto the first multi-virtual pinhole camera model for each key binocular image frame in the map, and the map points are said to be said. The reprojection points in the first multi-virtual pinhole camera model are acquired, and the map points are based on the re-projection points in the first multi-virtual pinhole camera model and the feature points corresponding to the map points. The step of determining the reprojection error and determining the left reprojection error based on the reprojection error of the map points associated with all the key binocular image frames, or the map points associated with the key binocular image frame. 2 Projected onto the multi-virtual pinhole camera model, the re-projected points of the map points in the second multi-virtual pinhole camera model are acquired, and the re-projected points of the map points in the second multi-virtual pinhole camera model and The reprojection error of the map point is determined based on the feature points corresponding to the map point, and the right reprojection error is determined based on the reprojection error of the map points associated with all the key binocular image frames. Associated with the key binocular image frame position and orientation and the key binocular image frame based on the step and the sum of the left reprojection error, the right reprojection error or the left reprojection error and the right reprojection error. With steps to update the position of all map points.

In some embodiments, the position and orientation of the binocular wide-field camera and creating a map based on the left distortion corrected image and the right distortion corrected image include a tracking step, the tracking step. Projects the map points onto the first multi-virtual pinhole camera model for each map point associated with the current binocular image frame, and reprojects the map points in the first multi-virtual pinhole camera model. The points are acquired and the reprojection error of the map points is determined based on the reprojection points of the map points in the first multi-virtual pinhole camera model and the feature points corresponding to the map points, and all the current current points are determined. The step of determining the left reprojection error based on the reprojection error of the map points associated with the binocular image frame, or the second multi of the map points by projecting the map points onto a second multi virtual pinhole camera model. The reprojection error of the map point is acquired based on the reprojection point of the virtual pinhole camera model and the reprojection point of the map point in the second multi-virtual pinhole camera model and the feature point corresponding to the map point. And the step of determining the right reprojection error based on the reprojection error of the map points associated with all the current binocular image frames, and the left reprojection error, the right reprojection error or the left reprojection. A step of updating the position and orientation of the current binocular image frame based on the sum of the projection error and the right reprojection error.

In some embodiments, the position and orientation of the binocular wide-field camera is determined and a map is created based on the left distortion corrected image and the right distortion corrected image. The creation steps include determining the feature points that match the current left distortion correction image and the current right distortion correction image with each other, the feature points of the current left distortion correction image, and the left camera of the current binocular wide-field camera. Based on the step of determining the direction vector corresponding to the first feature point based on the camera center of, and the feature point of the current right distortion corrected image and the camera center of the right camera of the current binocular wide-field camera. Triangular survey is performed on the step of determining the direction vector corresponding to the second feature point, the direction vector corresponding to the first feature point, and the direction vector corresponding to the second feature point, and the map point corresponding to the feature point. And a step to create a map based on the map points.

In some embodiments, the mapping step further comprises a local bundle optimization step, which is associated with the key binocular image frame for each key binocular image frame in the local map. The map point is projected onto the first multi-virtual pinhole camera model, the re-projected point of the map point in the first multi-virtual pinhole camera model is acquired, and the first multi-virtual pinhole camera model of the map point is acquired. The reprojection error of the map point is determined based on the reprojection point in and the feature point corresponding to the map point, and left reprojection based on the reprojection error of the map point associated with all the key binocular image frames. The step of determining the projection error, or the map point associated with the key binocular image frame is projected onto the second multi-virtual pinhole camera model, and the re-projection point of the map point in the second multi-virtual pinhole camera model is Acquired, the reprojection error of the map point is determined based on the reprojection point of the map point in the second multi-virtual pinhole camera model and the feature point corresponding to the map point, and all the key binocular images. The step of determining the right reprojection error based on the reprojection error of the map points associated with the frame and the sum of the left reprojection error, the right reprojection error or the left reprojection error and the right reprojection error. With a step of updating the position orientation of the key binocular image frame and the positions of all map points associated with the key binocular image frame based on.

In some embodiments, the position and orientation of the wide-field camera and creating a map based on the distortion-corrected image comprises a closed-loop detection processing step, wherein the closed-loop detection processing step is current. When the wide-field frame is a key wide-field frame, the step of determining a closed-loop wide-field frame similar to the current wide-field frame in the map database and the mutual of the current wide-field frame and the closed-loop wide-field frame The step of determining the feature points to be matched and the map points associated with the feature points for each matching feature point in the current wide-field frame are set to the multi-virtual pinhole camera corresponding to the closed-loop wide-field frame. Converted to the coordinate system of the model and then projected onto the imaging plane of the multi-virtual pinhole camera model to obtain the reprojected points of the map points in the closed-loop wide-field frame, the re-projected points and the closed-loop wide-field frame. The step of determining the first reprojection error based on the matching feature points in and the first cumulative reprojection error based on the first reprojection error of all matching feature points in the current wide-field frame. For the step to determine and each matching feature point in the closed-loop wide-field frame, the map points associated with the feature point are the coordinate system of the multi-virtual pinhole camera model corresponding to the current wide-field frame. And then projected onto the imaging plane of the multi-virtual pinhole camera model to obtain the reprojection points of the map points in the current wide-field frame, and in the re-projection points and the current wide-field frame. A step of determining the second reprojection error based on the matching feature points and a step of determining the second cumulative reprojection error based on the second reprojection error of all matching feature points in the closed-loop wide-field frame. And, using the first cumulative reprojection error and the second cumulative reprojection error, the key wide-field frame having a common field-of-view relationship with the current wide-field frame in the map and the map points associated with the key wide-field frame can be obtained. With steps to correct.
In some embodiments, the at least two different orientations are forward, upward, downward, left or right of the cube.

This application, on the other hand, provides a device that simultaneously performs self-position estimation and environment mapping, with at least one storage device containing a set of instructions and at least one processor communicating with the at least one storage device. When executing the set of instructions, the at least one processor has a step of acquiring a wide-field image by a wide-field camera and a multi-virtual pin in a device that simultaneously performs the self-position estimation and environment map creation. A step of acquiring a distortion-corrected image corresponding to the wide-field image based on the hall camera model, and a step of determining the position and orientation of the wide-field camera based on the distortion-corrected image and creating a map. To execute, the multi-virtual pinhole camera model includes at least two differently oriented virtual pinhole cameras, and the camera center of the at least two differently oriented virtual pinhole cameras overlaps the camera center of the wide-field camera. A device that estimates the position and creates an environment map at the same time.

Additional features in the present invention will be partially described in the following description. This description will reveal to those skilled in the art what is described in the drawings and embodiments below. The points of the invention in the present invention can be fully explained by implementing or using the methods, means, and combinations thereof described in the detailed examples discussed below.

The following drawings describe in detail exemplary embodiments disclosed in the present invention. In some of the drawings, the same reference numbers show a similar structure. Those skilled in the art will appreciate that these embodiments are non-limiting exemplary embodiments and that the drawings are for purposes of illustration and illustration only and are not intended to limit the scope of this disclosure. Let's do it. The intent of the present invention in the present invention is also satisfied. Please understand that the drawings are not drawn to scale.
Shows a system that simultaneously performs self-position estimation and environment map creation according to some examples of the present invention. Shows a flowchart of a method of simultaneously performing self-position estimation and environment map creation according to some embodiments of the present invention. Shows a multi-virtual pinhole camera model that includes two orientations according to some embodiments of the present invention. Shows a multi-virtual pinhole camera model that includes five orientations according to some embodiments of the present invention. Is a schematic diagram for performing distortion correction based on the multi-virtual pinhole camera model according to some examples of the present invention.

Shows the original monocular fisheye image according to some examples of the present invention, the conventional monocular fisheye image after distortion correction, and the monocular fisheye image after distortion correction by the method of the present disclosure. Shows the original binocular fisheye image according to some examples of the present invention and the conventional distortion-corrected binocular fisheye image. Shows a flowchart of camera position / orientation identification and map creation according to some embodiments of the present invention. Shows a schematic diagram of map points constructed by a monocular wide field camera according to some embodiments of the present invention. Is a schematic diagram of a polar line search of a binocular wide-field camera according to some examples of the present invention. Is a schematic diagram of creating map points of a binocular wide-field camera according to some examples of the present invention.

The following description provides specific application scenarios and requirements for this application in order for those skilled in the art to create and use content in this application. Various modifications to the disclosed embodiments will be apparent to those skilled in the art and the general principles defined herein can be applied to other embodiments without departing from the spirit and scope of the present disclosure. application. Therefore, the present disclosure is not limited to the illustrated embodiments, but is the broadest scope that is consistent with the claims.

The terms used herein are for the purposes of describing only certain exemplary embodiments and are not intended to be limiting. For example, as used herein, the singular forms "a," "an," and "the" may include the plural, unless the context explicitly indicates otherwise. The terms "contain", "contain", and / or "contain" as used in this specification mean that there are associated integers, steps, operations, elements, and / or components, but one. It does not preclude other features above, you can add integers, steps, operations, elements, components, and / or other features, integers, steps, operations, elements, components to your system / methods.

Considering the following description, these and other features of the present disclosure, as well as the behavior and function of the relevant components of the structure, as well as the combination of components and the economics of manufacture can be significantly improved. With reference to the drawings, all of which form part of this disclosure. However, it should be clearly understood that the drawings are for illustration and explanation purposes only and are not intended to define restrictions on disclosure.

The flowcharts used in the present disclosure show the operations performed by the system according to some embodiments of the present disclosure. You need to clearly understand that flowchart behavior can be implemented in any order. Instead, you can perform the operations in reverse order or at the same time. In addition, you can add one or more other actions to the flowchart. You can remove one or more actions from the flowchart.

One aspect of the present disclosure relates to a method of simultaneously performing self-position estimation and environment map creation. Specifically, the method is based on a multi-virtual pinhole camera model, a step of distortion-correcting a wide-field image acquired by a wide-field camera to obtain a distortion-corrected image, and a wide field based on the distortion-corrected image. Includes steps to determine the position and orientation of the field of view camera and create a map. The multi-virtual pinhole camera model includes at least two differently oriented virtual pinhole cameras, and the camera center of the at least two differently oriented virtual pinhole cameras overlaps the camera center of the wide-field camera.
FIG. 1 shows a system that simultaneously performs self-position estimation and environment map creation according to some examples of the present invention.

The system 100 that simultaneously estimates the self-position and creates the environment map can acquire a wide-field image and execute a method of simultaneously estimating the self-position and creating the environment map. For the method of simultaneously estimating the self-position and creating the environment map, the description of FIGS. 2 to 11 may be referred to.

As shown in the figure, the system 100 that simultaneously performs self-position estimation and environment map creation includes a wide-field camera 101 and a device 102 that simultaneously performs self-position estimation and environment map creation. The wide-field camera 101 and the device 102 that simultaneously estimates the self-position and creates the environment map may be mounted integrally or individually. For convenience of explanation of the invention of the present disclosure, the wide-field camera of the present disclosure is taken as an example of a fisheye camera.

The wide-field camera 101 acquires a fisheye image of the subject. In some embodiments, the wide-field camera 101 may be a fisheye camera, a reflection / refraction camera, or a panoramic camera. In some embodiments, the wide-field camera 101 may be a monocular wide-field camera, a binocular wide-field camera, or a multi-eye wide-field camera.

As an example, the wide field camera 101 includes a monocular fisheye camera and a binocular fisheye camera. The left camera of the binocular fisheye camera is called the left eye, and the right camera of the binocular fisheye camera is called the right eye. The image acquired by the left eye is called a left fisheye image (left visual field image), and the image acquired by the right eye is called a right fisheye image (right visual field image).

The device 102 that simultaneously performs self-position estimation and environment map creation is an exemplary computing device that can execute a method of simultaneously performing self-position estimation and environment map creation.

The device 102 for positioning and mapping can include a COM port 150 to facilitate data communication. The device 102 for simultaneous positioning and mapping may further include a processor 120 in the form of one or more processors for executing computer instructions. Computer instructions can include, for example, routines, programs, objects, components, data structures, procedures, modules, and functions that perform the specific functions described herein. For example, the processor 120 may determine a distorted image of a fisheye image based on a plurality of virtual pinhole camera models. As another example, the processor 120 may determine the orientation of the wide field camera 101 and build a map based on the distorted image.

In some embodiments, the processor 120 may include one or more hardware processors such as a microcontroller, microprocessor, reduced instruction set computer (RISC), application specific integrated circuit (ASIC), application specific, and the like. Instruction set processor (ASIP), central processing unit (CPU), graphics processing unit (GPU), physical processing unit (PPU), microcontroller unit, digital signal processor (DSP), field programmable gate array (FPGA)), advanced RISC A machine (ARM), a programmable logic device (PLD), a circuit or processor capable of performing one or more functions, or a combination thereof.

In some embodiments, the device 102 for simultaneous positioning and mapping is an internal communication bus 110, program storage, and different types of data storage (eg, disk 170, read-only memory (ROM) 130, or random access). May include. Memory (RAM) 140). Device 102 for simultaneous positioning and mapping may also include program instructions stored in ROM 130, RAM 140, and / or other types of non-temporary storage media executed by processor 120. The methods and / or processes of the present invention may be implemented as program instructions. The device 102 for simultaneous positioning and mapping also includes an I / O component 160 that supports input / output between the computer and other components (eg, user interface elements). The simultaneous positioning and mapping device 102 can also receive programming and data over network communication.

For purposes of illustration only, device 102 describes only one processor for simultaneous positioning and mapping in the present invention. However, it should be noted that the device 102 for simultaneous positioning and mapping in the present invention may also include multiple processors, and therefore the operation and / or method steps disclosed in the present invention are described herein. It can also be run jointly by multiple processors, which can be run by one processor. For example, if processor 120 of device 102 for positioning and mapping in the present invention performs steps A and B, it should be understood that steps A and B can be combined or separated by two different processors in information processing. .. Run (for example, the first processor performs step A and the second processor performs step B, or the first and second processors perform steps A and B together).

FIG. 2 shows a flowchart of a method of simultaneously performing self-position estimation and environment map creation according to some embodiments of the present invention. Process 200 can be performed as a set of instructions (in a non-temporary storage medium in device 102) that simultaneously estimates self-position and creates an environment map. The device 102 that simultaneously estimates the self-position and creates the environment map executes the set of instructions and executes the steps of the process 200 in association with each other.

The behavior of the illustrated process 200 presented below is intended to be exemplary and not limiting. In some embodiments, the process 200 may add one or more additional actions not described and / or remove one or more actions described herein. Furthermore, the order of operations shown in FIG. 2 and described below is not a limitation.
In 210, the device 102 that simultaneously estimates the self-position and creates the environment map can acquire a wide-field image by the wide-field camera 101.

When the wide-field camera 101 is a monocular wide-field camera, the monocular wide-field camera acquires a wide-field image, and when the wide-field camera 101 is a binocular wide-field camera, the binocular wide-field camera is a left-field image and a right-field image. Acquire a wide field of view image including the image.

In 220, the device 102 that simultaneously estimates the self-position and creates the environment map can acquire a distortion-corrected image corresponding to the wide-field image based on the multi-virtual pinhole camera model.

When the wide-field camera 101 is a monocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates an environment map acquires a distortion-corrected image corresponding to the wide-field image based on the multi-virtual pinhole camera model. Can be done.

The multi-virtual pinhole camera model includes at least two differently oriented virtual pinhole cameras, and the camera center of the at least two differently oriented virtual pinhole cameras overlaps the camera center of the monocular wide-field camera.

When the wide-field camera 101 is a binocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates an environment map produces a left distortion correction image corresponding to the left-field image based on the first multi-virtual pinhole camera model. Acquire and acquire a right distortion correction image corresponding to the right field image based on the second multi-virtual pinhole camera model. The first multi-virtual pinhole camera model and the second multi-virtual pinhole camera model may be the same or different.

The first multi-virtual pinhole camera model includes at least two differently oriented virtual pinhole cameras, and the camera center of the at least two differently oriented virtual pinhole cameras overlaps the left eye camera center of the wide-field camera 101. The second multi-virtual pinhole camera model includes at least two differently oriented virtual pinhole cameras, and the camera center of the at least two differently oriented virtual pinhole cameras is to the right of the wide-field camera 101. It may overlap with the center of the camera of the eye.

As an example, FIG. 3 shows a multi-virtual pinhole camera model that includes two orientations according to some embodiments of the present invention. The orientations of the two virtual pinhole cameras form a 90 ° angle, and the center of the camera overlaps the center of the wide-field camera at point C.

As an example, FIG. 4 shows a multi-virtual pinhole camera model that includes five orientations according to some embodiments of the present invention. As shown, the multi-virtual pinhole camera model includes a total of five orientations of the cube, forward, upward, downward, left and right. The camera centers of the five virtual pinhole cameras overlap with the camera centers of the wide-field cameras at point C. At this time, the distortion correction method is called a distortion correction method based on a cube map (cubemap-based unition method) (hereinafter, abbreviated as Cube or cube model).

Specifically, the device 102 that simultaneously estimates the self-position and creates the environment map converts the wide-field image (or left-field image or right-field image) into a multi-virtual pinhole camera model (or a first multi-virtual pinhole camera model, a first When projected onto a two-multi virtual pinhole camera model), the projections of at least two virtual pinhole cameras in different directions are acquired, and the projections of the virtual pinhole cameras in at least two different directions are expanded, the left fish A distortion-corrected image corresponding to an eye image can be acquired.

As shown in FIG. 5, it is a schematic diagram that performs distortion correction based on the multi-virtual pinhole camera model according to some examples of the present invention. Hereinafter, the first multi-virtual pinhole camera model and the left-field image will be taken as examples.

Point A is the center of the left eye of the binocular wide-field camera, and points B, C and D are exemplary pixels in the left-field image. The first multi-virtual pinhole camera 510 is a cube model and includes virtual pinhole cameras in five orientations, with the cubes facing forward, upward, downward, left and right, respectively. The camera centers of the virtual pinhole cameras in the five directions overlap at the point A.

As shown, the left-field image is projected onto the imaging planes of five differently oriented virtual pinhole cameras of the first multi-virtual pinhole camera model 510. Correspondingly, five different orientation projections are acquired. When the projection views in the five different directions are developed, a left distortion correction image is acquired.

As shown in FIG. 6, the original monocular fisheye image according to some examples of the present invention, the conventional monocular fisheye image after distortion correction, and the monocular fisheye image after distortion correction by the method of the present disclosure are shown.

FIG. 610 is a wide-field image acquired by a monocular fisheye camera. As can be seen from the illustration, the wide-field image has a wider field of view than the image acquired by an ordinary camera, but there is spatial distortion in all the images, and the dust and distortion away from the center of the image are large.

FIG. 620 is a distortion correction image obtained by performing distortion correction processing on the wide field image by a conventional distortion correction method. The angle of view of an image acquired by an ordinary camera is generally about 80 °, but the angle of view of FIG. 620 is 100 °. The angle of view of the image acquired by an ordinary camera is enlarged, but many angles of view are lost compared to the image before the distortion correction processing. Therefore, it is not possible to create a map of all angles of view including a wide-field image.

FIG. 630 is a wide-field distortion correction image developed after distortion correction based on a multi-virtual pinhole camera model in five directions according to an embodiment of the present invention, that is, a distortion correction image acquired by a cube model. As shown, FIG. 630 retains all angles of view of a wide field image. By performing SLAM based on the wide-field distortion-corrected image, a map including all the original angle-of-view contents can be created.
FIG. 7 shows an original binocular fisheye image according to some examples of the present invention and a conventional distortion-corrected binocular fisheye image.

As shown, the images 701 and 702 are the original left fisheye image and the right fisheye image acquired by the wide field camera 101 in the real world, respectively. The image 703 and the image 704 are a left distortion correction image and a right distortion correction image after the conventional distortion correction, respectively.

As a comparison with the left distortion corrected image and the right distortion corrected image (not shown) processed by the cube model, the single image obtained from the images 601 and 602 by the conventional distortion correction method is the vertical direction of the image. Both horizontal angles of view are only 100 °. As can be seen from the above, the distortion correction method according to the present invention can maintain a wide field of view and effectively prevent image distortion with respect to the wide field of view image acquired by the wide field camera 101.

In 230, the device 102 that simultaneously estimates the self-position and creates the environment map determines the position and orientation of the wide-field camera based on the distortion-corrected image and creates the map.

In some embodiments, in the case of a monocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates the environment map extracts the feature points of the distortion-corrected image and corresponds to the corresponding wide-field frame based on the extracted feature points. Then, based on the wide-field frame, the position and orientation of the monocular wide-field camera can be determined, and a map can be created.

Preferably, by extracting the feature points of the wide-field distortion-corrected image, that is, the key points and descriptors of the wide-field distortion-corrected image, the position and orientation of the movement of the camera are determined based on the feature points of the wide-field distortion-corrected image. Track and create maps. Preferably, the position and orientation of the camera movement are estimated directly based on the pixel luminance information in the wide-field distortion-corrected image without calculating the key points and descriptors, and a map is created.

The wide-field distortion-corrected image acquired by the distortion correction method based on the multi-virtual pinhole camera model retains all the angles of view of the original wide-field image. As a result, self-position estimation and environment map creation can be performed at the same time based on abundant shared features between wide-field images, and more efficient self-position estimation and more accurate map can be obtained. At the same time, the above method can avoid an increase in the computational cost of the system due to the complicated projection model of the wide-field camera.

In some embodiments, in the case of a binocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates the environment map extracts the feature points of the left distortion corrected image and the right distortion corrected image, and is based on the extracted feature points. The corresponding binocular image frame can be created, and then the position and orientation of the binocular wide-field camera can be determined based on the binocular image frame to create a map.

Since the wide-field frame (or binocular image frame) contains information on all the feature points in the distortion-corrected image (or left-handed distortion-corrected image, right-handed distortion-corrected image), the position and orientation of the movement of the wide-field camera 101 can be determined accordingly. You can track and create maps.

As an example, the device 102 that simultaneously estimates the self-position and creates the environment map enlarges / reduces the distortion correction image (or left distortion correction image, right distortion correction image), and scales the distortion correction image (or left distortion correction image, right distortion). The image pyramid (Image Pyramid) corresponding to the corrected image) is acquired. Corners are extracted from each scale image of the image pyramid and descriptors are calculated. The feature points of the image are composed of the corners and descriptors. The corner is a region having a high degree of recognition and being representative in the image, and is for showing the position information of the feature point in the image. The descriptor may be shown as a vector and is for describing the information of the pixels around the corner. Descriptors may be designed to give similar descriptors to features that are similar in appearance.

The feature points of the distortion-corrected image (or left-distortion-corrected image or right-handed distortion-corrected image) are extracted, and a corresponding wide-field frame (or binocular image frame) is created based on the extracted feature points. The wide-field frame (or binocular image frame) includes all feature points in the corresponding distortion-corrected image (or left-distortion-corrected image, right-handed distortion-corrected image). After the creation of the wide-field frame (or binocular image frame) is completed, the pixel data of the distortion correction image (or left distortion correction image, right distortion correction image) corresponding to the wide-field frame (or binocular image frame) is discarded, and the pixel data is discarded. Saves storage space and reduces system power consumption.
Further description of step 230 may be referred to FIG. 8 and related description.

When the wide-field camera 101 is a binocular wide-field camera, the optical axes of the left eye and the right eye of the binocular wide-field camera may not be parallel. Therefore, the process 200 may further include a step of parallelizing the optical axes of the left eye and the right eye of the wide field camera 101. For example, the device 102 that simultaneously estimates the self-position and creates the environment map can adjust the virtual optical axes of the left eye and the right eye of the binocular fisheye camera in parallel by the binocular camera correction program.

FIG. 8 shows a flowchart of camera position / orientation identification and map creation according to some embodiments of the present invention. Process 230 may be performed as a set of instructions in a non-temporary storage medium in device 102 that simultaneously estimates self-position and creates an environment map. The device 102, which simultaneously estimates the self-position and creates the environment map, can execute the set of instructions and execute the steps of the process 200 correspondingly.

The operations of process 230 described below are for illustration purposes only and are not limiting. In some embodiments, process 230 may add one or more operations not described and / or remove one or more operations described herein upon realization. Further, the order of operations described later shown in FIG. 8 is not limited.

In 810, the device 102 that simultaneously estimates the self-position and creates the environment map can execute the initialization step, and in the initialization step, the initial map can be created.

In the case of a monocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates an environment map acquires distortion-corrected images (or wide-field frames) at two different time points, and obtains distortion-corrected images (or wide-field frames) at the two different time points. It is possible to determine the feature points that match each other in the field frame) and create an initial map based on the feature points that match each other.

As an example, the device 102 that simultaneously estimates the self-position and creates the environment map acquires the distortion correction image (or wide field frame) corresponding to the first time point and the distortion correction image (or wide field frame) corresponding to the second time point. , The distortion-corrected image (or wide-field frame) corresponding to the first time point and the distortion-corrected image (or wide-field frame) corresponding to the second time point are determined to match each other, and the feature points matching each other are determined. You can create an initial map based on.

In some embodiments, the wide-field frame corresponding to the first time point and the wide-field frame corresponding to the second time point may be the current wide-field frame and the reference wide-field frame. The current wide-field frame and the reference wide-field frame may be continuous frames, and there may be one or more spacing frames between them. A predetermined parallax is required between the current wide-field frame and the reference wide-field frame to ensure smooth initialization.

In some embodiments, device 102, which simultaneously estimates self-position and creates an environment map, corresponds to a first time point based on a multi-virtual pinhole camera model (eg, the multi-virtual pinhole camera model shown in FIG. 4). The distortion-corrected image (or wide-field frame) and the distortion-corrected image (or wide-field frame) corresponding to the second time point can be decomposed into sub-field frames corresponding to each virtual pinhole camera. As a result, each virtual pinhole camera acquires two corresponding sub-field frames, and these two sub-field frames correspond to the distortion-corrected image (or wide-field frame) corresponding to the first time point and the second time point, respectively. This is the origin of the distortion-corrected image (or wide-field frame). By performing inter-frame matching for these two sub-field frames, feature points that match each other are determined.

In some embodiments, creating an initial map based on feature points that match each other is the feature points in the distortion-corrected image corresponding to the first time point and the camera center of the wide-field camera at the first time point. Based on the step of determining the direction vector corresponding to the first feature point, the matching feature points in the distortion-corrected image corresponding to the second time point, and the camera center of the wide-field camera at the second time point. Then, the step of determining the direction vector corresponding to the second feature point, the direction vector corresponding to the first feature point, and the direction vector corresponding to the second feature point are subjected to triangular survey to correspond to the feature point. It includes a step of determining a map point and a step of creating an initial map based on the map point.

Specifically, the device 102 that simultaneously estimates the self-position and creates the environment map uses the reference wide-field frame F1 as the sub-field frames F11 and F12 corresponding to each virtual pinhole camera based on the multi-virtual pinhole camera model, respectively. , F13, F14 and F15, and the current wide-field frame F2 is also divided into sub-field frames F21, F22, F23, F24 and F25 corresponding to each virtual pinhole camera based on the multi-virtual pinhole camera model. Disassemble. Here, the sub-field frames F11 and F21 correspond to forward-facing virtual pinhole cameras, the sub-field frames F12 and F22 correspond to upward virtual pinhole cameras, and the sub-field frames F13 and F23 correspond to downward virtual pinhole cameras. The sub-field frames F14 and F24 correspond to the left-facing virtual pinhole camera, and the sub-field frames F15 and F25 correspond to the right-facing virtual pinhole camera. Features that match the current wide-field frame and the reference wide-field frame with each other by performing frame-to-frame matching for the sub-field frames F11 and F21, F12 and F22, F13 and F23, F14 and F24, and F15 and F25. To determine. [Here, in the matching of the sub-field frames, the feature points that match each other of the two field frames are determined, and a new map point is created by performing triangulation based on the direction vector. Is such an explanation accurate? ]
Hereinafter, inter-frame matching will be described using the sub-field frames F11 and F21 as examples.

First, the feature points of the sub-field frames F11 and F21 are matched, it is detected whether or not the number of matching feature point pairs is equal to or greater than the initialization threshold value, and if it is less than the initialization threshold value, initialization fails. When the number of matching feature point pairs exceeds the initialization threshold, for example, using a random sample consensus algorithm (abbreviated as Random Sample Consensus, RANSAC), two frames based on the direction vector of the matching feature point pairs. Compute the Essential matrix between them. The initialization threshold indicates the minimum number of feature point pairs required to create the initial map, and the default value, for example 100, may be used directly or may be preset by the user.

Next, the essential matrix is decomposed to obtain the relative position and orientation of the current wide-field frame and the reference wide-field frame, and the relative position and orientation can be indicated by the position-orientation matrix. Based on the relative position and orientation of the current wide-field frame and the reference wide-field frame, triangulation is performed on the matching feature point pair, and the three-dimensional coordinates of the map point corresponding to the feature point pair, that is, the position of the map point is determined. get.

As shown in FIG. 9, the O1 point is the camera center of the virtual pinhole camera corresponding to the sub-field frame F11, the O2 point is the camera center of the virtual pinhole camera corresponding to the sub-field frame F12, and p1 and p2 are matching features. It is a point. The three-dimensional coordinates of the map point, that is, the position of the P point can be determined by the direction of the vector O1p1 and the direction of the vector O2p2. In SLAM, the vector O1p1 and the vector O2p2 may not intersect due to the influence of noise. In this case, for example, the coordinates of the point P that minimizes the error are obtained by the least squares method. The distance between O1 and O2 has a great influence on the error of triangulation. If the distance is too small, that is, the camera shift is too small, the angular error observed at point P leads to a large depth error. On the other hand, if the distance is too large, the overlapping portion of the scene is greatly reduced, and feature matching becomes difficult. Therefore, a predetermined parallax is required between the current wide-field frame and the reference wide-field frame. If the two selected wide-field frames do not meet this requirement, the initialization will fail, the two wide-field frames will be discarded, and reinitialization will be performed.

Finally, the three-dimensional coordinates of the map points are acquired based on the above triangulation to create the initial map points. The three-dimensional coordinates are used as the coordinates of the map points, and the descriptors of the feature points corresponding to the three-dimensional coordinates are used as the descriptors of the map points.

In the case of a binocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates the environment map may execute the initialization step of the monocular wide-field camera, and the left distortion correction image and the right distortion correction image at the same time point may be mutually executed. An initial map may be created based on matching feature points.

As an example, the device 102 that simultaneously estimates the self-position and creates the environment map determines the feature points that match the left distortion correction image and the right distortion correction image, and creates an initial map based on the feature points that match each other. Can be done.

In some embodiments, the device 102, which simultaneously estimates the self-position and creates the environment map, determines a pole line corresponding to a feature point in the left distortion corrected image in the right distortion corrected image, and then on the pole line. It is possible to search for feature points that match the feature points in the left distortion corrected image. The polar line is a multi-line segment polygonal line.

As shown in FIG. 10, it is a schematic diagram of the polar line search of the binocular wide-field camera according to some examples of the present invention. The left distortion correction image 1010 has a polar wire 1001, and the right distortion correction image 1020 has a polar wire 1002. The feature points that match the feature points of the left distortion correction image 1010 are always on the pole line 1002. On the other hand, the feature point matching the feature point of the right distortion correction image 1020 is on the pole line 1001. Therefore, the polar line search can quickly find feature points that match the left distortion corrected image and the right distortion corrected image.
As shown in the figure, the pole line 1001 and the pole line 1002 are three line segment polygonal lines, and include two inclined line segments and one horizontal line segment.

As shown in the figure, the left distortion correction image 1010 and the right distortion correction image 1020 hold all the angles of view of the left fisheye image and the right fisheye image, respectively. By simultaneously estimating the self-position and creating the environment map based on the left distortion correction image 1010 and the right distortion correction image 1020, it is possible to create a map including all the original angle-of-view contents.

In some embodiments, creating a map based on the above-mentioned mutually matching feature points is first based on the feature points in the left distortion corrected image and the camera center of the left eye of the wide-field camera 101. The direction vector corresponding to the first feature point is determined, and then the direction vector corresponding to the second feature point is determined based on the matching feature point in the right distortion corrected image and the camera center of the right eye of the wide-field camera 101. Then, based on the baseline of the binocular fish-eye camera, a triangular survey is performed on the direction vector corresponding to the first feature point and the direction vector corresponding to the second feature point to correspond to the feature point. A step of determining map points and finally creating a map based on the map points is included.

As shown in FIG. 11, it is a schematic diagram of map point creation of a binocular wide-field camera according to some examples of the present invention. Hereinafter, a map point in front of the wide-field camera 101 will be taken as an example.

The point O1 is the center of the camera of the left eye of the wide-field camera 101, and connects the feature points in the left distortion corrected image with the point O1 to acquire the direction vector corresponding to the first feature point. The point O2 is the center of the camera of the right eye of the wide-field camera 101, and connects the matching feature points and the points O2 in the right distortion corrected image to acquire the direction vector corresponding to the second feature point. In some embodiments, the direction vector corresponding to the first feature point and the direction vector corresponding to the second feature point are the united vectors.

The direction vector corresponding to the first feature point and the direction vector corresponding to the second feature point intersect at the point E, and the line segment O1E and the line segment O2E are acquired, respectively. The line segment O1O2 is acquired by connecting the point O1 and the point O2, and the length of the line segment O1O2 is b (that is, the baseline of the wide-field camera 101). The line segment O1O2, the line segment O1E, and the line segment O2E form a triangle. By solving the triangle, the length of the line segment O1E is d1, the length of the line segment O2E is d2, the angle between O1O2 and the line segment O1E is α1, the angle between O1O2 and the line segment O2E is α2, and further. Acquire the coordinates of the map point E corresponding to the feature point. Further, the map point E is converted from the coordinate system of the wide field camera 101 to the world coordinate system according to the current position and orientation of the wide field camera 101. Next, a map is created based on the position of point E in the world coordinate system.

Specifically, the device 102 that simultaneously estimates the self-position and creates the environment map performs triangulation based on the following equation. First, equations (1), (2) and (3) are obtained based on the sine / cosine theorem.

In some embodiments, the map points newly created by the monocular wide-field camera and the binocular wide-field camera need to be further associated. One map point can be observed by multiple key wide-field frames, and at the same time as associating the key wide-field frame that observed this map point with this map point, the feature points on the key wide-field frame associated with this map point, That is, the feature points that can be applied to the survey are recorded so as to acquire the map points. It is necessary to associate the two key wide-field frames created by the initialization with the map point acquired by the above initialization, and at the same time, on these two key wide-field frames associated with the map point. Record the feature points.
The created initial map includes the above two key wide-field frames, the above initial map points, and information on their association.

In some embodiments, the initialization step is a vector based on the Bag of Word model, based on the above two key wide-field frames, when the number of matching feature point pairs exceeds the initialization threshold. Further includes the step of creating and adding a vector based on the bag model of the word to the map database. In the word bag model, clustering is performed based on various image features. For example, eyes, nose, ears, mouth, edges and corners of various features are different feature classes. If there are 10000 classes, for each wide field frame, the included classes are analyzed and the presence is indicated by 1 and the absence is indicated by 0. In this case, this wide-field frame can be represented by one 10000-dimensional vector. For different wide-field frames, their similarity can be determined by comparing the vectors based on the bag model of those words. The map database is used to store vectors based on a bag model of words created based on key wide-field frames.

At 820, the device 102, which simultaneously estimates the self-position and creates the environment map, can perform the global bundle optimization step. Global bundle optimization optimizes all key wide-field frames (or key binocular image frames) and all map points in the map currently being created by SLAM (hereinafter abbreviated as the current map). For example, performing global bundle optimization on the initial map created in step 810 means performing global bundle optimization on a map having only the above two key wide-field frames and map points. In addition, except for the initial map, the global bundle optimization may be executed for the current map at any time during map creation. Bundle optimization fine-tunes the position and orientation of the key wide-field frame (or key binocular image frame) in the map and the position of the map points, and the key wide-field frame (or key) of the map points in the map created by SLAM. The purpose is to minimize the reprojection error in the binocular image frame) and optimize the map created thereby.

In the case of a monocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates an environment map projects each map point associated with the key wide-field frame in the map onto the multi-virtual pinhole camera model, and the above-mentioned of each map point. The reprojection points in the multi-virtual pinhole camera model are acquired, and the re-projection error of each map point is calculated based on the re-projection points of each map point in the multi-virtual pinhole camera model and the feature points corresponding to the map points. Determine and determine the reprojection error based on the reprojection error of the map points associated with all the key wide field frames, and based on the reprojection error, the position and orientation of the key wide field frame and the key wide. Update the position of all map points associated with the field frame.

In the present invention, the position / orientation of the frame (for example, the position / orientation of the key wide-field frame) is the position / orientation of the movement of the wide-field camera 101 at the time when the wide-field camera 101 acquires the frame. In addition, it is called the position and orientation of the frame.

Taking the multi-virtual pinhole camera model of the five orientations shown in FIG. 4 as an example, based on the multi-virtual pinhole camera model, map points are converted into the coordinate system of the corresponding virtual pinhole camera, for example, the map. The virtual pinhole camera corresponding to the point is a forward-facing virtual pinhole camera, and further projects onto the imaging plane of the forward-facing virtual pinhole camera to acquire a reprojection point of the map point. Since the multi-virtual pinhole camera model used here is the same as the multi-virtual pinhole camera model used for the wide-field image distortion correction process in step 220, the imaging plane is decomposed into forward-facing virtual pinhole cameras. Corresponds to the sub-field frame of the key wide-field frame. The reprojection point can be understood as an observation value of the map point based on the position and orientation of the sub-field frame. A feature point on the key wide-field frame associated with the map point (ie, a triangulation is used to obtain the feature point on which the map point is based) and a reprojection point of the map point of the map point. Determine the reprojection error. Ideally, the map created by SLAM is error-free and the reprojection error is zero. However, since errors such as survey errors inevitably occur under actual conditions, the reprojection error cannot be completely eliminated, and SLAM optimizes the created map by minimizing the reprojection error. To do.

In the case of a binocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates an environment map projects each map point associated with the key binocular image frame in the map onto the first multi-virtual pinhole camera model, and the map point. The reprojection points in the first multi-virtual pinhole camera model of the above are acquired, and the map is based on the re-projection points of the map points in the first multi-virtual pinhole camera model and the feature points corresponding to the map points. The point reprojection error is determined and the left reprojection error is determined based on the map point reprojection error associated with all said key binocular image frames.

Alternatively, the device 102 that simultaneously estimates the self-position and creates the environment map projects the map point associated with the key binocular image frame onto the second multi-virtual pinhole camera model, and the second multi-virtual pinhole of the map point. The reprojection point in the camera model is acquired, and the reprojection error of the map point is determined based on the reprojection point of the map point in the second multi-virtual pinhole camera model and the feature point corresponding to the map point. , The right reprojection error is determined based on the reprojection error of the map points associated with all said key binocular image frames.

Further, the device 102 that simultaneously estimates the self-position and creates the environment map sets the position and orientation of the key binocular image frame and all associated with the key binocular image frame based on the left reprojection error, the right reprojection error, or the sum thereof. You can update the position of the map points in. Specifically, in the case of a monocular map point, the device 102 that simultaneously estimates the self-position and creates the environment map associates the position and orientation of the key binocular image frame with the binocular image frame based on the left reprojection error or the right reprojection error. In the case of binocular map points, the device 102 that updates the positions of all the map points and simultaneously estimates the self-position and creates the environment map is a key binocular image based on the sum of the left reprojection error and the right reprojection error. The position and orientation of the frame and the positions of all map points associated with the binocular image frame can be updated.

In some embodiments, the device 102, which simultaneously estimates the self-position and creates the environment map, has a reprojection error (eg, left reprojection error, right reprojection error, sum of left reprojection error and right reprojection error). Based on this, the loss function can be determined. After obtaining the loss function, preferably, the key wide-field frame (or key binocular image frame) is repeatedly subjected to a gradient descent method such as the Gauss-Newton method (Gauss-Newton) or the Levenberg-Marquardt method (Levenberg-Marquardt). The gradient corresponding to each of the position and orientation and the position of the map point associated with it is obtained, and the position and orientation of the key wide-field frame (or key binocular image frame) and the position of the map point associated with it are determined by the corresponding gradient. Updates and finally puts the current map in an optimal state with minimal reprojection error.

The bundle optimization is based on a multi-virtual pinhole camera model similar to the distortion correction processing of a wide-field image, and converts a complex wide-field camera projection model into a plurality of virtual pinhole camera projection models. This avoids the complex optimization process of the complex projection model of the wide field camera, thereby improving the processing performance of the system.

At 830, the device 102, which simultaneously estimates the self-position and creates the environment map, can perform the tracking step. The tracking step optimizes the position and orientation of the current wide-field camera by minimizing the reprojection error of the map points in the current wide-field frame (or current binocular image frame). The tracking step optimizes only the current position and orientation of the wide-field camera and maintains the position and orientation of the wide-field camera and the position of the map points at other times. Step 830 may be performed at any time during map creation, for example, based on the initial map created in initialization step 810 or the map optimized in global bundle optimization step 820, SLAM is new. A wide-field frame (or binocular image frame) continuously tracks the position and orientation of the movement of the wide-field camera.

In the case of a monocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates an environment map projects each map point associated with the current wide-field frame onto the multi-virtual pinhole camera model, and the multi-virtual of the map point. The reprojection points in the pinhole camera model are acquired, and the reprojection error of the map points is determined based on the reprojection points of the map points in the multi-virtual pinhole camera model and the feature points corresponding to the map points. , The reprojection error is determined based on the reprojection error of the map points associated with all the current wide field frames, and the position and orientation of the current wide field frame is updated based on the reprojection error.

In some embodiments, the device 102, which simultaneously estimates the self-position and creates the environment map, can perform the tracking step by performing the following three sub-steps.
In tracking substep 1, the reference wide-field frame of the current wide-field frame is determined.
Preferably, the wide-field frame immediately preceding the current wide-field frame is determined as the reference wide-field frame.

Preferably, the key wide-field frame having the highest degree of common field of view with the current wide-field frame in the local map is selected as the reference wide-field frame. If the number of key wide-field frames in the current map is less than N (N is an integer greater than 2), the local map includes all key wide-field frames and all map points in the current map. N may directly use the default value, for example 10, or may be preset by the user. If the current map is a map obtained by initialization, the local map is the current map and contains the initial two key widefield frames and their associated map points. When the number of key wide-field frames in the current map is N or more, the local map has at least N key wide-field frames and at least the above-mentioned at least N key wide-field frames in the current map that have the highest degree of common field of view with the current wide-field frames. Includes map points associated with N-key wide-field frames.

Preferably, the key wide-field frame having the highest degree of common field of view with the wide-field frame immediately preceding the current wide-field frame in the local map is selected as the reference wide-field frame. Since the degree of common field of view between the current wide-field frame and the previous wide-field frame is generally high, the reference wide-field frame of the current wide-field frame can be selected based on the latter. Compared to the key wide-field frame that has the highest degree of common field of view with the current wide-field frame immediately after creation, the key wide-field frame that has the highest degree of common field of view with the previous wide-field frame is easier to select. .. Thereby, it is advantageous to realize the SLAM method smoothly.

Preferably, the reference wide-field frame is determined by global matching. First, we create a vector based on the word bag model based on the current wide-field frame. Next, the map database created in the initialization step 810 is queried based on the vector based on the bag model of the word, and the key wide-field frame matching the current wide-field frame is acquired as the reference wide-field frame. ..

In one example, the current wide-field frame and the previous wide-field frame are matched, and a matching feature point pair is acquired. If the number of matching feature point pairs is greater than the tracking threshold, the wide-field frame immediately preceding the current wide-field frame is determined as the reference wide-field frame. The tracking threshold indicates the minimum number of feature point pairs required for positioning and orientation tracking of the wide field camera, and a direct default value, for example 20, may be used or preset by the user.

If the number of matching feature point pairs between the current wide-field frame and the previous wide-field frame is less than or equal to the tracking threshold, then the current wide-field frame or the previous wide-field frame in the local map The key wide-field frame having the highest degree of common field of view is selected, the current wide-field frame is matched with the key wide-field frame, and a matching feature point pair is obtained. If the number of matching feature point pairs is greater than the tracking threshold, the key wide-field frame is determined as the reference wide-field frame.

When the number of matching feature point pairs between the current wide-field frame and the key wide-field frame is less than or equal to the tracking threshold, the reference wide-field frame is determined by global matching. The specific decision process is as described above, and for the sake of brevity, duplicate description will be omitted here.
As a result, it is possible to obtain a reference wide-field frame with higher referenceability of the current wide-field frame, SLAM tracking becomes more accurate, and mapping becomes more efficient.

In the tracking sub-step 2, the position and orientation of the current wide-field frame is determined based on the multi-virtual pinhole camera model by the current wide-field frame and the above-determined reference wide-field frame. In one example, the position and orientation of the current wide-field frame is determined by determining the relative position and orientation of the current wide-field frame and the reference wide-field frame.

Based on the multi-virtual pinhole camera model, the current wide-field frame is decomposed into sub-field frames corresponding to each virtual pinhole camera, and the same operation is performed for the reference wide-field frame. As a result, each virtual pinhole camera acquires two corresponding sub-field frames. Among the sub-field frame pairs corresponding to different virtual pinhole cameras, the sub-field frame pair having the largest number of matching feature point pairs is selected. By performing inter-frame matching on two sub-field frames in the sub-field frame pair, their relative positions and orientations are acquired. Since the specific inter-frame matching process for sub-field frames is the same as the inter-frame matching process in the initialization step 810, duplicate description will be omitted here for the sake of brevity.

Since the camera center of each virtual pinhole camera overlaps with the camera center of the wide-field camera, there is a constant rotation angle between each virtual pinhole camera and the wide-field camera in the multi-virtual pinhole camera model. The rotation angle of each virtual pinhole camera corresponds to one fixed rotation matrix. Thereby, the position / orientation matrix of the wide-field frame can be converted into the position / orientation matrix of the sub-field frame with the corresponding rotation matrix. On the other hand, the position / orientation matrix of the sub-field frame can also be converted into the position / orientation matrix of the wide-field frame by the corresponding rotation matrix.

The above technical proposal is a wide-field SLAM algorithm by converting position / orientation acquisition based on a complicated wide-field camera projection model into position / orientation acquisition based on a simple virtual pinhole camera projection model using a multi-virtual pinhole camera model. Is greatly simplified and the performance is greatly improved.
In the tracking sub-step 3, the position and orientation of the current wide-field frame acquired in the tracking sub-step 2 are updated.

The matching feature point pair of the current wide-field frame and the reference wide-field frame associates each matching feature point in the reference wide-field frame with the feature point based on the multi-virtual pinhole camera model. Converts the map points to the coordinate system of the corresponding virtual pinhole camera in the current wide-field frame. The map point is then projected onto the imaging plane of the virtual pinhole camera to obtain a reprojection point of the map point in the current wide field frame.

In one example, there is a large parallax between the current wide-field frame and the reference wide-field frame. Processing is performed based on the multi-virtual pinhole camera model of the five orientations shown in FIG. Specific matching feature points in the reference wide-field frame are on the imaging plane of the left-pointing virtual pinhole camera. The map points associated with the feature points are transformed based on the multi-virtual pinhole camera model and then associated with the coordinate system of the forward facing virtual pinhole camera of the current wide-field frame. The reprojection point of the map point is acquired on the imaging plane of the forward-looking virtual pinhole camera of the current wide-field frame. The map point can be observed by the left-facing virtual pinhole camera in the multi-virtual pinhole camera model in the position and orientation of the reference wide-field frame, and the forward-facing in the multi-virtual pinhole camera model in the position and orientation of the current wide-field frame. The map point can be observed by the virtual pinhole camera of.

The reprojection error of the map point is determined based on the reprojection point and the matching feature points in the current wide-field frame. Updates the current position and orientation of the wide-field frame based on the reprojection error of the map points associated with all matching feature points in the reference wide-field frame. The process of updating the position and orientation of the current wide-field frame based on the reprojection error calculation and the reprojection error in this step is the same as the process of global bundle optimization in step 820, so for the sake of brevity, here Then, the duplicate explanation is omitted.

By further optimizing and updating the position and orientation of the current wide-field frame, the reliability of the position and orientation of the current wide-field frame is improved and the tracking error is reduced. This makes SLAM tracking more accurate and mapping more efficient.

In the case of a binocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates an environment map projects each map point associated with the current binocular image frame onto the first multi-virtual pinhole camera model, and the above-mentioned map points. The reprojection points in the first multi-virtual pinhole camera model can be acquired, and the map points of the map points are based on the re-projection points of the map points and the feature points corresponding to the map points. The reprojection error can be determined and the left reprojection error can be determined based on the reprojection error of the map points associated with all said current binocular image frames.

Alternatively, the device 102 that simultaneously estimates the self-position and creates the environment map projects the map point onto the second multi-virtual pinhole camera model, and acquires the re-projected point of the map point in the second multi-virtual pinhole camera model. The reprojection error of the map point is determined based on the reprojection point of the map point in the second multi-virtual pinhole camera model and the feature point corresponding to the map point, and all the current binocular images of the current map point. The right reprojection error can be determined based on the reprojection error of the map points associated with the frame.

Further, the device 102 that simultaneously estimates the self-position and creates the environment map can update the current position and orientation of the binocular image frame based on the left reprojection error, the right reprojection error, or the sum thereof. For example, in the case of a monocular map point, the device 102 that simultaneously estimates the self-position and creates the environment map can update the position and orientation of the current binocular image frame based on the left reprojection error or the right reprojection error, and can update the position and orientation of the binocular map point. In this case, the device 102 that simultaneously estimates the self-position and creates the environment map can update the current position / orientation of the binocular image frame based on the sum of the left reprojection error and the right reprojection error.

Specifically, the device 102 that simultaneously estimates the self-position and creates the environment map obtains the left reprojection error, the right reprojection error, or the sum of the left reprojection error and the right reprojection error, and obtains the position of the wide-field camera 101. The posture increment can be determined, and then the current position and orientation of the wide-field camera 101 can be determined according to prior information.

In some embodiments, the prior information is the position / orientation of the previous frame wide-field camera 101, or the sum of the position / orientation of the previous frame wide-field camera 101 and the previous frame position / orientation increment. You may. The previous frame position / orientation increment is a position / orientation increment of the position / orientation of the previous frame wide-field camera 101 with respect to the position / orientation of the two-previous frame wide-field camera 101.

In some embodiments, the apparatus 102, which simultaneously estimates the self-position and creates the environment map, calculates the left projection error and / or the right projection error by the following plurality of equations to obtain the position / orientation increment. Equation (7) is as follows.

In the equation, P2 indicates the projection point of the map point P in the coordinate system of the multi-virtual pinhole camera model, and P1 indicates the projection point of the point P2 in the coordinate system of one surface of the multi-virtual pinhole camera model.
Therefore, the Jacobian matrix from u to the camera position and orientation can be derived by the chain rule. It is shown in the formula (9).

For the wide-field camera 101, the device 102 that simultaneously estimates the self-position and creates the environment map determines the left reprojection error of the wide-field camera 101 based on equations (7), (8), (9), and (10). The position and orientation of the wide-field camera 101 can be determined.

Based on the same principle, the device 102 that simultaneously estimates the self-position and creates the environment map determines the right reprojection error of the wide-field camera 101, and then the right reprojection error or the left reprojection error and the right. The position and orientation of the wide-field camera 101 can be determined based on the sum of the reprojection error.

In the 840, the device 102 that simultaneously performs self-position estimation and environment map creation can execute a map creation step (or a map update step). In the map creation step, the map is expanded according to the movement of the wide-field camera based on the current map. In other words, the map creation step inserts a new map point into the current map. Preferably, the mapping step 840 is performed after the tracking step 830 to determine its position or orientation in the tracking step 830 with respect to the current wide field frame (or current binocular image frame), i.e., at the current time point. The position and orientation of the movement of the wide-field camera can be determined.

In the case of a monocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates an environment map determines the feature points of the current wide-field frame and its reference frame that match each other, and the feature points of the current wide-field frame and the present The direction vector corresponding to the first feature point can be determined based on the camera center of the wide-field camera, and based on the matching feature point of the reference frame and the camera center of the wide-field camera corresponding to the reference frame. , The direction vector corresponding to the second feature point is determined, the direction vector corresponding to the first feature point and the direction vector corresponding to the second feature point are triangulated, and the map point corresponding to the feature point is obtained. Determine and create a map based on the map points.
In some embodiments, the device 102, which simultaneously estimates the self-position and creates the environment map, can perform the map creation step by performing three substeps.
In the map creation substep 1, it is determined whether or not the current wide-field frame is a key wide-field frame.

Since the wide-field camera moves continuously to collect data, performing a map update operation on each acquired wide-field frame results in a huge amount of calculation. Therefore, some important wide-field frames may be selected as the key wide-field frames, and the map update operation may be performed based on the key wide-field frames. Any known or future developed technology can be used to determine the key wide field frame. For example, based on the initial key wide-field frame, one out of every ten wide-field frames is selected as the key wide-field frame. That is, the 11th, 21st, 31st ... Are selected as the key wide-field frames. Further, for example, a wide-field frame having an appropriate parallax from the previous key wide-field frame is selected as the key wide-field frame.

If the current wide-field frame is a key wide-field frame, the process proceeds to the map update substep 2 and the map update process is performed based on the current wide-field frame. If the current wide-field frame is not the key wide-field frame, the process proceeds to the map update sub-step 3 and the map point association process is performed on the current wide-field frame.
In the map creation sub-step 2, when the current wide-field frame is the key wide-field frame, the map update process is performed based on the current wide-field frame.

For each key wide-field frame in the local map, based on the multi-virtual pinhole camera model, the key wide-field frame is decomposed into sub-field frames corresponding to each virtual pinhole camera, and the current wide field of view is obtained. Do the same for the frame. As a result, each virtual pinhole camera acquires two corresponding sub-field frames, and the two sub-field frames are matched between frames to create a new map point.

Preferably, in the process of performing inter-frame matching on the two sub-field frames, matching between feature points is accelerated by using a vector based on the word bag model. It detects whether or not the epipolar constraint is further satisfied for the feature point pair matched by the word bag model. For a feature point pair that satisfies the epipolar constraint, a three-dimensional coordinate point of a new map point is acquired by triangulation based on the feature point pair.

Here, the process of acquiring the three-dimensional coordinate points of the new map points by the inter-frame matching process of the subfield frame and the triangulation based on the feature point pair is the same as the corresponding process in the initialization step 810. For the sake of brevity, duplicate description is omitted here.

After creating a new map point, convert the new map point to a map point in the world coordinate system based on the position and orientation of the current wide-field frame and insert it into the current map to create the current wide-field frame. Insert into the current map. In general, the coordinate system of the first key wide-field frame for creating a map in initialization is the world coordinate system. After that, it is necessary to convert the camera coordinate system and the world coordinate system for the new map point.
Those skilled in the art will understand that the current map will gradually expand and complete with the constant insertion of new map points and new key wide-field frames.

Preferably, a vector based on the new word bag model is created based on the current wide field frame, and the vector based on the new word bag model is added to the map database. Based on the map database, vectors based on the word bag model speed up matching between feature points, thereby improving the efficiency of SLAM tracking and mapping.
In the map creation sub-step 3, if the current wide-field frame is not the key wide-field frame, the map point association process is performed on the current wide-field frame.

For each map point in the local map, the position and orientation of the current wide field of view frame, based on the multi-virtual pinhole camera model, the map point of the current wide field of view frame of the corresponding virtual pinhole camera. Convert to a coordinate system. Further, the map point is projected onto the imaging plane of the virtual pinhole camera to obtain a reprojection point of the map point in the current wide-field frame. If the projection fails, the map point cannot be observed in the current position and orientation of the wide-field frame. If the projection is successful, the map point can be observed in the current position and orientation of the wide-field frame, and the reprojection point of the map point is acquired. Of all the feature points in the current wide-field frame, the feature point that best matches the map point is selected from the feature points near the reprojection point and associated with the map point. Note that this step associates the current wide-field frame with map points that can be observed in the position and orientation of the current wide-field frame. As described above, when processing the next wide-field frame, the current wide-field frame is used for the tracking process as the wide-field frame immediately before the next wide-field frame. This makes SLAM tracking smoother, more accurate position estimation, and more accurate maps created.

In the case of a binocular wide-field camera, the device 102 that simultaneously estimates the self-position and creates an environment map may execute the map creation step of the monocular wide-field camera, and the left distortion correction image and the right distortion correction image at the same time point may be mutually executed. Maps may be created based on matching feature points.

In the latter case, the device 102 that simultaneously estimates the self-position and creates the environment map can determine the feature points that match the current left distortion correction image and the current right distortion correction image, and the current left distortion correction image. The direction vector corresponding to the first feature point is determined based on the feature point of the current right distortion correction image and the camera center of the left camera of the current binocular wide-field camera, and the feature point of the current right distortion correction image and the current binocular wide A direction vector corresponding to the second feature point is determined based on the camera center of the camera on the right side of the field camera, and a triangular survey is performed on the direction vector corresponding to the first feature point and the direction vector corresponding to the second feature point. This is performed, the map points corresponding to the feature points are determined, and a map is created based on the map points.

In some embodiments, in the case of a binocular wide-field camera, the apparatus 102, which simultaneously estimates the self-position and creates the environment map, refers to the related description in the initialization step 810, and the direction vector corresponding to the first feature point and the first The direction vector corresponding to the two feature points can be determined and triangulation can be performed.

If the current wide-field frame (or current binocular image frame) is a key wide-field frame (or key binocular image frame), mapping step 840 may further include local bundle optimization. Local bundle optimization fine-tunes the position orientation of the key wide-field frame (or key binocular image frame) in the local map and the position of the map points, and the key wide-field frame (or key binocular image) of the map points in the local map. The purpose is to minimize the reprojection error in the frame) and optimize the map created thereby.
In the case of a monocular wide-field camera, the bundle optimization process for each key wide-field frame in the local map is as follows.

For each map point associated with the key wide-field frame, the map point is converted into the coordinate system of the corresponding virtual pinhole camera based on the multi-virtual pinhole camera model, and further, the virtual pinhole camera The reprojection point of the map point is acquired by projecting onto the imaging plane of. Moreover, the reprojection error of the map point is determined based on the feature point associated with the map point and the reprojection point of the map point. Based on the reprojection error of all map points associated with the key wide-field frame, the position orientation of the key wide-field frame and the positions of all map points associated with the key wide-field frame are updated. Since the bundle optimization processing process of this step is the same as the processing process of the global bundle optimization step 820, duplicate description is omitted here for the sake of brevity.
In the case of a binocular wide-field camera, the bundle optimization process for each key binocular image frame in the local map is as follows.

The map point associated with the key binocular image frame is projected onto the first multi-virtual pinhole camera model, the re-projected point of the map point in the first multi-virtual pinhole camera model is acquired, and the map point is described. Based on the reprojection points in the first multi-virtual pinhole camera model and the feature points corresponding to the map points, the reprojection error of the map points is determined and of the map points associated with all the key binocular image frames. The left reprojection error is determined based on the reprojection error.

Alternatively, the map point associated with the key binocular image frame is projected onto the second multi-virtual pinhole camera model, the re-projected point of the map point in the second multi-virtual pinhole camera model is acquired, and the map point is obtained. Based on the reprojection points in the second multi-virtual pinhole camera model and the feature points corresponding to the map points, the reprojection error of the map points is determined and the map associated with all the key binocular image frames. The right reprojection error is determined based on the point reprojection error.

Further, it is associated with the position and orientation of the key binocular image frame and the key binocular image frame based on the left reprojection error, the right reprojection error, or the sum of the left reprojection error and the right reprojection error. Update the position of all map points.

At the 850, the device 102 that simultaneously estimates the self-position and creates the environment map can execute the closed loop detection processing step. The closed-loop detection processing steps of the monocular wide-field camera and the binocular wide-field camera may be the same, and the closed-loop detection processing of the monocular wide-field camera will be taken as an example below.

If the current wide-field frame is a key wide-field frame, a vector based on the word bag model detects closed-loop wide-field frames in the current map database that are similar to the current wide-field frame.

A matching feature point pair of the closed-loop wide-field frame and the current wide-field frame is determined. Preferably, a vector based on the word bag model is used to speed up matching between feature points.

Based on the matching feature point pair of the closed-loop wide-field frame and the current wide-field frame, the similarity transformation operator (Sim3 Solver) and the RANSAC algorithm are used to calculate the similarity transformation matrix between the closed-loop wide-field frame and the current wide-field frame. calculate.

For each matching feature point in the current wide-field frame, based on the multi-virtual pinhole camera model, the map points associated with the feature point are assigned to the corresponding virtual pinhole camera in the closed-loop wide-field frame. Convert to a coordinate system. Further, the map point is projected onto the imaging plane of the virtual pinhole camera to obtain a reprojection point of the map point in the closed-loop wide-field frame. The first reprojection error is determined based on the reprojection point and the matching feature points in the closed-loop wide-field frame. The first cumulative reprojection error is determined based on the first reprojection error of all matching feature points in the current wide-field frame.

For each matching feature point in the closed-loop wide-field frame, the map points associated with the feature point are assigned to the corresponding virtual pinhole camera in the current wide-field frame, based on the multi-virtual pinhole camera model. Convert to a coordinate system. Further, it is projected onto the imaging plane of the virtual pinhole camera to acquire the reprojection point of the map point in the current wide-field frame. The second reprojection error is determined based on the reprojection point and the matching feature points in the current wide-field frame. The second cumulative reprojection error is determined based on the second reprojection error of all matching feature points in the closed-loop wide-field frame.
The loss function is determined based on the first cumulative reprojection error and the second cumulative reprojection error. The similarity transformation matrix is optimized by minimizing the loss.

In order to eliminate the error accumulated in the closed-loop process, it is necessary to correct the key wide-field frame and its associated map points in the current map that have a common field-of-view relationship with the current wide-field frame. First, a key wide-field frame having a common field-of-view relationship with the current wide-field frame is acquired. If the number of common map points observed in the two wide-field frames is greater than the common-field relationship threshold, these two wide-field frames have a common-field relationship. The common field relationship threshold indicates the minimum number of common map points required to determine that two key wide field frames have a common field relationship, and a direct default value, such as 20, may be used. It may be preset by the user. Next, the position orientation of these key wide-field frames and the positions of the map points associated with these key wide-field frames are corrected by the similarity transformation matrix. Up to this point, the closed loop detection process is completed.

With the movement of the wide-field camera, an error occurs in both the position and orientation of the wide-field camera calculated by tracking and the position of the map point acquired by triangulation. Even if the optimization is performed by local bundle optimization or global bundle optimization, cumulative error occurs. The closed-loop detection process effectively eliminates cumulative error, which makes the SLAM-created map more accurate.

Preferably, the closed-loop detection process further comprises the step of further optimizing the position orientation of all key widefield frames and the positions of all map points in the current map by pose-graph optimization. Preferably, the closed-loop detection process further includes finding and deleting redundant keyframes and map points, thereby saving system storage space and avoiding redundant computational operations.

Steps 81 to 850 of the above embodiment provided an embodiment of step 230 of a wide field SLAM based on a multi-virtual pinhole camera model. Any known or future wide-field SLAM method can be adopted based on the distortion-corrected image acquired in step 220. For example, instead of the optimization update process for calculating the reprojection error based on the multi-virtual pinhole camera model, the optimization update process may be performed based on the unit direction vector error calculation. The error calculation based on the unit direction vector is a final optimization goal by minimizing the difference between the unit direction vector corresponding to the map point and the unit direction vector corresponding to the feature point associated with the map point. To achieve. The optimized target loss may be a distance between unit direction vectors, or an angle between unit vectors, or another index that describes a vector error.

Finally, the "left" and "right" mentioned in this application, such as "left eye", "right eye", "left fisheye image", "right fisheye image", "left distortion corrected image", "right" "Distorted Image", "Left Reprojection Error", and "Right Reprojection Error" are for illustration purposes only and do not limit the scope of protection for this application.

In summary, after reading this detailed disclosure, one of ordinary skill in the art can understand that the detailed disclosure described above may be presented as an example only and is not limiting. Although not explicitly stated herein, one of ordinary skill in the art can understand that the present invention is intended to cover various reasonable changes, improvements, and modifications to embodiments. These changes, improvements, and modifications are intended to be implied by this disclosure and are within the spirit and scope of the exemplary embodiments of this disclosure.

In addition, certain terms of the invention have been used to describe embodiments of the present disclosure. For example, "one embodiment", "embodiments", and / or "some embodiments" describe at least one embodiment of the present disclosure of a particular feature, structure, or property described in connection with an embodiment. It means that it can be included in the form. Therefore, it is emphasized that two or more references to "embodiments" or "one embodiment" or "alternative embodiments" in various parts of the specification do not necessarily refer to the same embodiment. Understandable. In addition, certain features, structures, or properties may be adequately combined in one or more embodiments of the present disclosure.

In the above description of embodiments of the present disclosure, the present invention sometimes combines various features in a single embodiment, drawing, or description thereof, for the purpose of simplifying the disclosure to aid in understanding the features. Should be understood. Alternatively, the invention disperses various features in a plurality of embodiments of the invention. However, this does not mean that a combination of these features is required, and one of ordinary skill in the art is likely to extract some of these features as separate embodiments when reading the invention. That is, the embodiment of the present invention can also be understood as the integration of a plurality of secondary embodiments. It is also true that the content of each secondary embodiment is less than all the features of a single previously disclosed embodiment.

In some embodiments, numbers representing quantities or properties used to describe and claim a particular embodiment of the invention are varied by the terms "about," "approximately," or "substantially." In some cases. For example, unless otherwise stated, "about," "approximately," or "substantially" may mean that the value described changes by ± 20%. Therefore, in some embodiments, the numerical parameters described in the description and the appended claims are approximate values that may vary depending on the desired properties required to be obtained by the particular embodiment. is there. In some embodiments, the numerical parameters should be interpreted by applying common rounding techniques based on the reported number of valid digits. While some embodiments described in the present invention estimate a wide range of numerical ranges and parameters, certain examples list the most accurate numerical values possible.

Each patent, patent application, publication of a patent application, and other material cited herein, such as articles, books, instructions, publications, documents, articles, etc., may be incorporated herein by reference. The entire content of all purposes except the history of the relevant prosecution document, those that are inconsistent with or may be inconsistent with this document, or the same prosecution document that may have a limiting effect on the widest range of claims. history. Currently or in the future, it will be associated with this document. For example, if there is a conflict or inconsistency in the description, definition, and / or use of a term associated with any of the materials contained in this document, the term will prevail.

Finally, it should be understood that the embodiments of the present invention disclosed herein are exemplary of the principles of the embodiments of the present invention. Other modified embodiments are also within the scope of the invention. Therefore, the embodiments disclosed in the present invention are merely examples and are not limited thereto. One of ordinary skill in the art can adopt an alternative configuration for carrying out the present invention in the present invention according to an embodiment of the present invention. Therefore, embodiments of the present invention are not limited to which embodiments are precisely described in the present invention.

Claims (20)

It is a method of performing self-position estimation and environment map creation at the same time.
The steps to acquire a wide-field image with a wide-field camera,
Based on the multi-virtual pinhole camera model, the step of acquiring the distortion-corrected image corresponding to the wide-field image, and
Including a step of determining the position and orientation of the wide-field camera and creating a map based on the distortion-corrected image.
The multi-virtual pinhole camera model includes at least two virtual pinhole cameras in different orientations, and the camera center of the at least two different orientations of the virtual pinhole camera overlaps the camera center of the wide-field camera. How to perform self-position estimation and environment map creation at the same time. The wide-field camera is a monocular wide-field camera, and the position and orientation of the wide-field camera are determined based on the distortion-corrected image, and a map is created. The above includes an initialization step, and the initialization step includes an initialization step. ,
The step of acquiring the distortion correction image corresponding to the first time point and the distortion correction image corresponding to the second time point,
A step of determining feature points that match the distortion correction image corresponding to the first time point and the distortion correction image corresponding to the second time point with each other.
The method of simultaneously performing self-position estimation and environment map creation according to claim 1, wherein an initial map is created based on feature points that match each other. Creating an initial map based on the feature points that match each other
A step of determining a direction vector corresponding to the first feature point based on the feature points in the distortion-corrected image corresponding to the first time point and the camera center of the wide-field camera at the first time point.
A step of determining a direction vector corresponding to the second feature point based on the matching feature points in the distortion-corrected image corresponding to the second time point and the camera center of the wide-field camera at the second time point.
A step of performing triangulation on the direction vector corresponding to the first feature point and the direction vector corresponding to the second feature point to determine a map point corresponding to the feature point.
The method of simultaneously performing self-position estimation and environment map creation according to claim 2, further comprising a step of creating an initial map based on the map points. The wide-field camera is a monocular wide-field camera, and the position and orientation of the wide-field camera are determined based on the distortion-corrected image, and a map is created. The above includes a global bundle optimization step, and the global bundle The optimization step is
For each key wide field frame in the map
Each map point associated with the key wide-field frame is projected onto the multi-virtual pinhole camera model, the re-projection point of the map point in the multi-virtual pinhole camera model is acquired, and the multi-virtual pin of the map point is obtained. Based on the reprojection points in the hall camera model and the feature points corresponding to the map points, the reprojection error of the map points is determined and based on the reprojection errors of the map points associated with all the key widefield frames. And the steps to determine the reprojection error
The first aspect of the present invention includes a step of updating the position and orientation of the key wide-field frame and the positions of all map points associated with the key wide-field frame based on the reprojection error. How to perform self-position estimation and environment map creation at the same time. The wide-field camera is a monocular wide-field camera, and the position and orientation of the wide-field camera are determined based on the distortion-corrected image, and a map is created. The above includes a tracking step, and the tracking step includes a tracking step.
For each map point associated with the current wide field frame
The map point is projected onto the multi-virtual pinhole camera model, the re-projection point of the map point in the multi-virtual pinhole camera model is acquired, the re-projection point of the map point in the multi-virtual pinhole camera model, and the above. The step of determining the reprojection error of the map point based on the feature points corresponding to the map points and determining the reprojection error based on the reprojection error of the map points associated with all the current widefield frames. When,
The method for simultaneously performing self-position estimation and environment map creation according to claim 1, further comprising a step of updating the position and orientation of the current wide-field frame based on the reprojection error. The wide-field camera is a monocular wide-field camera, and the position and orientation of the wide-field camera are determined based on the distortion-corrected image to create a map. The above includes a map creation step, and the map creation step includes a map creation step. ,
Steps to determine the matching feature points of the current wide-field frame and its reference frame,
A step of determining a direction vector corresponding to the first feature point based on the feature point of the current wide-field frame and the camera center of the current wide-field camera.
A step of determining a direction vector corresponding to the second feature point based on the matching feature points of the reference frame and the camera center of the wide-field camera corresponding to the reference frame.
A step of performing triangulation on the direction vector corresponding to the first feature point and the direction vector corresponding to the second feature point to determine a map point corresponding to the feature point.
The method for simultaneously performing self-position estimation and environment map creation according to claim 1, wherein a step of creating a map based on the map points is included. The mapping step further includes a local bundle optimization step, which includes the local bundle optimization step.
For each key wide field frame in the local map
Each map point associated with the key wide-field frame is projected onto the multi-virtual pinhole camera model, the re-projection point of the map point in the multi-virtual pinhole camera model is acquired, and the multi-virtual pin of the map point is obtained. Based on the reprojection points in the hall camera model and the feature points corresponding to the map points, the reprojection error of the map points is determined and based on the reprojection errors of the map points associated with all the key widefield frames. And the steps to determine the reprojection error
6. The sixth aspect of claim 6 comprises the step of updating the position and orientation of the key wide-field frame and the positions of all map points associated with the key wide-field frame based on the reprojection error. How to perform self-position estimation and environment map creation at the same time. The wide-field camera is a binocular wide-field camera.
The step of acquiring the left-field image and the right-field image by the binocular wide-field camera,
A step of acquiring a left distortion correction image corresponding to the left field of view image based on the first multi-virtual pinhole camera model, and
A step of acquiring a right distortion correction image corresponding to the right field of view image based on the second multi-virtual pinhole camera model, and
A step of determining the position and orientation of the binocular wide-field camera and creating a map based on the left distortion correction image and the right distortion correction image is included.
The first multi-virtual pinhole camera model includes at least two differently oriented virtual pinhole cameras, and the camera center of the at least two differently oriented virtual pinhole cameras is the camera center of the left camera of the binocular wide-field camera. Overlaps with
The second multi-virtual pinhole camera model includes at least two differently oriented virtual pinhole cameras, and the camera center of the at least two differently oriented virtual pinhole cameras is the camera center of the right camera of the binocular wide-field camera. The method for simultaneously performing self-position estimation and environment map creation according to claim 1, wherein the method overlaps with. Based on the left distortion correction image and the right distortion correction image, the position and orientation of the binocular wide-field camera is determined and a map is created. The above includes an initialization step, and the initialization step includes an initialization step.
A step of determining feature points that match the left distortion correction image and the right distortion correction image with each other,
The method of simultaneously performing self-position estimation and environment map creation according to claim 8, wherein an initial map is created based on the feature points that match each other. Determining the feature points that match the left distortion correction image and the right distortion correction image with each other
A step of determining a pole line corresponding to a feature point in the left distortion correction image in the right distortion correction image, and
A step of searching for a feature point matching the feature point in the left distortion corrected image on the pole line is included.
The method for simultaneously performing self-position estimation and environment map creation according to claim 9, wherein the pole line is a multi-line segment polygonal line. Creating an initial map based on the feature points that match each other
A step of determining a direction vector corresponding to the first feature point based on the feature points in the left distortion corrected image and the camera center of the left camera of the binocular wide-field camera.
A step of determining a direction vector corresponding to the second feature point based on the matching feature points in the right distortion corrected image and the camera center of the right camera of the binocular wide-field camera.
A step of performing triangulation on a direction vector corresponding to the first feature point and a direction vector corresponding to the second feature point based on the baseline of the binocular wide-field camera to determine a map point corresponding to the feature point. When,
The method of simultaneously performing self-position estimation and environment map creation according to claim 9, further comprising a step of creating an initial map based on the map points. Based on the left distortion correction image and the right distortion correction image, the position and orientation of the binocular wide-field camera is determined and a map is created. The above includes a global bundle optimization step, and the global bundle optimization step includes a global bundle optimization step. ,
For each key binocular image frame in the map
The map point associated with the key binocular image frame is projected onto the first multi-virtual pinhole camera model, the re-projected point of the map point in the first multi-virtual pinhole camera model is acquired, and the map point is described. Based on the reprojection points in the first multi-virtual pinhole camera model and the feature points corresponding to the map points, the reprojection error of the map points is determined and of the map points associated with all the key binocular image frames. The step of determining the left reprojection error based on the reprojection error, or projecting the map point associated with the key binocular image frame onto the second multi-virtual pinhole camera model and the second multi-virtual pin of the map point. The reprojection point in the hall camera model is acquired, and the reprojection error of the map point is determined based on the reprojection point of the map point in the second multi-virtual pinhole camera model and the feature point corresponding to the map point. And the step of determining the right reprojection error based on the reprojection error of the map points associated with all the key binocular image frames.
Based on the left reprojection error, the right reprojection error, or the sum of the left reprojection error and the right reprojection error, the position and orientation of the key binocular image frame and all associated with the key binocular image frame. The method of simultaneously performing self-position estimation and environment map creation according to claim 8, further comprising a step of updating the position of a map point. Based on the left distortion correction image and the right distortion correction image, the position and orientation of the binocular wide-field camera is determined and a map is created. The above includes a tracking step, and the tracking step includes a tracking step.
For each map point associated with the current binocular image frame
The map point is projected onto the first multi-virtual pinhole camera model, the re-projected point of the map point in the first multi-virtual pinhole camera model is acquired, and the map point is obtained in the first multi-virtual pinhole camera model. The reprojection error of the map point is determined based on the reprojection point and the feature points corresponding to the map point, and left reprojection based on the reprojection error of the map points associated with all the current binocular image frames. The step of determining the projection error, or the map point is projected onto the second multi-virtual pinhole camera model, the re-projection point of the map point in the second multi-virtual pinhole camera model is acquired, and the map point is described. Based on the reprojection points in the second multi-virtual pinhole camera model and the feature points corresponding to the map points, the reprojection error of the map points is determined and the map points associated with all the current binocular image frames. Steps to determine the right reprojection error based on the reprojection error of
It is characterized by including a step of updating the position and orientation of the current binocular image frame based on the left reprojection error, the right reprojection error, or the sum of the left reprojection error and the right reprojection error. The method for simultaneously performing self-position estimation and environment map creation according to claim 8. The position and orientation of the binocular wide-field camera are determined based on the left distortion correction image and the right distortion correction image, and a map is created. The map creation step includes a map creation step.
Steps to determine feature points that match the current left distortion corrected image and the current right distortion corrected image, and
A step of determining a direction vector corresponding to the first feature point based on the feature point of the current left distortion corrected image and the camera center of the left camera of the current binocular wide-field camera.
A step of determining a direction vector corresponding to the second feature point based on the feature point of the current right distortion corrected image and the camera center of the right camera of the current binocular wide-field camera.
A step of performing triangulation on the direction vector corresponding to the first feature point and the direction vector corresponding to the second feature point to determine a map point corresponding to the feature point.
The method of simultaneously performing self-position estimation and environment map creation according to claim 8, wherein a step of creating a map based on the map points is included. The mapping step further includes a local bundle optimization step, which includes the local bundle optimization step.
For each key binocular image frame in the local map
The map point associated with the key binocular image frame is projected onto the first multi-virtual pinhole camera model, the re-projected point of the map point in the first multi-virtual pinhole camera model is acquired, and the map point is described. Based on the reprojection points in the first multi-virtual pinhole camera model and the feature points corresponding to the map points, the reprojection error of the map points is determined and of the map points associated with all the key binocular image frames. The step of determining the left reprojection error based on the reprojection error, or projecting the map point associated with the key binocular image frame onto the second multi-virtual pinhole camera model and the second multi-virtual pin of the map point. The reprojection point in the hall camera model is acquired, and the reprojection error of the map point is determined based on the reprojection point of the map point in the second multi-virtual pinhole camera model and the feature point corresponding to the map point. And the step of determining the right reprojection error based on the reprojection error of the map points associated with all the key binocular image frames.
Based on the left reprojection error, the right reprojection error, or the sum of the left reprojection error and the right reprojection error, the position and orientation of the key binocular image frame and all associated with the key binocular image frame. The method of simultaneously performing self-position estimation and environment map creation according to claim 14, further comprising a step of updating the position of a map point. Based on the distortion-corrected image, the position and orientation of the wide-field camera and the map are created. The above includes a closed-loop detection processing step, and the closed-loop detection processing step includes a closed-loop detection processing step.
If the current wide-field frame is a key wide-field frame, the steps to determine a closed-loop wide-field frame similar to the current wide-field frame in the map database,
A step of determining feature points that match each other between the current wide-field frame and the closed-loop wide-field frame, and
For each matching feature point in the current wide-field frame, the map points associated with the feature point are converted into the coordinate system of the multi-virtual pinhole camera model corresponding to the closed-loop wide-field frame, and further described. Projected onto the imaging plane of the multi-virtual pinhole camera model, the reprojected points of the map points in the closed-loop wide-field frame are acquired, and the reprojected points and the matching feature points in the closed-loop wide-field frame are the first. 1 Steps to determine the reprojection error and
A step of determining the first cumulative reprojection error based on the first reprojection error of all matching feature points in the current wide-field frame.
For each matching feature point in the closed-loop wide-field frame, the map points associated with the feature point are converted to the coordinate system of the multi-virtual pinhole camera model corresponding to the current wide-field frame, and further. Projected onto the imaging plane of the multi-virtual pinhole camera model, the reprojected points of the map points in the current wide-field frame are acquired, and the re-projected points and matching feature points in the current wide-field frame are used. Steps to determine the second reprojection error based on
A step of determining the second cumulative reprojection error based on the second reprojection error of all matching feature points in the closed-loop wide-field frame.
Using the first cumulative reprojection error and the second cumulative reprojection error, the key wide-field frame having a common field-of-view relationship with the current wide-field frame in the map and the map points associated therewith are corrected. The method of simultaneously performing self-position estimation and environment map creation according to claim 1, wherein the step and the step are included. The method of simultaneously performing self-position estimation and environment map creation according to claim 1, wherein the at least two different orientations include forward, upward, downward, left or right orientation of the cube. It is a device that simultaneously estimates the self-position and creates an environment map.
With at least one storage device containing a set of instructions
A device comprising at least one processor communicating with the at least one storage device and executing the set of instructions, the at least one processor simultaneously performing the self-position estimation and the environment map creation.
The steps to acquire a wide-field image with a wide-field camera,
Based on the multi-virtual pinhole camera model, the step of acquiring the distortion-corrected image corresponding to the wide-field image, and
Based on the distortion-corrected image, the steps of determining the position and orientation of the wide-field camera and creating a map are executed.
The multi-virtual pinhole camera model includes at least two differently oriented virtual pinhole cameras, and the camera center of the at least two differently oriented virtual pinhole cameras is self-positioning that overlaps the camera center of the wide-field camera. A device that creates an environment map at the same time. The wide-field camera is a monocular wide-field camera, and in order to determine the position and orientation of the wide-field camera and create a map based on the distortion-corrected image, the at least one processor further estimates the self-position. And the device that creates the environment map at the same time is made to execute the initialization step, and the initialization step is
The step of acquiring the distortion correction image corresponding to the first time point and the distortion correction image corresponding to the second time point,
A step of determining feature points that match the distortion correction image corresponding to the first time point and the distortion correction image corresponding to the second time point with each other.
The device for simultaneously performing self-position estimation and environment map creation according to claim 18, further comprising a step of creating an initial map based on the feature points matching each other. A non-transitory computer-readable medium, including a computer program product, said computer program product containing several instructions, the instructions to a computing device.
The steps to acquire a wide-field image with a wide-field camera,
Based on the multi-virtual pinhole camera model, the step of acquiring the distortion-corrected image corresponding to the wide-field image, and
Based on the distortion-corrected image, the steps of determining the position and orientation of the wide-field camera and creating a map are executed.
The multi-virtual pinhole camera model includes at least two differently oriented virtual pinhole cameras, and the camera center of the at least two differently oriented virtual pinhole cameras is non-temporary overlapping the camera center of the wide-field camera. Computer readable medium.