A fast tracking based ORB-SLAM3 map repair method

Abstract

The present invention discloses an ORB-SLAM3 map repair method based on fast tracking, step S1: play all image frames before the current map is initialized; step S2: fast track the played image frames and perform map repair. The technical solution of the present invention is adopted, in the monocular visual inertial mode, a fast tracking method is used to start from the initial frame of the current map, reversely track to the image with a small timestamp, and estimate a more accurate posture to generate a complete map. The map contains all the input data, so that a complete map can be repaired in any initialized scenario regardless of the initialization effect, reducing the SLAM system's dependence on initialization, avoiding the map discard problem caused by insufficient initialization excitation, and making the robot more stable when only positioning and tracking is performed offline.

Description

Technical Field

[0001] The present invention relates to the field of robot synchronous positioning and map creation, and in particular to an ORB-SLAM3 map repair method based on fast tracking. Background Art

[0002] In the past decade, the development of simultaneous localization and mapping (SLAM) has been very rapid, and the focus of research has gradually shifted from the initial lidar SLAM to visual SLAM. Among them, the mapping effect of the SLAM system is considered to be one of the key issues in current research, which is of great significance to the practical application and application expansion of robots. An accurate and complete map will directly determine the accuracy of offline positioning and tracking of robots. Among the many SLAM frameworks currently available, ORB-SLAM3 is one of the best open source frameworks. In its monocular visual inertial mode, the method of motion recovery structure is used for initialization, and the essential matrix and basic matrix are calculated at the same time. A better model is selected for initialization based on the score, which can improve the success rate and quality of initialization. However, due to the nature of motion recovery structure, a sufficiently large perspective change is required to complete the initialization, so the information of many image frames is not used. Secondly, in the monocular inertial mode, the inertial measurement unit is easily under-excited or unstable during the initialization stage, which makes initialization more difficult and thus discards more frames.

[0003] The initialization method of the prior art mainly has the following two problems:

[0004] 1. Initialization of the structure-from-motion method will inevitably consume multiple frames to initialize, resulting in partial information loss.

[0005] 2. During the initialization process, the loss of local maps and insufficient excitation of the inertial measurement unit will cause the created initialization map to be frequently discarded, resulting in a large amount of initialization part pose information not being retained.

[0006] Therefore, in view of the defects of the prior art, it is necessary to propose a technical solution to solve the technical problems existing in the prior art. Summary of the invention

[0007] In view of this, it is necessary to provide an ORB-SLAM3 map repair method based on fast tracking, save all image frames before successful initialization, and then quickly reverse track these frames in another thread, use the bag-of-words model to match feature points, use MLPNP for pose estimation, and then insert the newly generated key frame into the key frame database, its map point into the map, and insert the pose of all frames tracked in reverse into the map trajectory, thereby repairing the map and making the final generated map a complete map.

[0008] In order to overcome the defects of the prior art, the technical solution of the present invention is as follows:

[0009] A fast-tracking-based ORB-SLAM3 map repair method comprises the following steps:

[0010] Step S1: Play all image frames before the current map is initialized;

[0011] Step S2: quickly track the played image frame and perform map repair; wherein step S1 further includes:

[0012] Step S11: creating a stack for storing image frames;

[0013] Step S12: putting each input image frame into a stack until the current map completes the initialization process of the inertial measurement unit;

[0014] Step S13: retaining all image frames before the initialization frame of the current active map in the stack, and removing image frames after the initialization frame;

[0015] Step S14: continuously popping frames from the stack until all image frames in the stack are processed;

[0016] Step S2 quickly tracks the image frame obtained in S1, and the step further includes:

[0017] Step S21: Determine the tracking mode; this step further includes:

[0018] Step S211: if the current frame is the first frame popped from the stack, jump to S22;

[0019] Step S212: if the current frame has been matched with the reference key frame, but the number of matching points or inliers is insufficient, jump to S22;

[0020] Step S213: In other cases, jump to S23;

[0021] Step S22: Accurate positioning and tracking; this step further includes:

[0022] Step S221: Bag of Words matching. Get the frame F popped from the stack. If F is the first frame popped from the stack, the current frame is only matched with the initial key frame KF0 of the current active map. Otherwise, the current frame is matched with its reference key frame, KF0, and key frames with more common nodes in the key frame database;

[0023] Step S222: using a key frame with the most matching points with the current frame in the above matching process, using MLPNP to perform pose estimation;

[0024] Step S223: Use chi-square distribution to remove outliers and define the reprojection error as:

[0025]

[0026] Where u is the position of the feature point of F on the pixel plane, is the position of the map point of KF0 projected onto the F pixel plane. The reprojection error follows the following Gaussian distribution:

[0027] e~N(0,∑)

[0028] Where ∑ is the covariance:

[0029]

[0030] Where s is the scale factor of the image pyramid and p is the standard deviation of the 0th layer.

[0031] The error term in is weighted by covariance and then the inner product is calculated to obtain an error scalar r = e T ∑ -1 e. Since the monocular projection has 2 degrees of freedom, the chi-square statistic threshold is 5.99 at a significance level of 0.05. We consider points greater than this threshold to be outliers and points less than this threshold to be inliers.

[0032] Step S224: Determine the number of inliers. If the number of inliers is sufficient, the estimated pose is used as the pose of the current frame and the process jumps to S25. Otherwise, more matching points are generated by reprojecting the map points, and the threshold is adjusted more strictly. The MLPNP pose estimation is re-executed, and the determination of this step is performed. If the number of inliers is still insufficient, the process jumps to S24.

[0033] Step S23: fast reference key frame tracking; this step further includes:

[0034] Step S231: Bag-of-words matching. In this process, the current frame is only matched with its reference key frame;

[0035] Step S232: Determine the number of matching points. If the number of matching points is less than the threshold, jump to S21;

[0036] Step S233: using the posture transformation of the previous frame as the initial value to accelerate the convergence speed of posture estimation using MLPNP;

[0037] Step S234: using chi-square distribution to remove outliers;

[0038] Step S235: Determine the number of inner points. If the number of inner points is sufficient, jump to S25, otherwise jump to S21;

[0039] Step S24: assigning the pose of the previous image frame to the current frame;

[0040] Step S25: Determine whether to generate a key frame; this step further includes:

[0041] Step S251: determine whether the number of frames from the current frame to the previous key frame is enough, if not, end S25;

[0042] Step S252: determine whether the distance to the nearest key frame is far enough, if not, end S25;

[0043] Step S253: determine whether the number of common view feature points in the current frame and the local map is less than a threshold. If not, end S25;

[0044] Step S254: construct the current frame into a key frame, insert its map points into the map, and use this frame as a reference key frame for subsequent image frames;

[0045] Step S26: Save the current frame pose to the map.

[0046] Compared with the prior art, the present invention has the following beneficial effects:

[0047] Efficient tracking: The present invention utilizes bag-of-words matching and MLPNP to estimate pose in reverse fast tracking, and divides the tracking process into precise positioning tracking and fast reference key frame tracking, which not only ensures the real-time tracking but also ensures high accuracy. At the same time, the tracking process is run in parallel with the main thread, so that unused image frames can be tracked one by one efficiently.

[0048] Complete mapping: Through the fast tracking algorithm, the unused frames of the current map can be estimated to have an accurate pose. Combined with the generation of new key frames, the final generated map can be complete, which utilizes the poses of all image frames.

[0049] The initialization effect can be ignored: the present invention avoids the problem of long initialization time or frequent map abandonment caused by the failure of local map tracking during initialization, insufficient excitation of the inertial measurement unit, etc., and can achieve map repair under any initialization situation. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 A framework flow chart of an ORB-SLAM3 map repair method based on fast tracking provided by the present invention;

[0051] Figure 2 A detailed flow chart of step S21 in an ORB-SLAM3 map repair acceleration method based on fast tracking provided by the present invention;

[0052] Figure 3 A detailed flow chart of step S22 in an ORB-SLAM3 map repair acceleration method based on fast tracking provided by the present invention;

[0053] Figure 4 A detailed flow chart of step S23 in an ORB-SLAM3 map repair acceleration method based on fast tracking provided by the present invention;

[0054] Figure 5 A detailed flow chart of step S25 in an ORB-SLAM3 map repair acceleration method based on fast tracking provided by the present invention;

[0055] Figure 6 A comparison chart of the map trajectories of the present invention and the ORB-SLAM3 algorithm on the Euroc dataset;

[0056] Figure 7 This is a comparison chart of the absolute trajectory errors of the present invention and the ORB-SLAM3 algorithm on the Euroc dataset;

[0057] The following specific embodiments will further illustrate the present invention in conjunction with the above-mentioned drawings. DETAILED DESCRIPTION

[0058] The technical solution provided by the present invention will be further described below in conjunction with the accompanying drawings.

[0059] Among the current monocular inertial SLAM systems, ORB-SLAM3 is one of the systems with the best performance. It uses a motion recovery structure to perform monocular initialization. However, when ORB-SLAM3 is initialized, if the inertial measurement unit is not sufficiently stimulated, it will cause frequent discarding of map information, thereby failing to establish a complete map. The present invention adopts a bag-of-words model to match feature points, and utilizes MLPNP to perform pose estimation, and designs a fast map repair algorithm. The bag-of-words model is currently one of the most efficient methods in two-frame 2D-2D matching. It uses a hierarchical K-means tree storage structure, is easy to use and has a fast matching speed. MLPNP is a fast, highly accurate pose estimation algorithm that does not rely on the camera model and is very suitable for use in SLAM systems. To this end, the present invention provides an ORB-SLAM3 map repair method based on fast tracking.

[0060] See also Figure 1-5 The present invention provides an ORB-SLAM3 map repair method based on fast tracking. Figure 1 The figure shows the architecture diagram of the ORB-SLAM3 map repair method based on fast tracking of the present invention. Generally speaking, the present invention includes two major steps: step S1: playing all image frames before the current map is initialized; step S2: fast tracking the played image frames and performing map repair;

[0061] Step S1 plays all frames before the current active map is successfully initialized one by one, from the frames with larger timestamps to the frames with smaller timestamps, and specifically includes the following steps:

[0062] Step S11: creating a stack for storing image frames;

[0063] Step S12: putting each input image frame into a stack until the current map completes the initialization process of the inertial measurement unit;

[0064] Step S13: retaining all image frames before the initialization frame of the current active map in the stack, and removing image frames after the initialization frame;

[0065] Step S14: continuously popping frames from the stack until all image frames in the stack are processed;

[0066] Step S2 quickly tracks the played image frames one by one and completes the map repair, which specifically includes the following steps:

[0067] Step S21: Determine the tracking mode;

[0068] Figure 2 The figure shows a detailed flow chart of the above steps in an ORB-SLAM3 map repair method based on fast tracking provided by the present invention, which further includes the following steps:

[0069] Step S211: if the current frame is the first frame popped from the stack, jump to S22;

[0070] Step S212: if the current frame has been matched with the reference key frame, but the number of matching points or inliers is insufficient, jump to S22;

[0071] Step S213: In other cases, jump to S23;

[0072] Step S22: precise positioning and tracking;

[0073] Figure 3 The figure shows a detailed flow chart of the above steps in an ORB-SLAM3 map repair method based on fast tracking provided by the present invention, which further includes the following steps:

[0074] Step S221: Bag of Words matching. Get the frame F popped from the stack. If F is the first frame popped from the stack, the current frame is only matched with the initial key frame KF0 of the current active map. Otherwise, the current frame is matched with its reference key frame, KF0, and key frames with more common nodes in the key frame database;

[0075] Step S222: using a key frame with the most matching points with the current frame in the above matching process, using MLPNP to perform pose estimation;

[0076] Step S223: Use chi-square distribution to remove outliers and define the reprojection error as:

[0077]

[0078] Where u is the position of the feature point of F on the pixel plane, is the position of the map point of KF0 projected onto the F pixel plane. The reprojection error follows the following Gaussian distribution:

[0079] e~N(0,∑)

[0080] Where ∑ is the covariance:

[0081]

[0082] Where s is the scale factor of the image pyramid and p is the standard deviation of the 0th layer.

[0083] The error term in is weighted by covariance and then the inner product is calculated to obtain an error scalar r = e T ∑ -1e. Since the monocular projection has 2 degrees of freedom, the chi-square statistic threshold is 5.99 at a significance level of 0.05. We consider points greater than this threshold to be outliers and points less than this threshold to be inliers.

[0084] Step S224: Determine the number of inliers. If the number of inliers is sufficient, the estimated pose is used as the pose of the current frame and the process jumps to S25. Otherwise, more matching points are generated by reprojecting the map points, and the threshold is adjusted more strictly. The MLPNP pose estimation is re-executed, and the determination of this step is performed. If the number of inliers is still insufficient, the process jumps to S24.

[0085] Step S23: fast reference key frame tracking;

[0086] Figure 4 The figure shows a detailed flow chart of the above steps in an ORB-SLAM3 map repair method based on fast tracking provided by the present invention, which further includes the following steps:

[0087] Step S231: Bag-of-words matching. In this process, the current frame is only matched with its reference key frame;

[0088] Step S232: Determine the number of matching points. If the number of matching points is less than the threshold, jump to S21;

[0089] Step S233: using the posture transformation of the previous frame as the initial value to accelerate the convergence speed of posture estimation using MLPNP;

[0090] Step S234: using chi-square distribution to remove outliers;

[0091] Step S235: Determine the number of inner points. If the number of inner points is sufficient, jump to S25, otherwise jump to S21;

[0092] Step S24: assigning the pose of the previous image frame to the current frame;

[0093] Step S25: Determine whether to generate a key frame;

[0094] Figure 5 The figure shows a detailed flow chart of the above steps in an ORB-SLAM3 map repair method based on fast tracking provided by the present invention, which further includes the following steps:

[0095] Step S251: determine whether the number of frames from the current frame to the previous key frame is enough, if not, end S25;

[0096] Step S252: determine whether the distance to the nearest key frame is far enough, if not, end S25;

[0097] Step S253: determine whether the number of common view feature points in the current frame and the local map is less than a threshold. If not, end S25;

[0098] Step S254: construct the current frame into a key frame, insert its map points into the map, and use this frame as a reference key frame for subsequent image frames;

[0099] Step S26: Save the current frame pose to the map.

[0100] The input data used in the present invention adopts images and IMU data from the Euroc dataset. The Euroc dataset is a visual inertial dataset collected by a micro-aircraft, which contains 11 binocular video sequences. The dataset scenes are indoor rooms and factories. According to the different motion modes in the initialization stage of each sequence, MH01-MH05 sequences are used as difficult initialization sequences, and V101-V203 are easy initialization sequences. The simulation experiment platform of this paper is a computer with Intel(R) Core(TM) i7-7700HQ CPU@2.80GHz and 24G memory. All video sequences in the dataset are applied to this experiment to observe the relationship between the map repaired by the algorithm of this paper and the unrepaired map.

[0101] In order to verify the technical effect of the present invention, the effectiveness of the present invention will be verified by algorithm comparison:

[0102] In the comparison between this method and the original ORB-SLAM3 method, both methods were run ten times and the median was taken as the final comparison result. In the difficult initialization sequence, the initialization time was too long due to the rapid time transformation and insufficient excitation of the inertial measurement unit, making the constructed map very incomplete; while in the easy initialization sequence, the system initialization time was short, so the loss of map information was not much. First, the map repair effect on all sequences of the Euroc dataset was compared based on the number of poses and trajectory lengths before and after the algorithm repair. Then, the MH05 sequence represented the difficult initialization sequence, and the V203 represented the easy initialization sequence. The root mean square error (RMSE) and standard deviation (SD) of the absolute trajectory were used as indicators to compare the improvement of accuracy and stability before and after the map repair.

[0103] See Table 1, Table 2, Figure 6 , Figure 7 As shown, the experimental data results of the present invention and the original ORB-SLAM3 algorithm are compared. Table 1 compares the number of poses and trajectory lengths of the maps generated by the present invention and the original ORB-SLAM3 algorithm; Table 2 compares the root mean square error and standard deviation of the absolute trajectory of the present invention and the original ORB-SLAM3 algorithm; Figure 6 The map trajectory comparison diagrams of the present invention and the original ORB-SLAM3 algorithm on MH05 and V203 respectively; Figure 7 The following are the comparison diagrams of the root mean square error between the present invention and the original ORB-SLAM3 algorithm in MH05 and V203 respectively.

[0104] Table 1 Comparison of the number of map poses and trajectory lengths between the present invention and ORB-SLAM3

[0105]

[0106] Table 2 Comparison of the root mean square error (RMSE) and standard deviation (SD) of the absolute trajectory of the present invention and ORB-SLAM3

[0107]

[0108] Table 1 above lists the data comparison of the number of map poses and trajectory lengths of the algorithm of the present invention and the original ORB-SLAM3 algorithm, which can intuitively show the different effects of the present invention on different sequences. In the sequence that is difficult to initialize, that is, the initialization time is too long, the repaired map poses are the most and the trajectory is the longest. In the sequence that is easy to initialize, the number of map poses and the trajectory length that are repaired are limited. Table 2 lists the data comparison of the root mean square error and standard deviation of the absolute trajectory of the algorithm of the present invention and the original ORB-SLAM3 algorithm. The comparison of RMSE and SD can reflect the improvement effect of the present invention on accuracy and stability. Figure 6 The map trajectories in the xyz direction generated by the present invention and the original ORB-SLAM3 algorithm on the MH05 and V203 sequences are shown, and the restored trajectory is on the left side of the vertical line. Figure 7 The box plot comparison of RMSE between the present invention and the original ORB-SLAM3 algorithm on the MH05 and V203 sequences is shown. In summary, the present invention uses a fast tracking method to repair the map of ORB-SLAM3, so that a complete and accurate map can be generated in any scenario regardless of the initialization effect. It has been experimentally verified that the root mean square error of the absolute trajectory error of ORB-SLAM3 on the Euroc dataset is reduced by an average of 47.51%, and the standard deviation is reduced by an average of 52.18%. On the difficult-to-initialize sequence, the trajectory length is repaired by an average of 11.68%, and the number of poses contained in the map is repaired by an average of 23.38%.

[0109] The above embodiments are only used to help understand the method and core idea of ​​the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made to the present invention without departing from the principles of the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

[0110] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A fast-tracking ORB-SLAM3 map repair method, characterized in that: The following steps are involved: Step S1: Play all image frames before the current map is initialized; Step S2: quickly track the played image frames and perform map repair; Wherein, the step S1 further comprises: Step S11: creating a stack for storing image frames; Step S12: putting each input image frame into a stack until the current map completes the initialization process of the inertial measurement unit; Step S13: retaining all image frames before the initialization frame of the current active map in the stack, and removing image frames after the initialization frame; Step S14: continuously popping frames from the stack until all image frames in the stack are processed; The step S2 further comprises: Step S21: Determine the tracking mode; Step S22: precise positioning and tracking; Step S23: fast reference key frame tracking; Step S24: assigning the pose of the previous image frame to the current frame; Step S25: Determine whether to generate a key frame; Step S26: Save the current frame pose to the map; Wherein, the step S21 further comprises: Step S211: if the current frame is the first frame popped from the stack, jump to S22; Step S212: if the current frame has been matched with the reference key frame, but the number of matching points or inliers is insufficient, jump to S22; Step S213: In other cases, jump to S23; The step S22 further comprises: Step S221: bag-of-words matching; get the frame F popped out of the stack, if F is the first frame popped out of the stack, then the current frame is only matched with the initial key frame KF0 of the current active map; otherwise, the current frame is matched with its reference key frame, KF0 and key frames with more common nodes in the key frame database; Step S222: using a key frame with the most matching points with the current frame in the above matching process, using MLPNP to perform pose estimation; Step S223: Use chi-square distribution to remove outliers and define the reprojection error as: Where u is the position of the feature point of F on the pixel plane, is the position of the map point KF0 projected onto the F pixel plane; the reprojection error obeys the following Gaussian distribution: e~M(0,∑) Where ∑ is the covariance: Where s is the scale factor of the image pyramid, and p is the standard deviation of the 0th layer; The error term in is weighted by covariance and then the inner product is calculated to obtain an error scalar r = e T ∑ -1 e. Since the monocular projection has 2 degrees of freedom, the chi-square statistic threshold is 5.99 at a significance level of 0.

05. We consider points greater than this threshold to be outliers and points less than this threshold to be inliers. Step S224: determine the number of inliers; if the number of inliers is sufficient, the estimated pose is used as the pose of the current frame and the process jumps to S25; otherwise, more matching points are generated by reprojecting the map points, and the threshold is adjusted more strictly. The MLPNP pose estimation is re-executed, and the judgment of this step is performed. If the number of inliers is still insufficient, the process jumps to S24; The step S23 further comprises: Step S231: bag-of-words matching; in this process, the current frame is only matched with its reference key frame; Step S232: determine the number of matching points; if the number of matching points is less than the threshold, jump to S21; Step S233: using the posture transformation of the previous frame as the initial value to accelerate the convergence speed of posture estimation using MLPNP; Step S234: using chi-square distribution to remove outliers; Step S235: determine the number of inner points; if the number of inner points is sufficient, jump to S25, otherwise jump to S21; The step S25 further comprises: Step S251: determine whether the number of frames from the current frame to the previous key frame is enough, if not, end S25; Step S252: determine whether the distance to the nearest key frame is far enough, if not, end S25; Step S253: determine whether the number of common view feature points in the current frame and the local map is less than a threshold. If not, end S25; Step S254: construct the current frame into a key frame, insert its map points into the map, and use this frame as a reference key frame for subsequent image frames.

Images