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

Abstract

The application discloses a kind of visual odometry methods based on feature point fusion and key frame selection optimization, step S1, extract ORB feature point and XFeat feature point from input image;Step S2, after image sequence is inserted into tracking thread, judge the system state at this time, according to system state, select appropriate feature point to initialize and tracking thread of system;Step S3, after tracking thread ends, new key frame is needed, at this time, the camera pose calculated by tracking thread is used to judge whether key frame needs to be generated using improved key frame selection condition;Step S4, according to the key frame filtered by tracking thread to carry out local mapping thread and loop detection thread.The technical scheme of the application uses the advantages of low-texture high-recognition rate of Xfeat feature point and high-computing-efficiency of ORB feature point fusion, to improve the accuracy and robustness of visual odometry in low-texture scene.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot simultaneous localization and mapping, and particularly relates to a visual odometry method based on feature point fusion and key frame selection optimization. BACKGROUND

[0002] Simultaneous Localization And Mapping (SLAM) has gradually become a hot topic in the field of artificial intelligence applications such as autonomous driving, AR / VR in recent years, and it plays an irreplaceable role. SLAM is divided into laser SLAM and visual SLAM according to the type of sensor. Compared with laser sensors, cameras are low in cost and rich in environmental information, so the sensor of SLAM is gradually changing from laser radar to vision. As one of the most advanced technologies for robot localization and mapping, Visual Simultaneous Localization and Mapping (VSLAM) mainly relies on the picture information provided by the camera sensor, so that the robot can calculate its own position and build an environmental map in an unknown environment, and the two are mutually dependent. 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 in performance, supporting monocular, binocular, and RGB-D cameras, and having a complete SLAM framework for visual odometry and back-end. However, when faced with low texture and rapid rotation, the visual odometry of ORB-SLAM3 cannot perform accurate tracking, which greatly reduces the accuracy and robustness of the entire SLAM system.

[0003] The existing SLAM method has the following two problems: (1) In the face of low texture, it is difficult to extract feature points, which leads to difficulty in feature matching and tracking thread loss of visual odometry. (2) In the face of rapid rotation of the camera, the key frame interval is large, which leads to difficulty in feature matching and tracking loss of visual odometry.

[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

[0005] Therefore, it is necessary to provide a visual odometer method based on feature point fusion and key frame selection optimization, which extracts ORB feature points and XFeat feature points from input images, fuses Xfeat feature points with low texture and high recognition rate and ORB feature points with high calculation efficiency, improves the accuracy and robustness of visual odometer in low texture scenes, inserts image sequences into a tracking thread, judges the system state at this time, selects feature point initialization and tracking according to the system state, uses an improved key frame selection condition to judge whether a key frame needs to be generated according to the pose transformation of the camera, improves the robustness of the camera in the case of fast rotation by using the pose transformation of the camera as the key frame selection condition, and finally performs local mapping and loop detection threads according to the key frames.

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

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

[0008] Step S1, ORB feature points and XFeat feature points are extracted from input images for subsequent system initialization and tracking;

[0009] Step S2, after inserting image sequences into a tracking thread, the system state at this time is judged, and appropriate feature points are selected for system initialization and tracking threads according to the system state;

[0010] Step S3, after the tracking thread ends, a key frame is created, and the camera pose calculated by the tracking thread is used to judge whether a key frame needs to be generated by using an improved key frame selection condition;

[0011] Step S4, local mapping and loop detection threads are performed according to the key frames selected by the tracking thread.

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

[0013] Step S201, when the system has no pose, the system state is set to “need initialization”, and then ORB features and XFeat features are used for initialization at the same time; if ORB initialization is successful, XFeat initialization is stopped; otherwise, if XFeat initialization is successful, XFeat feature points are tracked, and ORB continues to attempt initialization until ORB initialization is successful after the texture is rich, and then the main feature tracking is switched; if both of them are not successfully initialized, the system continues to maintain the need for initialization state;

[0014] Step S202: after the system is initialized, the system state is set as "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 temporary map point is generated according to the geometric relationship between the previous and next frames, projection matching is performed, the pose is calculated, and the pose is optimized by constructing a minimum re-projection error;

[0015] Step S203: when the system is normally tracked, the ratio of the number of currently tracked feature points to the total number of initial tracking feature points is less than p, generally p = 0.2, then the system state is reset as "ORB recently lost" or "XFeat recently lost"; if the main feature is ORB, the XFeat feature points of the current frame and the previous frame are extracted, and the pose is calculated; when the ORB feature tracking is lost, XFeat is used for tracking; otherwise, the ORB tracking thread is constructed in advance.

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

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

[0018] Step S301: a transformation matrix T between the current frame and the reference key frame is calculated, and then:

[0019]

[0020] wherein R is a rotation matrix of the reference key frame to the current frame, and t is a translation vector of the reference key frame to the current frame;

[0021] Step S302: the rotation vector R and the translation vector t are taken out from the transformation matrix T, R is converted into a rotation vector ω, and according to the Rodrigues formula:

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

[0023] wherein θ is the rotation angle, I is a unit matrix, and n is a rotation axis, and then

[0024]

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

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

[0027] ΔP = (1 - a) ||Δt|| + a||ΔR||.

[0028] wherein a is a weight coefficient used to balance the weight between rotation and translation, and the formula is:

[0029]

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

[0031] Step S304, a threshold ΔP is selected ％ , taking the rotation amount ||ΔR|| as the main consideration factor; when the camera view rotation exceeds 30° corresponding to ΔP ％ = 0.4, a key frame is created.

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

[0033] Step S401, the key frame selected by the tracking thread is inserted into the local mapping thread, and feature matching is performed on the new key frame and its co-view key frame;

[0034] Step S402, a new map point is generated by triangulation through the feature points matched by the new key frame and its co-view key frame;

[0035] Step S403, the new key frame is further transmitted to the loop detection thread, and the system performs fast matching on the historical key frame based on the bag-of-words model to find a loop candidate key frame;

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

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

[0038] Step S406, after the loop is successful, the loop thread performs global pose graph optimization to correct the long-time accumulated drift.

[0039] Compared with the prior art, the technical scheme of the present application can greatly improve the performance of the SLAM system in the case of low texture and fast camera rotation. The present application combines the advantages of the high recognition rate of XFeat feature points in low texture and the high efficiency of ORB feature points by using the system state, thereby solving the problems of feature matching difficulty and tracking loss caused by the lack of feature points in low texture. In the optimization of the selection of key frames, when the rotation exceeds a certain condition, a key frame is created, effectively solving the problem of tracking loss caused by the large interval between key frames due to too fast rotation. Attached Figure Description

[0040] Figure 1 The flowchart illustrates the framework of a visual odometry method based on feature point fusion and keyframe selection optimization provided by this invention.

[0041] Figure 2 This is a comparison diagram of the trajectories of the present invention and the ORB-SLAM3 algorithm with the ground truth in the XYZ directions on the TUM dataset;

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

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

[0044] Current visual SLAM systems mainly consist of visual odometry, backend optimization, loop closure detection, and mapping. Visual odometry estimates camera pose in real time by analyzing changes in feature points between adjacent image frames. This process is the core of SLAM and directly affects the system's localization accuracy. However, in low-texture scenarios, ORB feature point extraction becomes difficult, impacting the overall system's accuracy and robustness. XFeat feature points have good discriminative power in low-texture conditions, and fusing them with ORB feature points according to the system state is highly suitable for improving the accuracy and robustness of visual odometry. When the camera rotates too quickly, using rotation as a condition for adding keyframes and reducing the keyframe interval during rotation can effectively improve the robustness of visual odometry.

[0045] An embodiment of the present invention provides a visual odometry method based on feature point fusion and keyframe selection optimization. Figure 1 The diagram shows the architecture of the visual odometry method based on feature point fusion and keyframe selection optimization of the present invention, which includes the following steps: Step S1, extract 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 for system initialization and tracking thread based on the system state; Step S3, after the tracking thread ends, keyframes need to be created. At this time, the improved keyframe selection condition is used to determine whether keyframes need to be generated based on the camera pose calculated by the tracking thread; Step S4, perform local mapping thread and loop closure detection thread based on the keyframes selected by the tracking thread.

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

[0047] Step S21, the system state is set to "need initialization" when the system is initially without pose, and then the initialization is performed using both ORB feature points and XFeat feature points, with the ORB feature points as the main feature and the XFeat feature points as the auxiliary feature. When the image is full of delicate and complex patterns and details, the system uses the ORB feature points to perform multi-frame feature matching, and when the number of matches exceeds the set minimum threshold and the initial pose is successfully calculated based on the matching points, it is determined that the initialization is successful, and the initialization process of the XFeat feature point thread should be stopped. Conversely, when the image lacks changes and fine features, the system uses the XFeat feature points to perform tracking after the initialization is successful, and the ORB feature point thread continues to attempt initialization until the image is full of delicate and complex patterns and details, and then the ORB feature point tracking is switched. If both of them fail to initialize, the system continues to keep the state of "need initialization".

[0048] Step S22, after the initialization is completed, 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 temporary map point is generated according to the geometric relationship between the previous and next frames, projection matching is performed, the pose is calculated, and the pose is optimized by constructing a minimum reprojection error.

[0049] Step S23, when the system is in normal tracking, the ratio of the number of currently tracked feature points to the total number of initial tracking feature points is less than p (generally p = 0.2), and the system state is reset to "ORB recently lost" or "XFeat recently lost". If the main feature is ORB, the XFeat feature points of the current frame and the previous frame are extracted, and the pose is calculated. When the ORB feature tracking is lost, the XFeat is used for tracking. Conversely, the ORB tracking thread is constructed in advance.

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

[0051] In an embodiment of the present application, step S3 further comprises:

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

[0053]

[0054] Wherein, R is the rotation matrix of the reference key frame to the current frame, and t is the translation vector of the reference key frame to the current frame.

[0055] Step S32, the rotation vector R and the translation vector t are taken out from the transformation matrix T. R is converted into a 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. It can be obtained that

[0058]

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

[0060] Step S33, the parameter ΔP represents the size of the inter-frame relative motion amount, and the formula is:

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

[0062] where α is a weight coefficient for balancing the weight between rotation and translation, and the formula is:

[0063]

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

[0065] Step S34, a threshold value ΔP ％ is selected, and the rotation amount ∥ΔR∥ is mainly considered. When the camera view rotation exceeds 30°, corresponding to ΔP ％ = 0.4, a key frame is created.

[0066] In an embodiment of the present application, step S4 further includes:

[0067] Step S41, the key frame selected by the tracking thread is inserted into the local mapping thread, and feature matching is performed on the new key frame and its co-visible key frame.

[0068] Step S42, a new map point is generated by triangulation through the feature points matched by the new key frame and its co-visible key frame.

[0069] Step S43, the new key frame is passed into the loop detection thread, and the system performs fast matching on historical key frames based on the bag-of-words model to find a loop candidate key frame.

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

[0071] Step S45: When the co-view keyframes of the new keyframe and the candidate keyframes for loop closure also satisfy the current similarity transformation, the loop closure is considered successful.

[0072] Step S46: After successful loop closure, the loop closure thread will perform global attitude graph optimization to correct the drift accumulated over a long period of time.

[0073] The input data used in this invention was obtained from the Technical University of Munich (Technische University of Munich). The TUM dataset released by München includes datasets in the Robot SLAM and Structure vs. Texture directories. The Robot SLAM dataset is designed for ground robots to collect information in large indoor environments and contains a large number of samples with abrupt viewpoint changes, large rotations, and motion blur. The Structure vs. Texture dataset, on the other hand, divides multiple subsets of the dataset into weakly textured, weakly structured, richly textured, and richly structured scenes based on the texture and structural features of the scene. The simulation platform used in this paper is a computer with an Intel(R) Core(TM) i5-8300H CPU@2.30GHz and 16GB of RAM. All video sequences from the datasets were applied to this experiment, and ORB, XFeat, and fused feature points were used to observe the initialization and tracking effects of visual odometry. The robustness of the system was also observed after keyframe selection optimization.

[0074] To verify the technical effects of this invention, its effectiveness will be verified through algorithmic comparison:

[0075] In the comparison between this method and the original ORB-SLAM3 method, the median value of each method running ten times was used as the final comparison result. The root mean square error (RMSE) of the absolute trajectory was used as the measurement metric for comparison, and the improvement in accuracy and stability compared to the original algorithm was calculated. A detailed comparison was then performed using the str_tex_far image sequence.

[0076] See Table 1. Figure 2 , Figure 3 The table shows a comparison of experimental data results between the present invention and the original ORB-SLAM3 algorithm. Table 1 shows the accuracy comparison and improvement between the present invention and the original ORB-SLAM3 algorithm.

[0077] Here, Lost indicates that the tracking was lost, and None indicates that the initialization was successful. Figure 2 The figures show a comparison of the trajectories of this invention and the original ORB-SLAM3 algorithm in the str_notex_far sequence.

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

[0079]

[0080] The above Table 1 lists the accuracy and robustness of the present application compared with the ORB-SLAM2 original algorithm and the improvement, and the rotation scene is included in the three sequences of pioneer_360, pioneer_slam, pioneer_slam2, and the initialization can be completed using ORB, XFeat feature points, but due to the rotation of the camera, it cannot track enough feature points, so it causes tracking failure. After adding the key frame optimization selection, the system can track the entire process, and the robustness of the system is improved. From the floor and str_notex_far two low-texture sequences, it can be seen that the ORB feature points cannot track in low-texture, and when the XFeat feature points are added, the system can complete the entire process of tracking. For example 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 description of the embodiments is only used to help understand the method of the present application and its core idea. It should be noted that for those skilled in the art, without departing from the principles of the present application, some improvements and modifications can be made to the present application, and these improvements and modifications also fall within the protection scope of the claims of the present application.

[0082] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application will not be limited to the embodiments shown herein, but will conform 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 keyframe selection optimization, characterized in that, Includes the following steps: Step S1: Extract 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 current system state. Based on the system state, select appropriate feature points to initialize the system and the tracking thread. Step S3: After the tracking thread ends, a new keyframe is created. At this time, the camera pose calculated by the tracking thread is used to determine whether a keyframe needs to be generated. Step S4: Perform local mapping thread and loop closure detection thread based on the keyframes filtered by the tracking thread; Step S2 further includes: Step S201: If the system initially has no pose, the system state is set to "initialization required". Then, ORB features and XFeat features are used simultaneously for initialization. If ORB is successfully initialized, XFeat initialization stops. Otherwise, if XFeat is successfully initialized, XFeat feature point tracking is used, and ORB continues to attempt initialization until the texture is rich enough and ORB initialization is successful. Then, the system switches to main feature tracking. If neither of them is successfully initialized, the system remains in the "initialization required" state. Step S202: After the system completes initialization, the system state is set to "ORB normal tracking" or "XFeat normal tracking". If the main feature is ORB, constant velocity tracking or reference keyframe tracking is used. If the main feature is XFeat, geometric relationships are constructed based on the two frames before and after, temporary map points are generated, projection matching is performed, pose is calculated, and then the pose is optimized by minimizing the reprojection error. Step S203: During normal system tracking, the ratio of the number of currently tracked feature points to the initial total number of tracked feature points is less than... ,Pick =0.2, then reset the system state to "ORB recently lost" or "XFeat recently lost"; if the main feature is ORB, then extract the XFeat feature points of the current frame and the previous frame and calculate the pose; when ORB feature tracking is lost, XFeat is used for tracking; otherwise, an ORB tracking thread is built in advance. Step S24: When the system status is "Recently Lost", if both features are lost during tracking, the system status is reset to "Tracking Lost". At this time, since neither feature point can be tracked, the system status is reset to "Requires Initialization". Step S3 further includes: Step S301: Calculate the transformation matrix between the current frame and its reference keyframe. ,but: ； in, The rotation matrix from the reference keyframe to the current frame, The translation vector from the reference keyframe to the current frame; Step S302: Extract the rotation vector from the transformation matrix T. Translation vector ,Will Convert to rotation vector According to Rodrigues' formula: ； in The size of the rotation angle. It is the identity matrix. Let be the axis of rotation, then we get ； The rotation vector is represented as Then to and The modulus is used to obtain the relative rotation from the reference keyframe to the current frame. and relative translation ; Step S303, parameters The formula for representing the relative motion between frames is: ； in These are weighting coefficients used to balance the weights between rotation and translation; their formula is: ； Compared to translation, rotation is more likely to cause difficulties in feature point matching, making tracking more challenging. Therefore, weights are set. It increases exponentially with increasing rotational speed; Step S304, select a threshold , in terms of rotation The primary consideration is when the camera's field of view rotates more than 30°. Create a keyframe at that time; Step S4 further includes: Step S401: The keyframes selected by the tracking thread are inserted into the local mapping thread, and feature matching is performed on the new keyframes and their co-view keyframes. Step S402: Triangulate the feature points of the new keyframe and its co-view keyframe to generate new map points. In step S403, the new keyframe is then passed to the loop closure detection thread. The system uses the bag-of-words model to quickly match historical keyframes and find loop closure candidate keyframes. In step S404, after finding the closed-loop candidate keyframe, the system calculates the similarity transformation through feature point matching. Step S405: When the co-view keyframes of the new keyframe and the loop closure candidate keyframes also satisfy the current similarity transformation, the loop closure is considered successful. Step S406: After successful loop closure, the loop closure thread performs global attitude graph optimization to correct the drift accumulated over a long period of time.

Images