Visual odometer method based on feature point fusion and key frame selection optimization

Abstract

The invention discloses a visual odometer method based on feature point fusion and key frame selection optimization. The method comprises the following steps: S1, extracting ORB feature points and XFeat feature points from an input image; s2, after the image sequence is inserted into a tracking thread, the system state at the moment is judged, and according to the system state, proper feature points are selected for system initialization and thread tracking; s3, after the tracking thread is finished, a key frame needs to be newly built, and at the moment, whether the key frame needs to be generated or not is judged by using an improved key frame selection condition according to the camera pose calculated by the tracking thread; and S4, carrying out local mapping thread and closed-loop detection thread according to the key frames screened by the tracking thread. By adopting the technical scheme of the invention, the precision and robustness of the visual odometer in a low-texture scene are improved by utilizing the advantage fusion of low texture and high recognition rate of the Xfeat feature points and high calculation efficiency of the ORB feature points.

Description

Technical Field

[0001] The present invention relates to the field of robot synchronous positioning and map creation, and in particular to a visual odometer method based on feature point fusion and key frame selection optimization. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) has gradually become a hot topic in the fields of artificial intelligence applications such as autonomous driving, AR / VR, and it plays an irreplaceable role in recent years. SLAM is divided into laser SLAM and visual SLAM according to the type of sensor. Compared with laser sensors, cameras have low cost-effectiveness and rich environmental information, so SLAM sensors have gradually shifted from lidar to vision. As one of the most advanced technologies for robot positioning and mapping, visual simultaneous localization and mapping (VSLAM) mainly relies on the image information provided by the camera sensor, so that the robot can calculate its own position and build an environmental map in an unknown environment. The two are interdependent. Accurate positioning can build an accurate map, and an accurate map can get accurate positioning. ORB-SLAM3 is one of the best open source frameworks at present. It supports single, binocular, RGB-D cameras, and has a visual odometer and a complete back-end SLAM framework. However, when faced with low texture and fast rotation, ORB-SLAM3's visual odometer cannot track accurately, which greatly reduces the accuracy and robustness of the entire SLAM system.

[0003] The existing SLAM methods have the following two main problems: (1) When faced with low texture, feature points cannot be extracted, which makes it difficult for the visual odometer to perform feature matching and the tracking thread is easily lost. (2) When faced with the situation where the camera rotates too fast, the key frame interval is large, which makes it difficult for the visual odometer to perform feature matching and the tracking thread is lost.

[0004] 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

[0005] In view of this, it is necessary to provide a visual odometer method based on feature point fusion and key frame selection optimization, extract ORB feature points and XFeat feature points from the input image, and use the advantages of Xfeat feature points with low texture and high recognition rate and ORB feature points with high computational efficiency to improve the accuracy and robustness of visual odometer in low texture scenes. Then insert the image sequence into the tracking thread to determine the system state at this time, and select feature points for initialization and tracking based on the system state. Then use the improved key frame selection condition to determine whether a key frame needs to be generated based on the camera's pose transformation. By using the camera pose transformation as the key frame selection condition, the robustness of the camera in the case of fast rotation is improved. Finally, the local mapping thread and closed loop detection thread are performed based on the key frames.

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

[0007] A visual odometer method based on feature point fusion and key frame selection optimization includes the following steps:

[0008] Step S1, extracting ORB feature points and XFeat feature points from the input image for subsequent system initialization and tracking;

[0009] Step S2, after inserting the image sequence into the tracking thread, determine the system state at this time, and select appropriate feature points to initialize the system and track the thread according to the system state;

[0010] Step S3, after the tracking thread ends, a new key frame is created. At this time, the camera pose calculated by the tracking thread is used to use the improved key frame selection condition to determine whether a key frame needs to be generated;

[0011] Step S4, performing a local mapping thread and a closed-loop detection thread according to the key frames filtered by the tracking thread.

[0012] In a possible implementation manner, the step S2 further includes:

[0013] Step S201: When the system has no initial posture, the system state is set to "need to initialize", and the ORB feature and the XFeat feature are used for initialization at the same time. If ORB is successfully initialized, XFeat initialization is stopped; otherwise, after XFeat is successfully initialized, XFeat feature points are used for tracking, and ORB continues to try to initialize until the texture is rich and ORB is successfully initialized and then switched to main feature tracking; if both are not successfully initialized, the system continues to maintain the state of needing initialization;

[0014] Step S202: After the system is initialized, the system state is set to "ORB normal tracking" or "XFeat normal tracking"; if the main feature is ORB, constant speed tracking or reference key frame tracking is used; if the main feature is XFeat, a geometric relationship is constructed based on the previous and next two frames, and after generating temporary map points, projection matching is performed to calculate the pose, and then the pose is optimized by minimizing the reprojection error;

[0015] Step S203: When the system is tracking normally, if the ratio of the number of currently tracked feature points to the total number of initially tracked feature points is less than p, generally p=0.2, the system state is reset to "ORB recent loss" or "XFeat recent loss"; if the main feature is ORB, the XFeat feature points of the current frame and the previous frame are extracted to calculate the pose; when the ORB feature tracking is lost, XFeat is used for tracking; otherwise, the ORB tracking thread is built in advance;

[0016] Step S24: When the system state is recently lost, if both features are lost in tracking, the system state is reset to "tracking lost"; at this time, since both feature points cannot be tracked, the system state is reset to "needs initialization".

[0017] In a possible implementation manner, the step S3 further includes:

[0018] Step S301, calculate the transformation matrix T between the current frame and its reference key frame, then:

[0019]

[0020] Among them, R is the rotation matrix from the reference key frame to the current frame, and t is the translation vector from the reference key frame to the current frame;

[0021] Step S302, taking out the rotation vector R and the translation vector t from the transformation matrix T, converting R into the rotation vector ω, according to the Rodrigues formula:

[0022] R=cosθI+(1-cosθ)nn T +sinθn∧

[0023] Where θ is the rotation angle, I is the unit matrix, and n is the rotation axis.

[0024]

[0025] The rotation vector is expressed as ω=θn, and then the relative rotation ΔR and relative translation Δt from the reference key frame to the current frame can be obtained by taking the modulus of ω and t;

[0026] Step S303, parameter ΔP represents the relative motion between frames, and its formula is:

[0027] ΔP=(1-α)∥Δt∥+α∥ΔR∥

[0028] Where α is the weight coefficient, which is used to balance the weight between rotation and translation. The formula is:

[0029]

[0030] Compared with translation, rotation is more likely to cause difficulty in matching feature points, making tracking difficult, so the weight α is set to increase exponentially with the increase in the amount of rotation;

[0031] Step S304, select a threshold ΔP ％ , with the rotation amount ∥ΔR∥ as the main consideration; when the camera angle rotates more than 30°, it corresponds to ΔP ％ =0.4, create a keyframe.

[0032] In a possible implementation manner, the step S4 further includes:

[0033] Step S401, the key frames screened by the tracking thread are inserted into the local mapping thread, and feature matching is performed on the new key frames and their co-viewing key frames;

[0034] Step S402, generating new map points by triangulating the feature points after the new key frame and its common view key frame are matched;

[0035] Step S403, the new key frame is then passed to the closed-loop detection thread, and the system quickly matches the historical key frames based on the bag-of-words model to find the closed-loop candidate key frame;

[0036] Step S404, after finding the closed-loop candidate keyframe, the system calculates the similarity transformation through feature point matching;

[0037] Step S405, when the common view key frame of the new key frame and the closed-loop candidate key frame also satisfies the current similarity transformation, it is considered that the closed-loop is successful;

[0038] Step S406, after the loop is closed successfully, the closed-loop thread performs global posture graph optimization to correct the drift accumulated over a long period of time.

[0039] Compared with the prior art, the technical solution of the present invention can greatly improve the performance of the SLAM system under low texture and fast camera rotation conditions. The present invention adopts the advantages of high recognition rate of XFeat feature points under low texture and high efficiency of ORB feature points, and fuses the two through the system state, thereby solving the problem of difficult feature matching and easy tracking loss caused by missing feature points under low texture. When optimizing the selection of key frames, key frames are created when the rotation exceeds a certain level, which effectively solves the tracking loss caused by large key frame intervals due to too fast rotation. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 A framework flow chart of a visual odometer method based on feature point fusion and key frame selection optimization provided by the present invention;

[0041] Figure 2 It is a comparison chart of the trajectories of the present invention and the ORB-SLAM3 algorithm on the TUM data set with the true value in the XYZ direction;

[0042] Figure 3 This is a comparison chart of the absolute pose error between the present invention and the ORB-SLAM3 algorithm on the TUM dataset. DETAILED DESCRIPTION

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

[0044] The current visual SLAM system is mainly composed of visual odometry, backend optimization, loop detection and mapping. The visual odometry can estimate the camera's position and posture in real time by analyzing the changes in feature points between adjacent image frames. This process is the core of SLAM and directly affects the positioning accuracy of the system. However, when faced with low-texture conditions, it is difficult to extract ORB feature points, which affects the accuracy and robustness of the entire system. XFeat feature points have good recognition in low-texture conditions. Fusion with ORB feature points based on the state of the system is very suitable for improving the accuracy and robustness of the visual odometry. When the camera rotates too fast, using rotation as a condition for adding keyframes and reducing the keyframe interval during rotation can effectively improve the robustness of the visual odometry.

[0045] A visual odometer method based on feature point fusion and key frame selection optimization according to an embodiment of the present invention, Figure 1 The figure shows an architecture diagram of the visual odometer method based on feature point fusion and key frame selection optimization of the present invention, which includes the following steps: step S1, extracting ORB feature points and XFeat feature points from the input image for subsequent system initialization and tracking; step S2, after inserting the image sequence into the tracking thread, judging the system state at this time, and selecting appropriate feature points according to the system state to initialize the system and track the thread; step S3, after the tracking thread ends, a new key frame needs to be created, and at this time, the camera pose calculated by the tracking thread uses the improved key frame selection condition to judge whether a key frame needs to be generated; step S4, performing a local mapping thread and a closed-loop detection thread according to the key frame screened by the tracking thread.

[0046] In a specific embodiment of the present invention, step S2 further includes:

[0047] Step S21, when the system initially has no posture, the system state is set to "needs initialization", and the ORB feature points and XFeat feature points are used for initialization at the same time, with ORB feature points as the main and XFeat feature points as the auxiliary. When the image is full of delicate and complex patterns and details, the system uses ORB feature points for multi-frame feature matching. When the number of matches exceeds the set minimum threshold and the initial posture is successfully calculated based on these matching points, the initialization can be determined to be successful. At this time, the initialization process of the XFeat feature point thread should be stopped. On the contrary, when the image lacks changes and subtle features, the system uses XFeat feature points for tracking after successful initialization, and the ORB feature point thread continues to try to initialize until the image is full of delicate and complex patterns and details, and then ORB is successfully initialized and switched to ORB feature point tracking. If neither is successfully initialized, the system continues to maintain the state of needing initialization.

[0048] Step S22, after the system is initialized, the system state is set to "ORB normal tracking" or "XFeat normal tracking". If the main feature is ORB, constant speed tracking or reference key frame tracking is used. If the main feature is XFeat, the geometric relationship is constructed based on the previous and next two frames, and after generating temporary map points, projection matching is performed, the pose is calculated, and then the pose is optimized by minimizing the reprojection error.

[0049] Step S23, when the system is tracking normally, if the ratio of the number of currently tracked feature points to the total number of initially tracked feature points is less than p (generally p=0.2), the system state is reset to "ORB recent loss" or "XFeat recent loss". If the main feature is ORB, the XFeat feature points of the current frame and the previous frame are extracted to calculate the pose. When ORB feature tracking is lost, XFeat is used for tracking. In any case, the ORB tracking thread is built in advance.

[0050] Step S24: When the system state is recently lost, if both features are lost, the system state is reset to "tracking lost". At this time, since both feature points cannot be tracked, the system state is reset to "needs initialization".

[0051] In a specific embodiment of the present invention, step S3 further includes:

[0052] Step S31, calculate the transformation matrix T between the current frame and its reference key frame, then:

[0053]

[0054] Among them, R is the rotation matrix from the reference key frame to the current frame, and t is the translation vector from the reference key frame to the current frame.

[0055] Step S32, extract the rotation vector R and the translation vector t from the transformation matrix T. Convert R to the rotation vector ω according to the Rodrigues formula:

[0056] R=cosθI+(1-cosθ)nn T +sinθn∧

[0057] Where θ is the rotation angle, I is the unit matrix, and n is the rotation axis.

[0058]

[0059] The rotation vector can be expressed as ω=θn, and then the relative rotation ΔR and relative translation Δt from the reference key frame to the current frame can be obtained by taking the modulus of ω and t.

[0060] Step S33, parameter ΔP represents the relative motion between frames, and its formula is:

[0061] ΔP=(1-α)∥Δt∥+α∥ΔR∥

[0062] Where α is the weight coefficient, which is used to balance the weight between rotation and translation. The formula is:

[0063]

[0064] Compared with translation, rotation is more likely to cause difficulty in matching feature points, making tracking difficult, so the weight α is set to increase exponentially with the increase in the amount of rotation.

[0065] Step S34, select a threshold ΔP ％ , with the rotation amount ∥ΔR∥ as the main consideration. When the camera angle rotates more than 30°, it corresponds to ΔP ％ =0.4, create a keyframe.

[0066] In a specific embodiment of the present invention, step S4 further includes:

[0067] Step S41, the key frames selected by the tracking thread are inserted into the local mapping thread, and feature matching is performed on the new key frames and their co-viewing key frames.

[0068] Step S42, generating new map points by triangulating the feature points after the new key frame and its common view key frame are matched.

[0069] Step S43, after the new keyframe is passed into the closed-loop detection thread, the system quickly matches the historical keyframes based on the bag-of-words model to find the closed-loop candidate keyframes.

[0070] Step S44, after finding the closed-loop candidate key frame, the system calculates the similarity transformation through feature point matching.

[0071] Step S45: When the common view key frame of the new key frame and the closed-loop candidate key frame also satisfies the current similarity transformation, it is considered that the closed-loop is successful.

[0072] Step S46, after the loop is closed successfully, the closed-loop thread will optimize the global posture graph to correct the drift accumulated over a long period of time.

[0073] The input data used in this invention is from the Technische The datasets in the Robot SLAM directory and Structure vs. Texture directory under the TUM dataset released by the University of Munich (MU) are from the Robot SLAM directory and the Structure vs. Texture directory. The Robot SLAM dataset is designed for ground robots to collect information in large indoor environments and contains a large number of samples with sudden changes in perspective, large rotations, and motion blur. The Structure vs. Texture dataset divides multiple sub-datasets into scenes with weak texture, weak structure, rich texture, and rich structure according to the texture and structural features of the scene. The simulation experiment platform in this paper is a computer with an Intel(R) Core(TM) i5-8300H CPU@2.30GHz and 16G memory. All video sequences in the dataset are applied to this experiment, and experiments are conducted using ORB, XFeat, and fused feature points to observe the initialization and tracking effects of the visual odometer. The key frame selection optimization is added to observe the robustness of the system.

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

[0075] 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. The root mean square error (RMSE) of the absolute trajectory was used as the measurement indicator for comparison, and the accuracy and stability improved by this method compared with the original algorithm were calculated. The str_tex_far image sequence was then used for detailed comparison.

[0076] See Table 1. Figure 2 , Figure 3 As shown, the experimental data results of the present invention and the original ORB-SLAM3 algorithm are compared. Table 1 shows the accuracy comparison and improvement of the present invention and the original ORB-SLAM3 algorithm;

[0077] Among them, Lost means tracking is lost, and None means initialization is successful. Figure 2 Comparison diagram of the str_notex_far sequence trajectory between the present invention and the original ORB-SLAM3 algorithm;

[0078] Table 1 Comparison of accuracy and robustness between the present invention and ORB-SLAM3

[0079]

[0080] Table 1 above lists the accuracy and robustness comparison and improvement of the present invention and the original ORB-SLAM2 algorithm. The three sequences of pioneer_360, pioneer_slam, and pioneer_slam2 contain rotating scenes. Both ORB and XFeat feature points can be initialized, but due to the rotation of the camera, not enough feature points can be tracked, resulting in tracking failure. When the key frame optimization option is added, the system can track the entire process, which improves the robustness of the system. It can be seen from the two low-texture sequences of floor and str_notex_far that ORB feature points cannot be tracked under low textures. When XFeat feature points are added, the system tracking of the entire process can be completed well. Figure 2 It can be seen that the fusion of feature points and key frame selection optimization effectively solves the problem of tracking failure caused by low texture and fast camera rotation. Figure 3 It can be seen that the fusion of feature points and key frame selection optimization effectively improves the accuracy of the entire SLAM system.

[0081] 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.

[0082] 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 visual odometry method based on feature point fusion and key frame selection optimization, characterized in that: The following steps are involved: Step S1, extracting ORB feature points and XFeat feature points from the input image for subsequent system initialization and tracking; Step S2, after inserting the image sequence into the tracking thread, determine the system state at this time, and select appropriate feature points to initialize the system and track the thread according to the system state; Step S3, after the tracking thread ends, a new key frame is created. At this time, the camera pose calculated by the tracking thread is used to use the improved key frame selection condition to determine whether a key frame needs to be generated; Step S4, performing a local mapping thread and a closed-loop detection thread according to the key frames filtered by the tracking thread.

2. The visual odometer method based on feature point fusion and key frame selection optimization according to claim 1, characterized in that: The step S2 further comprises: Step S201: When the system has no initial pose, the system state is set to "needs initialization", and the ORB feature and the XFeat feature are used for initialization at the same time. If ORB is successfully initialized, XFeat initialization is stopped; otherwise, after XFeat is successfully initialized, XFeat feature points are used for tracking, and ORB continues to try to initialize until the texture is enriched and ORB is successfully initialized and then switched to main feature tracking; if both are not successfully initialized, the system continues to maintain the state of needing initialization; Step S202: After the system is initialized, the system state is set to "ORB normal tracking" or "XFeat normal tracking"; if the main feature is ORB, constant speed tracking or reference key frame tracking is used; if the main feature is XFeat, a geometric relationship is constructed based on the previous and next two frames, and after generating temporary map points, projection matching is performed to calculate the pose, and then the pose is optimized by minimizing the reprojection error; Step S203: When the system is tracking normally, if the ratio of the number of currently tracked feature points to the total number of initially tracked feature points is less than p, generally p=0.2, the system state is reset to "ORB recent loss" or "XFeat recent loss"; if the main feature is ORB, the XFeat feature points of the current frame and the previous frame are extracted to calculate the pose; when the ORB feature tracking is lost, XFeat is used for tracking; otherwise, the ORB tracking thread is built in advance; Step S24: When the system state is recently lost, if both features are lost, the system state is reset to "tracking lost"; at this time, since both feature points cannot be tracked, the system state is reset to "needs initialization".

3. The visual odometer method based on feature point fusion and key frame selection optimization according to claim 1, characterized in that: The step S3 further comprises: Step S301, calculate the transformation matrix T between the current frame and its reference key frame, then: Among them, R is the rotation matrix from the reference key frame to the current frame, and t is the translation vector from the reference key frame to the current frame; Step S302, taking out the rotation vector R and the translation vector t from the transformation matrix T, converting R into the rotation vector ω, according to the Rodrigues formula: R=cosθI+(1-cosθ)nn T +sinθn ∧ Where θ is the rotation angle, I is the unit matrix, and n is the rotation axis. The rotation vector is expressed as ω=θn, and then the relative rotation ΔR and relative translation Δt from the reference key frame to the current frame can be obtained by taking the modulus of ω and t; Step S303, parameter ΔP represents the magnitude of the relative motion between frames, and its formula is: ΔP=(1-α)∥Δt∥+α∥ΔR∥ Where α is the weight coefficient, which is used to balance the weight between rotation and translation. The formula is: Compared with translation, rotation is more likely to cause difficulty in matching feature points, making tracking difficult, so the weight α is set to increase exponentially with the increase in the amount of rotation; Step S304, select a threshold ΔP ％ , with the rotation amount ∥ΔR∥ as the main consideration; when the camera angle rotates more than 30°, it corresponds to ΔP ％ =0.4, create a keyframe.

4. The visual odometer method based on feature point fusion and key frame selection optimization according to claim 1, characterized in that: The step S4 further comprises: Step S401, the key frames screened by the tracking thread are inserted into the local mapping thread, and feature matching is performed on the new key frames and their co-viewing key frames; Step S402, generating new map points by triangulating the feature points after the new key frame and its common view key frame are matched; Step S403, the new key frame is then passed to the closed-loop detection thread, and the system quickly matches the historical key frames based on the bag-of-words model to find the closed-loop candidate key frame; Step S404, after finding the closed-loop candidate keyframe, the system calculates the similarity transformation through feature point matching; Step S405, when the common view key frame of the new key frame and the closed-loop candidate key frame also satisfies the current similarity transformation, it is considered that the closed-loop is successful; Step S406, after the loop is closed successfully, the closed-loop thread performs global posture graph optimization to correct the drift accumulated over a long period of time.

Images