Method for improving positioning precision in dynamic SLAM scene

Abstract

The invention discloses a method for improving positioning precision in a dynamic SLAM scene, and the method comprises the steps: obtaining a depth map of a current frame, and obtaining an optimized depth map after filtering and denoising; performing robust feature matching on the continuous frames to obtain a feature point corresponding relation, screening out static background feature points to form a 3D-3D point corresponding relation, and solving an initial camera pose through a nonlinear optimization method; determining dynamic feature points based on an adaptive dynamic threshold model and removing the dynamic feature points, and outputting a pure static feature point set; map points are divided into static points, potential dynamic points and dynamic points according to semantic categories, differentiated weights are distributed to all kinds of points, a semantic weighted factor graph optimization model is constructed on the basis of a pure static feature point set in combination with the semantic categories and optimization weights, the camera pose is gradually optimized in the factor graph model, and the camera pose is obtained. And finally, the optimized high-precision camera pose is obtained. Dynamic interference can be suppressed in real time.

Description

Technical Field

[0001] This invention relates to the field of 3D vision technology, and in particular to a method for improving positioning accuracy in dynamic SLAM scenes. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a key enabling technology for autonomous robot perception. By fusing multi-sensor data streams, this technology can achieve real-time localization and mapping in unknown environments, thus becoming a supporting technology for autonomous driving, augmented reality, and related fields. As application scenarios gradually extend from structured static environments to unstructured dynamic environments, traditional SLAM systems face a critical challenge: the ubiquitous presence of dynamic objects (such as pedestrians and vehicles) causes feature matching distortion, pose estimation drift, and map-building artifacts, leading to a sharp decline in localization accuracy. Dynamic visual SLAM technology is at the forefront of research addressing these core challenges—this field utilizes semantic understanding, motion modeling, and adaptive optimization techniques to proactively perceive and eliminate dynamic interference. This approach provides the necessary theoretical foundation and technical guarantee for the reliable operation of intelligent systems in complex environments.

[0003] In the field of visual SLAM, geometry-based methods directly eliminate dynamic features through motion consistency verification techniques to cope with dynamic environments. D.-H. Kim et al. proposed using depth information from an RGB-D camera to estimate background model parameters to effectively exclude dynamic objects. However, their photometric error function shows significant limitations in scenes with changing lighting, and the computational efficiency of the method is insufficient for real-time applications. Y. Sun et al. introduced an advanced dynamic object culling strategy, significantly improving localization accuracy in dynamic environments. This method is effective in handling moderately dynamic scenes, but its performance degrades in highly cluttered environments. S. Li et al. developed an iterative nearest-point algorithm based on object edge points for camera pose estimation. Although its frame-keyframe dynamic information recognition mechanism has potential, the system lacks semantic understanding capabilities, resulting in a fundamental limitation in distinguishing actively moving objects from passively displaced environmental elements. The StaticFusion framework provides a comprehensive solution by segmenting the scene into multiple coherent regions through pixel-level clustering. This unified framework simultaneously estimates camera pose and identifies dynamic objects, achieving accurate static background reconstruction. The main innovation of the system lies in the joint optimization method, but its approach of treating object categories as a whole has significant constraints. While clustering algorithms improve segmentation accuracy, this comes at the cost of increased computational complexity. ZJ Du et al. proposed a two-stage method combining GC-RANSAC dynamic feature detection and elimination with LC-CRF locally consistent conditional random fields to achieve map point relabeling. This method first estimates the initial camera pose using the remaining static features, and then optimizes the solution through a probabilistic relabeling process. However, extensive experimental verification revealed a key drawback: the system frequently misclassifies features in highly dynamic scenes, while in static environments it abnormally misclassifies stationary points as dynamic. R. Wang et al. developed a multi-stage detection process that integrates clustering and geometric constraints. Their innovative method first performs K-means clustering segmentation on the depth map, and then identifies dynamic regions through feature point distribution analysis and dual-essence matrix constraints. This purely mathematical modeling strategy successfully avoids the high computational overhead of deep learning methods, but at the cost of decreased localization accuracy, especially in complex real-world environments with multiple moving objects.

[0004] In deep learning-based methods, DS-SLAM combines SegNet with ORB-SLAM2 to suppress dynamic interference while generating semantic maps. However, its reliance on epipolar geometric constraints makes it difficult to effectively remove dynamic features in highly dynamic scenes. DynaSLAM uses Mask R-CNN for dynamic object segmentation, but suffers from real-time limitations and cannot reliably identify potential dynamic objects. YOLO-SLAM replaces the backbone of YOLOv3 with Darknet53 and adopts a lightweight object detection network. This method combines depth parallax and RANSAC to distinguish dynamic features and performs well in highly dynamic sequences, but it cannot effectively remove passively moving objects, and its real-time performance improvement is limited. DLD-SLAM accurately removes dynamic feature points through semantic extraction and depth data fusion, but its heavy reliance on depth information and single threshold model limit its detection accuracy. DRM-SLAM proposes a dynamic region masking system that uses only RGB data, combining semantic segmentation (YOLOv5-seg) with geometric constraints (epipole verification). This method fails in sparsely textured regions because geometric constraints are no longer effective. AHY-SLAM combines YOLOv5 with adaptive threshold-based feature extraction for dynamic keypoint detection and introduces an innovative keyframe selection strategy to improve efficiency. However, this method aggressively removes almost all keypoints within the detected bounding box, resulting in poor performance in low-dynamic scenes. DHP-SLAM achieves high accuracy through a three-layer filtering method, but it cannot handle objects undergoing state transitions and, due to the removal of feature points from all potential dynamic categories, suffers from an over-reliance on semantic information.

[0005] In summary, geometry-based methods struggle to effectively handle dynamic feature points in high-speed, dynamic scenes, resulting in low localization accuracy and a tendency for degradation in the epipolar direction. While deep learning-based methods show promise, their real-time performance remains insufficient. Furthermore, existing methods largely limit the utilization of semantic information to object detection and segmentation, failing to fully explore its potential for optimization. Summary of the Invention

[0006] Current dynamic SLAM systems face the challenge of significantly reduced positioning accuracy under strong motion interference. The core difficulty lies in how to efficiently and robustly distinguish between dynamic and static scene elements. To address this issue, this invention proposes a method to improve positioning accuracy in dynamic SLAM scenarios. It employs a three-level collaborative architecture of perception-decision-optimization to suppress dynamic interference in real time, overcoming the core limitations of existing technologies.

[0007] To achieve the above objectives, the present invention provides a method for improving positioning accuracy in dynamic SLAM scenarios, comprising:

[0008] The RGB image of the current frame is obtained, a semantic mask is generated using an instance segmentation model, the depth map of the current frame is obtained, and after filtering and denoising, an optimized depth map is obtained. The semantic mask and the optimized depth map constitute preprocessed data with semantic-geometric information.

[0009] Robust feature matching is performed on consecutive frames to obtain the correspondence of feature points. The semantic mask is used to exclude feature points in the dynamic object region and filter out static background feature points. The static background feature points are back-projected to three-dimensional space according to the depth information to form a 3D-3D point correspondence. The initial camera pose is solved by nonlinear optimization method.

[0010] The camera motion information is calculated based on the initial camera pose. The depth validity and back projection of the matching feature points are checked. The scene flow vector is calculated in combination with the camera motion compensation. An adaptive dynamic threshold model is constructed based on scene flow consistency, depth difference, motion direction and camera motion angle. Dynamic feature points are determined and removed based on the adaptive dynamic threshold model, and a clean set of static feature points is output.

[0011] Map points are classified into static points, potential dynamic points, and dynamic points according to semantic categories, and differentiated weights are assigned to each type of point. Based on the pure set of static feature points, a semantically weighted factor graph optimization model is constructed by combining the semantic categories and optimization weights. The camera pose is progressively optimized in the factor graph optimization model to obtain the final optimized high-precision camera pose.

[0012] Preferably, for stereo images, a deep learning-based monocular depth estimation model is used to estimate depth information from the RGB image to generate the depth map.

[0013] Preferably, robust feature matching is performed on consecutive frames, including:

[0014] The forward and backward optical flow tracking method is adopted, and mismatch points are adaptively filtered by calculating the backward tracking error. Specifically:

[0015] ;

[0016] In the formula, Let be the characteristic coordinates at time point t. Forward optical flow tracking function, For directional optical flow tracking function, To be equal to the Euclidean distance, This is the adaptive reverse error threshold.

[0017] Preferably, the nonlinear optimization method is a graph-based 3D-3D pose optimization method, and the error term of the 3D-3D pose optimization method is the Euclidean distance between the 3D map point in the world coordinate system and the current frame observation point after pose transformation.

[0018] Preferably, determining dynamic feature points based on the adaptive dynamic threshold model includes:

[0019] Calculate the scene flow direction similarity and motion amplitude consistency between the matched feature point and other feature points in the corresponding spatial neighborhood to obtain a local scene flow consistency score;

[0020] Based on the depth values ​​of feature points, the depth sensor error is modeled to obtain the depth adaptive threshold component;

[0021] The local scene flow consistency score, the depth adaptive threshold component, and the angle information between the feature point motion direction and the camera motion direction are fused to generate a dynamic judgment threshold for the corresponding feature point.

[0022] Preferably, obtaining a local scene flow consistency score includes:

[0023] Calculate the average direction vector: ;

[0024] Calculate the angle difference: ;

[0025] Calculate the angle similarity score: ;

[0026] Consistency of calculation magnitude: ;

[0027] Calculate the eventual consistency score: ;

[0028] In the formula, Let be the average direction vector, n be the total number of local feature points, and i be the nth feature point. Let i be the direction vector of the i-th feature point. Due to the angle difference, The direction vector of the center point, Scoring is based on angular similarity. For the sake of amplitude consistency, It is a direction vector. The consistency score is given for the scene flow.

[0029] Preferably, in the semantically weighted factor graph optimization model, the static points are assigned high weights, the potential dynamic points are assigned low weights, and the dynamic points do not participate in the optimization.

[0030] An incremental optimization strategy is adopted, which uses a relaxed threshold to retain more constraints in the first two stages and a strict threshold to achieve precise convergence in the last two stages.

[0031] Preferably, the incremental optimization strategy is:

[0032] ;

[0033] In the formula, This represents the Lie group addition operator. For the camera pose at time k+1, Let k be the camera pose. For a tiny pose increment on the Lie algebra, This is the transpose of the Jacobian matrix. This is the weight matrix. For Jacobian matrices, The damping factor, It is the identity matrix. This is the residual vector.

[0034] The present invention also provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the method for improving positioning accuracy in a dynamic SLAM scenario.

[0035] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the method for improving positioning accuracy in a dynamic SLAM scenario.

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

[0037] (1) This invention integrates geometric motion cues (scene flow) with semantic segmentation through a bidirectional optical flow verification mechanism. Lightweight SOLOv2 instance segmentation generates object-level prior masks to narrow the detection range, while 3D motion vector analysis enables pixel-level tracking and dynamic target recognition, effectively solving the problem of false detection in textureless areas and avoiding misclassification of static objects.

[0038] (2) The present invention simultaneously constructs a cascaded ICP-3D-3D G2O optimizer, achieves coarse registration through RANSAC-ICP, and performs fine optimization with the help of 3D-3D G2O. In addition, an adaptive dynamic threshold model based on depth-motion direction-local consistency is proposed.

[0039] (3) The dialectical cluster adjustment mechanism of the present invention applies strict Cauchy kernel function constraints to static point clouds, while adopting relaxed Huber kernel suppression for dynamic features, thereby achieving a balance between attitude estimation accuracy and system stability.

[0040] (4) This invention can accurately identify and eliminate dynamic feature points through semantically guided scene flow calculation and adaptive dynamic threshold model, effectively suppressing the interference caused by moving objects. Attached Figure Description

[0041] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0042] Figure 1 This is a flowchart of a method for improving positioning accuracy in a dynamic SLAM scenario according to an embodiment of the present invention;

[0043] Figure 2 This is a schematic diagram of instance segmentation according to an embodiment of the present invention;

[0044] Figure 3 This is a schematic diagram of bilateral filtering according to an embodiment of the present invention;

[0045] Figure 4 This is a schematic diagram of optical flow reverse tracking according to an embodiment of the present invention;

[0046] Figure 5 This is a schematic diagram of robust feature matching output according to an embodiment of the present invention;

[0047] Figure 6 This is a schematic diagram illustrating the 3D-3D G2O optimization principle of an embodiment of the present invention;

[0048] Figure 7 This is a comparison and analysis chart of trajectory errors between ICP and 3D-3D G2O in an embodiment of the present invention:

[0049] Figure 8 This is a comparison chart of the cumulative trajectory errors of ICP and 3D-3D G2O in an embodiment of the present invention;

[0050] Figure 9 This is an overall flowchart of dynamic target detection and removal according to an embodiment of the present invention;

[0051] Figure 10 This is a visualization diagram illustrating scene flow consistency in an embodiment of the present invention;

[0052] Figure 11 This is an example diagram of pseudocode for calculating the threshold in an embodiment of the present invention;

[0053] Figure 12 This is a bundle adjustment factor diagram of semantic constraints in an embodiment of the present invention;

[0054] Figure 13This is a visual comparison diagram of the algorithm performance on the TUM and KITTI datasets according to an embodiment of the present invention;

[0055] Figure 14 This is a diagram showing the time consumption comparison of embodiments of the present invention;

[0056] Figure 15 This is a comparison chart of ATE and RPE of ORB-SLAM2 and Ours(SC) in low dynamic scenes according to an embodiment of the present invention;

[0057] Figure 16 This is a comparison chart of ATE and RPE of ORB-SLAM2 and Ours(SC) in a high dynamic scene according to an embodiment of the present invention. Detailed Implementation

[0058] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

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

[0060] This embodiment proposes a method to improve positioning accuracy in dynamic SLAM scenarios, such as... Figure 1 ,include:

[0061] The RGB image of the current frame is obtained, a semantic mask is generated using an instance segmentation model, the depth map of the current frame is obtained, and after filtering and denoising, an optimized depth map is obtained. The semantic mask and the optimized depth map constitute preprocessed data with semantic-geometric information.

[0062] Robust feature matching is performed on consecutive frames to obtain the correspondence of feature points. The semantic mask is used to exclude feature points in the dynamic object region and filter out static background feature points. The static background feature points are back-projected to three-dimensional space according to the depth information to form a 3D-3D point correspondence. The initial camera pose is solved by nonlinear optimization method.

[0063] The camera motion information is calculated based on the initial camera pose. The depth validity and back projection of the matching feature points are checked. The scene flow vector is calculated in combination with the camera motion compensation. An adaptive dynamic threshold model is constructed based on scene flow consistency, depth difference, motion direction and camera motion angle. Dynamic feature points are determined and removed based on the adaptive dynamic threshold model, and a clean set of static feature points is output.

[0064] Map points are classified into static points, potential dynamic points, and dynamic points according to semantic categories, and differentiated weights are assigned to each type of point. Based on the pure set of static feature points, a semantically weighted factor graph optimization model is constructed by combining the semantic categories and optimization weights. The camera pose is progressively optimized in the factor graph optimization model to obtain the final optimized high-precision camera pose.

[0065] Specifically, firstly, this embodiment obtains corresponding points through continuous inter-frame feature point matching and backprojects these points to 3D coordinates using depth information. Next, a segmentation mask is used to filter static background points, and the camera pose is iteratively optimized using the ICP algorithm, supplemented by G2O optimization to improve accuracy. Then, the scene flow vector after motion compensation is calculated, and an adaptive dynamic threshold model is constructed by combining depth information, local consistency, and the angle between the motion direction and camera movement. Finally, the proportion of dynamic feature points within the detected specific object category regions is statistically analyzed, and the object's motion state is determined through a significance test. Feature points inside dynamic objects are removed to improve the robustness of the SLAM system.

[0066] Furthermore, for binocular images, a deep learning-based monocular depth estimation model is used to estimate depth information from the RGB image to generate the depth map.

[0067] Specifically, in this embodiment, data processing serves as the preprocessing stage in the visual SLAM workflow, responsible for receiving and optimizing raw sensor data. This process supports both RGB-D and binocular working modes, and its core processing employs a lightweight SOLOv2 instance segmentation model to accurately segment objects in the image (such as...). Figure 2 (As shown). To address the lack of depth maps in stereo mode, the Monodepth2 model is introduced to reconstruct stereo depth information. The generated depth map is then processed by bilateral filtering (the effect is shown). Figure 3 This process effectively suppresses noise while maintaining the integrity of the edge structure. The final output is an optimized depth map and a dynamic object mask, providing denoised and semantically segmented perceptual data for subsequent pose estimation and scene understanding modules.

[0068] Furthermore, robust feature matching is performed on consecutive frames, including:

[0069] The forward and backward optical flow tracking method is adopted, and mismatch points are adaptively filtered by calculating the backward tracking error. Specifically:

[0070] ;

[0071] In the formula, Let be the characteristic coordinates at time point t. Forward optical flow tracking function, For directional optical flow tracking function, The value is the Euclidean distance (L2 norm). This is the adaptive reverse error threshold.

[0072] Specifically, optical flow reverse tracking matching, such as Figure 4 As shown, the algorithm achieves robust feature tracking based on the assumption of constant brightness of optical flow and the principle of local motion consistency: First, the coordinate scale of the feature points is adjusted according to the image pyramid theory as the initial tracking points, and the Lucas-Kanade optical flow method is used for forward matching between adjacent frames to satisfy the grayscale invariance constraint, as shown in equation (1):

[0073] (1);

[0074] In the formula, For image intensity function, For pixel coordinates, In terms of time dimension, For optical flow vector, For time intervals.

[0075] To adapt to the characteristics of different sensors (binocular / RGBD), this algorithm dynamically calculates the reverse tracking error threshold based on the image diagonal ratio, thereby enhancing the system's adaptability. The dynamic displacement threshold is obtained through median displacement statistics and fused with depth information to construct a perspective projection model, as shown in Equation (2). This allows the threshold to be adaptively adjusted according to the object distance, conforming to the geometric principle of three-dimensional spatial motion projection onto the image plane.

[0076] (2);

[0077] In the formula, For dynamic displacement threshold, For camera focal length, The feature point depth value. It indicates a direct proportion.

[0078] This embodiment also introduces a bidirectional optical flow verification mechanism, which utilizes the principle of closed-loop consistency of motion trajectory, as shown in formula (3), to filter out abnormal points that exceed the displacement limit or have reverse errors. Then, valid matching points are selected within the image boundary for output, as shown in formula (3). Figure 5 As shown.

[0079] (3);

[0080] In the formula, Let be the characteristic coordinates at time point t. Forward optical flow tracking function, For directional optical flow tracking function, The value is the Euclidean distance (L2 norm). This is the adaptive reverse error threshold.

[0081] Finally, by fusing a spatial weighting strategy based on human visual attention mechanisms (strict in the central region and lenient in the peripheral region) with boundary constraints, effective matching points are output (as shown in Equations (4) and (5)). The entire process achieves robust feature tracking under varying lighting conditions and complex motion scenarios.

[0082] (4);

[0083] In the formula, These represent the width and height of the image, respectively. Weighting the central region – simulating the high-resolution characteristics of the fovea in human vision. Edge region weighting - used to compensate for lens distortion and sensor noise. These are adaptive weighting coefficients;

[0084] (5);

[0085] In the formula, For the image spatial domain.

[0086] Furthermore, the nonlinear optimization method is a graph-based 3D-3D pose optimization method, and the error term of the 3D-3D pose optimization method is the Euclidean distance between the 3D map point in the world coordinate system and the current frame observation point after pose transformation.

[0087] Specifically, in this embodiment, the pose optimization process includes:

[0088] The initial pose is calculated based on static background points, and inliers are selected. When the number of valid static points exceeds a threshold, a secondary 3D-3D G2O pose optimization is performed using the inlier set. Figure 6 This diagram illustrates the principle of 3D-3D G2O pose optimization. The left sub-figure shows the initial registration state, where blue circles represent reference point clouds (3D points in the world coordinate system); red triangles represent target point clouds (3D points observed in the current camera frame); and green crosshairs indicate the transformed positions of the reference point clouds after applying the initial pose estimate. Black dashed lines connect the reference points (blue) to their transformed counterparts (green), demonstrating the point correspondence.

[0089] Before optimization begins, the reference point cloud (blue) is transformed to the current frame coordinate system using the initial camera pose calculated by ICP, resulting in green crosshairs. Due to errors in pose estimation, there is a misalignment between these transformed points (green) and the observed target points (red). The black dashed line visually illustrates the initial transformation relationship, while the distance between the green crosshairs and the red triangles quantifies the initial registration error.

[0090] Figure 6 The left-hand subplot shows the state before optimization: inaccurate pose estimation caused a deviation between the transformed point and the observed point. The subsequent optimization process minimizes these residuals by iteratively adjusting the camera pose, driving the transformed point (green) to gradually approach the optimal alignment with the target point (red).

[0091] Figure 6 The right-hand subplot illustrates the graph structure for G2O optimization. The central blue circle represents the camera pose vertex (SE3 vertex), i.e., the variable to be optimized (6-DOF pose: rotation and translation). The six surrounding red rectangles each represent a 3D-3D edge connecting the pose vertex to the observation 3D point. Connecting lines represent edges radiating from the central vertex to the red rectangles. In the G2O framework, the problem is modeled as a graph structure, where:

[0092] Vertex: Variable to be optimized, here it is a single SE(3) vertex (camera pose);

[0093] Edge: Constraints (error terms).

[0094] Each 3D-3D edge applies a constraint between a reference point (world coordinate system) and its corresponding observation point (camera coordinate system), with the goal of minimizing the difference between the transformed reference point and the observation.

[0095] The error function for each edge is shown in formula (6):

[0096] (6);

[0097] In the formula, This is the observation point (red triangle) in the camera coordinate system. This is the reference point in the world coordinate system (blue circle). Camera pose (parameters to be optimized). This is the residual (the difference between the observation point and the transformation point).

[0098] The optimization objective is shown in formula (7):

[0099] (7);

[0100] In the formula, For information matrix, This refers to the camera pose.

[0101] Compared with the traditional ICP method, the 3D-3D G2O optimization algorithm shows significant improvements in both accuracy and robustness. For example... Figure 7As shown in the trajectory error comparison chart, the average pose estimation error of 3D-3D G2O decreased significantly from 0.0194 in ICP to 0.0151, a reduction of 22.3%, while the standard deviation decreased by 33%. The more concentrated error distribution (fewer outliers in the box plot) confirms its superior local accuracy. Figure 8 The cumulative trajectory error curves shown further reveal that, throughout the 800-frame long sequence, G2O consistently maintains a smaller cumulative error than ICP, effectively suppressing SLAM system drift. The comprehensive data validates that G2O optimization significantly improves global consistency and system stability by continuously constraining error propagation.

[0102] Further, determining dynamic feature points based on the adaptive dynamic threshold model includes:

[0103] Calculate the scene flow direction similarity and motion amplitude consistency between the matched feature point and other feature points in the corresponding spatial neighborhood to obtain a local scene flow consistency score;

[0104] Based on the depth values ​​of feature points, the depth sensor error is modeled to obtain the depth adaptive threshold component;

[0105] The local scene flow consistency score, the depth adaptive threshold component, and the angle information between the feature point motion direction and the camera motion direction are fused to generate a dynamic judgment threshold for the corresponding feature point.

[0106] Specifically, the dynamic target detection and removal process includes:

[0107] like Figure 9 As shown, the overall process of dynamic target detection and removal is summarized. Its core working logic can be summarized as follows: First, camera displacement and velocity are calculated using camera pose changes and time intervals. Next, depth validity is checked on matching feature points between consecutive frames. 3D point coordinates are obtained through back projection, and predicted positions are calculated using camera motion compensation, thus obtaining the scene flow vector and its magnitude and direction. Then, depth difference and motion amplitude thresholds are applied for outlier filtering. Next, local scene flow consistency is calculated, and adaptive thresholds are generated based on multi-dimensional features. Dynamic points are determined using rules such as absolute motion thresholds, relative depth motion analysis, orientation angle verification, and co-motion checks. For detected instances (such as vehicles / pedestrians), the scene flow, depth value, threshold, and motion-camera orientation angle of their internal feature points are statistically analyzed. The decision threshold is dynamically adjusted using depth adaptive parameters, and a z-score test is performed to ensure statistical significance. Finally, based on the instance-level dynamic determination results, feature points within the contour are located at each level of the image pyramid, achieving dynamic point removal and outputting a filtered static keypoint set, thereby achieving dynamic target perception capability.

[0108] Furthermore, Figure 10The principle of scene flow consistency calculation is demonstrated: Using the central feature point (red dot) and its 3D motion vector (red arrow) as a baseline, evaluation is performed within a 5×5 neighborhood window (blue dashed circle). Feature points within the neighborhood are displayed as colored arrows (representing the 2D projection of their 3D motion vectors), with direction angles indicated by HSV color mapping, showing clear directional consistency. Excluded outliers (gray arrows) exhibit random directions that do not conform to the central point.

[0109] The algorithm quantifies scene flow reliability through three computational stages: First, it collects normalized motion direction vectors of neighboring points, calculates the arithmetic mean and normalizes it to obtain the average direction of the neighborhood; second, it calculates the angle between the center point direction vector and the average direction using inverse cosine operation based on dot product, and then uses the Gaussian kernel function to calculate the angle similarity; simultaneously, it compares the center point motion amplitude with the neighborhood average using an exponential decay function to evaluate amplitude consistency. Finally, the algorithm linearly fuses angle similarity (70% weight) and amplitude consistency (30% weight) to generate a scene flow consistency score of 0-1. The higher the score, the stronger the consistency with the surrounding motion pattern, and the more likely it is to belong to a static background structure. The mathematical expression of the algorithm is shown in formulas (8)-(12):

[0110] In this embodiment, the direction vector of the center point is denoted as... The set of direction vectors of neighboring points is represented as :

[0111] Calculate the average direction vector: (8);

[0112] Calculate the angle difference: (9);

[0113] Calculate the angle similarity score: (10);

[0114] Consistency of calculation magnitude: (11);

[0115] Calculate the eventual consistency score:

[0116] (12);

[0117] In the formula, Let be the average direction vector, n be the total number of local feature points, and i be the nth feature point. Let i be the direction vector of the i-th feature point. Due to the angle difference, The direction vector of the center point, Scoring is based on angular similarity. For the sake of amplitude consistency, It is a direction vector. The consistency score is given for the scene flow.

[0118] like Figure 11 As shown in the figure, this embodiment designs a unified dynamic thresholding framework for stereo vision and RGB-D sensors. This framework integrates multi-dimensional information such as depth error modeling, camera motion, orientation compensation, edge position and optical flow consistency to achieve autonomous optimization of feature extraction thresholds in different scenarios.

[0119] For stereo mode: Construct a nonlinear depth error model that scales with distance, synthesize it with camera displacement (velocity × time) to form a base threshold, and dynamically adjust the sensitivity through orientation compensation—a linear enhancement strategy is used for motion in the same direction, and cosine scaling is applied for large angular differences.

[0120] In RGB-D mode: Depth error is converted into 3D spatial error using camera intrinsic parameters, and then calibration and optimization are performed by combining edge region relaxation constraints and scene flow consistency factors. By quantifying perception uncertainty through sensor-geometry-motion fusion, this algorithm achieves adaptive threshold control in response to scene dynamics and sensor characteristics.

[0121] Furthermore, the process of semantic pose optimization includes:

[0122] In SLAM systems, even with dynamic point culling at the front end, semantically constrained G2O optimization remains indispensable. This is because front-end filtering cannot completely solve the problem of dynamic interference—semantic segmentation may misclassify semi-static objects (such as temporarily parked vehicles), while the ambiguity of potential dynamic objects requires differentiated processing mechanisms.

[0123] First, the maximum a posteriori probability estimation model under semantic constraints is established as follows:

[0124] (13);

[0125] In the formula, Z represents the observed data, and S represents the semantic information. Given a target state T and a scene point X i Under the conditions, the observed data Z i The likelihood probability, For semantic prior probability, Let T be the prior probability distribution of the target state T;

[0126] Among them, semantic prior probability Map points are divided into the following three categories:

[0127] static point High confidence observations;

[0128] Potential dynamic points Low confidence observations;

[0129] Dynamic Points Observation elimination.

[0130] Next, the maximum a posteriori probability estimation is transformed into a weighted robust least squares problem, with the following formula:

[0131] (14);

[0132] In the formula, To obtain the optimal pose estimate by minimizing the objective function, For semantic weights, For the adaptive Huber kernel function, Let be the residual vector for the i-th data point, representing the difference between the observed value and the model's predicted value. This is an information matrix.

[0133] Among them, information matrix Encode semantic weights into the covariance structure; Adaptive Huber kernel function threshold Determined by the chi-square distribution: Among them, the static point corresponds to Potential dynamic points correspond This configuration ensures that... Outlier removal is achieved at a certain confidence level.

[0134] Finally, the following four-stage asymptotic optimization is performed on the SE(3) Lie group manifold:

[0135] (15);

[0136] In the formula, This represents the Lie group addition operator. For the camera pose at time k+1, Let k be the camera pose. For a tiny pose increment on the Lie algebra, This is the transpose of the Jacobian matrix. This is the weight matrix. For Jacobian matrices, The damping factor, It is the identity matrix. This is the residual vector.

[0137] This gradual strategy dynamically adjusts the chi-square threshold. :

[0138] (16);

[0139] This configuration allows the optimization process to transition from maintaining a larger convergence region (preserving more constraints) to high-precision convergence (strict filtering).

[0140] Figure 12 The structural evolution of the semantically enhanced visual odometry factor graph model over time is visually presented. Center-aligned blue circular nodes represent camera pose sequences to be optimized, interconnected by temporal motion constraints represented by gray squares. Each camera pose node is connected upwards to green circular static landmarks, forming strong constraints through light green high-weight static factors; downwards, it is associated with yellow potential dynamic landmarks through light yellow low-weight potential dynamic factors. Notably, the system actively suppresses dynamic elements: although red dynamic landmarks exist in the framework, their optimization paths to camera poses are completely severed by red dynamic factors, and the connection between the potential dynamic landmark layer and the underlying red dynamic landmarks is ignored. This hierarchical constraint mechanism based on semantic labels achieves accurate and robust pose estimation by effectively suppressing dynamic interference.

[0141] This algorithm enhances the pose optimization performance of visual SLAM by fusing semantic information: First, map points are divided into three categories—static, potentially dynamic, and dynamic—based on semantic labels, and differentiated weights are assigned to each category. Then, a factor graph containing camera pose vertices and semantically weighted edges is constructed. A strict Huber kernel function is applied to static points, while a relaxed threshold is used for potentially dynamic points. Finally, a four-stage progressive optimization strategy is executed—the first two stages use relaxed optimization, and the last two stages switch to strict refinement—which significantly improves the localization robustness in dynamic environments by minimizing the weighted projection error within the G2O framework.

[0142] As shown in Table 1, Table 2 and Figure 13 As shown, "SC" represents semantic constraints. Experiments on the TUM and KITTI datasets demonstrate that introducing semantic constraint optimization (Ours(SC)) significantly improves the localization accuracy of the SLAM system: in the TUM high-dynamic scene, the optimized RMSE is reduced by up to 23.25% (sequence fr3-w-static), with an average improvement of 5.51%; the improvement is even more significant in the KITTI urban road scene—KITTI 05 error decreases by 31.7%, KITTI 03 by 25.6%, and the average error of all sequences decreases by 17.3%. This optimization is particularly effective in complex environments with frequent dynamic objects, such as dense pedestrian areas and temporary parking areas, verifying that semantic constraints, through dynamic point weighting and adaptive thresholding mechanisms, can effectively suppress the impact of dynamic interference on pose estimation.

[0143] Table 1

[0144]

[0145] Table 2

[0146]

[0147] This embodiment also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements a method for improving positioning accuracy in a dynamic SLAM scenario.

[0148] This embodiment also provides an electronic device, including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement a method for improving positioning accuracy in a dynamic SLAM scene.

[0149] To more clearly illustrate the technical solution of this invention, specific experiments are provided below for comparative explanation:

[0150] To comprehensively verify the performance of the proposed algorithm, experiments were conducted on a computing platform equipped with Ubuntu 18.04, an Intel i7-9750H processor, and 32GB of memory, using the TUM RGB-D high-dynamic dataset and the KITTI autonomous driving dataset. By comparing advanced dynamic SLAM methods such as ORB-SLAM2, YOLO-SLAM, and SG-SLAM, core metrics such as absolute trajectory error (ATE) were quantitatively evaluated. The TUM dataset focuses on robustness verification under dynamic object interference (such as fr3 / walking sequences), while KITTI was used to evaluate performance in dynamic urban street scenes and highway environments.

[0151] Absolute trajectory error (ATE): The root mean square error calculated through global trajectory alignment.

[0152] (17);

[0153] In the formula, Indicates the true pose. Represents estimated pose. Operators are used to extract translation components. For absolute trajectory error, this metric quantifies the positioning accuracy in the global coordinate system.

[0154] Relative pose error (RPE): Calculated at fixed time intervals Relative motion error within:

[0155] (18);

[0156] In the formula, and These represent the actual pose and the estimated pose at timestamp k, respectively. Represents a time interval. The F-norm, The relative pose error is used to quantify the cumulative drift error in local motion estimation. For timestamp The actual position of the location. For timestamp The estimated pose at the location.

[0157] Table 3 shows the computation time. Analysis indicates that dynamic feature recognition and removal have the highest computational intensity (33.1 milliseconds, accounting for 38.7% of the total processing time), while data processing (21.2 milliseconds) and pose estimation (20.5 milliseconds) constitute the basic computational load. Notably, semantically optimized pose estimation exhibits a significant efficiency advantage (requiring only 10.8 milliseconds). With a total latency of 85.6 milliseconds for all collaborative operations, approximately 11.7 FPS can be achieved, meeting real-time requirements. This demonstrates an effective balance between dynamic environment processing capability and computational efficiency—the time cost of dynamic feature processing confirms the aforementioned technical trade-off, which is precisely what enables the breakthrough in dynamic scene performance. Figure 14 The computation time of ORB-SLAM2 was compared with that of several advanced dynamic SLAM systems.

[0158] Table 3

[0159]

[0160] Performance evaluation on the TUM RGB-D dataset:

[0161] Tables 4 and 5 show the experimental results on the TUM dataset. The algorithm in this embodiment achieves a significant breakthrough in the field of dynamic environment SLAM. In highly dynamic scenes (such as the fr3_w_xyz sequence), its absolute trajectory error (ATE) is only 0.014 meters, a 98.6% reduction compared to ORB-SLAM2's 1.027 meters; the relative pose error (RPE) is sharply reduced from 0.506 meters to 0.012 meters (a 97.7% reduction), successfully overcoming pose drift caused by dynamic objects. In the fr3_w_static sequence, despite the presence of moving interference, the ATE is still controlled at 0.013 meters (a 95.2% improvement), and the RPE is reduced to 0.006 meters (a 97.01% reduction), demonstrating strong robustness to local dynamic disturbances.

[0162] Table 4

[0163]

[0164] Table 5

[0165]

[0166] Facing the challenging scenario fr3_w_rpy characterized by rotational motion, the algorithm in this embodiment achieves significant performance gains: ATE is reduced to 0.032 m and RPE is reduced to 0.018 m, representing improvements of 97.0% and 96.9% respectively compared to the baseline, significantly outperforming the failure performance of traditional methods in rapid turns. In the semi-dynamic scenario (fr3_w_half), ATE and RPE remain stable within 0.019 m and 0.012 m respectively, with improvements of no less than 96.8%, verifying the algorithm's strong generalization ability in mixed dynamic environments. Under low dynamic conditions, the algorithm in this embodiment continues to optimize performance metrics: ATE (0.005 m) and RPE (0.004 m) for the fr3_s_static sequence are reduced by 37.2% and 52.2% respectively compared to ORB-SLAM2; ATE (0.016 m) and RPE (0.012 m) for the fr3_s_half sequence are improved by 42.2% and 46.7% respectively, highlighting the stability of the algorithm implementation. The data collectively confirm the technological breakthrough of the algorithm in the field of dynamic SLAM in this embodiment: while improving the positioning accuracy of high dynamic scenes by two orders of magnitude, it maintains centimeter-level error control capability in all test scenarios, providing a reliable navigation solution for robot systems in complex environments.

[0167] like Figure 15 and Figure 16 As shown, the two sets of visualized experimental data clearly demonstrate that in high-dynamic sequences (such as fr3_w_xyz), the trajectory of ORB-SLAM2 (green line) deviates significantly from the blue ground truth trajectory, with a maximum offset exceeding 1.5 meters; while the trajectory of the algorithm in this embodiment (red "Ours" and yellow "Ours(SC)") almost perfectly matches the ground truth. The translation error curves plotted below more intuitively reveal this contrast: the blue error line of ORB-SLAM2 fluctuates dramatically, while the red and green error lines of the algorithm in this embodiment remain stable and close to the zero baseline. In low-dynamic scenarios (such as fr3_s_static), although the trajectory deviation of ORB-SLAM2 is smaller, the algorithm in this embodiment further compresses the error. It is particularly noteworthy that the trajectory error curves of "Ours" and "Ours(SC)" almost overlap, which confirms the algorithm's excellent robustness to dynamic disturbances (improvement of over 95% in high-dynamic scenarios) and its continuous optimization ability in static environments (error reduction of over 37%).

[0168] Based on the experimental data shown in Table 6, in comparison with mainstream dynamic SLAM systems (DS-SLAM, Dyna-SLAM, SG-SLAM, Amos-SLAM, DRM-SLAM, Crowd-SLAM, and YOLO-SLAM), the algorithm in this embodiment demonstrates leading SLAM accuracy with a comprehensive average absolute trajectory error (ATE) of 0.0163 meters. It exhibits comprehensive advantages in highly dynamic scenarios, with its ATE value significantly lower than all seven comparison systems. For example, in the rotating challenge scenario fr3 / w / rpy sequence (ATE: 0.0313 meters), the error is only 7% of the DS-SLAM result (0.4442 meters) and 14.5% of YOLO-SLAM (0.2164 meters). In the highly dynamic fr3_w_xyz sequence (ATE: 0.0140 meters), it shares the lowest error record with Amos-SLAM. In low-dynamic scenarios, although the algorithm in this embodiment is slightly inferior to Amos-SLAM (0.005411 meters) on the fr3_s_half sequence (0.0159 meters), it is still one of the few systems that can output valid results. Notably, it consistently maintains its leading position in static environments, achieving the lowest ATE (0.0053 meters) on the fr3_s_static sequence. These results collectively demonstrate the excellent robustness of the algorithm in this embodiment to high-dynamic disturbances and its superior stability across all test scenarios.

[0169] Table 6

[0170]

[0171] Performance evaluation on the KITTI dataset:

[0172] The experimental results on the KITTI dataset shown in Tables 7 and 8 demonstrate the significant advantages of the algorithm in complex scenarios. The algorithm achieves up to a 22% improvement in trajectory accuracy in highly dynamic urban environments (KITTI sequences 06 / 07) and a 12.3% reduction in error in high-speed motion scenarios (KITTI sequences 01 / 02). In key sequences such as KITTI 06, the root mean square error is optimized by 34.06% (from 0.7897 meters to 0.5207 meters). Although the accuracy is comparable to the baseline method in highly structured static environments (such as KITTI sequence 00), the overall results validate its robust performance under a variety of challenging conditions.

[0173] Table 7

[0174]

[0175] Table 8

[0176]

[0177] According to the comparison data in Table 9 with mainstream dynamic SLAM systems, the algorithm in this embodiment demonstrates comprehensive superior performance on the KITTI dataset: our method (SC), which employs a scene fusion strategy, achieves the lowest absolute trajectory error (ATE) in all eight sequences. Especially in highly dynamic urban environments (KITTI 06 / 07), it further reduces the error by 27.7% (0.72 m → 0.5207 m) and 17.5% (0.47 m → 0.3877 m) compared to DynaSLAM2; on high-speed structured roads (KITTI02), the root mean square error of our method (SC) is only 2.7495 m, achieving a 52.9% reduction (5.837 m → 2.7495 m) compared to the suboptimal Dynamic SLAM solution. It also shows breakthrough optimization in complex static regions (KITTI 03 / 04 / 05)—based on KITTI... Taking the 03 sequence as an example, the error dropped sharply to 0.2963 meters, a 67.8% improvement over DynaSLAM2 (0.92 meters → 0.2963 meters). Most significantly, in extremely challenging scenarios (such as the KITTI 01 highway), our method (SC) significantly outperformed other dynamic SLAM schemes (DynaSLAM2: 10.05 meters) with an accuracy of 5.6767 meters, representing a 43.5% reduction in error. These results validate the algorithm's scene-adaptive optimization strategy, successfully addressing the performance degradation issues of SLAM caused by dynamic interference, high-speed motion blur, and feature-sparse environments.

[0178] Table 9

[0179]

[0180] This embodiment, through trajectory comparison and visualization analysis of eight sequences (KITTI 00-07), shows that in high-speed motion scenarios (such as KITTI 01 / 02), the algorithm trajectory remains highly parallel to the ground truth, completely avoiding the progressive divergence phenomenon that occurs in traditional methods. Facing dense dynamic interference (such as urban roads in KITTI 06 / 07), the trajectory almost perfectly overlaps with the ground truth during turning maneuvers, effectively eliminating the systematic external drift present in the comparison algorithm. In structured static environments (such as KITTI 00 / 03), the algorithm trajectory always closely follows the centerline of the ground truth.

[0181] Particularly noteworthy is its enhanced performance in extreme scenarios (such as the KITTI 01 highway)—even when navigating complex areas like highway ramps and dense traffic intersections, the trajectory maintains minimal fluctuations. This verifies that the algorithm, through a collaborative mechanism of dynamic object perception and motion consistency constraints, fundamentally solves the trajectory distortion problem caused by dynamic object interference, high-speed motion blur, and complex road structures.

[0182] The above are merely preferred embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for improving positioning accuracy in dynamic SLAM scenarios, characterized in that, include: The RGB image of the current frame is obtained, a semantic mask is generated using an instance segmentation model, the depth map of the current frame is obtained, and after filtering and denoising, an optimized depth map is obtained. The semantic mask and the optimized depth map constitute preprocessed data with semantic-geometric information. Robust feature matching is performed on consecutive frames to obtain the correspondence of feature points. The semantic mask is used to exclude feature points in the dynamic object region and filter out static background feature points. The static background feature points are back-projected to three-dimensional space according to the depth information to form a 3D-3D point correspondence. The initial camera pose is solved by nonlinear optimization method. The camera motion information is calculated based on the initial camera pose. The depth validity and back projection of the matching feature points are checked. The scene flow vector is calculated in combination with the camera motion compensation. An adaptive dynamic threshold model is constructed based on scene flow consistency, depth difference, motion direction and camera motion angle. Dynamic feature points are determined and removed based on the adaptive dynamic threshold model, and a clean set of static feature points is output. Map points are classified into static points, potential dynamic points, and dynamic points according to semantic categories, and differentiated weights are assigned to each type of point. Based on the pure set of static feature points, a semantically weighted factor graph optimization model is constructed by combining the semantic categories and optimization weights. The camera pose is progressively optimized in the factor graph optimization model to obtain the final optimized high-precision camera pose.

2. The method for improving positioning accuracy in dynamic SLAM scenarios according to claim 1, characterized in that, For stereo images, a deep learning-based monocular depth estimation model is used to estimate depth information from the RGB image to generate the depth map.

3. The method for improving positioning accuracy in dynamic SLAM scenarios according to claim 1, characterized in that, Robust feature matching for consecutive frames includes: The forward and backward optical flow tracking method is adopted, and mismatch points are adaptively filtered by calculating the backward tracking error. Specifically: ； In the formula, Let be the characteristic coordinates at time point t. Forward optical flow tracking function, For directional optical flow tracking function, To be equal to the Euclidean distance, This is the adaptive reverse error threshold.

4. The method for improving positioning accuracy in dynamic SLAM scenarios according to claim 1, characterized in that, The nonlinear optimization method is a graph-based 3D-3D pose optimization method. The error term of the 3D-3D pose optimization method is the Euclidean distance between the 3D map point in the world coordinate system and the current frame observation point after pose transformation.

5. The method for improving positioning accuracy in dynamic SLAM scenarios according to claim 1, characterized in that, Determining dynamic feature points based on the aforementioned adaptive dynamic threshold model includes: Calculate the scene flow direction similarity and motion amplitude consistency between the matched feature point and other feature points in the corresponding spatial neighborhood to obtain a local scene flow consistency score; Based on the depth values ​​of feature points, the depth sensor error is modeled to obtain the depth adaptive threshold component; The local scene flow consistency score, the depth adaptive threshold component, and the angle information between the feature point motion direction and the camera motion direction are fused to generate a dynamic judgment threshold for the corresponding feature point.

6. The method for improving positioning accuracy in a dynamic SLAM scene according to claim 5, characterized in that, Obtain a local scene flow consistency score, including: Calculate the average direction vector: ; Calculate the angle difference: ; Calculate the angle similarity score: ; Consistency of calculation magnitude: ; Calculate the eventual consistency score: ; In the formula, Let be the average direction vector, n be the total number of local feature points, and i be the nth feature point. Let i be the direction vector of the i-th feature point. Due to the angle difference, The direction vector of the center point, Scoring is based on angular similarity. For the sake of amplitude consistency, It is a direction vector. The consistency score is given for the scene flow.

7. The method for improving positioning accuracy in dynamic SLAM scenarios according to claim 1, characterized in that, In the semantically weighted factor graph optimization model, the static points are assigned high weights, the potential dynamic points are assigned low weights, and the dynamic points do not participate in the optimization. An incremental optimization strategy is adopted, which uses a relaxed threshold to retain more constraints in the first two stages and a strict threshold to achieve precise convergence in the last two stages.

8. The method for improving positioning accuracy in a dynamic SLAM scene according to claim 7, characterized in that, The incremental optimization strategy is as follows: ； In the formula, This represents the Lie group addition operator. For the camera pose at time k+1, Let k be the camera pose. For a tiny pose increment on the Lie algebra, This is the transpose of the Jacobian matrix. This is the weight matrix. For Jacobian matrices, The damping factor, It is the identity matrix. This is the residual vector.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1-8.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory, characterized in that, When the processor executes the computer program, it implements the method as described in any one of claims 1-8.

Images