Visual odometry method fusing mid-level semantic information

Abstract

The application discloses a visual odometer method fusing mid-term semantic information, comprising the following steps: S1. a tracking thread receives a binocular image stream, and delivers a monocular image to a semantic segmentation process; the semantic segmentation process performs semantic segmentation on the monocular image, and delivers a semantic segmentation graph to the tracking thread; S2. the tracking thread constructs a current frame according to the semantic segmentation graph and generates an initial map; local map points are matched with feature points of the current frame; the pose of the current frame is optimized; a key frame is determined; key frame information and the semantic segmentation graph are transmitted to a local mapping thread; S3. the local mapping thread updates map points according to the feature points of the key frame, constructs a mid-term sliding window and updates semantic map points of the mid-term sliding window, establishes a mid-term constraint according to the semantic map points and optimizes the pose of the key frame; S4. the local mapping thread reversely projects the map points to the semantic segmentation graph by using the optimized pose, updates semantic information of the semantic map points, and manages the semantic map points according to the quality of the semantic map points.

Description

Technical Field

[0001] This invention relates to the field of vision-based intelligent vehicle positioning and navigation, and more specifically, to an intelligent vehicle visual odometry method that integrates semantic feature mid-term data association. Background Technology

[0002] Intelligent vehicles are mobile terminals equipped with multiple high-precision sensors, involving technologies such as driving environment perception, vehicle navigation and positioning, motion planning and decision-making, and vehicle control, capable of autonomous and safe driving on roads. Navigation and positioning are key technologies for vehicle intelligence, primarily responsible for estimating the vehicle's attitude and position, providing spatial information for path planning and decision-making control in driving tasks, and are an important prerequisite for achieving autonomous driving in intelligent vehicles. Odometer-based positioning methods are important methods for calculating position changes between adjacent time points based on sensor data. Among these, cameras are the most common and inexpensive sensors, leading to extensive and in-depth research on visual odometry. Visual odometry acquires image sequences of the environment from onboard cameras and determines the vehicle's position and attitude changes between adjacent time points based on the multi-view geometric relationships between images, thereby obtaining the vehicle's pose information. Currently, the tasks of visual odometry mainly consist of three parts: feature extraction, data association, and pose estimation. Currently, feature extraction employs feature point extraction or direct extraction methods. The collected features are low-level features derived from pixel grayscale relationships, which change with distance and can only be used for short-term data association. If there are errors in pose estimation, these errors will gradually accumulate, leading to severe pose drift. Furthermore, low-level features have poor stability in environments with large variations in lighting conditions and missing texture features, making it impossible to correctly match and establish reliable associations. This results in increased inter-frame pose estimation errors, causing positioning deviations in intelligent vehicles.

[0003] In recent years, the rapid development of artificial intelligence technology has led to revolutionary progress in many key problems in computer vision. For example, intelligent agents can now identify boundaries between different types of objects in a scene (semantic segmentation). These technologies enable agents to more easily acquire semantic features of the environment, resulting in a deeper understanding of it. Semantic features include object detection categories and semantic segmentation pixel-level categories. Semantic segmentation pixel-level category information refines scene categories down to the pixel level, and its data format is relatively consistent with the feature points in visual odometry. This feature is almost unaffected by distance, remains stable over a long period, and can be observed within a certain distance, establishing data associations across several frames. It establishes constraints between the current frame and earlier keyframes, thereby mitigating the accumulation of pose estimation errors and improving odometry trajectory drift. Furthermore, semantic features have low dependence on changes in lighting conditions and texture information. Therefore, an increasing number of researchers have proposed fusion methods that incorporate semantic information into visual odometry to improve positioning accuracy.

[0004] Researchers have proposed a keyframe sliding window structure to establish semantic error edges. This method first constructs semantic error edges and ORB feature reprojection error edges, and then builds a visual odometry that fuses the semantic errors. Next, it adds the few keyframes closest to the current keyframe to the sliding window, and then constructs feature associations with the current frame based on map points observed from these keyframes. The drawback of this method is that the semantic feature associations between keyframes in the sliding window and the current keyframe largely overlap with the constraints of ORB feature-based feature associations, resulting in minimal improvement in system localization accuracy and failing to establish reasonable and effective semantic feature associations.

[0005] Researchers proposed VisualSemantic Odometry (VSO), which incorporates semantic feature associations into Bundle Adjustment (BA) optimization. This method uses a distance transformation function to model the likelihood observation model of semantic features, establishing a semantic error term and forming a visual odometry system that fuses semantics. The drawbacks of this method are that it requires multiple binarization operations on the semantic segmentation map before calculating the distance transformation function, and the distance transformation function itself needs to calculate the nearest distance from all pixels to all categories, resulting in low computational efficiency and redundancy in the likelihood observation model. Summary of the Invention

[0006] The purpose of this invention is to solve the problems of weak semantic feature association and redundancy of semantic observation likelihood models in the prior art, and to provide a visual odometry method that integrates mid-term semantic information. This method can establish a more efficient semantic feature observation model and form a more targeted semantic error term to improve the pose estimation accuracy of visual odometry that integrates semantics.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0008] A visual odometry method incorporating intermediate semantic information includes the following steps:

[0009] S1. The tracking thread receives the stereo image stream from the stereo camera and passes the monocular image to the semantic segmentation process. The semantic segmentation process performs semantic segmentation on the monocular image and passes the semantic segmentation map to the tracking thread.

[0010] S2. The tracking thread constructs the current frame based on the semantic segmentation map and generates an initial map to track the pose of the stereo camera. It matches the local map points with the feature points of the current frame, optimizes the pose of the current frame, determines the key frame, and transmits the key frame information and semantic segmentation map to the local mapping thread.

[0011] S3. The local mapping thread updates map points based on the feature points of keyframes, constructs a mid-term sliding window and updates the semantic map points of the mid-term sliding window, establishes mid-term constraints based on the semantic map points and optimizes the pose of keyframes.

[0012] S4. The local mapping thread uses the optimized pose to back-project map points onto the semantic segmentation map, updates the semantic information of the semantic map points, and manages the semantic map points based on their quality information.

[0013] Furthermore, in step S1, the semantic segmentation process uses a lightweight neural network ERFNet to perform semantic segmentation on the monocular image, extracts semantic features, and passes the semantic segmentation map to the tracking thread.

[0014] Further, in step S2, the tracking thread extracts ORB feature points from the image pyramid, initializes map points through triangulation, calculates the initial camera pose through inter-frame feature point matching, matches local map points with feature points in the current frame, establishes appropriate data associations, optimizes the pose of the current frame by minimizing reprojection error, and selects whether to establish a keyframe based on the observation quality of the current frame and the state of the system. If a keyframe is established, it is passed to the local mapping thread.

[0015] Further, in step S3, a mid-term sliding window is constructed and its semantic map points are updated. Mid-term constraints are established based on the semantic map points, and keyframe poses are optimized. Specifically:

[0016] S31. Use a set number of keyframes before the current keyframe as a sliding window container for managing the semantic information of map points, select the first few frames as the intermediate sliding window, and continuously update the sliding window over time;

[0017] S32. Triangulate the ORB feature points in the intermediate sliding window to obtain semantic map points;

[0018] S33. Design the semantic map point semantic categories as a multi-weighted pattern;

[0019] S34. Establish semantic observations between the selected semantic map points and the current keyframe, and calculate the semantic observation error;

[0020] S35. Define the image frame that has a common map point with the current keyframe as a local keyframe, and obtain the local map point by triangulation of the ORB feature points in the local keyframe.

[0021] S36. Calculate the reprojection error based on the local map points and the current keyframe;

[0022] S37. Incorporate semantic observation error and reprojection error into the local BA optimization graph optimization model to optimize the pose and map points of keyframes.

[0023] Further, in step S34, semantic observations are established between the selected semantic map points and the current keyframe, and the semantic observation error is calculated, specifically as follows:

[0024] S341. Define the semantic observation likelihood model as follows:

[0025] p(S k |Z i =c,X i ,T k );

[0026] In the formula, k is the keyframe number, c is a semantic category, i is the map point number, and T k For the vehicle's pose, X i Z represents the location of a point on the map. i For map point categories, S k This is the semantic segmentation map of the current frame;

[0027] S342. Based on the vehicle pose, calculate the pixel coordinates of the map points projected onto the current keyframe. i,k Represents the coordinates of the projected pixel point i on the map;

[0028] Marginalizing the observation likelihood with respect to the projected pixel coordinates, according to Bayes' theorem, is as follows:

[0029]

[0030] S343. Semantic segmentation map of the current frame S k Independent of other variables, the projected pixels are determined by the map point positions and the vehicle pose, resulting in:

[0031]

[0032] S344. Assuming semantic segmentation graph S k The prior probabilities of each class are the same. According to Bayes' theorem, p(S) can be calculated. k |u i,k Z i =c) is represented as:

[0033] p(S k |u i,k Z i =c)∝p(u i,k Z i =c|S k )=p(u i,k |S k Z i =c)p(Z i =c|S k );

[0034] S345.p(ui,k |X i ,T k ) represents the error likelihood of map point reprojection in the current keyframe. Within the acceptable error range, the projection probability is always 1, resulting in:

[0035] p(S k |Z i =c,X i ,T k )=p(u i,k |S k Z i =c)p(Z i =c|S k );

[0036] S346.p(ui,k|S k Z i =c) indicates that given map point i is of class c and the semantic segmentation map of frame k is S k When the pixel coordinates of map point i are projected, the pixel coordinates are u. i,k The probability that the projection point of the map point is far from S k The closer the semantic region (class c) is, the higher the probability. A pruning-optimized breadth-first search nearest neighbor algorithm is used, and this probability is represented by the local nearest neighbor distance:

[0037]

[0038] In the formula, S represents k The distance between class c pixels and the projected pixel u i,k The minimum distance, σ represents the pixel offset predicted by semantic segmentation, α k,c It is a normalization factor that makes the sum of the probabilities of all pixels equal to 1;

[0039] S347.p(Z i =c|S k ) represents the semantic segmentation graph S k The probability of a pixel of class c occurring is represented by the Gaussian probability of the pixel under the given semantic category:

[0040]

[0041] In the formula, α k It is α k,c The sum of normalization factors for different categories c;

[0042] S348. The semantic observation error is:

[0043]

[0044] S349. Exponential probabilities have a better mathematical form under negative logarithms, which can be used to represent the semantic-based pose estimation problem as follows:

[0045]

[0046] Where S represents all semantic segmentation maps in the system, θ represents all map points to be optimized and vehicle poses, and E sem (θ) represents the sum of all semantic observation errors, and we obtain

[0047]

[0048] Wherein, π(T) k ,X i ) represents pose T k Click X on the map below i The coordinates of the projected pixel, e sem (k,i) represents the error edge formed by the pose observation of the i-th map point in the k-th frame, which is related to the semantic segmentation confidence and the nearest neighbor distance.

[0049] Furthermore, α k,c and α k The calculation formula is:

[0050]

[0051] Furthermore, in step S37, the semantic observation error and reprojection error are added to the graph optimization model of local BA optimization to optimize the pose and map points of the keyframes, specifically as follows:

[0052] Because neural network predictions have inherent errors, map point projection pixels may fall into multiple classes. The observation error requires calculating nearest neighbor distances for these classes. A Gaussian probability representation of the pixel's class weight under a given semantic class is used to calculate the semantic observation error through a weighted summation. The semantic error model considering map point class weights is as follows:

[0053]

[0054] in, This represents the semantic weight of map point i in category c.

[0055] Furthermore, to reduce the impact of semantic feature mismatches, the system removes error edges with excessive semantic errors and performs optimization in two stages;

[0056] The first stage sets the Huber kernel function to limit excessive error and solves the graph optimization problem. Then, based on the running status of the tracing thread, it is decided whether to perform the second stage of optimization.

[0057] In the second stage, error edges with large semantic errors are removed, and no kernel function is set to solve the graph optimization problem.

[0058] Furthermore, in step S4, the local mapping thread uses the optimized pose to back-project map points onto the semantic segmentation map, updating the semantic information of the semantic map points, specifically:

[0059] Using the optimized pose from the keyframes, the map points in the keyframes are back-projected onto the semantic segmentation map of the keyframes.

[0060] The local mapping thread uses projection relationships and semantic segmentation maps to update the semantic weight information of map points in keyframes. The calculation formula is as follows:

[0061]

[0062] Where α is the normalization factor, ensuring When a keyframe is added to the local mapping thread, the map point weights are updated once according to the above formula.

[0063] Further, in step S4, the semantic map points are managed based on their quality information, specifically as follows:

[0064] If a map point cannot be continuously observed across multiple frames of images since its generation, then that map point is removed from the semantic map points.

[0065] If the semantic weight of a map point remains dispersed after multiple frames of observation since its generation, then that map point is removed from the semantic map points.

[0066] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0067] 1. This invention uses a mid-term keyframe sliding window structure as a semantic feature manager. Compared to previous methods that establish feature associations using the few keyframes closest to the current keyframe, this invention can establish semantic feature associations that better conform to the mid-term constraints of semantic features, estimating more accurate poses with the same number of semantic error edges. The semantic feature associations selected by this method can complement the error edges of ORB features, enhancing the mid-term association of odometry, effectively mitigating odometry pose drift, and improving the positioning accuracy of visual odometry.

[0068] 2. This invention proposes a semantic feature observation likelihood model based on local nearest neighbor distance. Compared with previous methods that use distance transformation functions to model semantic feature observation likelihood models, this invention uses a breadth-first search method within a small area of ​​the map projection point to calculate local nearest neighbor distances. This eliminates the image binarization preprocessing required for establishing the semantic error term and reduces the calculation of meaningless projection point distances during the modeling process, making the semantic feature observation likelihood model more concise and efficient. Furthermore, this invention only calculates the nearest neighbors of the most important category of the projection point, simplifying the calculation of the semantic error term and ensuring that this modeling method effectively constrains the pose, thus significantly improving the computational efficiency of establishing the semantic error term. Attached Figure Description

[0069] Figure 1 This is a flowchart illustrating the visual odometry method that integrates intermediate semantic information according to the present invention.

[0070] Figure 2 This is a schematic diagram illustrating the specific process of the visual odometry method that integrates intermediate semantic information according to the present invention.

[0071] Figure 3 This is a partial schematic diagram of the visual odometry method that integrates intermediate semantic information according to the present invention. Detailed Implementation

[0072] The visual odometry method integrating intermediate semantic information of the present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0073] Please see Figure 1 and Figure 2 This invention discloses a visual odometry method that integrates intermediate semantic information, comprising the following steps:

[0074] S1. The tracking thread receives the stereo image stream from the stereo camera and passes the monocular image to the semantic segmentation process. The semantic segmentation process performs semantic segmentation on the monocular image and passes the semantic segmentation map to the tracking thread.

[0075] S2. The tracking thread constructs the current frame based on the semantic segmentation map and generates an initial map to track the pose of the stereo camera. It matches the local map points with the feature points of the current frame, optimizes the pose of the current frame, determines the key frame, and transmits the key frame information and semantic segmentation map to the local mapping thread.

[0076] S3. The local mapping thread updates map points based on the feature points of keyframes, constructs a mid-term sliding window and updates the semantic map points of the mid-term sliding window, establishes mid-term constraints based on the semantic map points and optimizes the pose of keyframes.

[0077] S4. The local mapping thread uses the optimized pose to back-project map points onto the semantic segmentation map, updates the semantic information of the semantic map points, and manages the semantic map points based on their quality information.

[0078] This invention can accurately track the camera's pose while performing semantic segmentation of the scene and building a local map containing semantic information. The method uses the local nearest neighbor distance of map projection points as a semantic observation likelihood model, constructing an error term for semantic observation and achieving nonlinear optimization of the stereo camera pose. Compared to previous methods that require calculating the global nearest neighbor distance of all pixels in the entire image before constructing the semantic error term, this invention can construct the semantic error term while reducing the number of pixel nearest neighbor distance calculations, ensuring pose estimation accuracy. Addressing the issue of insufficient specificity in semantic feature association, this invention constructs a map point management structure based on a mid-term sliding window, building semantic feature associations suitable for mid-term constraints, and achieving effective constraint of the semantic error term on the nonlinear pose optimization process.

[0079] Specifically, in step S1, the tracking thread receives the stereo image stream from the stereo camera, then preprocesses the left image from the stereo camera, adjusts its size to 1024*256, and then sends the adjusted image to the ERFNet prediction node under PyTorch for semantic segmentation through the Robot Operating System (ROS) node.

[0080] The semantic segmentation process uses a lightweight neural network ERFNet to predict images, extract semantic features, and convert the semantically segmented information into ROS image message format, then passes the semantic segmentation map to the tracking thread.

[0081] In step S2, after receiving the semantic segmentation map, the tracking thread resizes it to the original image size, converts the input image into a grayscale image, and then uses the semantic segmentation map to construct the current frame. During the construction of the current frame, ORB (Oriented Fast and Rotated BRIEF) features are first extracted uniformly from the image pyramid using a quadtree structure for both the left and right images. Then, the fastest approximate nearest neighbor algorithm is used to match feature points between adjacent frames.

[0082] After the current frame is constructed, an attempt is made to initialize tracking. If the extracted feature points in the image are rich enough and there are enough direct feature point matches between the stereo images, then triangulation is used to generate initial map points, and initialization is complete. Otherwise, subsequent frames are used to continue attempting initialization.

[0083] After initialization, the system begins tracking the stereo camera. If the camera has recorded velocity during the current frame tracking, the pose of the previous frame is first updated based on the relative pose between the previous frame and the previous reference frame. Then, the pose of the current frame is predicted using a constant velocity model. After matching, a 3D-2D pose estimation is performed to obtain the pose of the current frame. Then, it is matched with points in the local map to further optimize the 3D-2D pose. If the constant velocity model has not yet been generated, such as after initialization or after relocalization, it needs to be matched with the reference keyframe. The matching is accelerated using a bag of words [Dorian Gálvez-López, JuanDTardos. Bags of binary words for fast place recognition in imagesequences. IEEE Transactions on Robotics, 2012, 28(5): 1188-1197.]. After matching, it is also matched with the local map to finally optimize and obtain the pose of the current frame. If tracking fails, relocalization is performed.

[0084] After tracking is complete, the system selects whether to designate the current frame as a keyframe based on the observation quality and system status. If a keyframe is generated, the system stores the data in the keyframe's data structure and passes it to the local mapping thread.

[0085] Please see Figure 2 and Figure 3 In step S3, a mid-term sliding window is constructed and its semantic map points are updated. Mid-term constraints are established based on the semantic map points, and keyframe poses are optimized. Specifically:

[0086] S31. Use a set number of keyframes before the current keyframe as a sliding window container for managing the semantic information of map points, select the first few frames as the intermediate sliding window, and continuously update the sliding window over time;

[0087] S32. Triangulate the ORB feature points in the intermediate sliding window to obtain semantic map points;

[0088] S33. Design the semantic map point semantic categories as a multi-weighted pattern;

[0089] S34. Establish semantic observations between the selected semantic map points and the current keyframe, and calculate the semantic observation error;

[0090] S35. Define the image frame that has a common map point with the current keyframe as a local keyframe, and obtain the local map point by triangulation of the ORB feature points in the local keyframe.

[0091] S36. Calculate the reprojection error based on the local map points and the current keyframe;

[0092] S37. Incorporate semantic observation error and reprojection error into the local BA optimization graph optimization model to optimize the pose and map points of keyframes.

[0093] In step S34, semantic observations are established between the selected semantic map points and the current keyframe, and the semantic observation error is calculated, specifically as follows:

[0094] S341. Define the semantic observation likelihood model as follows:

[0095] p(S k |Z i =c,X i ,T k );

[0096] In the formula, k is the keyframe number, c is a semantic category, i is the map point number, and T k For the vehicle's pose, X i Z represents the location of a point on the map. i For map point categories, S k This is the semantic segmentation map of the current frame;

[0097] S342. Based on the vehicle pose, calculate the pixel coordinates of the map points projected onto the current keyframe. i,k Represents the coordinates of the projected pixel point i on the map;

[0098] Marginalizing the observation likelihood with respect to the projected pixel coordinates, according to Bayes' theorem, is as follows:

[0099]

[0100] S343. Semantic segmentation map of the current frame S k Independent of other variables, the projected pixels are determined by the map point positions and the vehicle pose, resulting in:

[0101]

[0102] S344. Assuming semantic segmentation graph S k The prior probabilities of each class are the same. According to Bayes' theorem, p(S) can be calculated. k |u i,k Z i =c) is represented as:

[0103] p(S k |u i,k Z i =c)∝p(u i,k Z i =c|S k )=p(u i,k |S k Z i =c)p(Zi =c|S k );

[0104] S345.p(u i,k |X i ,T k ) represents the error likelihood of map point reprojection in the current keyframe. Within the acceptable error range, the projection probability is always 1, resulting in:

[0105] p(S k |Z i =c,X i ,T k )=p(u i,k |S k Z i =c)p(Z i =c|S k );

[0106] S346.p(ui,k|S k Z i =c) indicates that given map point i is of class c and the semantic segmentation map of frame k is S k When the pixel coordinates of map point i are projected, the pixel coordinates are u. i,k The probability that the projection point of the map point is far from S k The closer the semantic region (class c) is, the higher the probability. A pruning-optimized breadth-first search nearest neighbor algorithm is used, and this probability is represented by the local nearest neighbor distance:

[0107]

[0108] In the formula, S represents k The distance between class c pixels and the projected pixel u i,k The minimum distance, σ represents the pixel offset predicted by semantic segmentation, α k,c It is a normalization factor that makes the sum of the probabilities of all pixels equal to 1;

[0109] S347.p(Z i =c|S k ) represents the semantic segmentation graph S k The probability of a pixel of class c occurring is represented by the Gaussian probability of the pixel under the given semantic category:

[0110]

[0111] In the formula, α k It is α k,c The sum of normalization factors for different categories c;

[0112] S348. The semantic observation error is:

[0113]

[0114] In the formula, S represents k The distance between class c pixels and the projected pixel u i,k The minimum distance, σ represents the pixel offset predicted by semantic segmentation, α k,c α is the normalization factor that makes the sum of the probabilities of all pixels equal to 1. k It is α k,c The sum of normalization factors for different categories c;

[0115] S349. Exponential probabilities have a better mathematical form under negative logarithms, which can be used to represent the semantic-based pose estimation problem as follows:

[0116]

[0117] Where S represents all semantic segmentation maps in the system, θ represents all map points to be optimized and vehicle poses, and E sem (θ) represents the sum of all semantic observation errors, and we obtain

[0118]

[0119] Wherein, π(T) k ,X i ) represents pose T k Click X on the map below i The coordinates of the projected pixel, e sem (k,i) represents the error edge formed by the pose observation of the i-th map point in the k-th frame, which is related to the semantic segmentation confidence and the nearest neighbor distance.

[0120] α k,c and α k The calculation formula is:

[0121]

[0122] In step S37, semantic observation error and reprojection error are added to the graph optimization model of local BA optimization to optimize the pose and map points of keyframes, specifically as follows:

[0123] Because neural network predictions have inherent errors, map point projection pixels may fall into multiple classes. The observation error requires calculating nearest neighbor distances for these classes. A Gaussian probability representation of the pixel's class weight under a given semantic class is used to calculate the semantic observation error through a weighted summation. The semantic error model considering map point class weights is as follows:

[0124]

[0125] in, This represents the semantic weight of map point i in category c.

[0126] To reduce the impact of semantic feature mismatches, the system removes error edges with excessive semantic errors and performs optimization in two stages.

[0127] The first stage sets the Huber kernel function to limit excessive error and solves the graph optimization problem. Then, based on the running status of the tracing thread, it is decided whether to perform the second stage of optimization.

[0128] In the second stage, error edges with large semantic errors are removed, and no kernel function is set to solve the graph optimization problem.

[0129] After the local mapping thread obtains keyframes, it first adds the new keyframes to the local map. It then calculates the corresponding bag-of-words vectors from the descriptors of the feature points in the current keyframe, iterates through all map points in the current frame, and updates the normal vectors, descriptors, and other information of the map points. If the current map point is not an observation from the current frame (but from the local map), it adds an observation to the current map point and updates the average observation direction, observation distance range, and best descriptor of the map point. If the current map point is a newly generated map point during stereo camera tracking, it is added to the recently added map point queue, awaiting verification in subsequent steps. After updating the normal vectors, descriptors, and other information of the map points, the new keyframes are added to the shared view, the edges of keyframes with shared viewing relationships are updated, and then the keyframes are inserted into the map.

[0130] After a new keyframe is added to the local map, the newly added map points are checked. If the ratio of the number of frames in which the map point can be observed (with matching feature points) to the number of frames in which the map point can be back-projected into the range of ordinary frame images is less than 25%, or if at least two keyframes have passed since the map point was created but the number of frames in which the point has been observed does not exceed the threshold, then the map point is considered to be of poor quality and is removed from the map.

[0131] After removing new map points of poor quality, the adjacent keyframe with the highest degree of co-visibility is found in the co-visible keyframes of the current keyframe. Then, feature matching pairs are searched between the two keyframes and erroneous matches are removed using epipolar constraints. Finally, triangulation is performed to generate new map points.

[0132] After triangulation generates new map points, the two adjacent keyframes at each level in the co-view are obtained for the current keyframe. The map points of the current frame are projected onto the two adjacent keyframes respectively, and matching points are found. Then, the map points of the two adjacent keyframes are projected onto the current keyframe respectively, and the map points corresponding to the matching points are found and merged. Finally, the descriptor, depth, average viewing direction, and other attributes of the map points in the current frame are updated, and the co-view connectivity between the current frame and other frames is updated.

[0133] The keyframe sliding window is moved forward a corresponding distance based on the number of newly generated keyframes entering the local mapping thread. The earliest 5 keyframes in the sliding window are selected as intermediate sliding window keyframes, and the number of semantic map point observations within the intermediate sliding window is adjusted based on the map point observations of newly added and removed intermediate sliding window keyframes. If the number of semantic map point observations is zero, it is removed from the semantic map points, and the sliding window and semantic map are continuously updated.

[0134] After updating the mid-term sliding window and semantic map points, the semantic categories of map points are designed as a multi-weighted mode, and a weighted summation method is used to calculate the semantic observation error:

[0135]

[0136] In the semantic constraint pose optimization during the fusion phase of the local mapping thread, the system calculates the reprojection error by reprojecting local map points to their corresponding local keyframes. Then, it obtains the semantic error e by backprojecting semantic map points to the semantic segmentation map of the latest keyframe. sem (k,i) Finally, the two types of error edges are added to the graph optimization model optimized in the local BA for pose optimization. To reduce the impact of semantic feature mismatch, the system removes error edges with excessive semantic errors and performs optimization in two stages: the first stage of optimization sets the Huber kernel function to limit excessive errors, solves the graph optimization problem, and then selects whether to perform optimization based on the running status of the tracking thread; the second stage of optimization removes error edges with large semantic errors in the graph optimization model, no longer sets the kernel function, and solves the graph optimization problem.

[0137] Please see Figure 2 and Figure 3 In step S4, the local mapping thread uses the optimized pose to back-project map points onto the semantic segmentation map, updating the semantic information of the semantic map points, specifically:

[0138] Using the optimized pose from the keyframes, the map points in the keyframes are back-projected onto the semantic segmentation map of the keyframes.

[0139] The local mapping thread uses projection relationships and semantic segmentation maps to update the semantic weight information of map points in keyframes. The calculation formula is as follows:

[0140]

[0141] Where α is the normalization factor, ensuring

[0142] Each time a keyframe is added in the local mapping thread, the map point weights are updated according to the above formula. Then, if a map point exists for more than 5 frames but cannot be observed in 4 consecutive keyframes, it is removed from the semantic map points; if a map point exists for more than 5 frames and can be observed in at least 4 consecutive keyframes but its semantic weights are still scattered (the largest category weight of the map point is still less than 0.75), it is removed from the semantic map points.

[0143] Semantic map points are obtained through feature point triangulation. This triangulation process introduces errors, necessitating a filtering method. The semantic information of these points is predicted by a neural network, a process also subject to error. To reduce the number of unreliable points with large errors, a filtering principle is established.

[0144] Semantic map points are managed based on their quality information, specifically as follows:

[0145] The first filtering principle is: if a map point exists for more than 5 frames and cannot be observed by 4 consecutive keyframes, then the map point is removed from the semantic map points.

[0146] The second selection principle is: if a map point exists in more than 5 frames and can be observed in at least 4 consecutive keyframes, but the semantic weight of the map point is still scattered (the largest category weight of the map point is still less than 0.75), then the map point is removed from the semantic map points.

[0147] To further demonstrate the effectiveness of this invention, experiments were conducted using the KITTI dataset to verify its positioning accuracy. The algorithm's experimental platform parameters were configured as follows: software environment: Ubuntu 16.04; processor (CPU): Intel Core I7-9750H, 6 cores, 2.6GHz; memory: 16GB; graphics card: NVIDIA GTX 1660Ti; parallel computing framework: CUDA 10.1; deep learning framework: PyTorch; third-party libraries: OpenCV 3.4.3, g2o, etc. The error weight λ and neural network prediction confidence σ were selected empirically in each experimental group.

[0148] To verify the positioning accuracy of the system, the reference system of this invention establishes semantic constraints, known as CVSO, by selecting map points from a set number of keyframes prior to the latest keyframe. This invention uses the root mean square error (RMSE) of relative pose error (RPE) and absolute trajectory error (ATE) as evaluation criteria. ATE measures the absolute deviation of the pose on two trajectories. Experimental results are shown in Table 1.

[0149] Table 1. Absolute Trajectory Error (ATE) on the KITTI dataset

[0150]

[0151] In the experiment, except for the 05 sequence, the estimation accuracy of absolute trajectory for other sequences was improved, with the absolute trajectory error decreasing by an average of 11.37% and a maximum of 16.29%.

[0152] RPE calculates the deviation between the estimated pose change and the actual pose change over the same distance (time) interval to obtain the relative pose error, which can be used to evaluate the pose drift phenomenon of the odometer. The relative pose translation error per 100 meters was evaluated experimentally, and the experimental results are shown in Table 2.

[0153] Table 2. Relative Pose Error (RPE) on the KITTI dataset.

[0154]

[0155] Since semantic constraints can provide intermediate constraints to reduce odometry pose drift, LVSO reduces RPE by an average of 1.67% and up to 4.76% compared to CVSO. Using intermediate sliding windows to establish semantic constraints has a certain improvement effect on pose drift.

[0156] In summary, the present invention has the following advantages and beneficial effects:

[0157] 1. This invention uses a mid-term keyframe sliding window structure as a semantic feature manager. Compared to previous methods that establish feature associations using the few keyframes closest to the current keyframe, this invention can establish semantic feature associations that better conform to the mid-term constraints of semantic features, estimating more accurate poses with the same number of semantic error edges. The semantic feature associations selected by this method can complement the error edges of ORB features, enhancing the mid-term association of odometry, effectively mitigating odometry pose drift, and improving the positioning accuracy of visual odometry.

[0158] 2. This invention proposes a semantic feature observation likelihood model based on local nearest neighbor distance. Compared with previous methods that use distance transformation functions to model semantic feature observation likelihood models, this invention uses a breadth-first search method within a small area of ​​the map projection point to calculate local nearest neighbor distances. This eliminates the image binarization preprocessing required for establishing the semantic error term and reduces the calculation of meaningless projection point distances during the modeling process, making the semantic feature observation likelihood model more concise and efficient. Furthermore, this invention only calculates the nearest neighbors of the most important category of the projection point, simplifying the calculation of the semantic error term and ensuring that this modeling method effectively constrains the pose, thus significantly improving the computational efficiency of establishing the semantic error term.

[0159] The above description is a detailed description of the preferred embodiments of the present invention. However, the embodiments are not intended to limit the scope of the patent application of the present invention. All equivalent changes or modifications made under the technical spirit disclosed in the present invention should fall within the patent scope covered by the present invention.

Claims

1. A visual odometry method incorporating intermediate semantic information, characterized in that, Includes the following steps: S1. The tracking thread receives the stereo image stream from the stereo camera and passes the monocular image to the semantic segmentation process. The semantic segmentation process performs semantic segmentation on the monocular image and passes the semantic segmentation map to the tracking thread. S2. The tracking thread constructs the current frame based on the semantic segmentation map and generates an initial map to track the pose of the stereo camera. It matches the local map points with the feature points of the current frame, optimizes the pose of the current frame, determines the key frame, and transmits the key frame information and semantic segmentation map to the local mapping thread. S3. The local mapping thread updates map points based on the feature points of keyframes, constructs a mid-term sliding window and updates the semantic map points of the mid-term sliding window, establishes mid-term constraints based on the semantic map points and optimizes the pose of keyframes. S4. The local mapping thread uses the optimized pose to back-project map points onto the semantic segmentation map, updates the semantic information of the semantic map points, and manages the semantic map points according to their quality information. In step S2, the tracking thread extracts ORB feature points from the image pyramid, initializes map points through triangulation, calculates the initial camera pose through inter-frame feature point matching, matches local map points with feature points in the current frame, establishes appropriate data associations, optimizes the pose of the current frame by minimizing reprojection error, and selects whether to establish a key frame based on the observation quality of the current frame and the state of the system. If a key frame is established, it is passed to the local mapping thread. In step S3, a mid-term sliding window is constructed and its semantic map points are updated. Mid-term constraints are established based on the semantic map points, and keyframe poses are optimized. Specifically: S31. Use a set number of keyframes before the current keyframe as a sliding window container for managing the semantic information of map points, select the first few frames as the intermediate sliding window, and continuously update the sliding window over time; S32. Triangulate the ORB feature points in the intermediate sliding window to obtain semantic map points; S33. Design the semantic map point semantic categories as a multi-weighted pattern; S34. Establish semantic observations between the selected semantic map points and the current keyframe, and calculate the semantic observation error; S35. Define the image frame that has a common map point with the current keyframe as a local keyframe, and obtain the local map point by triangulation of the ORB feature points in the local keyframe. S36. Calculate the reprojection error based on the local map points and the current keyframe; S37. Incorporate semantic observation error and reprojection error into the graph optimization model of local BA optimization to optimize the pose and map points of keyframes. In step S4, semantic map points are managed based on their quality information, specifically as follows: If a map point cannot be continuously observed across multiple frames of images since its generation, then that map point is removed from the semantic map points. If the semantic weight of a map point remains dispersed after multiple frames of observation since its generation, then that map point is removed from the semantic map points.

2. The visual odometry method fusion of intermediate semantic information according to claim 1, characterized in that, In step S1, the semantic segmentation process uses a lightweight neural network ERFNet to perform semantic segmentation on the monocular image, extracts semantic features, and passes the semantic segmentation map to the tracking thread.

3. The visual odometry method fusion of intermediate semantic information according to claim 1, characterized in that, In step S34, semantic observations are established between the selected semantic map points and the current keyframe, and the semantic observation error is calculated, specifically as follows: S341. Define the semantic observation likelihood model as follows: p(S k |Z i =c,X i ,T k ); In the formula, k is the keyframe number, c is a semantic category, i is the map point number, and T k For the vehicle's pose, X i Z represents the location of a point on the map. i For map point categories, S k This is the semantic segmentation map of the current frame; S342. Based on the vehicle pose, calculate the pixel coordinates of the map points projected onto the current keyframe. i,k Represents the coordinates of the projected pixel point i on the map; Marginalizing the observation likelihood with respect to the projected pixel coordinates, according to Bayes' theorem, is as follows: S343. Semantic segmentation map of the current frame S k Independent of other variables, the projected pixels are determined by the map point positions and the vehicle pose, resulting in: S344. Assuming semantic segmentation graph S k The prior probabilities of each class are the same. According to Bayes' theorem, p(S) can be calculated. k |u i,k Z i =c) is represented as: p(S k |u i,k ,Z i =c)∝p(u i,k ,Z i =c|S k )=p(u i,k |S k ,Z i =c)p(Z i =c|S k ); S345.p(u i,k |X i ,T k ) represents the error likelihood of map point reprojection in the current keyframe. Within the acceptable error range, the projection probability is always 1, resulting in: p(S k ∣Z i =c,X i ,T k )=p(u i,k |S k ,Z i =c)p(Z i =c|S k ); S346.p(u i,k |S k Z i =c) indicates that given map point i is of class c and the semantic segmentation map of frame k is S k When the pixel coordinates of map point i are projected, the pixel coordinates are u. i,k The probability that the projection point of the map point is far from S k The closer the semantic region (class c) is, the higher the probability. A pruning-optimized breadth-first search nearest neighbor algorithm is used, and this probability is represented by the local nearest neighbor distance: In the formula, S represents k The distance between class c pixels and the projected pixel u i,k The minimum distance, σ represents the pixel offset predicted by semantic segmentation, α k,c It is a normalization factor that makes the sum of the probabilities of all pixels equal to 1; S347.p(Z i =c|S k ) represents the semantic segmentation graph S k The probability of a pixel of class c occurring is represented by the Gaussian probability of the pixel under the given semantic category: In the formula, α k It is α k,c The sum of normalization factors for different categories c; S348. The semantic observation error is: S349. Exponential probabilities have a better mathematical form under negative logarithms, which can be used to represent the semantic-based pose estimation problem as follows: Where S represents all semantic segmentation maps in the system, θ represents all map points to be optimized and vehicle poses, and E sem (θ) represents the sum of all semantic observation errors, and we obtain Wherein, π(T) k ,X i ) represents pose T k Click X on the map below i The coordinates of the projected pixel, e sem (k,i) represents the error edge formed by the pose observation of the i-th map point in the k-th frame, which is related to the semantic segmentation confidence and the nearest neighbor distance.

4. The visual odometry method fusion of intermediate semantic information according to claim 3, characterized in that, α k,c and α k The calculation formula is:

5. The visual odometry method fusion of intermediate semantic information according to claim 3, characterized in that, In step S37, semantic observation error and reprojection error are added to the graph optimization model of local BA optimization to optimize the pose and map points of keyframes, specifically as follows: Because neural network predictions have inherent errors, map point projection pixels may fall into multiple classes. The observation error requires calculating nearest neighbor distances for these classes. A Gaussian probability representation of the pixel's class weight under a given semantic class is used to calculate the semantic observation error through a weighted summation. The semantic error model considering map point class weights is as follows: in, This represents the semantic weight of map point i in category c.

6. The visual odometry method fusion of intermediate semantic information according to claim 5, characterized in that, To reduce the impact of semantic feature mismatches, the system removes error edges with excessive semantic errors and performs optimization in two stages. The first stage sets the Huber kernel function to limit excessive error and solves the graph optimization problem. Then, based on the running status of the tracing thread, it is decided whether to perform the second stage of optimization. In the second stage, error edges with large semantic errors are removed, and no kernel function is set to solve the graph optimization problem.

7. The visual odometry method fusion of intermediate semantic information according to claim 3, characterized in that, In step S4, the local mapping thread uses the optimized pose to back-project map points onto the semantic segmentation map, updating the semantic information of the semantic map points, specifically: Using the optimized pose from the keyframes, the map points in the keyframes are back-projected onto the semantic segmentation map of the keyframes. The local mapping thread uses projection relationships and semantic segmentation maps to update the semantic weight information of map points in keyframes. The calculation formula is as follows: Where α is the normalization factor, ensuring When a keyframe is added to the local mapping thread, the map point weights are updated once according to the above formula.

Images