Visual odometer method fusing dynamic object detection network and geometric constraint

Abstract

The invention discloses a visual odometer method fusing a dynamic object detection network and geometric constraints, and relates to the technical field of visual odometers. According to the method, the interference of the dynamic object on the visual odometer is eliminated through effective identification of the dynamic object detection network and combination with a geometric method, so that the problems of feature point matching error and trajectory drift caused by the dynamic object are avoided, and the system can still keep high precision in dynamic scenes such as dense pedestrians and busy vehicles; the dynamic object detection network adopts a lightweight deep learning model and has efficient and real-time detection capability, and meanwhile, the modular design of the system enables the system to have good expandability, and the performance can be further improved in combination with sensors such as a laser radar; after the dynamic object is removed, map construction is based on high-quality static feature points, so that ghosting and false features caused by the dynamic object are avoided, the generated map is more accurate and more consistent, and subsequent positioning and path planning are facilitated.

Description

Technical Field

[0001] The present invention relates to the technical field of visual odometers, and in particular to a visual odometer method integrating a dynamic object detection network and geometric constraints. Background Art

[0002] With the rapid development of technologies such as autonomous driving, robot navigation, and augmented reality, visual odometry, as an important positioning technology, has been widely used in various intelligent systems. Visual odometry mainly relies on the image information collected by the camera to estimate the camera's motion trajectory, and uses the matching of feature points between images to realize the estimation of the camera's position and posture.

[0003] However, traditional visual odometry methods have significant challenges in dealing with dynamic environments, especially when there are moving objects (such as pedestrians, vehicles, etc.) in the environment, which can easily lead to unstable pose estimation or trajectory drift. Traditional visual odometry methods usually rely on local feature points in the image (such as SIFT, ORB, etc.) for matching and tracking. These methods assume that most objects in the scene are stationary and the movement of feature points only comes from the movement of the camera. However, in a dynamic environment, the movement of objects in the scene will produce visual changes that are unrelated to the camera motion. The movement of these dynamic objects may introduce mismatches or pseudo features, thereby affecting the camera's pose estimation. Dynamic objects not only increase the complexity of feature matching, but may also cause serious errors in pose estimation, which in turn affects the accuracy and robustness of the entire visual odometry system. To this end, we propose a visual odometry method that integrates a dynamic object detection network and geometric constraints. Summary of the invention

[0004] The purpose of the present invention is to solve the problems mentioned in the above background technology. The present invention provides a visual odometer method that integrates dynamic object detection network and geometric constraints.

[0005] In order to achieve the above-mentioned purpose, the present invention specifically adopts the following technical solutions:

[0006] A visual odometry method integrating a dynamic object detection network and geometric constraints comprises the following steps:

[0007] Step 1, data acquisition, collecting RGB-D data I rgb_d with depth information through an RGB-D camera, and collecting corresponding data Dimu through an I MU sensor at the same time, wherein I rgb_d is a combination of an image array Irgb containing three RGB color channels and a depth two-dimensional array I d, and Dimu is a vector [p, v, R], wherein p is the state position of the I MU sensor, v is the speed of the I MU sensor, and R is the rotation amount of the I MU sensor;

[0008] Step 2: Feature point extraction and target detection. Extract FAST corner points in the image and calculate the BRIEF descriptor for each corner point. For FAST corner detection, each pixel point in the image is detected. By comparing the gray values with other pixels in the neighborhood, corner points are determined. For each corner point, a pair of pixels is selected and their gray value difference is calculated. A binary descriptor is generated through the comparison results. Use the YOLO target detection algorithm for target detection to obtain the target detection box and category, and identify the dynamic objects in the image;

[0009] Step 3: Segment the foreground and background, including the following steps:

[0010] Step 31: Normalize the depth values within the detection box:

[0011]

[0012] where I(x, y) is the depth value of the depth image, (x, y) is the position coordinate of the depth value I(x, y) in the depth image, I max is the maximum depth value within the depth image, I min is the minimum depth value within the depth image, I norm(x,y) is the pixel value after normalizing the depth image;

[0013] Step 32: Calculate the gray histogram of the image: Traverse the pixel values of all pixels within the detection box, count the number of pixels for each pixel value. The probability that the pixel value is i is where N i is the number of pixels with the pixel value i, and N is the total number of pixels in the target detection box;

[0014] Step 33: Calculate the global mean μ T ,

[0015] Step 34: Initialize the cumulants: background probability w 0 (0), foreground probability w 1 (0), background mean μ 0 (0), foreground mean μ 1 (0), between-class variance maximum variance and the optimal threshold t * ;

[0016] Step 35: Traverse all possible thresholds. For each threshold t, update the cumulative probabilities w 0 (t), w 1 (t) and the cumulative means μ 0 (t), μ1 (t), calculate the between-class variance Compare the between-class variance with the maximum between-class variance. If it is greater, update the maximum between-class variance and the optimal threshold. The relevant formula is as follows:

[0017] w 0 (t) = w 0 (t - 1)+P(t), w 1 (t) = 1 - w 0 (t)

[0018]

[0019] Determine the optimal threshold t after the traversal ends * , and divide the foreground class and the background class according to the optimal threshold;

[0020] Step 36: Judge whether the current variance is greater than the maximum variance. If so, update the maximum variance and the optimal threshold; if not, judge whether the iteration cut-off condition is reached. If the condition is not reached, return to the previous step;

[0021] Step 37: If the iteration cut-off condition is reached, filter the outliers in the foreground through the chi-square distribution model

[0022] Assume that the foreground point depth conforms to a Gaussian distribution. The definition of the chi-square distribution model is:

[0023]

[0024] In the above formula, δ d is the chi-square value, d represents the depth value of the pixel, and the threshold is set to Points with a chi-square value greater than the threshold are regarded as background points, and vice versa as foreground points;

[0025] Step 38: Generate a mask;

[0026] Step 39: Optimize the mask image by dilation and erosion.

[0027] Step 4: Perform a motion consistency test based on geometric constraints on the foreground feature points within the detection frame;

[0028] Step 5: IMU data processing and state prediction. Calculate the state of the camera through numerical integration. Among them, the IMU state usually includes position (p), velocity (v), and rotation (R);

[0029] Step 6: Feature matching based on improved RANSAC, including the following steps:

[0030] Step 61: Use the feature matching algorithm FLANN to match the matching points between two frames of images;

[0031] Step 62: For the BRI EF in the feature points which is a binary descriptor, use the Hamming distance as the metric to calculate the Hamming distance between each pair of matching points. If the Hamming distance of this pair of matching points is less than twice the minimum Hamming distance, then retain this pair of matching points; otherwise, it is regarded as a wrong match. Finally, obtain the pair of matching points after preliminary screening;

[0032] Step 63: Randomly select 4 pairs of the number of matching points to generate a sample model M;

[0033] Step 64: Use the sample model M to calculate the Euclidean distance dist between the matching points, calculate the threshold T according to the absolute median deviation, and obtain the number I of matching points less than the threshold T. If the current I is greater than the best I, then update the model; otherwise, continue to iterate until the adaptive termination condition is met and then end;

[0034] Step 7: Pose estimation;

[0035] Step 8: Joint optimization of the pose by vision and IMU. The joint optimization of the pose by vision and IMU performs state estimation through two-step iteration: prediction and update. Set the state of the camera as x = [p, v, R]. The IMU data provides high-frequency dynamic information, while the vision data provides low-frequency observation information.

[0036] Furthermore, after the data collection, preprocess the data, including the following steps:

[0037] Step a: Image distortion correction. Use the internal parameters and distortion coefficients of the RGB-D camera, and the reverse transformation pixel position radial distortion correction formula:

[0038] r corrected = r·(1 + k 1 r 2 + k 2 r 4 + k 2 r 6 ),

[0039] where r is the radial distance of the pixel,

[0040] and k 1 , k 2 , k 3 are the radial distortion coefficients,

[0041] Tangential distortion correction formula:

[0042]

[0043] where p 1 , p 2 are the tangential distortion coefficients,

[0044] The x and y are pixel coordinates in the image;

[0045] Step b: Image noise removal. Gaussian filtering is used to perform weighted averaging on the image, and the weights are given by the Gaussian function:

[0046]

[0047] The G(x, y) is the Gaussian kernel function, representing the weight of the pixel;

[0048] Step c: IMU noise removal. A low-pass filter is used for the acceleration data to remove high-frequency noise.

[0049] Furthermore, the FAST corner detection formula for extracting corners in the image is:

[0050] |I(p) - I(p i )| > t

[0051] In the formula, the I(p) is the pixel value of the candidate point p, the I(p i ) is the pixel value of the neighboring pixel, and the t is the threshold.

[0052] Furthermore, the motion consistency check for foreground feature points within the detection box based on geometric constraints includes the following steps:

[0053] Step 41: For each foreground feature point p i , calculate its motion vector v i = (x i , y i ), representing the motion direction of the feature point;

[0054] Step 42: Calculate the average motion v avg of all foreground feature points, that is, the mean value of all motion vectors;

[0055] Step 43: Calculate the angle between the motion direction of each feature point and the average motion direction:

[0056] If the angle is greater than 30°, the feature point is considered a dynamic point. If the number of dynamic points is greater than 20% of all feature points in the foreground, remove the feature points within the foreground;

[0057] Step 44: Obtain the static feature points in the image.

[0058] Furthermore, the IMU data processing and state prediction include;

[0059] Position update: p k+1 = p k + v k Δt;

[0060] Acceleration and velocity update: v k+1 = v k + a k Δt, where the a k is the acceleration measurement of the IMU, and the Δt is the time interval;

[0061] Rotation update (by angular velocity): R k+1 = R k exp(ω k Δt), where the ω k is the angular velocity of the IMU, and the exp(ω k Δt) is the rotation matrix obtained by integrating the angular velocity.

[0062] Furthermore, the pose estimation calculates the pose based on PnP, including the following steps:

[0063] Given N pairs of known 3D points P and corresponding 2D points p i , and the camera intrinsic matrix K is known;

[0064] Normalize the 2D point p i through the intrinsic matrix K to obtain p i ' = K -1 p i ;

[0065] Construct the relationship between the 3D point P and the corresponding 2D point p according to the projection model of the camera:

[0066] The R and the t are obtained by solving with the least squares method.

[0067] Furthermore, the visual and IMU joint optimization of the pose includes the following steps:

[0068] Step 81: Update the state using IMU data: x k+1|k = f(x k , u k ) + w k , where the f(x k , u k ) is the IMU dynamics model, the u k is the IMU measurement (acceleration and angular velocity), and the w k is the process noise;

[0069] Step 82: Update the state using visual observations: z k = h(x k ) + v k , where the h(x k) is the observation value obtained from state prediction (e.g., through the projection model of the camera), and the v k is the observation noise;

[0070] Step 83, Update step: x k+1|k+1 = x k+1|k + K k (z k - h(x k+1|k ))), where the K k is the Kalman gain.

[0071] A visual odometer device integrating a dynamic object detection network and geometric constraints, including a processor, a memory, and a computer program stored on the memory and executable on the processor. When the computer program is executed by the processor, it implements the steps of the visual odometer method integrating a dynamic object detection network and geometric constraints as described in any one of the above.

[0072] The beneficial effects of the present invention are as follows:

[0073] 1. The positioning accuracy of the present invention is significantly improved in a dynamic environment: The dynamic object detection network effectively identifies and combines geometric methods to eliminate the interference of dynamic objects on the visual odometer, avoiding problems such as feature point matching errors and trajectory drift caused by dynamic objects, so that the system can still maintain high accuracy in dynamic scenes such as crowded pedestrians and busy vehicles.

[0074] 2. The present invention is applicable to a variety of complex scenes, including dynamic environments, low-texture scenes, and scenes with large lighting changes.

[0075] 3. The present invention supports real-time performance and scalability: The dynamic object detection network adopts a lightweight deep learning model and has efficient real-time detection capabilities. At the same time, the modular design of the system enables it to have good scalability and can further improve performance by combining sensors such as lidar.

[0076] 4. The present invention improves the accuracy and consistency of the map: After eliminating dynamic objects, the map construction is based on high-quality static feature points, thus avoiding ghosts and false features caused by dynamic objects, making the generated map more accurate and consistent, which is beneficial for subsequent positioning and path planning. Brief Description of the Drawings

[0077] Figure 1 is the overall flowchart of the present invention;

[0078] Figure 2 is the flowchart of estimating pose from visual information in the present invention;

[0079] Figure 3 is the flowchart of foreground and background segmentation in the present invention;

[0080] Figure 4 is the feature matching flowchart in the present invention;

[0081] Figure 5 is the flowchart of the improved RANSAC algorithm in the present invention. Detailed implementation manners

[0082] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

[0083] Please refer to Figure 1 - Figure 5 , the present invention provides a visual odometry method integrating a dynamic object detection network and geometric constraints, including the following steps:

[0084] Step 1, data acquisition, collecting RGB-D data I rgb_d with depth information through an RGB-D camera, and simultaneously collecting corresponding data Dimu through an I MU sensor. The I rgb_d is a combination of an image array Irgb including three RGB color channels and a depth two-dimensional array Id. The D imu is a vector [p, v, R], where p is the state position of the I MU sensor, v is the speed of the IMU sensor, and R is the rotation amount of the I MU sensor; after data acquisition, preprocess the data, including the following steps:

[0085] Step a, image distortion correction, using the internal parameters and distortion coefficients of the RGB-D camera, and inversely transforming the pixel position radial distortion correction formula:

[0086] r corrected = r·(1 + k 1 r 2 + k 2 r 4 + k 2 r 6 ),

[0087] where r is the radial distance of the pixel,

[0088] where k 1 , k 2 , k 3 are radial distortion coefficients,

[0089] Tangential distortion correction formula:

[0090]

[0091] where p 1 , p 2 are tangential distortion coefficients,

[0092] The x and y are pixel coordinates in the image;

[0093] Step b, Image noise removal. Gaussian filtering is used to perform weighted averaging on the image, and the weight is given by the Gaussian function:

[0094]

[0095] The G(x, y) is the Gaussian kernel function, representing the weight of the pixel;

[0096] Step c, IMU noise removal. A low-pass filter is used for the acceleration data to remove high-frequency noise.

[0097] Step 2, Feature point extraction and target detection. FAST corner points in the image are extracted, and the BRIEF descriptor of each corner point is calculated. For FAST corner point detection, each pixel point in the image is detected. By comparing the gray values with other pixels in the neighborhood, the corner points are determined. For each corner point, a pair of pixels is selected and their gray value difference is calculated. A binary descriptor is generated through the comparison results. The YOLO target detection algorithm is used for target detection to obtain the target detection box and category, and to identify the dynamic objects in the image. The formula for detecting FAST corner points in the image is:

[0098] |I(p) - I(p i )| > t

[0099] In the formula, the I(p) is the pixel value of the candidate point p, the I(p i ) is the pixel value of the neighboring pixel, and the t is the threshold.

[0100] In this embodiment, preferably, the motion consistency check based on geometric constraints for the foreground feature points within the detection box includes the following steps:

[0101] Step 41, For each foreground feature point p i , calculate its motion vector v i = (x i , y i ), representing the motion direction of the feature point;

[0102] Step 42, Calculate the motion average v avg of all foreground feature points, that is, the mean value of all motion vectors;

[0103] Step 43, Calculate the included angle between the motion direction of each feature point and the average motion direction:

[0104] If the included angle is greater than 30°, the feature point is considered a dynamic point. If the number of dynamic points is greater than 20% of all feature points in the foreground, remove the feature points in the foreground;

[0105] Step 44: Obtain the static feature points in the image.

[0106] Step 3: Segment the foreground and background, including the following steps:

[0107] Step 31: Normalize the depth values within the detection box:

[0108]

[0109] where I(x, y) is the depth value of the depth image, (x, y) is the position coordinate of the depth value I(x, y) in the depth image, I max is the maximum depth value within the depth image, I min is the minimum depth value within the depth image, I norm(x,y) is the pixel value after normalizing the depth image;

[0110] Step 32: Calculate the grayscale histogram of the image: Traverse the pixel values of all pixels within the detection box and count the number of pixels for each pixel value. The probability that the pixel value is i is where N i is the number of pixels with the pixel value i, and N is the total number of pixels in the target detection box;

[0111] Step 33: Calculate the global mean μ T ,

[0112] Step 34: Initialize the cumulants: background probability w 0 (0), foreground probability w 1 (0), background mean μ 0 (0), foreground mean μ 1 (0), between-class variance maximum variance and the optimal threshold t * ;

[0113] Step 35: Traverse all possible thresholds. For each threshold t, update the cumulative probabilities w 0 (t), w 1 (t) and the cumulative means μ 0 (t), μ 1 (t), and calculate the between-class variance Compare the between-class variance with the maximum between-class variance. If it is greater, update the maximum between-class variance and the optimal threshold. The relevant formulas are as follows:

[0114] w 0 w(t) = w 0 (t - 1)+P(t), w 1 w'(t) = 1 - w 0 w'(t)

[0115]

[0116] Determine the optimal threshold t at the end of traversal * , and divide the foreground class and background class according to the optimal threshold;

[0117] Step 36: Determine whether the current variance is greater than the maximum variance. If so, update the maximum variance and the optimal threshold; if not, determine whether the iteration cutoff condition is reached. If the condition is not reached, return to the previous step;

[0118] Step 37: If the iteration cutoff condition is reached, filter out the outliers in the foreground through the chi-square distribution model

[0119] Assume that the depth of foreground points conforms to a Gaussian distribution. The definition of the chi-square distribution model is:

[0120]

[0121] In the above formula, δ d is the chi-square value, d represents the depth value of the pixel, and the threshold is set to Points with a chi-square value greater than the threshold are regarded as background points, and vice versa as foreground points;

[0122] Step 38: Generate a mask;

[0123] Step 39: Optimize the dilation and erosion of the mask image.

[0124] Step 4: Use geometric constraint-based motion consistency test for foreground feature points within the detection box; identify dynamic objects through the object detection network and remove dynamic feature points to reduce their interference with camera pose estimation and trajectory calculation, which can improve the robustness in a dynamic environment.

[0125] Step 5: IMU Data Processing and State Prediction. Calculate the state of the camera through numerical integration. The IMU state usually includes position (p), velocity (v), and rotation (R). The IMU can provide high-frequency output, enabling it to respond in real time and accurately measure the motion state. The IMU integrates an accelerometer, a gyroscope, and a magnetometer, and can provide six-degree-of-freedom (6DoF) measurement information including attitude, heading, and position. The IMU can work properly under any weather and geographical conditions. It is an independent data source and can be used for short-term navigation and verification of other sensors' information. It will not fail due to weather, lens dirt, radar and lidar signal reflection, or urban canyon effect. It has strong environmental adaptability. IMU data can be combined with other sensors (such as cameras or lidar) to improve positioning accuracy by integrating multiple data sources, enabling the device to navigate in a dynamically changing environment.

[0126] IMU data processing and state prediction include;

[0127] Position update: p k+1 = p k + v k Δt;

[0128] Acceleration and velocity update: v k+1 = v k + a k Δt, where the a k is the acceleration measurement of the IMU, and the Δt is the time interval;

[0129] Rotation update (through angular velocity): R k+1 = R k exp(ω k Δt), where the ω k is the angular velocity of the IMU, and the exp(ω k Δt) is the rotation matrix obtained by integrating the angular velocity.

[0130] Step 6: Feature matching based on improved RANSAC. By reasonably reducing the number of elements in the sample set, the proportion of inliers in the sample can be increased, thereby improving the running efficiency and estimation accuracy of the algorithm. On the basis of completing the initial feature matching using the SIFT algorithm, unreasonable initial parameter models are quickly discarded through pre-checking, further reducing the number of iterations of the algorithm and improving its running efficiency. By combining the internal shape descriptor algorithm and the FPFH algorithm to obtain feature descriptors, the improved RANSAC algorithm can quickly and accurately eliminate mismatched points, solve the affine transformation matrix, and eliminate the need for secondary registration. The improved RANSAC algorithm using the three-dimensional grid segmentation method improves its robustness in large-scale three-dimensional point cloud registration, while ensuring accuracy and significantly improving the registration efficiency. It not only improves the matching accuracy but also significantly reduces the running time, including the following steps:

[0131] Step 61: Use the feature matching algorithm FLANN to match the matching points between two frames of images;

[0132] Step 62: For BRIEF in the feature points which are binary descriptors, use the Hamming distance as a metric to calculate the Hamming distance between each pair of matching points. If the Hamming distance of this pair of matching points is less than twice the minimum Hamming distance, then retain this pair of matching points; otherwise, it is regarded as a wrong match. Finally, the pair of matching points after preliminary screening is obtained;

[0133] Step 63: Randomly select 4 pairs of matching point numbers to generate a sample model M;

[0134] Step 64: Use the sample model M to calculate the Euclidean distance dist between the matching points, calculate the threshold T according to the absolute median deviation, and obtain the number I of matching points less than the threshold T. If the current I is greater than the best I, then update the model; otherwise, continue to iterate until the adaptive termination condition is met and then end;

[0135] Step 7: Pose estimation; It has a fast calculation speed and is suitable for application scenarios that require real-time response. Compared with feature matching methods, it has better adaptability to light changes and occlusions, does not require complex sensor devices, thereby reducing costs, and supports visual odometers in multiple scenarios, including dynamic environments (such as scenes with dense pedestrians and vehicles) and low-texture environments (such as smooth indoor walls), enhancing multi-scenario applicability. It includes the following steps:

[0136] Given N pairs of known 3D points P and corresponding 2D points p i , the camera internal parameter matrix K is known;

[0137] Normalize the 2D point p i through the internal parameter matrix K to obtain p i ' = K -1 pi ;

[0138] Construct the relationship between the 3D point P and the corresponding 2D point p according to the projection model of the camera:

[0139] The described R and the described t are obtained by solving with the least squares method.

[0140] Step 8, Joint optimization of visual and IMU poses. Utilize the high-quality semantic information provided by the object detection network, and combine geometric constraints for multi-level feature analysis, thereby optimizing the pose estimation of the camera, which can improve the accuracy of visual odometry. The joint optimization of visual and IMU poses is performed through two-step iteration: prediction and update for state estimation. Set the state of the camera as x = [p, v, R]. The IMU data provides high-frequency dynamic information, while the visual data provides low-frequency observation information.

[0141] It includes the following steps:

[0142] Step 81, Update the state using IMU data: x k+1|k = f(x k , u k ) + w k , where the f(x k , u k ) is the IMU dynamics model, the u k is the IMU measurement (acceleration and angular velocity), and the w k is the process noise;

[0143] Step 82, Update the state using visual observations: z k = h(x k ) + v k , where the h(x k ) is the observation value obtained from the state prediction (e.g., through the projection model of the camera), and the v k is the observation noise;

[0144] Step 83, Update step: x k+1|k+1 = x k+1|k + K k (z k - h(x k+1|k ))), where the K k is the Kalman gain

[0145] A visual odometry device that fuses a dynamic object detection network and geometric constraints, including a processor, a memory, and a computer program stored on the memory and executable on the processor. When the computer program is executed by the processor, it implements the steps of the visual odometry method that fuses a dynamic object detection network and geometric constraints as described in any one of the above.

[0146] The foregoing description of the disclosed embodiments enables those skilled in the art to practice or use the present invention. Various modifications to these embodiments will be readily apparent to those 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. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A visual odometry method integrating dynamic object detection network and geometric constraints, characterized in that: The steps include: Step 1, data acquisition, collecting RGB-D data Irgb_d with depth information through an RGB-D camera, and collecting corresponding data Dimu through an IMU sensor at the same time, wherein Irgb_d is a combination of an image array Irgb containing three RGB color channels and a depth two-dimensional array Id, and Dimu is a vector [p, v, R], wherein p is the state position of the IMU sensor, v is the speed of the IMU sensor, and R is the rotation amount of the IMU sensor; Step 2: Feature point extraction and target detection. Extract FAST corner points in the image and calculate the BRIEF descriptor of each corner point. FAST corner point detection detects each pixel in the image and determines the corner point by comparing the grayscale value with other pixels in the field. For each corner point, select a pair of pixels and calculate their grayscale difference. Generate a binary descriptor by comparing the results. Use the YOLO target detection algorithm for target detection, obtain the target detection box and category, and identify dynamic objects in the image. Step 3: Segment the foreground and background, including the following steps: Step 31: Normalize the depth value in the detection frame: Wherein, I(x, y) is the depth value of the depth image, (x, y) is the position coordinate of the depth value I(x, y) in the depth image, and I max is the maximum depth value in the depth image, I min is the minimum depth value in the depth image, I norm(x,y) is the pixel value after normalizing the depth image; Step 32, calculate the grayscale histogram of the image: traverse the pixel values ​​of all pixels in the detection frame, count the number of pixels of each pixel value, and the probability that the pixel value is i is Among them, the N i is the number of pixels of the pixel value i, and N is the total number of pixels of the target detection frame; Step 33: Calculate the global mean μ of all pixels in the target detection box T , Step 34, initialize the cumulants: background probability w0(0), foreground probability w1(0), background mean μ0(0), foreground mean μ1(0), between-class variance Maximum variance and the optimal threshold t * ; Step 35: Traverse all possible thresholds, update the cumulative probability w0(t), w1(t) and cumulative mean μ0(t), μ1(t) for each threshold t, and calculate the inter-class variance Compare the inter-class variance with the maximum inter-class variance. If it is greater than the maximum inter-class variance, update the maximum inter-class variance and the optimal threshold. The relevant formulas are as follows: w0(t)=w0(t-1)+P(t),w1(t)=1-w0(t) The traversal ends and determines the optimal threshold t * , divide the foreground class and background class according to the optimal threshold; Step 36: determine whether the current variance is greater than the maximum variance. If so, update the maximum variance and the optimal threshold. If not, determine whether the iteration cutoff condition is met. If not, return to the previous step. Step 37: If the iteration cutoff condition is met, the chi-square distribution model is used to filter out the outliers in the foreground. Assuming that the foreground point depth conforms to the Gaussian distribution, the chi-square distribution model is defined as: In the above formula, δ d is the chi-square value, d represents the depth value of the pixel, and the threshold is set to Points with chi-square values ​​greater than the threshold are considered background points, and those with chi-square values ​​less than the threshold are considered foreground points; Step 38, generating a mask; Step 39: Mask image expansion and erosion optimization. Step 4: Use geometric constraint-based motion consistency check on the foreground feature points in the detection frame; Step 5: IMU data processing and state prediction: calculating the state of the camera by numerical integration, where the IMU state usually includes position (p), velocity (v), and rotation (R); Step 6: Feature matching based on improved RANSAC, including the following steps: Step 61, using feature matching algorithm FLANN to match matching points between two frames of images; Step 62: For the binary descriptor of BRIEF in the feature point, use the Hamming distance as a metric to calculate the Hamming distance between each matching point pair. If the Hamming distance of this matching point pair is less than twice the minimum Hamming distance, the matching point pair is retained. Otherwise, it is regarded as a wrong match, and finally the matching point pairs after preliminary screening are obtained. Step 63: Randomly select 4 pairs of matching points to generate a sample model M; Step 64: Use the sample model M to calculate the Euclidean distance dist between the matching points, calculate the threshold T according to the absolute median deviation, and obtain the number of matching points I that is less than the threshold T. If the current I is greater than the optimal I, update the model, otherwise continue to iterate until the adaptive termination condition is met; Step 7: Pose estimation; Step 8: Joint optimization of posture by vision and IMU. The joint optimization of posture by vision and IMU is performed through two steps of iteration: prediction and update to perform state estimation. The state of the camera is set to x = [p, v, R]. The IMU data provides high-frequency dynamic information, while the visual data provides low-frequency observation information.

2. A visual odometer method integrating dynamic object detection network and geometric constraints according to claim 1, characterized in that: After the data is collected, the data is preprocessed, including the following steps: Step a, image distortion correction, using the intrinsic parameters and distortion coefficients of the RGB-D camera, reversely transform the pixel position radial distortion correction formula: r corrected =r·(1+k1r 2 +k2r 4 +k2r 6 ), The r is the radial distance of the pixel, The k1, k2, k3 are radial distortion coefficients, Tangential distortion correction formula: The p1 and p2 are tangential distortion coefficients, The x, y are pixel coordinates in the image; Step b: Image noise removal: Gaussian filtering is used to perform weighted averaging on the image, with the weight given by the Gaussian function: The G(x,y) is a Gaussian kernel function, which represents the weight of the pixel; Step c: IMU noise removal: A low-pass filter is applied to the acceleration data to remove high-frequency noise.

3. The visual odometer method integrating dynamic object detection network and geometric constraints according to claim 1, characterized in that: The FAST corner point detection formula in the extracted image is: |I(p)-I(p i )|>t Where, I(p) is the pixel value of the candidate point p, and I(p i ) is the pixel value of the neighborhood pixel, and t is the threshold.

4. The visual odometer method integrating dynamic object detection network and geometric constraints according to claim 1, characterized in that: The motion consistency check based on geometric constraints for the foreground feature points in the detection frame comprises the following steps: Step 41: For each foreground feature point p i , calculate its motion vector v i =(x i ,y i ), indicating the moving direction of the feature point; Step 42: Calculate the moving average v of all foreground feature points avg , which is the mean of all motion vectors; Step 43: Calculate the angle between the moving direction of each feature point and the average moving direction: If the angle is greater than 30°, the feature point is considered to be a dynamic point. If the number of dynamic points is greater than 20% of all feature points in the foreground, the feature points in the foreground are removed; Step 44: Obtain static feature points in the image.

5. The visual odometer method integrating dynamic object detection network and geometric constraints according to claim 1, characterized in that: The IMU data processing and state prediction include: Location update: p k+1 =p k +v k Δt; Acceleration and velocity update: v k+1 =v k +a k Δt, where a k is the acceleration measurement of I MU, and Δt is the time interval; Rotation update (via angular velocity): R k+1 =R k exp(ω k Δt), where ω k is the angular velocity of the IMU, the exp(ω k Δt) is the rotation matrix obtained by integrating the angular velocity.

6. The visual odometer method integrating dynamic object detection network and geometric constraints according to claim 1, characterized in that: The pose estimation is based on PnP pose calculation, and includes the following steps: Given N pairs of known 3D points P and corresponding 2D points p i , the camera intrinsic parameter matrix K is known; The 2D point p i Normalized by the internal parameter matrix K, we get p i '=K -1 p i ; Construct the relationship between the 3D point P and the corresponding 2D point p according to the camera's projection model: The R and t are obtained by least square method.

7. The visual odometer method integrating dynamic object detection network and geometric constraints according to claim 1, characterized in that: The joint optimization of vision and IMU posture includes the following steps: Step 81: Update the state using IMU data: x k+1|k =f(x k ,u k )+w k , where f(x k ,u k ) is the IMU dynamic model, the u k is the IMU measurement (acceleration and angular velocity), the w k is the process noise; Step 82: Update the state using visual observations: z k =h(x k )+v k , where h(x k ) is the observation obtained from the state prediction (e.g., through the camera projection model), the v k is the observation noise; Step 83, Update step: x k+1|k+1 =x k+1|k +K k (z k -h(x k+1|k )), wherein the K k is the Kalman gain.

8. A visual odometer device integrating a dynamic object detection network and geometric constraints, characterized in that: The invention comprises a processor, a memory and a computer program stored in the memory and executable on the processor, wherein when the computer program is executed by the processor, the steps of the visual odometer method for fusing a dynamic object detection network and geometric constraints as described in any one of claims 1 to 8 are implemented.

Images