阅读说明：本技术 视觉slam的增量建图方法、装置、机器人及可读存储介质 (Incremental mapping method and device for visual SLAM, robot and readable storage medium ) 是由 李一龙 范圣印 刘志励 王璀 张煜东 于 2021-09-29 设计创作，主要内容包括：本申请涉及一种视觉SLAM的增量建图方法、装置、机器人及可读存储介质,其中,所述方法包括：基于传感器数据获取视觉特征和里程计数据；获取第一地图,并基于所述第一地图、所述视觉特征和所述里程计数据对第二地图进行初始化；利用当前帧在所述第一地图和初始化后的第二地图中分别进行协同定位,得到第一定位信息和第二定位信息；基于所述第二定位信息对所述初始化后的第二地图进行融合建图,得到第三定位信息；基于所述第一定位信息和所述第三定位信息对融合建图后的第二地图进行矫正,得到增量地图。本申请具有改善增量建图中原有地图尺度改变、新建地图不收敛的效果。(The application relates to an incremental mapping method, an incremental mapping device, a robot and a readable storage medium for visual SLAM, wherein the method comprises the following steps: obtaining visual characteristics and odometry data based on the sensor data; acquiring a first map, and initializing a second map based on the first map, the visual features and the odometry data; performing cooperative positioning on the first map and the initialized second map by using the current frame to obtain first positioning information and second positioning information; performing fusion map building on the initialized second map based on the second positioning information to obtain third positioning information; and correcting the second map after the map is built by fusion based on the first positioning information and the third positioning information to obtain an incremental map. The method and the device have the effects of improving the original map scale change in the incremental map building and the unconvergence of the newly built map.)

1. An incremental mapping method for visual SLAM, comprising:

obtaining visual characteristics and odometry data based on the sensor data;

acquiring a first map, and initializing a second map based on the first map, the visual features and the odometry data;

performing cooperative positioning on the first map and the initialized second map by using the current frame to obtain first positioning information and second positioning information;

performing fusion map building on the initialized second map based on the second positioning information to obtain third positioning information;

and correcting the second map after the map is built by fusion based on the first positioning information and the third positioning information to obtain an incremental map.

2. The method of claim 1, wherein initializing a second map based on the first map, the visual feature, and the odometry data comprises:

constructing a fixed area of a second map based on the keyframes and map points in the first map;

creating a temporary map based on the visual features and the odometry data;

relocating in the second map based on the visual feature;

if the position reliability of the current frame in the second map exceeds a first preset threshold tau, aligning the second map with the temporary map;

and fusing and replacing the second map subjected to the alignment processing with the temporary map.

3. The method of claim 2, wherein the building a temporary map based on the visual features and the odometry data comprises:

semantic matching and geometric constraint inspection are carried out on the image feature points of the current frame and the previous frame to obtain a first matching relation;

calculating the relative pose provided by the odometer corresponding to the current frame and the previous frame based on the first matching relation, and initializing the temporary map by using a triangulation method;

and establishing key frames and map points of the newly added area of the second map by utilizing the matching relation between the current frame and the reference key frame and the relative poses provided by the odometer corresponding to the current frame and the reference key frame.

4. The method of claim 2, wherein the relocating in the second map based on the visual feature comprises:

searching a key frame similar to the current frame as a candidate key frame, and performing semantic matching and geometric constraint inspection on image feature points of the current frame and the candidate key frame to obtain a second matching relation;

calculating the relative pose provided by the odometer corresponding to the current frame and the candidate key frame based on the second matching relation;

and acquiring a local map of the second map based on the relative pose provided by the odometer corresponding to the current frame and the candidate key frame, and optimizing the pose of the current frame based on the local map.

5. The method of claim 2, wherein the aligning the second map with the temporary map if the position reliability of the current frame in the second map exceeds a first preset threshold τ comprises:

if the number of map points observed in the second map by the current frame exceeds the first preset threshold tau, the positioning pose in the second map based on the current frame is determinedAnd the pose of the current frame in the temporary mapCalculating the relative pose of the temporary map and the second map

Based on the relative poseRotating the temporary map to be under the coordinate system of the second map.

6. The method according to any one of claims 1 to 5, wherein the obtaining the first positioning information and the second positioning information by using the current frame to perform the co-positioning in the first map and the initialized second map respectively comprises:

positioning on the first map by using the visual features to obtain the positioning information of the current frame in the first map

If the location informationDegree of confidence inGreater than a second predetermined threshold τ_CThen position information will be locatedAs initial positioning information of the current frame in the second mapInitial positioning information using a partial map of a second mapOptimizing to obtain the positioning information of the current frame in the second map

If the location informationDegree of confidence inIs not greater than a second preset threshold tau_CThen, based on the visual characteristic and the odometry data, obtaining an initial pose of the current frame in the second map, and positioning by using a matching relation between a local map of the second map and the current frame on the basis of the initial pose to obtain positioning information of the current frame in the second map

Positioning informationAs initial positioning information of the current frame in the first mapInitial positioning information using a partial map of a first mapOptimizing to obtain the positioning information of the current frame in the first map

If the location informationDegree of confidence inGreater than confidenceThen the positioning information will beAs the first positioning information, and positioning informationAs the second positioning information.

7. The method of claim 3, wherein the fusing the initialized second map based on the second positioning information to obtain third positioning information comprises:

establishing map points of the newly added area based on the second positioning information and the key frames in the fixed area;

establishing map points of the newly added area based on the second positioning information and the key frame of the newly added area;

if the map points and/or the key frames of the newly added area accord with preset redundancy conditions, deleting the map points and/or the key frames which accord with the preset redundancy conditions;

and carrying out local optimization on the pose of the newly added region to obtain the third positioning information.

8. The method of claim 7, wherein the establishing the map point of the newly added area based on the second positioning information and the keyframes in the fixed area comprises:

matching image feature points of the current key frame and adjacent key frames in the fixed area based on semantic conditions and geometric constraint conditions;

for two successfully matched image feature points, if the map points corresponding to the two image feature points are both empty, obtaining map points p1 corresponding to the two image feature points based on the positions of the two image feature points in the two frames of images and the poses of the two frames of images, assigning the map points p1 to the two image feature points, and adding the map points p1 into the newly added region; if the map points corresponding to the two image feature points are empty and only one map point is empty, assigning a non-empty value to the map point with the empty value; if the map points corresponding to the two image feature points are both non-empty, judging whether map points belonging to a fixed area exist in the map points corresponding to the two image feature points, if so, replacing another map point by the map point belonging to the fixed area, and if not, optionally replacing another map point by one map point.

9. The method of claim 7, wherein the establishing the map point of the newly added region based on the second positioning information and the keyframe of the newly added region comprises:

matching the image feature points of the current key frame and the adjacent key frames in the newly added region based on semantic conditions and geometric constraint conditions;

for two successfully matched image feature points, if the map points corresponding to the two image feature points are both empty, obtaining map points p2 corresponding to the two image feature points based on the positions of the two image feature points in the two frames of images and the poses of the two frames of images, assigning the map points p2 to the two image feature points, and adding the map points p2 into the newly added region; if the map points corresponding to the two image feature points are empty and only one map point is empty, assigning a non-empty value to the map point with the empty value; if the map points corresponding to the two image feature points are both non-empty, judging whether map points belonging to the newly added area exist in the map points corresponding to the two image feature points, if so, replacing another map point by the map point belonging to the newly added area, and if not, optionally replacing another map point by one map point.

10. The method according to claim 7, wherein if the map points and/or the key frames of the newly added area meet a preset redundancy condition, the deleting process for the map points and/or the key frames meeting the preset redundancy condition comprises:

traversing the map points of the newly added area, and deleting the map points if the visibility rate of the map points is lower than a third preset threshold; and/or the presence of a gas in the gas,

and traversing the key frame of the newly added area, and removing the key frame if the visual map point of the key frame and other key frames have a preset overlapping area.

11. The method according to any one of claims 1 to 5 and 7 to 10, wherein the rectifying the second map after the fused map based on the first positioning information and the third positioning information to obtain an incremental map comprises:

if the confidence of the current key frame in the first positioning information is not less than a fourth preset threshold, judging whether the difference between the first positioning information and the third positioning information of the current key frame meets a preset difference condition;

if so, setting the positioning pose of the current key frame in the first map as the pose of the current key frame in the second map, and finishing pose alignment;

updating the visual relationship of the second map based on the visual relationship of the current keyframe to the visual map point in the first map;

and performing a dose graph optimization and a global optimization on the newly added region.

12. The method according to claim 11, wherein the preset difference condition comprises at least one of the following conditions:

the translation amount difference of the poses in the first positioning information and the third positioning information exceeds a fourth preset threshold;

the difference between the rotation angles of the poses in the first positioning information and the third positioning information exceeds a preset angle;

and the ratio of the number of map points observed by the current key frame in the fixed area of the second map to the absolute value of the number of map points observed by the current key frame in the first map is less than a fifth preset threshold value.

13. An incremental mapping apparatus for visual SLAM, comprising:

an acquisition module for acquiring visual characteristics and odometer data based on sensor data;

the initialization module is used for acquiring a first map and initializing a second map based on the first map, the visual features and the odometry data;

the cooperative positioning module is used for respectively performing cooperative positioning on the first map and the initialized second map by using the current frame to obtain first positioning information and second positioning information;

a fused map building module, configured to perform fused map building on the initialized second map based on the second positioning information, so as to obtain third positioning information;

and the correction module is used for correcting the second map after the map is built by fusion based on the first positioning information and the third positioning information to obtain the incremental map.

14. A robot comprising a memory, a processor, a communication bus, a communication interface, and a sensor device;

the memory, the processor and the communication interface are connected through the communication bus;

the sensor equipment is connected with the communication interface and used for acquiring sensor data;

the memory has stored thereon a computer program that can be loaded by the processor and that executes the method according to any of claims 1 to 12.

15. A computer-readable storage medium, in which a computer program is stored which can be loaded by a processor and which executes the method of any one of claims 1 to 12.

Technical Field

The present application relates to the field of computer image processing technologies, and in particular, to an incremental mapping method and apparatus for a visual SLAM, a robot, and a readable storage medium.

Background

In recent years, SLAM technology is widely applied to the fields of automatic driving, automatic robots and the like, and is used for solving the problems of self positioning and mapping in a position scene. Compared with a laser SLAM technology, the visual SLAM technology has the characteristics of low sensor cost and rich information, but cannot provide the scale difference with a real space, and has the phenomenon of uneven scale distribution.

In order to solve the problem of uneven scale distribution generated by the visual SLAM, a visual incremental mapping technology is adopted, and external observations such as a wheel encoder, an IMU, an RTK and the like are generally selected as the basis for scale recovery and participate in the processes of pose calculation and map optimization so as to reduce the scale difference of the visual map as much as possible.

However, the existing visual incremental map building technology can change the original map scale, and needs to re-build a navigation map generated based on a visual map, thereby causing the waste of computing resources; and when the external observation is selected to carry out scale restoration on the pose of the visual map or incremental map building is carried out on the visual map with poor positioning effect, the newly-built key frame is changed due to scene, illumination and observation and is separated from the incidence relation with the original map, so that the newly-built map is not converged and the phenomenon of map bifurcation is generated.

Disclosure of Invention

In order to solve the problems of scale change of an original map and non-convergence of a newly-built map in an incremental mapping, the application provides an incremental mapping method, an incremental mapping device, a robot and a readable storage medium for a visual SLAM.

In a first aspect, the present application provides an incremental mapping method for a visual SLAM, which adopts the following technical scheme:

an incremental mapping method for visual SLAM comprises the following steps:

obtaining visual characteristics and odometry data based on the sensor data;

acquiring a first map, and initializing a second map based on the first map, the visual features and the odometry data;

performing cooperative positioning on the first map and the initialized second map by using the current frame to obtain first positioning information and second positioning information;

performing fusion map building on the initialized second map based on the second positioning information to obtain third positioning information;

and correcting the second map after the map is built by fusion based on the first positioning information and the third positioning information to obtain an incremental map.

Optionally, the initializing a second map based on the first map, the visual feature, and the odometry data comprises:

constructing a fixed area of a second map based on the keyframes and map points in the first map;

creating a temporary map based on the visual features and the odometry data;

relocating in the second map based on the visual feature;

if the position reliability of the current frame in the second map exceeds a first preset threshold tau, aligning the second map with the temporary map;

and fusing and replacing the second map subjected to the alignment processing with the temporary map.

Optionally, the creating a temporary map based on the visual features and the odometry data comprises:

semantic matching and geometric constraint inspection are carried out on the image feature points of the current frame and the previous frame to obtain a first matching relation;

calculating the relative pose provided by the odometer corresponding to the current frame and the previous frame based on the first matching relation, and initializing the temporary map by using a triangulation method;

and establishing key frames and map points of the newly added area of the second map by utilizing the matching relation between the current frame and the reference key frame and the relative poses provided by the odometer corresponding to the current frame and the reference key frame.

Optionally, the relocating in the second map based on the visual features comprises:

searching a key frame similar to the current frame as a candidate key frame, and performing semantic matching and geometric constraint inspection on image feature points of the current frame and the candidate key frame to obtain a second matching relation;

calculating the relative pose provided by the odometer corresponding to the current frame and the candidate key frame based on the second matching relation;

and acquiring a local map of the second map based on the relative pose provided by the odometer corresponding to the current frame and the candidate key frame, and optimizing the pose of the current frame based on the local map.

Optionally, if the position reliability of the current frame in the second map exceeds a first preset threshold τ, aligning the second map with the temporary map includes:

if the number of map points observed in the second map by the current frame exceeds the first preset threshold tau, the positioning pose in the second map based on the current frame is determinedAnd the pose of the current frame in the temporary mapCalculating the relative pose of the temporary map and the second map

Based on the relative poseRotating the temporary map to be under the coordinate system of the second map.

Optionally, the obtaining the first positioning information and the second positioning information by using the current frame to perform the co-positioning in the first map and the initialized second map respectively includes:

positioning on the first map by using the visual features to obtain the positioning information of the current frame in the first map

If the location informationDegree of confidence inGreater than a second predetermined threshold τ_CThen position information will be locatedAs initial positioning information of the current frame in the second mapInitial positioning information using a partial map of a second mapOptimizing to obtain the positioning information of the current frame in the second map

If the location informationDegree of confidence inIs not greater than a second preset threshold tau_CThen, based on the visual characteristic and the odometry data, obtaining an initial pose of the current frame in the second map, and positioning by using a matching relation between a local map of the second map and the current frame on the basis of the initial pose to obtain positioning information of the current frame in the second map

Positioning informationAs initial positioning information of the current frame in the first mapInitial positioning information using a partial map of a first mapOptimizing to obtain the positioning information of the current frame in the first map

If the location informationDegree of confidence inGreater than confidenceThen the positioning information will beAs the first positioning information, and positioning informationAs the second positioning information.

Optionally, the fusing and map building the initialized second map based on the second positioning information to obtain third positioning information includes:

establishing map points of the newly added area based on the second positioning information and the key frames in the fixed area;

establishing map points of the newly added area based on the second positioning information and the key frame of the newly added area;

if the map points and/or the key frames of the newly added area accord with preset redundancy conditions, deleting the map points and/or the key frames which accord with the preset redundancy conditions;

and carrying out local optimization on the pose of the newly added region to obtain the third positioning information.

Optionally, the establishing a map point of the newly added region based on the second positioning information and the keyframe in the fixed region includes:

matching image feature points of the current key frame and adjacent key frames in the fixed area based on semantic conditions and geometric constraint conditions;

for two successfully matched image feature points, if the map points corresponding to the two image feature points are both empty, obtaining map points p1 corresponding to the two image feature points based on the positions of the two image feature points in the two frames of images and the poses of the two frames of images, assigning the map points p1 to the two image feature points, and adding the map points p1 into the newly added region; if the map points corresponding to the two image feature points are empty and only one map point is empty, assigning a non-empty value to the map point with the empty value; if the map points corresponding to the two image feature points are both non-empty, judging whether map points belonging to a fixed area exist in the map points corresponding to the two image feature points, if so, replacing another map point by the map point belonging to the fixed area, and if not, optionally replacing another map point by one map point.

Optionally, the establishing a map point of the newly added region based on the second positioning information and the keyframe of the newly added region includes:

matching the image feature points of the current key frame and the adjacent key frames in the newly added region based on semantic conditions and geometric constraint conditions;

for two successfully matched image feature points, if the map points corresponding to the two image feature points are both empty, obtaining map points p2 corresponding to the two image feature points based on the positions of the two image feature points in the two frames of images and the poses of the two frames of images, assigning the map points p2 to the two image feature points, and adding the map points p2 into the newly added region; if the map points corresponding to the two image feature points are empty and only one map point is empty, assigning a non-empty value to the map point with the empty value; if the map points corresponding to the two image feature points are both non-empty, judging whether map points belonging to the newly added area exist in the map points corresponding to the two image feature points, if so, replacing another map point by the map point belonging to the newly added area, and if not, optionally replacing another map point by one map point.

Optionally, if the map points and/or the key frames of the newly added area meet a preset redundancy condition, the deleting process of the map points and/or the key frames meeting the preset redundancy condition includes:

traversing the map points of the newly added area, and deleting the map points if the visibility rate of the map points is lower than a third preset threshold; and/or the presence of a gas in the gas,

and traversing the key frame of the newly added area, and removing the key frame if the visual map point of the key frame and other key frames have a preset overlapping area.

Optionally, the correcting the second map after the fused map is built based on the first positioning information and the third positioning information to obtain the incremental map includes:

if the confidence of the current key frame in the first positioning information is not less than a fourth preset threshold, judging whether the difference between the first positioning information and the third positioning information of the current key frame meets a preset difference condition;

if so, setting the positioning pose of the current key frame in the first map as the pose of the current key frame in the second map, and finishing pose alignment;

updating the visual relationship of the second map based on the visual relationship of the current keyframe to the visual map point in the first map;

and performing a dose graph optimization and a global optimization on the newly added region.

Optionally, the preset difference condition includes at least one of the following conditions:

the translation amount difference of the poses in the first positioning information and the third positioning information exceeds a fourth preset threshold;

the difference between the rotation angles of the poses in the first positioning information and the third positioning information exceeds a preset angle;

and the ratio of the number of map points observed by the current key frame in the fixed area of the second map to the absolute value of the number of map points observed by the current key frame in the first map is less than a fifth preset threshold value.

In a second aspect, the present application provides an incremental mapping apparatus for visual SLAM, which adopts the following technical solution:

an incremental mapping device for visual SLAM, comprising:

an acquisition module for acquiring visual characteristics and odometer data based on sensor data;

the initialization module is used for acquiring a first map and initializing a second map based on the first map, the visual features and the odometry data;

the cooperative positioning module is used for respectively performing cooperative positioning on the first map and the initialized second map by using the current frame to obtain first positioning information and second positioning information;

a fused map building module, configured to perform fused map building on the initialized second map based on the second positioning information, so as to obtain third positioning information;

and the correction module is used for correcting the second map after the map is built by fusion based on the first positioning information and the third positioning information to obtain the incremental map.

In a third aspect, the present application provides a robot, which adopts the following technical solution:

a robot comprising a memory, a processor, a communication bus, a communication interface, and a sensor device;

the memory, the processor and the communication interface are connected through the communication bus;

the sensor equipment is connected with the communication interface and used for acquiring sensor data;

the memory has stored thereon a computer program that can be loaded by the processor and that performs the method of any of the first aspects.

In a fourth aspect, the present application provides a computer-readable storage medium, which adopts the following technical solutions:

a computer readable storage medium storing a computer program that can be loaded by a processor and executed to perform the method of any of the first aspects.

By adopting the technical scheme, the function of incremental map building is realized on the premise of not changing the scale distribution of the original visual map, and the support of incremental map building is provided for data which can be strongly positioned on the original map through cooperative positioning and fusion map building; for data which are weakly positioned or lost on the original map, the correction module is used for aligning the scale distribution of the newly added map to the scale distribution of the original map, the phenomena of unconvergence, bifurcation and the like of incremental map construction caused by uneven scale distribution are avoided, and the quality of the visual incremental map construction is improved.

Drawings

Fig. 1 is a schematic flowchart of an incremental mapping method of a visual SLAM according to an embodiment of the present disclosure.

Fig. 2 is a flowchart illustrating a sub-step of step S200 in the incremental mapping method for visual SLAM according to an embodiment of the present disclosure.

Fig. 3 is a flowchart illustrating the sub-step of step S400 in the incremental mapping method for visual SLAM according to the embodiment of the present application.

Fig. 4 is a block diagram of an incremental mapping apparatus of a visual SLAM according to an embodiment of the present application.

Fig. 5 is a block diagram of a robot according to an embodiment of the present application.

Detailed Description

In order to make the objects, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application.

Fig. 1 is a schematic flowchart of an incremental mapping method of a visual SLAM according to this embodiment. As shown in fig. 1, the main flow of the method is described as follows (steps S100 to S500):

step S100, acquiring visual characteristics and odometer data based on sensor data;

in this embodiment, the sensor data includes image data and scale observation data. The image form of the image data can be monocular, binocular or multi-view vision, and can be acquired by image acquisition devices such as a common camera and a fish-eye camera.

Visual descriptors are extracted from image data, wherein the visual features can be FAST corners, Harris corners, SIFT, SURF and variants thereof, and the descriptors can be selected from HOG descriptors, BRIEF descriptors and the like, and deep learning descriptors can also be used. In order to ensure the descriptor extraction efficiency, a combination of FAST corners and BRIEF descriptors can be selected for describing the visual features.

The scale observation data is acquired by a scale observation odometer, the scale observation odometer can be formed by one or more devices of a wheel type encoder, the wheel type encoder, an inertia measurement unit, an RTK and a GPS, and the scale observation odometer can be formed as long as the odometer with real scale information can be formed, and the embodiment is not particularly limited.

Taking a wheel type encoder and an inertia measurement unit as an example, the wheel type encoder is used for collecting displacement, the inertia measurement unit is used for collecting rotation, the displacement and the rotation form an odometer, and odometer data is used as an effective sensor for relative positioning of the mobile robot, so that real-time pose information is provided for the robot, and the odometer is used for subsequent scale recovery.

Step S200, acquiring a first map, and initializing a second map based on the first map, the visual features and the odometry data;

in this embodiment, an original map, which is a reference map for incremental map creation, is referred to as a first map, and an incremental map is referred to as a second map. The first map is provided by other mapping modules, and is constructed by mapping data such as images, wheel speeds and the like.

The starting method for initializing the second map into the incremental map building only needs to meet the condition that the current frame obtains unique positioning with high confidence level on the first map.

As an alternative implementation manner of this embodiment, as shown in fig. 2, step S200 includes:

step S210, constructing a fixed area of a second map based on the key frames and map points in the first map;

the second map is initialized by using the first map, and the key frames and map points in the first map are copied into the second map to be the fixed area of the second map, so that the elements of the fixed area of the first map and the fixed area of the second map have one-to-one correspondence relationship.

Step S220, establishing a temporary map based on the visual characteristics and the odometry data;

semantic matching and geometric constraint inspection are carried out on the image feature points of the current frame and the previous frame to obtain a first matching relation; calculating the relative pose provided by the odometer corresponding to the current frame and the previous frame based on the first matching relation, and initializing the temporary map by using a triangulation method; and establishing key frames and map points of the newly added area of the second map by utilizing the matching relation between the current frame and the reference key frame and the relative poses provided by the odometer corresponding to the current frame and the reference key frame.

In this embodiment, the frame represents a data structure generated by each frame image, and the key frame refers to a frame stored in the map after being filtered. Although the frame is not present in the map, the frame can be located on the map. For a frame, the key frame with the most common view with it is called a reference key frame.

Step S230, repositioning in a second map based on the visual characteristics;

firstly, a key frame similar to the current frame is searched as a candidate key frame, and semantic matching and geometric constraint inspection are carried out on image feature points of the current frame and the candidate key frame to obtain a second matching relation. The candidate keyframes may be screened by methods such as a bag-of-words model and a VLAD series method, which are not specifically limited in this embodiment.

Then, calculating the relative pose provided by the odometer corresponding to the current frame and the candidate key frame based on the second matching relation; specifically, the relative pose may be calculated by using a method of RANSAC + PnP, which is not specifically limited in this embodiment.

And then acquiring a local map of the second map based on the relative pose provided by the odometer corresponding to the current frame and the candidate key frame, and optimizing the pose of the key frame based on the local map to complete the repositioning operation.

Step S240, if the position reliability of the current frame in the second map exceeds a first preset threshold value tau, aligning the second map with the temporary map;

in this embodiment, the number of map points observed in the second map by the current frame may be selected as a method for calculating the location reliability, that is, if the number of map points observed in the second map by the current frame exceeds the first preset threshold τ, and the location has a high reliability, the method is based on the location pose of the current frame in the second mapAnd the pose of the current frame in the temporary mapCalculating the relative pose of the temporary map and the second mapBased on relative poseAnd rotating the temporary map to be under the coordinate system of the second map.

Optionally, the first preset threshold τ is calculated as follows:

positioning on the first map by using map building data of the first map, and recording the number of map points observed in each frame to generate a set N; mean value mu based on set N_NAnd variance σ_NCalculating a first preset threshold τ by the formula τ ═ μ_N-3*σ_N. However, since the positioning operation is performed only on the first map, only the image data in the map data need be createdCan be prepared.

And when the positioning position reliability does not meet the condition, namely the number of map points observed in the second map by the current frame does not exceed the first preset threshold tau, repeating the operation until positioning with high reliability is obtained, and then performing alignment processing.

And step S250, fusing and replacing the second map after the alignment processing and the temporary map.

Set of map points observed in a second map using a current frameSet of map points observed in the temporary map with the current frameAnd performing fusion, replacement and local optimization to complete initialization of the second map.

Wherein N is the number of feature points extracted from the current frame,andrepresenting map points in a second map, temporary map, associated with the ith feature point in the current frame, when the ith feature point of the current frame does not have a corresponding map point in the temporary map,is empty.

Step S300, performing cooperative positioning in the first map and the initialized second map by using the current frame to obtain first positioning information and second positioning information;

the method comprises the following steps of obtaining a first map scale, obtaining an initial pose of a second map according with the first map scale, providing convenience for subsequent map building operation, and providing a reference pose in the first map for subsequent correction operation.

First, positioning is performed on a first map using visual featuresObtaining the positioning information of the current frame in the first mapIn a common positioning method, a relative position relationship between a current frame and a reference key frame is given through RANSAC + PnP or other methods, an initial value of the current frame is determined according to the position of the reference key frame, and positioning is completed by using an optimization method by combining a matching relationship between points in a local map and the current frame.

If the location informationDegree of confidence inGreater than a second predetermined threshold τ_CThen position information will be locatedAs initial positioning information of the current frame in the second mapInitial positioning information using a partial map of a second mapOptimizing to obtain the positioning information of the current frame in the second map

Since the second map contains all the information of the first map, that is, there is a one-to-one correspondence between the elements of the fixed area of the first map and the elements of the fixed area of the second map, the positioning result of the current frame on the first map can be used as the initial value for positioning on the second map, and the operation can always be successful.

If the location informationDegree of confidence inIs not greater than a second preset threshold tau_CObtaining the initial pose of the current frame in the second map based on the visual features and the odometry data, and positioning by utilizing the matching relation between the local map of the second map and the current frame on the basis of the initial pose to obtain the positioning information of the current frame in the second mapSince the second map includes the newly added area, the confidence of the second mapAlways higher, the positioning result is trusted.

Positioning informationAs initial positioning information of the current frame in the first mapInitial positioning information using a partial map of a first mapOptimizing to obtain the positioning information of the current frame in the first mapIf the location informationDegree of confidence inGreater than confidenceThen the positioning information will beAs the first positioning information, and the positioning informationAs second positioning information.

Step S400, fusing and building the initialized second map based on the second positioning information to obtain third positioning information;

as an alternative implementation manner of this embodiment, as shown in fig. 3, step S400 includes:

step S410, establishing map points of the newly added area based on the second positioning information and the key frames in the fixed area;

and matching the image feature points of the current key frame and the adjacent key frames in the fixed area based on semantic conditions and geometric constraint conditions. At the current key frame K_iAnd its adjacent key frameIn, use characteristicsRepresenting a key frame K_iThe m-th feature of (1)Representing a key frame K_jThe nth feature of (1). When the Hamming distance of the two feature descriptors is larger than a given threshold, the two features are judged to meet the semantic condition.

In the geometric constraint condition judgment, the positions of the feature points in the images, the internal reference of the camera and the relative poses corresponding to the two frames of images are known, and the feature is judged by using the Simpson errorAndwhether the geometric constraint is satisfied. In thatConsidering the feature under the condition of simultaneously satisfying the semantic condition and the geometric constraint conditionAndand (6) matching.

In this embodiment, the adjacent key frame refers to a key frame having a certain number of visual points with the current key frame.

For two successfully matched image feature points, if the map points corresponding to the two image feature points are empty, obtaining map points p1 corresponding to the two image feature points based on the positions of the two image feature points in the two frames of images and the poses of the two frames of images, assigning the map points p1 to the two image feature points, and adding the map points p1 into a newly added region; if the map points corresponding to the two image feature points are empty and only one map point is empty, assigning a non-empty value to the map point with the empty value; if the map points corresponding to the two image feature points are both non-empty, judging whether map points belonging to a fixed area exist in the map points corresponding to the two image feature points, if so, replacing the other map point by the map point belonging to the fixed area, and if not, optionally replacing the other map point by one map point.

Step S420, establishing map points of the newly added area based on the second positioning information and the key frames of the newly added area;

and the map point building comprises two parts of screening matching feature points and triangularization feature points.

First, matching image feature points of a current key frame and adjacent key frames in a newly added area based on semantic conditions and geometric constraint conditions.

For two successfully matched image feature points, if the map points corresponding to the two image feature points are empty, obtaining map points p2 corresponding to the two image feature points based on the positions of the two image feature points in the two frames of images and the poses of the two frames of images, assigning the map points p2 to the two image feature points, and adding the map points p2 into a newly added region; if the map points corresponding to the two image feature points are empty and only one map point is empty, assigning a non-empty value to the map point with the empty value; and if the map points corresponding to the two image feature points are both non-empty, judging whether the map points corresponding to the two image feature points have map points belonging to the newly added area, if so, replacing the other map point by the map point belonging to the newly added area, and if not, optionally replacing the other map point by one map point.

Step S430, if the map points and/or key frames of the newly added area meet the preset redundancy condition, deleting the map points and/or key frames meeting the preset redundancy condition;

specifically, traversing the map points of the newly added area, if the visibility of the map points is lower than a third preset threshold, determining that the amount of information carried by the map points is low, and deleting the map points; and/or traversing the key frames of the newly added area, and removing the key frames if the visible map points of the key frames and other key frames have a preset overlapping area, such as 95%, so as to reduce the redundancy of the map.

And step S440, carrying out local optimization on the pose of the newly added area to obtain third positioning information.

In the process of local optimization, locking a fixed area in a second map, wherein the fixed area participates in pose optimization calculation but does not update the pose; and updating the pose of the newly added area in the second map through local optimization, and outputting third positioning information.

Through step S400, triangularization is performed on the current key frame and the key frames with different priorities, so as to ensure that the newly added key frame and the map point more fit the scale of the first map.

And S500, correcting the second map after the map is built by the fusion based on the first positioning information and the third positioning information to obtain an incremental map.

The map rectification can be divided into three links: pose alignment, updating of visual relations, and position graph optimization and global optimization. The key frames participating in the alignment and visual relationship adjustment are the key frames with high-confidence-degree positioning in the first map in the current frame and the adjacent frames, and the key frames are all from the newly added part of the second mapThe set of these key frames is denoted as

First, it is required to determine whether the confidence of the current key frame in the first positioning information is not less than a fourth preset threshold, and if so, determine whether the difference between the first positioning information and the third positioning information of the current key frame meets a preset difference condition.

Optionally, the preset difference condition includes at least one of the following conditions:

(1) the translational amount difference of the poses in the first positioning information and the third positioning information exceeds a fourth preset threshold, for example, the fourth preset threshold may be set to 0.5;

(2) the rotation angle difference of the poses in the first positioning information and the third positioning information exceeds a preset angle, for example, the preset angle may be set to 3 degrees;

(3) the ratio of the absolute value of the number of map points observed by the current keyframe in the fixed area of the second map to the number of map points observed by the current keyframe in the first map is less than a fifth predetermined threshold, e.g.,

and if the preset difference condition is met, setting the positioning pose of the current key frame in the first map as the pose of the key frame in the second map, and finishing pose alignment. In particular, for key frame K_iPositioning pose of the map in the first mapSet as its pose on a second mapNamely, it isAnd finishing pose alignment.

The visual relationship of the second map is then updated based on the visual relationship of the current keyframe to the visual map points in the first map.

In particular, key frame K is used_iVisual map points in a first map For its visual map points in the second mapCarrying out fusion whereinFor the current frame K_iThe number of the feature points of (a),respectively representing key frames K_iThe jth feature point of (a) corresponds to the visual map points in the first and second maps.

If it is notThe presence of the one or more of the one,at corresponding map points of the second mapFrom a new area of the second map, andandinstead, map points are usedInstead of the formerAnd removing map points in the map

And finally, performing dose graph optimization and global optimization on the newly added region.

In the case of the fuse graph optimization, acquiring a key frame set of a second mapAnd a fixed partIs set of key framesFixed key frame setPose in and current keyframe K_cThe pose of (1). Using pre-update poseUsing the updated pose as a targetAs an observation, optimization was performed with the objective function as follows:

in the global optimization, the pose of a key frame of the second map and the position of a map point are used as optimization variables, the visual relation between the key frame and the map point is used as constraint, and the reprojection error is used as an objective function to perform local map optimization.During the optimization process, the fixed part from the second map is fixedThe pose of the keyframe and the location of the map point. The objective function is as follows:

wherein the content of the first and second substances,indicating the number of key frames in the second map,representing the number of map points in the second map, z_ijRepresents the measurement, T, of the ith keyframe observation jth map point_iRepresents the pose of the ith keyframe, p_jRepresents the jth map point, g (T)_i，p_j) Representing map points p_jIn the position and posture T_iAnd (4) the observed value of (a).

The fuse graph optimization and the global optimization are a common method, and actually, the local map corresponding to the key frames from the current key frame to the last newly-built key frame with high-confidence-level positioning is optimized, and the pose and the position of a fixed part in the local map are fixed in the optimization process.

By the correction method, the situation of scale change of a newly-built map caused by poor positioning effect of the first map, short-term positioning loss or long-term positioning loss due to map area expansion due to illumination and environmental difference can be solved, and the robustness of the system is improved.

It should be noted that the confidence threshold calculation method varies depending on the confidence calculation method, and for the same confidence calculation method, there may be a plurality of residual calculation methods, such as the three-sigma criterion under gaussian distribution, hypothesis testing, and the like. The visual SLAM main body frame used in the embodiment of the application can be a series frame such as ORB _ SLAM and VINS; as long as the SLAM framework used in incremental map building is consistent with the framework used in the original map building, this embodiment is not specifically limited.

Fig. 4 is a block diagram of an incremental mapping apparatus 600 for a visual SLAM according to an embodiment of the present disclosure. As shown in fig. 4, the incremental mapping apparatus 600 for visual SLAM mainly includes:

an acquisition module 610 for acquiring visual features and odometry data based on sensor data;

an initialization module 620 for obtaining a first map and initializing a second map based on the first map, visual features and odometry data;

a co-location module 630, configured to perform co-location on the first map and the initialized second map by using the current frame, respectively, to obtain first location information and second location information;

a fused map building module 640, configured to perform fused map building on the initialized second map based on the second positioning information to obtain third positioning information;

and the correcting module 650 is configured to correct the second map after the fused map is built based on the first positioning information and the third positioning information, so as to obtain an incremental map.

The functional modules in the embodiments of the present application may be integrated together to form an independent unit, for example, integrated into a processing unit, or each module may exist alone physically, or two or more modules are integrated to form an independent unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit. The functions, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application or portions thereof that substantially contribute to the prior art may be embodied in the form of a software product stored in a storage medium and including instructions for causing an electronic device (which may be a personal computer, a server, a network device, or the like) or a processor (processor) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: u disk, removable hard disk, read only memory, random access memory, magnetic or optical disk, etc. for storing program codes.

Various changes and specific examples in the method provided by the embodiment of the present application are also applicable to the incremental mapping apparatus for a visual SLAM provided by the embodiment of the present application, and through the foregoing detailed description of the incremental mapping method for a visual SLAM, those skilled in the art can clearly know the implementation method of the incremental mapping apparatus for a visual SLAM in the present embodiment, and for the sake of brevity of the description, details are not described here again.

Fig. 5 is a block diagram of a robot 700 according to an embodiment of the present disclosure. As shown in fig. 5, the robot 700 includes a memory 701, a processor 702, a communication bus 703, a communication interface 704, a sensor device 705; the memory 701, processor 702, and communication interface 704 are connected by a communication bus 703.

The sensor device 705 is connected to the communication interface 704 for collecting sensor data. The sensor apparatus 705 may include, but is not limited to, an image capturing device, which may be a general camera, a fish-eye camera, etc., and a scale-observation odometer, which may be composed of one or more of a wheel encoder, a wheel encoder + inertial measurement unit, RTK, GPS.

The memory 701 may be used to store an instruction, a program, code, a set of codes, or a set of instructions. The memory 701 may include a storage program area and a storage data area, wherein the storage program area may store instructions for implementing an operating system, instructions for at least one function, and instructions for implementing the incremental mapping method of the visual SLAM provided by the above-described embodiments, and the like; the storage data area can store data and the like involved in the incremental mapping method of the visual SLAM provided by the above embodiment.

The processor 702 may include one or more processing cores. The processor 702 executes the various functions and processes data of the present application by executing or executing instructions, programs, code sets, or instruction sets stored in the memory 701 to invoke data stored in the memory 701. The Processor 702 may be at least one of an Application Specific Integrated Circuit (ASIC), a Digital Signal Processor (DSP), a Digital Signal Processing Device (DSPD), a Programmable Logic Device (PLD), a Field Programmable Gate Array (FPGA), a Central Processing Unit (CPU), a controller, a microcontroller, and a microprocessor. It is understood that the electronic devices for implementing the functions of the processor 702 may be other devices, and the embodiments of the present application are not limited thereto.

Communication bus 703 may include a path that transfers information between the above components. The communication bus 703 may be a PCI (Peripheral Component Interconnect) bus, an EISA (Extended Industry Standard Architecture) bus, or the like. The communication bus 703 may be divided into an address bus, a data bus, a control bus, etc. For ease of illustration, only one double-headed arrow is shown in FIG. 5, but that does not indicate only one bus or one type of bus.

Wherein, robots include but are not limited to: the robot comprises an automatic driving vehicle, a logistics robot, a sweeping robot, a mobile robot and an AR robot. The robot shown in fig. 5 is only an example, and should not bring any limitation to the functions and the range of use of the embodiments of the present application.

Embodiments of the present application provide a computer-readable storage medium storing a computer program capable of being loaded by a processor and executing the incremental mapping method for visual SLAM provided in the above embodiments.

In this embodiment, the computer readable storage medium may be a tangible device that retains and stores instructions for use by an instruction execution device. The computer readable storage medium may be, but is not limited to, an electronic memory device, a magnetic memory device, an optical memory device, an electromagnetic memory device, a semiconductor memory device, or any combination of the foregoing. In particular, the computer readable storage medium may be a portable computer diskette, a hard disk, a U-disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a podium random access memory (SRAM), a portable compact disc read-only memory (CD-ROM), a Digital Versatile Disc (DVD), a memory stick, a floppy disk, an optical disk, a magnetic disk, a mechanical coding device, and any combination thereof.

The computer program in the present embodiment includes a program code for executing the method shown in fig. 1, and the program code may include instructions corresponding to the method steps provided in the foregoing embodiments. The computer program may be downloaded to the respective computing/processing device from a computer-readable storage medium, or may be downloaded to an external computer or external storage device via a network, such as the internet, a local area network, a wide area network, and/or a wireless network. The computer program may execute entirely on the user's computer, as a stand-alone software package.

In the embodiments provided in the present application, it should be understood that the disclosed system, apparatus and method may be implemented in other ways. For example, the above-described apparatus embodiments are merely illustrative, and for example, the division of the modules or units is only one logical division, and there may be other divisions when actually implemented, for example, a plurality of units or components may be combined or may be integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may be in an electrical, mechanical or other form.

In addition, it is to be understood that relational terms such as first and second, and the like, are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. The terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus.

The above description is only a preferred embodiment of the present application and is not intended to limit the present application, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, improvement and the like made within the spirit and principle of the present application shall be included in the protection scope of the present application.