CN110378997A - A kind of dynamic scene based on ORB-SLAM2 builds figure and localization method - Google Patents

Abstract

The invention discloses a kind of dynamic scenes based on ORB-SLAM2 to build figure and localization method, including local map tracking process, dynamic pixel reject process, sparse mapping process, closed loop detection process and building Octree map process；This method has the function of dynamic pixel rejecting, combines the depth image of new key frame to be quickly detected mobile object in the image information of camera by object detection method and constructs a clean static background Octree map in complicated dynamic environment.

Description

A dynamic scene mapping and positioning method based on ORB-SLAM2

technical field

The invention relates to the technical field of robot synchronous mapping and positioning slam, in particular to a dynamic scene mapping and positioning method based on orb-slam 2.

Background technique

SLAM (simultaneous localization and map reconstruction) has been a hot topic in the field of computer vision and robotics, and it has also attracted the attention of many high-tech companies. SLAM technology is to build a map in an unknown environment and be able to locate it in real time on the map. The framework of modern visual SLAM systems is very mature, such as ORB-SLAM2, LSD-SLAM; the most advanced Visual Synchronous Localization and Mapping (V-SLAM) systems have high-precision positioning capabilities, but most of these systems assume that the operating environment is static, thus limiting their application.

For the establishment of static maps in dynamic scenes, existing algorithms have their shortcomings. For example, DynaSLAM cannot be real-time. DynaSLAM only removes pixels of predefined objects, but cannot remove undefined objects or only part of predefined objects. , the StaticFusion system cannot eliminate people who have been stationary for a long time, and none of the above algorithms can build a clean static map in real time in a complex dynamic environment.

Contents of the invention

In view of the problems existing in the above-mentioned prior art, the object of the present invention is to provide a dynamic scene mapping and positioning method based on ORB-SLAM2, which has the function of dynamic pixel elimination, and collects the depth of new key frames through the target detection method Image quickly detects moving objects in the camera's image information and builds a clean static background octree map in complex dynamic environments.

In order to achieve the above tasks, the present invention adopts the following technical solutions:

A dynamic scene mapping and positioning method based on ORB-SLAM2, comprising the following steps:

Step 1, local map tracking

Use the image information captured by the camera carried by the robot to initialize the camera pose; the first frame of image captured by the camera is used as the key frame during initialization; after the initial pose is obtained, the local map is tracked to optimize the camera pose and production new keyframe;

Step 2, dynamic pixel culling

Use the target detection algorithm to detect predefined dynamic objects in the color image of the new key frame, and then combine the depth image of the new key frame to identify dynamic pixels; the dynamic pixels detected by these two methods are all eliminated;

Step 3, Sparse Mapping

For the keyframes with dynamic pixels removed, optimize the robot pose of the keyframes, add new map points, and maintain the quality and scale of the keyframe collection;

Step 4, closed loop detection

Perform loop closure detection for each new key frame, and once a closed loop is detected, perform pose graph optimization;

Step 5, build an octree map

Use the octree to divide the map points into voxels, and store these voxels through the octree structure to construct the octree map; determine whether the voxel is occupied by calculating the occupancy probability of the voxel. Fork tree diagram for visualization.

Further, the tracking of the local map to optimize the camera pose and generate new keyframes includes:

The local map refers to the 3D points observed by the key frame whose distance and viewing angle are close to the current frame; more matching 3D points are obtained through reprojection, so as to optimize the camera pose and generate new key frames with the minimum error:

Project the 3D points on the local map onto the current frame to obtain 3D-2D feature matching;

Limit the area of searching for 2D matching points in the current frame to reduce false matching, and then compare the pixels in the current frame with the positions obtained by projecting the 3D points according to the current estimated camera pose to construct the least squares method problem, so that It minimizes and then finds the best camera pose for localization;

Determine whether to generate a new keyframe according to preset conditions.

Further, the step 2 described in combination with the depth image of the new key frame to identify dynamic pixels includes:

Project the remaining pixels of the target detection algorithm to remove the predefined objects to the world coordinates to create 3D points; divide the 3D points into multiple clusters, and select M pixels evenly from each cluster; for each pixel, the The pixel is projected to the N keyframes closest to the new keyframe for comparison to detect whether the pixel is a dynamic pixel:

Back-project pixel u to a 3D point ^pw in world coordinates using the depth from the keyframe's depth image and the new keyframe's robot pose;

Project the 3D point p ^w onto the color image of the jth keyframe near the new keyframe;

If the pixel u' of the jth key frame has a valid depth value z' on the corresponding depth image, then the pixel u' is back-projected to the 3D point p ^w' in the world coordinates;

Whether a pixel u is dynamic is judged by comparing the distance d between ^pw′ and ^pw with a set threshold _dmth :

Search the pixels in the square around u′ so that d takes the minimum value d _min ; if d _min is greater than the threshold d _mth , the pixel u is preliminarily judged to be static, otherwise it is preliminarily judged to be dynamic.

Further, assuming that pixel u is in the preliminary judgment results of all nearby key frames, the number of static results is N _S , the number of dynamic results is N _d , and the number of invalid results is N _i , the final attributes of pixel u are as follows:

If (N _S ≥ N _d , N _S >0), then the pixel u is a static pixel;

If (N _d ≥ N _s , N _d > 0), then the pixel u is a dynamic pixel;

If (N _s =N _d =0), then pixel u is invalid.

Further, the dynamic pixels detected by the two methods successively are all eliminated, and after the dynamic pixels are identified by combining the depth image of the new key frame, the method of elimination is as follows:

Among M pixels uniformly selected in a cluster, assuming that the number of static pixels is N _s ', the number of dynamic pixels is N _d ', and the number of invalid pixels is N _i ', the final properties of the cluster are as follows:

If (N _S '≥N _d '), the cluster is a static cluster, and the cluster is reserved;

If (N _d '≥N _s '), then the cluster is a dynamic cluster and will be eliminated.

Further, the optimization of the robot pose of the key frame, adding new map points, and maintaining the quality and scale of the key frame set includes:

Calculate the Bow vector of the current keyframe and update the map point of the current keyframe;

Apply sliding window local BA to optimize the pose of the robot, and the optimized object is the pose in the current frame;

Redundant keyframes are detected and eliminated. If 90% of the pixels on a keyframe can be observed by more than three arbitrary keyframes, it is considered a redundant keyframe and deleted.

Further, the calculation of the occupancy probability of the voxel is used to determine whether the voxel is occupied, and if it is occupied, it is visualized in the octree diagram, including:

Let Z _t represent the observation result of voxel n at time t, the probability log value of voxel n starting from time t is L(n|Z _1:t ), and then at time t+1, voxel n’s The log probability value is obtained by the following formula:

L(n|Z _1:t+1 )＝L(n|Z _1:t-1 )+L(n|Z _1:t ) Formula 8

L(n|Z _t ) is τ if voxel n is observed at time t, otherwise 0; increment τ is a predetermined value;

Define p∈[0,1] to represent the probability that a voxel is occupied, and l∈R to represent the logarithmic value of the probability, and l is calculated by logit transformation:

The inverse transformation of the above formula is:

The occupancy probability p of a voxel is calculated by substituting the logarithm value into Equation 10; only if the occupancy probability p is greater than a predetermined threshold, voxel n is considered occupied and will be visualized in the octree.

The present invention has the following technical characteristics:

1. Rapidity

The algorithm uses the CornerNet-Squeeze target detection algorithm to detect dynamic objects and uses the k-means variant Mini Batch K-Means clustering algorithm to cluster the depth information of the image, which is faster than existing algorithms. Because the CornerNet-Squeeze target detection algorithm only needs 34ms to process a picture, which is faster than YOLOv3 and other algorithms, the test hardware environment is: 1080ti GPU+Intel Core i7-7700k CPU. For clustering methods with big data larger than 10,000, the k-means variant Mini Batch K-Means is three times faster than k-means itself, and the performance is not much different.

In addition, the establishment of the octree map also shortens the update time of the map.

2. A very clean static map can be built in a complex environment

The algorithm combines the target detection method and the probability logarithm of the octree map to update voxels to detect and eliminate dynamic pixels.

Description of drawings

Fig. 1 is the flowchart of the inventive method;

Figure 2 is a diagram of the local BA optimization process;

Figure 3 is an octree map model.

Detailed ways

ORB-SLAM2 is a complete SLAM solution based on monocular, binocular and RGB-D cameras. It enables map reuse, loop detection and relocation. But it assumes that the operating environment is static, which limits its application.

The algorithm of the present invention is proposed on the basis of the ORB-SLAM2 algorithm, which can quickly establish a clean static octree map in a dynamic environment in real time. It mainly consists of five steps: local map tracking, dynamic pixel removal, and sparse Mapping, loop closure detection, and creation of octree maps, the overall flow chart is shown in Figure 1. The specific content is as follows:

Step 1, local map tracking

Use the image information captured by the camera carried by the robot to initialize the camera pose; the first frame of image captured by the camera is used as the key frame during initialization; after the initial pose is obtained, the local map is tracked to optimize the camera pose and production New keyframe; the specific steps are as follows:

Step 1.1, extract and match ORB feature points from the original RGB image information (including color image and depth image) captured by Kinect2, and then track and initialize the camera position through the three modes of motion mode, key mode and repositioning mode in turn Pose, that is, positioning; when initializing, set the first frame as a key frame, and this step 1.1 is the same as ORB-SLAM2.

Step 1.2, after obtaining the initial pose, track the local map. The local map refers to the key frame whose distance and viewing angle (within the distance set to 4m and the angle set to 1rad) are close to the current frame (the photo currently taken by the camera) (Local keyframe) Observed 3D points; get more matching 3D points through reprojection, so as to optimize the camera pose with minimum error:

(1) Definition:

Transformation matrix from camera coordinate system c to robot coordinate system r:

The transformation matrix from the robot coordinate system r to the world coordinate system w (that is, the pose of the robot):

The projection relationship from the 3D point P _c corresponding to a frame of picture C to the 2D point u on this picture:

From the 2D point u on a frame of picture C combined with its corresponding depth information z back-projection to the corresponding 3D point P _c back-projection relationship of this picture:

(2) Reprojection to get feature matching:

Let the pose of the robot (that is, the pose of the current frame) be 3D points on the local map Projected onto the current frame, 3D-2D feature matching can be obtained:

(3) Optimize the camera pose:

In a dynamic scene, there will be a large number of mismatches in feature matching. In order to solve this problem, the present invention uses a region (a circle with a radius set to 3 pixels) that limits the search for 2D matching points (ie pixels) in the current frame to reduce Mismatch. Then compare the pixel u _i in the current frame with the position u _i ' of the 3D point projected according to the current estimated camera pose (Eq. Good camera poses for localization:

(4) Finally, judge whether to generate a new key frame according to the preset condition; this preset condition is the same as the orb-slam2 algorithm.

Step 2, dynamic pixel culling

To build a static map in a dynamic scene, the identification and deletion of dynamic pixels is the most critical. Because only the key frame is used to construct the octree map, only the newly selected key frame (new key frame) in the previous step is used to remove dynamic pixels.

This step first uses the object detection method to detect predefined dynamic objects in the color image of the new key frame, and then combines the depth image of the new key frame to identify dynamic pixels; the dynamic pixels detected by these two methods successively are eliminated. Proceed as follows:

Step 2.1. First, for predefined objects such as people, tables, chairs, etc., this solution can use the CornerNet-Squeeze target detection algorithm in CornerNet-Lite to detect predefined dynamic objects in the color image of the new key frame, if When a dynamic object is detected, the pixels of the dynamic object are removed.

The CornerNet-Squeeze target detection algorithm only needs 34ms to process a picture, which is faster than YOLOv3 and other algorithms. The test hardware environment is: 1080ti GPU+Intel Core i7-7700k CPU.

Step 2.2, for some undefined dynamic objects such as books, boxes, etc., or part of predefined objects that cannot be detected by the target detection algorithm, such as human hands, etc., this solution uses the following method to process the previous step Combine the depth image of the new keyframe on the color image to detect dynamic pixels:

2.2.1 Project the remaining pixels after the target detection algorithm eliminates the predefined objects to the world coordinates to create 3D points.

2.2.2 Use a clustering algorithm to divide 3D points into several clusters

Divide the 3D points into multiple clusters; the number of clusters k is determined according to the number of 3D points s _p : k=s _p /n _pt , n _pt is the average number of points in the cluster to be adjusted, and s _p represents the size of the point cloud, starting from each M pixels are evenly selected from the clusters.

Since the number of pixels is huge and the clustering speed needs to be as fast as possible, the Mini Batch K-Means method, a variant clustering method of K-means, is used in this scheme. Among them, reducing n _pt can ensure a better approximation, but it will also increase the calculation burden. In this solution, n _pt is set to 6000 to balance calculation consumption and accuracy.

Because this scheme focuses on removing dynamic pixels and building static maps without tracking dynamic objects, clusters are assumed to be rigid bodies, which means that pixels in the same cluster have the same motion properties; therefore, this scheme only needs to detect which clusters is a dynamic cluster; in order to speed up the process of dynamic cluster detection, this scheme uniformly selects M=100 pixels for each cluster.

In the following steps, the selected dynamic and static attributes are judged; if there are more dynamic pixels than static pixels, the cluster is determined to be dynamic to be eliminated, otherwise it is determined to be static and retained.

2.2.4 Determine whether a pixel is a dynamic pixel

This solution detects whether the pixel is a dynamic pixel by projecting the M pixels selected in step 2.2.2 to the N=6 frame key frames closest to the new key frame (near the new key frame) for comparison. Specific steps are as follows:

(1) Use the depth z in the depth image of the keyframe and the robot pose of the new keyframe n Backproject pixel u to 3D point p ^w in world coordinates:

(2) Project the 3D point p ^w onto the color image of the jth keyframe near the new keyframe:

in is the robot pose of the jth keyframe near the keyframe.

(3) If the pixel u' of the jth key frame has a valid depth value z' on the corresponding depth image, then the pixel u' is back-projected to the 3D point p ^w' in the world coordinates:

(4) Determine whether the pixel u is dynamic by comparing the distance d between ^pw′ and ^pw with the set threshold _dmth :

Because there are errors in the depth image and pose of the key frame, u′ may not be the pixel corresponding to u, so this scheme searches for pixels within a square around u′ (set the side length S of the square to 10 pixels according to experience), Make d take the minimum value d _min ; if d _min is greater than the threshold d _mth (threshold d _mth is set to grow linearly with the depth value z′), the pixel u is preliminarily judged to be static, otherwise it is preliminarily judged to be dynamic; in other cases Either no valid depth value can be found in the square search area, or u is outside the bounds of the image, in which case pixel u is judged invalid.

In view of the fact that the result of a key frame is not reliable enough and may produce invalid results, this program applies the above-mentioned preliminary judgment process (1)-(4) to all nearby key frames of the new key frame (select 6 key frames in this embodiment), and finally , the final situation of pixel u is determined by voting: Suppose pixel u is in the preliminary judgment results of all nearby key frames, the number of static results is N _S , the number of dynamic results is N _d , the number of invalid results is N _i , pixel The final properties of u are as follows:

If (N _S ≥N _d , N _S >0), then the pixel u is a static pixel;

If (N _d ≥ N _s , N _d >0), then the pixel u is a dynamic pixel;

If (N _s =N _d =0), then pixel u is invalid.

2.2.5 Determine whether a cluster is dynamic

This step also adopts the voting method in the previous step to determine the attributes of the clusters; among M pixels uniformly selected in a cluster, it is assumed that the number of static pixels is N _s ', the number of dynamic pixels is N _d ', and the number of invalid pixels is N _i ', the final properties of the cluster are as follows:

If (N _S '≥N _d '), the cluster is a static cluster, and the cluster is reserved;

If (N _d '≥N _s '), then the cluster is a dynamic cluster and will be eliminated.

Step 3, Sparse Mapping

The main purpose of sparse mapping is to receive and process keyframes that have eliminated dynamic pixels, optimize the robot pose of this keyframe, add new map points, and maintain the quality and scale of the keyframe set. Specific steps are as follows:

Step 3.1, process newly introduced keyframes

Calculate the Bow vector of the current keyframe and update the map point of the current keyframe;

Step 3.2, Local BA

Apply the sliding window local BA to optimize the robot pose. The optimization framework is shown in Figure 2. The optimization object is the pose in the current frame. Participating in the optimization are:

(1) All the poses in the keyframes connected to the current keyframe in the sliding window; in order to balance time and precision, this scheme sets n in the sliding window to 7;

(2) Two black map points created before the keyframe in the sliding window;

(3) The two white map points created after the key frame in the sliding window are not used as variables and are already fixed. After local BA optimization, the pose of a new keyframe is optimized to create a new map point, and this new keyframe will be used to construct the octree map.

Step 3.3, local key frame screening

In order to control the reconstruction density and the complexity of BA optimization, this step also includes the process of detecting and eliminating redundant key frames; through reprojection, if 90% of the pixels on the key frame can be observed by more than three arbitrary key frames, then are considered redundant keyframes and are removed.

Step 4, closed loop detection

The bag-of-words model Dbow2 is used to perform loop closure detection for each new keyframe. Once a closed loop is detected, the pose graph optimization will be performed; this process is the same as the ORB-SLAM2 algorithm, and the specific pose graph optimization process belongs to the existing technology and will not be repeated here.

Step 5, build an octree map

Use the octree to divide the above-created and optimized map points into voxels (or small squares), and store these voxels through the octree structure to construct an octree map; judge by calculating the occupancy probability of voxels Whether the voxel is occupied, if so, it is visualized in the octree diagram.

As shown in Figure 3, the construction of an octree map is to continuously update whether these voxels are occupied; the octree map uses the form of probability to indicate whether a voxel is occupied, unlike map points that only use 0 to represent blanks, 1 means occupied; the probability log value method is used to describe below. Specific steps are as follows:

Step 5.1, let Z _t represent the observation result of voxel n at time t (the result can be obtained by reprojection), the logarithm value of the probability of voxel n starting from time t is L(n|Z _1:t ), then At time t+1, the logarithm value of voxel n can be obtained by the following formula:

L(n|Z _1:t+1 )＝L(n|Z _1:t-1 )+L(n|Z _1:t ) Formula 8

L(n|Z _t ) is τ if voxel n is observed at time t, and 0 otherwise; increment τ is a predetermined value. This formula states that the logarithmic value of a voxel will increase when the voxel is repeatedly observed to be occupied, and decrease otherwise.

In step 5.2, define p∈[0,1] to represent the probability that a voxel is occupied, and l∈R to represent the logarithm value of the probability, l can be calculated by logit transformation:

The inverse transformation of the above formula is:

The occupancy probability p of a voxel is calculated by substituting the probability log value obtained in the previous step into Equation 10; only when the occupancy probability p is greater than a predetermined threshold, voxel n is considered occupied and will be visualized in the octree diagram.

In other words, voxels that have been observed to be occupied multiple times are considered as stable occupied voxels, and in this way, our scheme can well handle the problem of mapping in dynamic environments. In complex situations, the octree map helps to strengthen the culling of dynamic pixels and minimize the impact of dynamic objects.

Claims (7)

1. a kind of dynamic scene based on ORB-SLAM2 builds figure and localization method, which comprises the following steps:

Step 1, local map tracks

Camera pose is initialized using camera captured image information entrained by robot；Camera is caught when initialization The first frame image obtained is as key frame；After obtaining initial pose, local map is tracked, thus optimize camera pose with And the key frame that production is new；

Step 2, dynamic pixel is rejected

Predefined dynamic object is detected in the color image of new key frame using algorithm of target detection, then in conjunction with new key frame Depth image identify dynamic pixel；The dynamic pixel successively detected by both methods is all removed；

Step 3, sparse mapping

For eliminating the key frame of dynamic pixel, optimizes the robot pose of key frame, increase new point map, maintenance is crucial The quality and scale of frame set；

Step 4, closed loop detects

Closed loop detection is carried out to each new key frame, once detecting closed loop, carries out the optimization of pose figure；

Step 5, Octree map is constructed

Point map is divided into voxel using Octree, and these voxels are stored by octree structure, with constructing Octree Figure；Judge whether voxel is occupied by calculating the occupation probability of voxel, as occupied, is visualized in Octree figure.

2. the dynamic scene based on ORB-SLAM2 builds figure and localization method as described in claim 1, which is characterized in that described Local map is tracked, to optimize camera pose and the new key frame of production, comprising:

The local map refers to the point of 3D observed by the key frame of distance and visual angle close to present frame；Pass through re-projection More matched 3D points are obtained, so that minimal error optimizes camera pose and generates new key frame:

3D point in local map is projected on present frame, 3D-2D characteristic matching is obtained；

The region of the search 2D match point of present frame is limited in reduce error hiding, then presses the pixel in present frame with 3D Least Square Solution is constructed according to the error that the position that the camera pose currently estimated is projected compares, makes it It minimizes, best camera pose is then looked for, to be positioned；

It is determined whether to generate new key frame according to preset condition.

3. the dynamic scene based on ORB-SLAM2 builds figure and localization method as described in claim 1, which is characterized in that step 2 The depth image of the new key frame of the combination identifies dynamic pixel, comprising:

Get off to create 3D point to world coordinates the predefined remaining pixel projection of object is rejected by algorithm of target detection；It will 3D point is divided into multiple clusters, and M pixel is selected uniformly at from each cluster；For each pixel, pixel projection is closed to from new It is for no dynamic pixel that the nearest N frame key frame of key frame, which is compared to detection pixel:

Using the robot pose of depth and new key frame in the depth image of key frame by pixel u back projection to world coordinates Under 3D point p^w；

By 3D point p^wIt projects on the color image of j-th of key frame near new key frame；

If there are effective depth value z ', pixel u ' instead to throw on corresponding depth image by the pixel u ' of j-th of key frame 3D point p under shadow to world coordinates^w′；

By by p^w′And p^wThe distance between d and setting threshold value d_mthCompare to judge whether pixel u is dynamic:

The pixel in the square around u ' is searched for, so that d is minimized d_min；If d_minGreater than threshold value d_mth, then preliminary judgement Pixel u is judged as static, otherwise tentatively judges that it is dynamic.

4. the dynamic scene based on ORB-SLAM2 builds figure and localization method as claimed in claim 3, which is characterized in that assuming that For pixel u in all preliminary judging results of key frame nearby, the quantity of static result is N_S, the quantity of dynamic result is N_d, nothing The quantity for imitating result is N_i, the final attribute of pixel u is as follows:

If (N_S≥N_d,N_S> 0), then pixel u is static pixels；

If (N_d≥N_s,N_d> 0), then pixel u is dynamic pixel；

If (N_S=N_d=0), then pixel u is invalid.

5. the dynamic scene based on ORB-SLAM2 builds figure and localization method as claimed in claim 4, which is characterized in that described The successive dynamic pixel detected by both methods be all removed, wherein the depth image in conjunction with new key frame identifies After dynamic pixel, the method rejected are as follows:

In a cluster in M pixel of uniform design, it is assumed that static pixels number is N_s', dynamic pixel number is N_d', inactive pixels Number is N_i', the final attribute of cluster is as follows:

If (N_S'≥N_d'), then the cluster is static cluster, retains the cluster；

If (N_d'≥N_s'), then the cluster is Dynamic Cluster, is rejected.

6. the dynamic scene based on ORB-SLAM2 builds figure and localization method as described in claim 1, which is characterized in that described Optimization key frame robot pose, increase new point map, safeguard the quality and scale of key frame set, comprising:

The Bow vector for calculating current key frame, updates the point map of current key frame；

The optimization to robot pose is carried out using sliding window part BA, optimization object is the pose in present frame；

Detection redundancy key frames are simultaneously rejected, if 90% pixel can be exceeded three any key frame observations on key frame It arrives, is then considered as redundancy key frames, and be deleted.

7. the dynamic scene based on ORB-SLAM2 builds figure and localization method as described in claim 1, which is characterized in that described The occupation probability by calculating voxel judge whether voxel is occupied, as occupied, visualized, wrapped in Octree figure It includes:

If Z_tIndicate voxel n time t observation as a result, probability logarithm of the voxel n since the time is t be L (n | Z_1:t), so Afterwards when the time is t+1, the probability logarithm of voxel n is obtained by following formula:

L(n|Z_1:t+1)=L (n | Z_1:t-1)+L(n|Z_1:t) formula 8

If voxel n is observed in time t, and L (n | Z_t) it is τ, it is otherwise 0；Increment τ is predetermined value；

Defining the general and l ∈ R that p ∈ [0,1] indicates that voxel is occupied indicates probability logarithm, and l is obtained by logit transformation calculations It arrives:

Above equation inverse transformation are as follows:

The occupation probability p of voxel is calculated by the way that probability logarithm is substituted into formula 10；Only when acquistion probability p is greater than predetermined threshold When, just think that voxel n is occupied and will visualize in Octree figure.