Dynamic vision SLAM optimization method and device

Abstract

The invention relates to a dynamic vision SLAM (Simultaneous Localization and Mapping) optimization method and device. A bounding box detection result is divided into a prior dynamic object detection box and a static object detection box according to semantic information in an RGB (Red, Green and Blue) image; motion consistency analysis is carried out on the prior dynamic object detection frame through an optical flow method, a first type of the prior dynamic object detection frame is determined, and the first type comprises a real dynamic object class and a static prior dynamic object class; determining a second type of a feature point according to the depth value of the feature point in the prior dynamic object detection frame of each real dynamic object class in the depth image, wherein the second type comprises a foreground point and a background point; calculating an initial pose result; and optimizing the initial pose result to obtain a final pose result. According to the method, the distribution characteristics of reprojection errors are analyzed in real time, and robust kernel function parameters are dynamically adjusted, so that the adaptability of an algorithm to a detection failure scene is enhanced, interference of non-Gaussian noise is effectively suppressed, and the positioning precision and robustness in a dynamic environment are improved.

Description

Technical Field

[0001] This invention relates to the field of visual SLAM technology, and more particularly to a dynamic visual SLAM optimization method and apparatus. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a key technology for mobile robots to achieve autonomous navigation in unknown environments. It involves sensing the environment through sensors, estimating the robot's pose in real time, and building an environmental map; the two are interdependent and complementary. Depending on the type of sensor, SLAM systems can be divided into laser SLAM and visual SLAM. Visual SLAM is favored due to the advantages of its camera sensors—economical cost, compact size, low power consumption, and rich information—and has become a current research focus. However, traditional visual SLAM systems typically assume a static environment, but the presence of dynamic objects in real-world scenes introduces noise and outliers, leading to localization errors and inaccurate map building, and even introducing erroneous information into subsequent optimization processes.

[0003] In recent years, the rapid development of deep learning technology has provided new solutions for visual SLAM. By combining techniques such as object detection and semantic segmentation with visual SLAM, it is possible to adapt to dynamic environments. However, although semantic segmentation and instance segmentation networks have high detection accuracy, their computational complexity is high, making it difficult to meet the real-time requirements of practical applications. In contrast, object detection algorithms are more efficient, but their bounding box-based global culling mechanism may lead to the erroneous removal of valid feature points outside dynamic regions. Furthermore, existing visual SLAM backend optimization methods typically rely on fixed robust kernel parameters, making it difficult to adapt to the complex non-Gaussian noise distribution characteristics in dynamic scenes, thus limiting the accuracy of pose estimation. Therefore, a visual SLAM algorithm that is accurate and robust in dynamic scenes is needed, which is of great significance for the intelligent application of mobile robots in dynamic environments. Summary of the Invention

[0004] To address the problems existing in the prior art, this invention provides a dynamic visual SLAM optimization method and apparatus. By analyzing the distribution characteristics of reprojection errors in real time and dynamically adjusting the robust kernel function parameters, the algorithm's adaptability to detection failure scenarios is enhanced, and interference from non-Gaussian noise is effectively suppressed, thereby improving positioning accuracy and robustness in dynamic environments.

[0005] To solve any of the above-mentioned technical problems, the specific technical solution of the present invention is as follows: This invention provides a dynamic visual SLAM optimization method, comprising: Obtain the current frame image of the target image, wherein the current frame image includes an RGB image and a depth image; Based on the semantic information in the RGB image, the bounding box detection results of the current frame image are divided into prior dynamic object detection boxes and static object detection boxes; Motion consistency analysis of the prior dynamic object detection box is performed using optical flow method to determine the first type of the prior dynamic object detection box, which includes real dynamic object class and static prior dynamic object class. The second type of a feature point is determined based on the depth value of the feature point in the depth image of the prior dynamic object detection box of each real dynamic object class. The second type includes foreground points and background points. The preliminary pose estimation result is calculated based on the feature points in the static object detection box, the feature points in the prior dynamic object detection box of the first type being static, and the feature points of the second type being background points in the feature points in the prior dynamic object detection box of the real static object type. The preliminary pose estimation result is optimized to obtain the final pose estimation result.

[0006] Furthermore, by performing motion consistency analysis on the prior dynamic object detection box using optical flow, the first type of the prior dynamic object detection box further includes: Calculate the optical flow vector of each feature point in the prior dynamic object detection frame; Calculate the average optical flow corresponding to the prior dynamic object detection box; Based on the average optical flow and the optical flow vector of each feature point, determine whether each feature point is an anomaly. The first type of the prior dynamic object detection box is determined based on the ratio of the number of outliers to the total number of feature points in the prior dynamic object detection box and a first threshold.

[0007] Furthermore, determining whether each feature point is an outlier based on the average optical flow and the optical flow vector of each feature point further includes: Determine whether the optical flow vector of the feature point satisfies the formula If yes, then the first type of the feature point is a real dynamic object; if not, then the feature point is an anomaly. The optical flow vector representing the feature point. u Indicates the pixel corresponding to the feature point in x Speed ​​of motion on the axis v Indicates the pixel corresponding to the feature point in y Speed ​​of motion on the axis Indicates average optical flow. Second threshold.

[0008] Further, determining the first type of the prior dynamic object detection box based on the ratio of the number of outliers to the total number of feature points in the prior dynamic object detection box and a first threshold further includes: If the ratio is greater than the first threshold, then the first type of the prior dynamic object detection box is a real dynamic object class; If the ratio is less than or equal to the first threshold, then the first type of the prior dynamic object detection box is the prior dynamic object class of static objects.

[0009] Furthermore, determining the second type of a feature point based on the depth value of the feature point in the depth image within the prior dynamic object detection box of each real dynamic object class further includes: Calculate the mean depth value and standard deviation depth value based on the depth values ​​of each feature point in the prior dynamic object detection box of the real dynamic object class; Calculate the chi-square value of the feature point based on its depth value, the mean depth value, and the standard deviation of the depth value. The second type of the feature point is determined based on the chi-square value and the third threshold.

[0010] Furthermore, determining the second type of the feature point based on the chi-square value and the third threshold further includes: If the chi-square value of the feature point is less than the third threshold, then the second type of the feature point is a foreground point; If the chi-square value of the feature point is greater than or equal to the third threshold, then the second type of the feature point is a background point.

[0011] Furthermore, optimizing the preliminary pose estimation result to obtain the final pose estimation result further includes: The reprojection error of each observed map point in the current frame image is calculated based on the camera pose and map points in the preliminary pose estimation results. The median absolute deviation is calculated based on the reprojection error and the median value of the reprojection error for each map point. Adjust the robustness parameter of the Huber kernel function based on the absolute deviation of the median; The camera pose and map points in the preliminary pose estimation result are constructed based on the Huber kernel function with the robustness parameter adjusted. The reprojection error is weighted and summed based on the weight function to obtain the optimization objective function. The Levenberg-Marquardt algorithm is used to solve the optimization objective function to obtain the optimized camera pose and optimized map points. The optimized camera pose and optimized map points are used as the final pose estimation result.

[0012] Furthermore, based on the aforementioned absolute deviation of the median, the formula for adjusting the robustness parameter of the Huber kernel function is as follows: ; ; Wherein, the vector of the robustness parameters is , s This represents the proportionality coefficient. This indicates the reprojection error. i Represents an image frame. j Represents the map points observed in the image frame. Indicates the camera pose to be optimized. and map points Indicates the first j A map point, Indicates the first i The camera pose corresponding to the frame, MAD(·) represents the reprojection error. The absolute deviation of the median, It is the quantile function of the standard normal distribution.

[0013] Furthermore, the formula for constructing the weight function of the camera pose and map points in the preliminary pose estimation result based on the Huber kernel function with adjusted robustness parameters is as follows: ; in, Indicates weight, This represents the first derivative of the Huber kernel function.

[0014] On the other hand, embodiments of this specification also provide a dynamic visual SLAM optimization device, the device comprising: An image acquisition unit is used to acquire the current frame image of the target image, wherein the current frame image includes an RGB image and a depth image; The semantic information processing unit is used to divide the bounding box detection results of the current frame image into prior dynamic object detection boxes and static object detection boxes based on the semantic information in the RGB image. The first type identification unit is used to perform motion consistency analysis on the prior dynamic object detection box using optical flow method to determine the first type of the prior dynamic object detection box, wherein the first type includes real dynamic object class and static prior dynamic object class. The second type recognition unit is used to determine the second type of the feature point in the depth image based on the depth value of the feature point in the prior dynamic object detection box of each real dynamic object class. The second type includes foreground points and background points. The preliminary pose estimation result calculation unit is used to calculate the preliminary pose estimation result based on the feature points in the static object detection box, the feature points in the prior dynamic object detection box of the first type being static, and the feature points of the second type being background points in the feature points in the prior dynamic object detection box of the real static object type. The pose optimization unit is used to optimize the preliminary pose estimation result to obtain the final pose estimation result.

[0015] Compared with the prior art, the present invention has the following advantages: (1) This invention adds a target detection thread to the traditional visual SLAM method and runs it in parallel with the tracking thread. This allows for the rapid acquisition of semantic information of the scene without blocking the tracking thread. Combined with optical flow motion consistency analysis and a foreground-background segmentation method based on depth information, it achieves accurate removal of dynamic feature points in the foreground within the target box, retaining only valid static feature points for inter-frame matching to solve the camera pose. This is a method that fully utilizes prior knowledge of target detection, optical flow, and depth information, and has the advantages of being simple, fast, and effective, while improving the positioning accuracy of the system. (2) To address the performance degradation of existing back-end optimization algorithms in non-Gaussian noise scenarios, this invention proposes a back-end optimization method based on adaptive robust kernel parameters. By analyzing the distribution characteristics of reprojection errors in real time and dynamically adjusting the kernel function parameters, the robustness of the algorithm to outliers is enhanced, thereby further improving the estimation accuracy and stability of the system. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0017] Figure 1 The diagram shown is a flowchart of a dynamic visual SLAM optimization method according to an embodiment of the present invention. Figure 2 The image shows a comparison of the dynamic feature point recognition and removal results between ORB-SLAM3 and the present invention. Figure 3 The figure shown is a comparison of the reprojection error and robustness parameters between ORB-SLAM3 and the present invention. Figure 4 The figure shown is a comparison of the absolute trajectory errors of ORB-SLAM3 and the present invention on the TUM dataset; Figure 5The figure shown is a comparison of the relative trajectory errors of ORB-SLAM3 and the present invention on the TUM dataset; Figure 6 The image shown is a diagram of the appearance and main components of the intelligent car. Figure 7 The image shows a comparison of trajectory estimation between ORB-SLAM3 and the present invention in a real dynamic scene. Figure 8 The diagram shown is a structural schematic of a dynamic visual SLAM optimization device according to an embodiment of the present invention. Figure 9 The diagram shown is a structural schematic of a computer device in an embodiment of the present invention.

[0018] [Explanation of Figure Markers]: 801, Image Acquisition Unit; 802. Semantic Information Processing Unit; 803, First type of identification unit; 804. Second type of identification unit; 805. Preliminary pose estimation result calculation unit; 806, Pose Optimization Unit; 902. Computer equipment; 904. Processing equipment; 906. Storage resources; 908. Drive system; 910. Input / Output Module; 912. Input devices; 914. Output devices; 916. Presentation equipment; 918. Graphical User Interface; 920. Network interface; 922. Communication link; 924. Communication bus. Detailed Implementation

[0019] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0020] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, apparatus, product, or device that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or devices.

[0021] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0022] To address the problems existing in the prior art, this invention provides a dynamic visual SLAM optimization method. By analyzing the distribution characteristics of reprojection errors in real time and dynamically adjusting the robust kernel function parameters, the algorithm's adaptability to detection failure scenarios is enhanced, and interference from non-Gaussian noise is effectively suppressed, thereby improving positioning accuracy and robustness in dynamic environments. Figure 1 The diagram illustrates a flowchart of a dynamic visual SLAM optimization method according to an embodiment of the present invention. This diagram depicts the process of constructing a 3D model of a flexible cable, but based on conventional or non-creative labor, it may include more or fewer operational steps. The order of steps listed in the embodiment is merely one possible execution order among many and does not represent the only possible execution order. In actual system or device products, the methods shown in the embodiment or the accompanying drawings can be executed sequentially or in parallel. Specifically, as shown... Figure 1 As shown, the method may include: Step 101: Obtain the current frame image of the target image, wherein the current frame image includes an RGB image and a depth image; Step 102: Based on the semantic information in the RGB image, divide the bounding box detection results of the current frame image into prior dynamic object detection boxes and static object detection boxes; Step 103: Perform motion consistency analysis on the prior dynamic object detection box using optical flow method to determine the first type of the prior dynamic object detection box. The first type includes real dynamic object class and static prior dynamic object class. Step 104: Determine the second type of the feature point in the depth image based on the depth value of the feature point in the prior dynamic object detection box of each real dynamic object class. The second type includes foreground points and background points. Step 105: Calculate the preliminary pose estimation result based on the feature points in the static object detection box, the feature points in the prior dynamic object detection box of the first type being static, and the feature points of the second type being background points in the feature points in the prior dynamic object detection box of the real static object type. Step 106: Optimize the preliminary pose estimation result to obtain the final pose estimation result.

[0023] In the embodiments of this specification, bounding box detection can be performed on RGB images using existing methods to obtain bounding box detection results. Then, the YOLOv8 object detection algorithm can be used to obtain semantic information in the RGB image scene. Based on the semantic information, the bounding box detection results of the current frame image are divided into prior dynamic object detection boxes and static object detection boxes. The semantic information includes the category and location information of the bounding boxes; the prior dynamic objects are objects with motion characteristics, such as pedestrians, vehicles, and animals.

[0024] YOLOv8 is the latest version in the YOLO series. Compared to its predecessors, YOLOv8 has made several improvements in model structure, training strategies, and inference efficiency, making it perform exceptionally well in tasks such as object detection, image classification, and semantic segmentation. YOLOv8 offers several model variants, including YOLOv8n, YOLOv8s, YOLOv8m, YOLOv8l, and YOLOv8x, which differ in parameter count and performance. Generally, larger models achieve higher detection accuracy but at a lower inference speed. Considering the limitations of experimental platform resources and the need for dynamic object detection in indoor scenes, the lightweight model YOLOv8n was chosen as the object detection module. YOLOv8n achieves an average accuracy (mAP) of 37.3% on the COCO dataset, while its inference speed on the A100 TensorRT is only 0.99 milliseconds, meeting real-time requirements while maintaining high accuracy. In addition, YOLOv8n has only 3.2M parameters, far less than YOLOv8x (68.2M), making it more suitable for resource-constrained hardware platforms.

[0025] Then, motion consistency analysis is performed on the prior dynamic object detection boxes using optical flow to determine a first type of the prior dynamic object detection boxes. This first type includes real dynamic object classes and static prior dynamic object classes. Specifically, determining the first type of the prior dynamic object detection boxes by performing motion consistency analysis using optical flow further includes: Calculate the optical flow vector of each feature point in the prior dynamic object detection frame; Calculate the average optical flow corresponding to the prior dynamic object detection box; Based on the average optical flow and the optical flow vector of each feature point, determine whether each feature point is an anomaly. The first type of the prior dynamic object detection box is determined based on the ratio of the number of outliers to the total number of feature points in the prior dynamic object detection box and a first threshold.

[0026] In the embodiments of this specification, to improve the real-time performance of the system, a sparse optical flow method is used to determine the true motion state of the target. The sparse optical flow method is represented by Lucas-Kanade (LK) optical flow. When calculating the optical flow vector, a constant brightness simulacrum is typically used, therefore: , Based on the assumption of time continuity, by performing a Taylor series expansion and retaining the first-order terms, we obtain: , Combining the two equations above and dividing by dt, we get: , in, Representing pixels in x and y The velocity of motion on the axis is denoted as u and v . Is the image at this point? x and y The gradients in the direction reflect the rate of change of image brightness in space. Let be the rate of change of image grayscale over time. In LK optical flow, considering that pixels within a small window typically have similar motion characteristics, a [formula / value] can be constructed regarding... u and v The system of overdetermined linear equations. By solving this system of equations, the optical flow vector can be obtained. The optimal estimate.

[0027] Calculate the average optical flow. In practical applications, due to camera motion, the static component of the image generates an optical flow vector. This static component is defined as the feature points of non-prior dynamic objects. Then, calculate the average optical flow vector of the static component of the image: , in, N The number of pixels.

[0028] In the embodiments of this specification, determining whether each feature point is an outlier based on the average optical flow and the optical flow vector of each feature point further includes: Determine whether the optical flow vector of the feature point satisfies the formula If yes, then the first type of the feature point is a real dynamic object; if not, then the feature point is an anomaly. The optical flow vector representing the feature point. u Indicates the pixel corresponding to the feature point in x Speed ​​of motion on the axis v Indicates the pixel corresponding to the feature point in y Speed ​​of motion on the axis Indicates average optical flow. Second threshold.

[0029] In this embodiment of the specification, determining the first type of the prior dynamic object detection box based on the ratio of the number of outliers to the total number of feature points in the prior dynamic object detection box and a first threshold further includes: If the ratio is greater than the first threshold, then the first type of the prior dynamic object detection box is a real dynamic object class; If the ratio is less than or equal to the first threshold, then the first type of the prior dynamic object detection box is the prior dynamic object class of static objects.

[0030] In this embodiment of the specification, determining the second type of a feature point based on the depth value of the feature point in the depth image of the prior dynamic object detection box of each real dynamic object class further includes: Calculate the mean depth value and standard deviation depth value based on the depth values ​​of each feature point in the prior dynamic object detection box of the real dynamic object class; Calculate the chi-square value of the feature point based on its depth value, the mean depth value, and the standard deviation of the depth value. The second type of the feature point is determined based on the chi-square value and the third threshold. In this embodiment of the specification, determining the second type of the feature point based on the chi-square value and the third threshold further includes: If the chi-square value of the feature point is less than the third threshold, then the second type of the feature point is a foreground point; If the chi-square value of the feature point is greater than or equal to the third threshold, then the second type of the feature point is a background point.

[0031] Specifically, first, define the set of foreground points in the dynamic frame as... , while other points are Iterate through all object detection boxes, validating the effective depth value for each pixel location in each RGB frame. Sort the depth values ​​in ascending order and set a depth threshold. Container D is defined as The storage depth values ​​are located between 0 and T for feature points. Assuming container D follows a normal distribution, we use... This indicates that, for subsequent processing, these depth values ​​are normalized to a standard normal distribution with a mean of [value missing]. and standard deviation The calculation formula is as follows: ; Then, a chi-square distribution is used to identify outliers. For each point, the corresponding chi-square value is: ; in, d This represents the depth value of the point. Since the foreground and background are segmented based on depth values, the categorical variable is divided into two groups in the chi-square test: the foreground group (smaller depth values) and the background group (larger depth values). The degrees of freedom are set to 1, and the confidence level is... The chi-square critical value is found to be 2.706 from the table. Based on this, a third threshold is set. If the chi-square value at a certain point... If the value is less than the third threshold, the point is determined to be a foreground point and is added to the set. Otherwise, add the point to the background set. Finally, by iteratively processing all points in the dynamic bounding box, feature points determined to be dynamic foreground elements are filtered out and removed.

[0032] Then, based on the feature points in the static object detection box, the feature points in the prior dynamic object detection box of the first type being static, and the feature points of the second type being background points in the feature points in the prior dynamic object detection box of the real static object type, a preliminary pose estimation result is calculated.

[0033] It should be noted that the calculation of the preliminary pose estimation result is common knowledge in the field, and will not be described in detail in the embodiments of this specification.

[0034] Finally, the preliminary pose estimation result is optimized to obtain the final pose estimation result. Specifically, Back-end optimization in visual SLAM can typically be formulated as a nonlinear least squares problem, aiming to minimize reprojection error. Depending on the number of keyframes to optimize, back-end optimization can be divided into local optimization and global optimization: local optimization refines the pose and map points of a few keyframes, while global optimization optimizes all keyframes after loop closure detection. The mathematical expression for the optimization problem is: ; in, This represents the optimal value of the camera pose and map points. L It is about The cost function, n The number of keyframes to be optimized. m This represents the number of map points observed in the frame. This indicates the reprojection error. i Represents an image frame. j Represents the map points observed in the image frame. Indicates the camera pose to be optimized. and map points , Indicates the first A map point, Indicates the first The camera pose corresponding to the frame. Weights Indicates the first The importance of individual projection errors.

[0035] In practical applications, noise from visual sensors, dynamic environments, and changes in lighting conditions can cause observation errors to exhibit a non-Gaussian or sub-Gaussian distribution, leading to a significant performance degradation. To address this issue, robust kernel functions are widely used in the back-end optimization of visual SLAM. Robust kernel functions assign lower weights to outliers, This effectively reduces its impact on the optimization process. The cost function based on the robust kernel function can be expressed as: , in, This represents the robust kernel. The Huber kernel function is a widely used robust kernel function in visual SLAM, and its expression and first derivative are as follows: , in, The threshold parameter (robustness parameter) of the Huber kernel function determines the degree of influence of error samples on the optimization process. e For error. When error e Less than or equal to When the error is high, the optimization problem can be viewed as a least squares problem; when the error is low... e Greater than At this point, the influence of error samples is significantly suppressed. The choice of robust kernel parameters determines the robustness and sensitivity of the back-end optimization algorithm.

[0036] However, in existing technologies, the parameters of robust kernel functions are typically set to fixed values ​​to adapt to the noise characteristics of specific scenarios. This approach has limitations in practical applications: when the scenario changes, the fixed kernel parameters struggle to adapt to the new noise distribution, leading to a decrease in the robustness and mapping accuracy of the backend optimization algorithm.

[0037] To address this issue, this method proposes an adaptive backend optimization algorithm that dynamically adjusts the parameters of the robust kernel function according to changes in the scenario, thereby improving the robustness and accuracy of the system. Specifically, it includes the following steps: Based on the camera pose in the preliminary pose estimation results and map points Calculate the reprojection error of each observed map point in the current frame image to obtain the error set. And calculate the median value of the reprojection error. ,in Represents the median function; The Median Absolute Deviation (MAD) is calculated based on the reprojection error and the median value of the reprojection error for each map point. To measure the dispersion of the reprojection error distribution, the median absolute deviation (MAD) of all reprojection errors is calculated. The formula for calculating MAD is: ; Adjust the robustness parameter of the Huber kernel function based on the absolute deviation of the median; set up The vector is ,but The calculation formula is: ; in, s It is a proportionality coefficient used to control the adjustment range of the robustness parameter. It is the quantile function of the standard normal distribution. Through this adaptive mechanism, when the dispersion of the reprojection error is large, Increasing the value helps to better suppress the influence of outliers; conversely, when the error distribution is more concentrated, The value is reduced to improve the positioning accuracy of the system.

[0038] The camera pose and map points in the preliminary pose estimation result are constructed based on the Huber kernel function with the robustness parameter adjusted. The reprojection error is weighted and summed based on the weight function to obtain the optimization objective function. Specifically, in determining robustness parameters Then, for each reprojection error in the current frame Calculate its corresponding weight The error is weighted and summed based on the weights to construct the optimization objective function.

[0039] The weighting function is defined as: ;in, It is the first derivative of the Huber kernel function.

[0040] The Levenberg-Marquardt algorithm is used to solve the optimization objective function to obtain the optimized camera pose and optimized map points. The optimized camera pose and optimized map points are used as the final pose estimation result.

[0041] In the embodiments of this specification, the Levenberg-Marquardt (LM) algorithm is used to solve the optimization problem. The LM algorithm updates the camera pose iteratively. and map points This gradually approaches the optimal solution. Specifically, the objective function is expressed with respect to... The derivative is set to zero to obtain the update amount of the optimization variable. .

[0042] ; For projection equations Performing Taylor expansion yields the linearized form: ; in, The Jacobian matrix represents the optimized value obtained in the previous iteration. .

[0043] The projection error is then: ; in, It is usually represented by pixel coordinates. In the i Frame observations, This represents the difference between the current observation and the value optimized in the previous iteration. The reprojection error.

[0044] Based on this, through The following can be compiled: ; By arranging the data in matrix form, we can obtain the update amounts of the optimization variables. : ; Where J represents the projection equation The Jacobian matrix after Taylor expansion, where W represents the error term. The weight matrix formed Indicates by The error matrix is ​​composed of damping coefficients. Used to control the stability and convergence of the optimization process, the update amount of the optimization variables... for: ; Then iterate continuously, based on the update volume. To update and optimize variables .when When the inequality approaches zero, the LM algorithm is similar to the Gauss-Newton method; when... When the value is large, the LM algorithm tends to favor gradient descent. By adjusting... (Empirical evidence) The LM algorithm can achieve a balance between the Gauss-Newton method and the gradient descent method, thereby ensuring the stability and convergence of the optimization process.

[0045] For example, the present invention provides a dynamic visual SLAM optimization method based on semantic information and adaptive robust kernel parameters, and uses the publicly available TUM RGB-D dataset and real dynamic scenes to evaluate the scheme of this embodiment.

[0046] The TUM RGB-D dataset, released by the Technical University of Munich (TUM) laboratory, contains 39 sequences acquired by the Microsoft Kinect sensor. These sequences cover a wide range of scenes, including indoor environments, corridors, and outdoor settings. The dataset boasts broad scene coverage and high-quality data, making it particularly suitable for research in dynamic object detection and 3D reconstruction. This invention selects five sequences from the dynamic object category of the dataset, denoted as: fr3 / w / xyz, fr3 / w / static, fr3 / w / rpy, fr3 / w / half, and fr3 / s / static. The first four are high-dynamic scene sequences, and the fifth is a low-dynamic scene sequence. These sequences are 640*480 resolution images depicting a scene in an office environment, including a desk and monitor, where two researchers are walking and conversing.

[0047] In this embodiment, a personal laptop computer was selected for the verification of the dataset scene. The personal laptop computer was configured with a 12th Gen Intel(R) Core(TM) i7-12700H 2.30 GHz CPU, an NVIDIA GeForce RTX 3060 GPU, and 16GB of memory. A smart car was used for the real-time dynamic scene verification of the visual SLAM algorithm. The smart car was equipped with an Nvidia Jetson Xavier NX development board and a RealSense D435i depth camera. The Nvidia Jetson Xavier NX contained a 6-core ARM Carmel CPU, an Nvidia Volta GPU, and 384 CUDA cores. The RealSense D435i depth camera had a resolution of 640*480 and a sampling frequency of 30fps.

[0048] Figure 2 This invention demonstrates the results of detecting and removing dynamic object regions within a dynamic detection frame. The invention utilizes object detection algorithms to obtain semantic prior information in the scene and combines optical flow and foreground / background segmentation techniques to effectively distinguish and remove static and dynamic feature points in the scene, achieving pseudo-semantic segmentation. In contrast, ORB-SLAM3 failed to remove dynamic feature points on the human body, retaining a large number of feature points, including dynamic humans. These dynamic feature points can cause errors in subsequent pose estimation and map construction, reducing the robustness of the system. Figure 3 The diagram illustrates the relationship between the reprojection error and the robustness parameter of the adaptive robust kernel parameter proposed in this invention. The robustness parameter of this invention can be dynamically adjusted according to the change of the reprojection error, while the robustness parameter of the robust Huber kernel method of ORB-SLAM3 remains unchanged.

[0049] This invention uses the EVO tool or officially provided tools to compare the trajectory output by the algorithm with the high-precision ground truth trajectory provided in the dataset, aiming to evaluate the deviation between the two. Commonly used error metrics include Absolute Trajectory Error (ATE) and Relative Pose Error (RPE). ATE is mainly used to measure the global accuracy and trajectory consistency of the algorithm, and usually only translation error is calculated because rotation error will indirectly affect subsequent translation trajectories; while RPE is used to evaluate local accuracy and pose estimation drift, considering both translation and rotation errors. This invention experimentally calculates the root mean square error (RMSE) and standard deviation (STD) of pose estimation. RMSE reflects the robustness of the system, while STD is used to evaluate the dispersion of the trajectory and the stability of the system.

[0050] The results of this method are quantitatively compared with those of ORB-SLAM3, a representative traditional visual SLAM system, running the TUM dynamic dataset. The root mean square error (RMSE) and standard deviation (STD) of the ATE and RPE indices are calculated respectively. As shown in Tables 1, 2 and 3.

[0051] Table 1 Comparison of ATE between ORB-SLAM3 and the algorithm of this invention

[0052] Table 2 Comparison of displacement components in RPE of ORB-SLAM3 and the algorithm of this invention

[0053] Table 3 Comparison of the rotational components in RPE of ORB-SLAM3 and the algorithm of this invention.

[0054] Quantitative comparison results show that the present invention has significantly improved positioning accuracy and robustness compared with traditional visual SLAM in high and low dynamic scenes.

[0055] Figure 4 and Figure 5 The results show the absolute and relative trajectory errors of ORB-SLAM3 and this invention under five test sequences in the TUM dataset. The trajectory estimated by ORB-SLAM3 deviates significantly from the true trajectory, while the estimated values ​​of this method are in good agreement with the true values, with smaller deviations and a more concentrated error distribution.

[0056] Figure 6 Showcases the appearance and main components of the intelligent vehicle; Figure 7 The comparison of camera trajectories between ORB-SLAM3 and the present invention in real dynamic scenes is shown. In areas with frequent human activity, ORB-SLAM3 is limited by its inability to effectively handle dynamic factors, resulting in multiple offsets in the estimated solid line trajectory, which deviates significantly from a straight line. The dashed line trajectory estimated by the present invention is similar to the preset straight line true value and has less drift, further verifying the effectiveness of the positioning accuracy and robustness of the present invention.

[0057] Based on the same inventive concept, embodiments of the present invention also provide a dynamic visual SLAM optimization device, such as... Figure 8 As shown, it includes: Image acquisition unit 801 is used to acquire the current frame image of the target image, wherein the current frame image includes an RGB image and a depth image; The semantic information processing unit 802 is used to divide the bounding box detection results of the current frame image into prior dynamic object detection boxes and static object detection boxes based on the semantic information in the RGB image. The first type identification unit 803 is used to perform motion consistency analysis on the prior dynamic object detection box using optical flow method to determine the first type of the prior dynamic object detection box. The first type includes real dynamic object class and static prior dynamic object class. The second type recognition unit 804 is used to determine the second type of the feature point in the depth image based on the depth value of the feature point in the prior dynamic object detection box of each real dynamic object class. The second type includes foreground points and background points. The preliminary pose estimation result calculation unit 805 is used to calculate the preliminary pose estimation result based on the feature points in the static object detection box, the feature points in the prior dynamic object detection box of the first type being static, and the feature points of the second type being background points in the feature points in the prior dynamic object detection box of the real static object type. The pose optimization unit 806 is used to optimize the preliminary pose estimation result to obtain the final pose estimation result.

[0058] The beneficial effects obtained by the above-described device are the same as those obtained by the above-described method, and will not be described in detail in the embodiments of the present invention.

[0059] like Figure 9 The diagram shown is a structural schematic of a computer device according to an embodiment of this specification. The methods described in this specification can be applied to the computer device of this embodiment.

[0060] Specifically, such as Figure 9 As shown, computer device 902 may include one or more processing devices 904, such as one or more central processing units (CPUs), each of which may implement one or more hardware threads. Computer device 902 may also include any storage resource 906 for storing information of any kind, such as code, settings, data, etc. Non-limitingly, for example, storage resource 906 may include any combination of one or more of the following: any type of RAM, any type of ROM, flash memory devices, hard disks, optical disks, etc. More generally, any storage resource can use any technology to store information.

[0061] Furthermore, any storage resource can provide volatile or non-volatile retention of information.

[0062] Furthermore, any storage resource can represent a fixed or removable component of the computer device 902. In one case, when the processing device 904 executes associated instructions stored in any storage resource or combination of storage resources, the computer device 902 can perform any operation of the associated instructions. The computer device 902 also includes one or more drive systems 908 for interacting with any storage resource, such as a hard disk drive system, an optical disk drive system, etc.

[0063] Computer device 902 may also include an input / output module 910 (I / O) for receiving various inputs (via input device 912) and providing various outputs (via output device 914). A specific output mechanism may include a presentation device 916 and an associated graphical user interface (GUI) 918. In other embodiments, the input / output module 910 (I / O), input device 912, and output device 914 may be omitted, and the device may function solely as a computer device within a network. Computer device 902 may also include one or more network interfaces 920 for exchanging data with other devices via one or more communication links 922. One or more communication buses 924 couple the components described above together.

[0064] Communication link 922 can be implemented in any way, such as via a local area network (LAN), a wide area network (WAN) (e.g., the Internet), a point-to-point connection, or any combination thereof. Communication link 922 may include any combination of hardwired links, wireless links, routers, gateway functions, name servers, etc., governed by any protocol or combination of protocols.

[0065] This invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described method.

[0066] This invention also provides a computer-readable instruction, wherein when a processor executes the instruction, the program therein causes the processor to perform the above-described method.

[0067] It should be understood that, in various embodiments of the present invention, the order of the above-mentioned process numbers does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0068] It should also be understood that, in the embodiments of the present invention, the term "and / or" is merely a description of the relationship between associated objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Furthermore, in the present invention, the character " / " generally indicates that the preceding and following associated objects have an "or" relationship.

[0069] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this invention can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of each example have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0070] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0071] In the embodiments provided by this invention, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative. For instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed may be indirect coupling or communication connection through some interfaces, devices, or units, or may be electrical, mechanical, or other forms of connection.

[0072] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of the embodiments of the present invention, depending on actual needs.

[0073] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0074] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0075] Specific embodiments have been used to illustrate the principles and implementation methods of this invention. The descriptions of the embodiments above are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this invention should not be construed as a limitation of this invention.

Claims

1. A dynamic visual SLAM optimization method, characterized in that, The method includes: Obtain the current frame image of the target image, wherein the current frame image includes an RGB image and a depth image; Based on the semantic information in the RGB image, the bounding box detection results of the current frame image are divided into prior dynamic object detection boxes and static object detection boxes; Motion consistency analysis of the prior dynamic object detection box is performed using optical flow method to determine the first type of the prior dynamic object detection box, which includes real dynamic object class and static prior dynamic object class. The second type of a feature point is determined based on the depth value of the feature point in the depth image of the prior dynamic object detection box of each real dynamic object class. The second type includes foreground points and background points. The preliminary pose estimation result is calculated based on the feature points in the static object detection box, the feature points in the prior dynamic object detection box of the first type being static, and the feature points of the second type being background points in the feature points in the prior dynamic object detection box of the real static object type. The preliminary pose estimation result is optimized to obtain the final pose estimation result.

2. The method according to claim 1, characterized in that, Motion consistency analysis of the prior dynamic object detection box using optical flow method to determine the first type of the prior dynamic object detection box further includes: Calculate the optical flow vector of each feature point in the prior dynamic object detection frame; Calculate the average optical flow corresponding to the prior dynamic object detection box; Based on the average optical flow and the optical flow vector of each feature point, determine whether each feature point is an anomaly. The first type of the prior dynamic object detection box is determined based on the ratio of the number of outliers to the total number of feature points in the prior dynamic object detection box and a first threshold.

3. The method according to claim 2, characterized in that, Determining whether a feature point is an outlier based on the average optical flow and the optical flow vector of each feature point further includes: Determine whether the optical flow vector of the feature point satisfies the formula If yes, then the first type of the feature point is a real dynamic object; if not, then the feature point is an anomaly. The optical flow vector representing the feature point. u Indicates the pixel corresponding to the feature point in x Speed ​​of motion on the axis v Indicates the pixel corresponding to the feature point in y Speed ​​of motion on the axis Indicates average optical flow. Second threshold.

4. The method according to claim 2, characterized in that, Determining the first type of the prior dynamic object detection box based on the ratio of the number of outliers to the total number of feature points in the prior dynamic object detection box and a first threshold further includes: If the ratio is greater than the first threshold, then the first type of the prior dynamic object detection box is a real dynamic object class; If the ratio is less than or equal to the first threshold, then the first type of the prior dynamic object detection box is the prior dynamic object class of static objects.

5. The method according to claim 1, characterized in that, Determining the second type of a feature point based on the depth value of the feature point in the depth image within the prior dynamic object detection box of each real dynamic object class further includes: Calculate the mean depth value and standard deviation depth value based on the depth values ​​of each feature point in the prior dynamic object detection box of the real dynamic object class; Calculate the chi-square value of the feature point based on its depth value, the mean depth value, and the standard deviation of the depth value. The second type of the feature point is determined based on the chi-square value and the third threshold.

6. The method according to claim 5, characterized in that, Determining the second type of the feature point based on the chi-square value and the third threshold further includes: If the chi-square value of the feature point is less than the third threshold, then the second type of the feature point is a foreground point; If the chi-square value of the feature point is greater than or equal to the third threshold, then the second type of the feature point is a background point.

7. The method according to claim 1, characterized in that, Optimizing the preliminary pose estimation result to obtain the final pose estimation result further includes: The reprojection error of each observed map point in the current frame image is calculated based on the camera pose and map points in the preliminary pose estimation results. The median absolute deviation is calculated based on the reprojection error and the median value of the reprojection error for each map point. Adjust the robustness parameter of the Huber kernel function based on the absolute deviation of the median; The camera pose and map points in the preliminary pose estimation result are constructed based on the Huber kernel function with the robustness parameter adjusted. The reprojection error is weighted and summed based on the weight function to obtain the optimization objective function. The Levenberg-Marquardt algorithm is used to solve the optimization objective function to obtain the optimized camera pose and optimized map points. The optimized camera pose and optimized map points are used as the final pose estimation result.

8. The method according to claim 7, characterized in that, The formula for adjusting the robustness parameter of the Huber kernel function based on the aforementioned median absolute deviation is as follows: ； Wherein, the vector of the robustness parameters is , s This represents the proportionality coefficient. This indicates the reprojection error. i Represents an image frame. j Represents the map points observed in the image frame. Indicates the camera pose to be optimized. and map points Indicates the first j A map point, Indicates the first i The camera pose corresponding to the frame, MAD(·) represents the reprojection error. The absolute deviation of the median, It is the quantile function of the standard normal distribution.

9. The method according to claim 8, characterized in that, The formula for constructing the weight function of the camera pose and map points in the preliminary pose estimation result based on the Huber kernel function with adjusted robustness parameters is as follows: ;in, Indicates weight, This represents the first derivative of the Huber kernel function.

10. A dynamic visual SLAM optimization device, characterized in that, The device includes: An image acquisition unit is used to acquire the current frame image of the target image, wherein the current frame image includes an RGB image and a depth image; The semantic information processing unit is used to divide the bounding box detection results of the current frame image into prior dynamic object detection boxes and static object detection boxes based on the semantic information in the RGB image. The first type identification unit is used to perform motion consistency analysis on the prior dynamic object detection box using optical flow method to determine the first type of the prior dynamic object detection box, wherein the first type includes real dynamic object class and static prior dynamic object class. The second type recognition unit is used to determine the second type of the feature point in the depth image based on the depth value of the feature point in the prior dynamic object detection box of each real dynamic object class. The second type includes foreground points and background points. The preliminary pose estimation result calculation unit is used to calculate the preliminary pose estimation result based on the feature points in the static object detection box, the feature points in the prior dynamic object detection box of the first type being static, and the feature points of the second type being background points in the feature points in the prior dynamic object detection box of the real static object type. The pose optimization unit is used to optimize the preliminary pose estimation result to obtain the final pose estimation result.

Images