A visual slam method based on dynamic feature compensation and weak constraint reuse

Abstract

The application provides a visual SLAM method based on dynamic feature compensation and weak constraint reuse. In view of the problems of insufficient constraint caused by feature elimination and failure of epipolar constraint caused by pure rotation, an IMU-assisted adaptive geometric model selection strategy is proposed. According to the adaptive switching between the basis matrix and the homography matrix according to the translation module length, the pure rotation degeneration is overcome. The adaptive classification of static and dynamic is realized by using the Gaussian mixture model and the RANSAC iteration refinement. The single-frame misjudgment is inhibited by combining the semantic priori and the pixel-level motion foreground double verification. The dynamic features are decomposed into local motion consistent sub-clusters through motion consistency density clustering. The object motion is decoupled by using the depth information and SVD decomposition, and three-dimensional compensation is performed to restore the quasi-static observation. According to the sufficient degree of static points, the compensation observation reuse weight is adaptively adjusted. The accuracy is maintained in the conventional scene, and the tracking loss is prevented in the extreme scene. The application significantly improves the system robustness in the dynamic environment while ensuring the positioning accuracy.

Description

Technical Field

[0001] This invention relates to the fields of computer vision and simultaneous localization and mapping (SLAM), and in particular to a visual SLAM method based on dynamic feature compensation and weak constraint reuse. Background Technology

[0002] Visual SLAM is a core technology for mobile robots to achieve autonomous localization and environmental modeling in unknown environments, with broad application prospects in fields such as autonomous driving, augmented reality, and robot navigation. Traditional visual SLAM methods rely on the static assumption of the environment, that is, feature points in the scene only change in projection with camera movement between frames. However, dynamic objects such as pedestrians and vehicles inevitably exist in real-world scenes. When dynamic objects occupy a large portion of the field of view, the static assumption of traditional SLAM systems is severely violated. Dynamic feature points introduce a large number of outliers during inter-frame matching, leading to pose estimation drift or even tracking loss. How to maintain robust localization and mapping capabilities in highly dynamic environments is a key obstacle for visual SLAM technology to move from laboratory settings to practical deployment.

[0003] To address the above issues, existing research has mainly developed three technical approaches: Detection and elimination strategies: Most existing methods adopt the approach of "detect first, then eliminate", directly excluding identified dynamic features. Although this strategy is simple and effective, in highly dynamic scenes, after a large number of features are removed, the available geometric constraints are significantly reduced, which can easily lead to degradation of local bundle adjustment or even tracking loss.

[0004] The epipolar geometry-based dynamic-static classification strategy utilizes epipolar constraints to distinguish between dynamic and static points. However, when the camera undergoes pure rotational motion, the fundamental matrix degenerates, the epipolar geometry constraints fail, and it becomes difficult to reliably distinguish between dynamic and static points. Pure rotation is a common scenario in real robot motion (such as turning in place or gimbal rotation), which severely limits the reliability of this type of method in practical deployment.

[0005] Strategies based on explicit motion modeling: Some methods attempt to explicitly model the motion of dynamic objects and incorporate them into joint optimization, but each additional dynamic object introduces at least 6 additional degrees of freedom, and the computational burden increases linearly with the number of dynamic objects in the scene, making it difficult to meet real-time requirements.

[0006] In summary, existing dynamic SLAM methods face a triple dilemma when dealing with highly dynamic scenes: insufficient constraints due to feature removal, failure of geometric classification due to pure rotation, and computational overload due to explicit modeling. How to effectively maintain geometric constraints in highly dynamic scenes without excessively increasing computational burden, and prevent tracking loss in extreme dynamic scenes, is a pressing technical challenge in the field of visual SLAM. Summary of the Invention

[0007] To address the problems of insufficient geometric constraints due to feature removal in dynamic scenes and epipolar constraint failure caused by pure rotation in existing methods, this invention proposes a visual SLAM method based on dynamic feature compensation and weak constraint reuse. Unlike the detection-then-removal strategy, this method performs motion compensation on dynamic features and retains and reuses them in a weighted manner, effectively maintaining scene geometric constraints in highly dynamic scenes.

[0008] First, adaptive static / dynamic classification is achieved using IMU-assisted geometric constraints and a Gaussian mixture model. Two adjacent image frames are read, ORB feature points are extracted and matched to obtain matching feature point pairs. The relative pose between adjacent frames is calculated using IMU pre-integration. The motion mode is determined by the translational modulus; if the translational modulus is greater than a threshold, the fundamental matrix model is selected; otherwise, the homography matrix model is chosen to avoid pure rotational degradation. Based on the selected geometric model, the epipolar distance or projective distance of each matching feature point is calculated. The distance error of all feature points is fitted using a two-component Gaussian mixture model, and the adaptive threshold is obtained using the EM algorithm, completing the initial static / dynamic classification. Using the initial static points as seeds, the geometric model is re-estimated using RANSAC and iteratively refined to eliminate the initial IMU error, resulting in the final static point set and dynamic candidate point set.

[0009] Secondly, a dual verification process combining YOLO semantic priors and pixel-level moving foreground is employed to suppress single-frame misjudgments. An affine transformation matrix is ​​estimated using a static point set to eliminate background shifts caused by camera motion, generating a compensation image aligned with the reference frame. The PBAS algorithm is then used to extract the moving foreground mask from the compensation image, and overlap matching is performed using YOLO object detection boxes to filter out the true moving target regions. The portion of dynamic candidate points falling within these true moving target regions is selected for subsequent processing.

[0010] Furthermore, a density clustering algorithm with motion direction constraints is introduced to automatically decompose dynamic features into locally consistent subclusters according to motion patterns at the two-dimensional motion residual level. For the selected feature points, the local motion residual vectors are extracted using inverse affine transformation, a motion difference metric is defined, and a density clustering algorithm with motion consistency constraints is run to divide them into locally motion-consistent subclusters.

[0011] Then, using depth information and known camera motion, the object's own motion is decoupled through SVD decomposition, and 3D compensation is performed to restore the quasi-static observation. For each sub-cluster, the current frame pixel coordinates and depth values ​​are back-projected onto the camera coordinate system to obtain 3D observation points, and the corresponding 3D points from the previous frame are obtained. Based on the known inter-frame camera motion, the points from the previous frame are transformed to the current frame coordinate system, and a motion decoupling objective function is constructed. The rigid body motion parameters of the object itself are solved using closed-form SVD decomposition. The object's motion components are separated from the mixed observations using the inverse transformation of the object's own motion parameters. The 3D coordinates of the assumed object are stationary are calculated, and then reprojected onto the image plane using the camera projection function to obtain the compensated quasi-static observation points.

[0012] Finally, the participation of compensation observations in the tracking thread is adaptively adjusted based on the sufficiency of static feature points in the scene. The variance of motion residuals and semantic confidence factors for each cluster are calculated to determine the geometric / semantic joint weights of the compensation observations. When there are sufficient static points, pose estimation is performed solely based on static points; when the number of static points is below a preset threshold, compensation observations are incorporated into pose estimation as weak constraints with joint weights, providing supplementary constraints for tracking and preventing front-end degradation due to insufficient static points.

[0013] To achieve the above objectives, this invention provides a visual SLAM method based on dynamic feature compensation and weak constraint reuse, the specific steps of which include: S1: Acquire two adjacent frames of images, extract feature points and match them to obtain matching feature point pairs; S2: Use IMU pre-integration to obtain relative pose information between two adjacent frames, determine the motion mode of the current frame based on the translational modulus in the relative pose information, and adaptively select the fundamental matrix model or homography matrix model as the initial geometric model according to the motion mode. S3: Based on the initial geometric model, calculate the geometric distance error of each matching feature point, and perform adaptive dynamic and static classification on the distance error distribution of all feature points to obtain a preliminary static point set and a dynamic candidate point set; S4: Using the initial static point set as a seed, the geometric model is re-estimated and iteratively refined using the RANSAC algorithm to eliminate the initial error of the IMU and obtain the final static point set and dynamic candidate point set. S5: Based on the dynamic candidate point set, cross-validation is performed by combining semantic prior information and pixel-level motion foreground to select feature points in the real motion target area as effective dynamic feature points, and the effective dynamic feature points are clustered by motion pattern to be divided into several local motion consistent sub-clusters. S6: For each of the local motion consistent subclusters, use depth information to decouple the three-dimensional motion from the known camera motion, separate the object's own motion component from the mixed observation, perform three-dimensional coordinate compensation on the separated feature points, and restore them to quasi-static observation points. S7: Based on the number of feature points in the final static point set in the current frame, adaptively adjust the participation of the quasi-static observation points in pose estimation; when the number of static feature points is sufficient, the final static point set is used first for pose estimation, and the quasi-static observation points do not participate in the pose estimation optimization function; when the number of static feature points is lower than a preset threshold, the quasi-static observation points are included as weak constraints in the pose estimation optimization function to provide supplementary constraints for the tracking thread.

[0014] Further, in step S1, two adjacent frames of images are acquired, feature points are extracted and matched to obtain matching feature point pairs. The specific process is as follows: The video stream is acquired, and two adjacent frames are read sequentially. ORB feature points are extracted from the previous frame and descriptors are calculated. In the next frame, nearest neighbor and second nearest neighbor searches are performed based on the descriptor distance. If the ratio of the nearest distance to the second nearest distance is less than a preset threshold, it is initially determined to be a matching point pair. Symmetric constraints are used for bidirectional matching verification, and only feature point pairs that match in both forward and reverse directions are retained, resulting in a robust set of adjacent frame matching point pairs.

[0015] Furthermore, the initial geometric model selection process in step S2 includes: Relative rotation between two adjacent frames is obtained using IMU pre-integration. Peaceful relocation And calculate the translational modulus: ; The motion mode is determined based on the translational modulus length. It was determined that the camera exhibited significant translational movement, and the fundamental matrix model was chosen as the initial geometric model, expressed as: ;like To determine that the camera is in an approximately pure rotational state, a homography matrix model is selected as the initial geometric model, expressed as: ; in, Represents the camera intrinsic parameter matrix. This represents an antisymmetric matrix operator constructed from three-dimensional vectors. This indicates the preset translation modulus threshold. Represents the rotation matrix. This represents the translation vector.

[0016] Furthermore, step S3, which involves adaptive dynamic-static classification of the distance error distribution of all feature points to obtain a preliminary static point set and a dynamic candidate point set, specifically includes: A two-component Gaussian mixture model is used to model the distance error distribution, and the expression is: ,in, Indicates geometric distance error. , These represent the combined weights of the two components. , Let represent the mean and variance of each Gaussian component, respectively. Represents the Gaussian distribution function; The model parameters are estimated iteratively using the expectation-maximization algorithm. Based on the Bayesian minimum error rate criterion, the optimal decision boundary is solved using the model parameters to generate an adaptive classification threshold. The boundary satisfies: ; Feature points with a distance error greater than the adaptive classification threshold are marked as dynamic candidate points, and the rest are marked as preliminary static points, thus obtaining a preliminary static point set and a dynamic candidate point set.

[0017] Furthermore, step S5, which involves cross-validating semantic prior information with pixel-level moving foreground information based on the dynamic candidate point set to filter feature points within the real moving target region, specifically includes: S51: Utilize the final static point set to estimate the affine transformation matrix, eliminate the background shift caused by camera motion, and generate a compensated image aligned with the reference frame; S52: Extract a pixel-level motion foreground mask from the compensated image and obtain the bounding rectangular region of the motion foreground mask. D' ; S53: Perform target detection on the current frame, obtain semantic target detection boxes, match the semantic target detection boxes with the motion foreground mask to filter out the real moving target regions; S54: The dynamic candidate points that fall within the actual moving target area are taken as effective dynamic feature points.

[0018] Furthermore, in step S52, the PBAS algorithm is used to extract the pixel-level moving foreground mask; in step S53, target detection is implemented using a YOLO series neural network; the calculation method for the overlap rate matching is as follows: Calculate each YOLO object detection box Rectangular region circumscribed by the motion foreground mask D The overlap rate of ': Where S() represents the area function of the region; if Greater than the preset overlap rate threshold δ If the target is detected, the region corresponding to the semantic target detection box is determined to be a real moving target region; otherwise, it is discarded as static background.

[0019] Furthermore, step S5, which involves clustering the effective dynamic feature points into motion pattern subclusters, specifically includes: The local motion residual vector of each effective dynamic feature point is extracted using the inverse transformation of the affine transformation matrix, expressed as: ,in, Indicates the actual observed coordinates of the current frame. Let A represent the coordinates of the matching point in the previous frame, and let A represent the affine transformation matrix. Based on the local motion residual vector, a motion difference metric is defined, and a density clustering algorithm based on motion consistency constraints is run to divide the effective dynamic feature points into several locally motion-consistent subclusters.

[0020] Furthermore, the density clustering algorithm based on motion consistency constraints divides the effective dynamic feature points into several locally motion-consistent sub-clusters, specifically including: The similarity of the motion residuals between two points is quantified using Mahalanobis distance: ,in, Representing feature points i and feature points j The difference in motion scores between them and Representing points respectively i and points j The local motion residual vector, The inverse matrix of the covariance matrix; Based on the properties of the chi-square distribution, at the first significance level Determine the initial clustering threshold At the second significance level Determine the cluster merging threshold ,in ; Perform initial clustering and cluster expansion based on the initial clustering threshold, and use the cluster merging threshold to merge spatially adjacent and similarly moving clusters, remove noise points, and obtain locally consistent sub-clusters.

[0021] Furthermore, step S6 specifically includes the following processes: S61: For the first in a locally motion-consistent subcluster i Each feature point is determined based on its pixel coordinates in the current frame. and corresponding depth values Using the camera intrinsic parameter matrix Perform a back-projection transformation to map the feature point to the current frame camera coordinate system, thus obtaining the 3D observation point of the feature point in the current frame camera coordinate system. And obtain the 3D point corresponding to the matching point of the feature point in the previous frame through ORB descriptor matching. ; S62: Rotation matrix of inter-frame camera motion provided by IMU pre-integration or initial pose estimation. Translation vector , take the previous frame's 3D points Transform to the current frame coordinate system to obtain the static desired position under pure camera motion. The expression is: ,in, Represents the rotation matrix. Represents the translation vector; S63: Constructing the objective function for motion decoupling The rigid body motion parameters of the object itself are solved by singular value decomposition in a closed form. S64: By using the inverse transformation of the object's own motion parameters, the motion components of the object are separated from the mixed observations, and the three-dimensional coordinates are calculated assuming the object is stationary. ,in, This represents the compensated three-dimensional coordinates, i.e., the quasi-static point. It represents the amount of translation of the object itself. This represents the object's own rotation; and is then reprojected onto the image plane using a camera projection function to obtain the compensated quasi-static observation point. .

[0022] Furthermore, before incorporating the quasi-static observation point as a weak constraint into the pose estimation optimization function in step S7, a joint weight calculation step is also included, specifically: Calculate the variance of the intra-cluster motion residuals for each locally motion-consistent sub-cluster. Taking the k-th dynamic sub-cluster as an example, the expression is: ,in, Indicates the first sub-cluster i The motion residual vector of each feature point This represents the average motion residual vector of all feature points within the sub-cluster. Indicates the first k Each independent sub-cluster This represents the total number of feature points within the sub-cluster; The reuse weights of compensated observations are determined based on geometric consistency and semantic priors: ,in, The semantic confidence factor is determined based on the category of the moving target corresponding to the local motion-consistent subcluster. These are the normalization coefficients; reuse weights The information matrix scaling factor or robust kernel function weight of the quasi-static observation point in the pose estimation optimization function is used to achieve weak constraint access.

[0023] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention effectively solves the problem of unreliable dynamic and static classification caused by the failure of epipolar geometric constraints under pure rotational motion by adaptively selecting the geometric model through IMU-assisted translational modulus length determination. Through motion compensation and 3D decoupling, dynamic features are repaired into quasi-static observation points and reused with reduced weights, avoiding insufficient geometric constraints and BA degradation caused by the removal of a large number of features in highly dynamic scenes. By adopting an adaptive switching strategy that relies on static points when they are sufficient and accesses compensated observations when they are insufficient, the tracking loss rate in extreme dynamic scenes is significantly reduced while ensuring that the positioning accuracy in conventional scenes is not affected. Furthermore, this invention uses SVD decomposition to solve for object motion parameters in a closed-form manner, eliminating the need for iterative optimization and avoiding the computational burden problem of variable dimensions increasing linearly with the number of dynamic objects in explicit motion modeling. Attached Figure Description

[0024] Figure 1 This is a flowchart of a visual SLAM method based on dynamic feature compensation and weak constraint reuse.

[0025] Figure 2 For the TUM RGB-D dataset fr3 / walking xyz Comparison of ATE trajectories for the sequence.

[0026] Figure 3 For the TUM RGB-D dataset fr3 / walking halfsphere Comparison of ATE trajectories for the sequence. Detailed Implementation

[0027] To enable those skilled in the art to better understand the technical solutions in this specification, the technical solutions in 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 this specification, and not all embodiments. Based on the embodiments in this specification, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this specification.

[0028] Figure 1 A flowchart illustrating the steps of a visual SLAM method based on dynamic feature compensation and weak constraint reuse, as provided in this embodiment of the invention, includes the following steps: S1: Acquire two adjacent frames of images, extract feature points and match them to obtain matching feature point pairs; S2: Use IMU pre-integration to obtain relative pose information between two adjacent frames, determine the motion mode of the current frame based on the translational modulus in the relative pose information, and adaptively select the fundamental matrix model or homography matrix model as the initial geometric model according to the motion mode. S3: Based on the initial geometric model, calculate the geometric distance error of each matching feature point, and perform adaptive dynamic and static classification on the distance error distribution of all feature points to obtain a preliminary static point set and a dynamic candidate point set; S4: Using the initial static point set as a seed, the geometric model is re-estimated and iteratively refined using the RANSAC algorithm to eliminate the initial error of the IMU and obtain the final static point set and dynamic candidate point set. S5: Based on the dynamic candidate point set, cross-validation is performed by combining semantic prior information and pixel-level motion foreground to select feature points in the real motion target area as effective dynamic feature points, and the effective dynamic feature points are clustered by motion pattern to be divided into several local motion consistent sub-clusters. S6: For each of the local motion consistent subclusters, use depth information to decouple the three-dimensional motion from the known camera motion, separate the object's own motion component from the mixed observation, perform three-dimensional coordinate compensation on the separated feature points, and restore them to quasi-static observation points. S7: Based on the number of feature points in the static point set in the current frame, adaptively adjust the participation of the quasi-static observation points in pose estimation; when the number of static feature points is sufficient, prioritize the use of the final static point set for pose estimation, and the quasi-static observation points do not participate in the pose estimation optimization function; when the number of static feature points is lower than a preset threshold, incorporate the quasi-static observation points as weak constraints into the pose estimation optimization function to provide supplementary constraints for the tracking thread.

[0029] In one specific implementation of this embodiment, step S1 involves acquiring two adjacent frames of images, extracting feature points, and matching them to obtain matching feature point pairs. This is specifically achieved through the following steps: (1) Read in the video stream and read two adjacent frames of images in sequence. Extract ORB feature points and calculate descriptors for the previous frame. In the next frame, search for the two closest candidate matching points by descriptor distance. If the ratio of the second closest distance to the closest distance is less than a set threshold (0.8 in the preferred embodiment of the present invention), it is determined to be a matching point pair.

[0030] (2) To remove mismatches, symmetric constraints are used for bidirectional matching verification. For ORB feature points in the previous frame, the forward matching method is used to find their matching points in the current frame, and the reverse matching method is used to find the matching points in the previous frame in the current frame. Only bidirectional consistent feature point pairs are retained to obtain a robust and accurate set of matching feature point pairs.

[0031] In one specific implementation of this embodiment, step S2 utilizes IMU pre-integration to obtain relative pose information between two adjacent frames, determines the motion mode of the current frame based on the translational modulus in the relative pose information, and adaptively selects either the fundamental matrix model or the homography matrix model as the initial geometric model based on the motion mode. This is specifically achieved through the following steps: (1) Obtain the relative rotation between adjacent frames using IMU pre-integration. Peaceful relocation And calculate the translational modulus: .

[0032] (2) Determine the motion mode based on the translational modulus length, if If the camera is determined to have significant translation, the fundamental matrix model is selected as the initial geometric model; if The camera is determined to be in an approximately pure rotational state, and the homography matrix model is selected as the initial geometric model.

[0033] (3) Calculate the initial geometric matrix according to the selected model. The calculation formula is shown in equation (1).

[0034] (1) In the formula, Represents the camera intrinsic parameter matrix; Represents an antisymmetric matrix operator constructed from three-dimensional vectors; This represents a preset translational modulus threshold. In a preferred embodiment of the present invention, the threshold... ε Take 0.01m; Represents the rotation matrix; This represents the translation vector.

[0035] In one specific embodiment of this example, step S3 calculates the geometric distance error of each matching feature point based on the initial geometric model. The specific steps are as follows: (1) If the basic matrix model is used in step S2, then calculate the epipolar distance between each matching feature point. Epipolar distance Defined as: (2) in F Based on the matrix, and To match the homogeneous pixel coordinates of point pairs, ( F )1 and ( F )2 represents the two components of the epipolar vector, with the numerator being the algebraic distance from the point to the epipolar line and the denominator used for normalization. This distance is used to quantify the degree to which the feature point violates the static assumption.

[0036] (2) If the homography matrix model is selected in step S2, the distance error is expressed as the Euclidean distance between the original feature point and the corresponding projected feature point, as follows: (3) in( This indicates that the feature points from the previous frame are projected onto the current frame using the homography matrix. ) represents the actual observed coordinates of the feature points matched in the current frame.

[0037] In one specific embodiment of this example, step S3 involves adaptive static / dynamic classification of the distance error distribution of all feature points to obtain a preliminary static point set and a dynamic candidate point set. The specific steps are as follows: (1) Distance error distribution modeling In most dynamic scenes, the static background and dynamic foreground often correspond to distance error distributions at different scales. The overall error typically exhibits an approximately bimodal structure, with small and concentrated distance errors at static points and large and dispersed distance errors at dynamic points. A two-component Gaussian mixture model is used to model the distance error distribution, expressed as: (4) In the formula, Indicates geometric distance error. , These represent the combined weights of the two components. , Let represent the mean and variance of each Gaussian component, respectively. This represents the Gaussian distribution function.

[0038] (2) Iterative estimation of model parameters Model parameters are iteratively estimated using the Expectation-Maximization (EM) algorithm. After convergence, the optimal decision boundary is solved based on the Bayesian minimum error rate criterion to generate an adaptive classification threshold. The threshold It satisfies equation (5), and the threshold can be adaptively adjusted according to the residual distribution of the current frame, thus avoiding the problem of insufficient adaptability of the fixed threshold in different dynamic scenarios.

[0039] (3) Preliminary classification of dynamic and static states Based on adaptive threshold Complete preliminary dynamic and static classification, based on feature points. For example, if its distance error If a point is identified as a dynamic candidate point, it is marked as a preliminary static point; otherwise, it is marked as a preliminary static point. This yields the preliminary static point set and the dynamic candidate point set.

[0040] In one specific implementation of this embodiment, step S4 uses the initial static point set as a seed, re-estimates the geometric model using the RANSAC algorithm, iteratively refines it, eliminates the initial IMU error, and obtains the final static point set and dynamic candidate point set. This is specifically achieved through the following: During implementation, considering the existence of error sources such as zero bias in the IMU sensor, the initial geometric model provided by the IMU pre-integration in step S2 may have biases. Therefore, this embodiment uses the obtained preliminary static point set as a seed, re-estimates the fundamental matrix or homography matrix using the RANSAC algorithm, recalculates the distance error for all matching feature points based on the new model, and performs GMM adaptive classification again. This process is repeated 2-3 times until the static point set structure stabilizes, resulting in the final static point set and dynamic candidate point set.

[0041] In one specific implementation of this embodiment, in step S5, based on the dynamic candidate point set, semantic prior information and pixel-level moving foreground are combined for cross-validation to select feature points within the real moving target area as effective dynamic feature points. The specific steps are as follows: (1) Estimate the affine transformation matrix using a static point set, eliminate the background shift caused by camera motion, and generate a compensation image aligned with the reference frame, specifically: Using the static point set obtained in step S4, the global affine transformation matrix is ​​estimated using the least squares method. Let the coordinates of the feature points in the current frame be... Its matching point coordinates in the previous frame are Then the following condition is met: in for Homogeneous affine matrix, parameters to Encoding rotation and scaling, Encoding shift. Utilizing Global motion compensation is performed on the current frame, and a compensated image aligned with the background of the reference frame is generated through bilinear interpolation. .

[0042] (2) The PBAS algorithm is used to extract the moving foreground mask on the compensated image, and the overlap rate is matched with the YOLO target detection box to filter out the real moving target region, specifically: ① For compensated images pixel values Calculate the minimum distance between it and the sample set: If with Distance less than adaptive threshold If the number of samples reaches a set threshold, it is classified as background; otherwise, it is classified as foreground, thus obtaining a binary foreground mask. .

[0043] in, Indicates compensation on the image Pixel value at that location, Maintained for this pixel A collection of historical background samples This is the minimum distance between the current pixel value and all background samples. If Less than the adaptive threshold ( For pixels The adaptive discrimination threshold at the current location is used when a pixel is determined to be background. With history The weighted moving average is used for updating; when a pixel is determined to be foreground, the weighted moving average is used. If the number of samples of a pixel that matches the background model reaches a set threshold, it is considered a background pixel; otherwise, it is considered a foreground pixel, thus obtaining a binary foreground mask. .

[0044] ② For binary foreground mask Perform connected component analysis to extract the minimum bounding rectangle of each motion region, and obtain the set of candidate motion regions.

[0045] ③ Use YOLOv8 to perform object detection on the current frame to obtain a set of semantic target boxes. For each YOLO target bounding box Select the bounding rectangle region of the moving foreground mask. And calculate the overlap rate: In the formula, S() represents the area function of the region. In a preferred embodiment of the present invention, if (Pick If so, then determine If the target area is a real moving object, it will be excluded as a static background.

[0046] ④ Compare the dynamic candidate points obtained in step S4 with the actual moving target area ( Spatial location comparison is performed to filter out feature points that fall within the actual moving target area, which are then used as effective dynamic feature points for subsequent processing.

[0047] In one specific implementation of this embodiment, step S5 involves clustering the effective dynamic feature points according to their motion patterns, dividing them into several locally motion-consistent subclusters. The specific steps are as follows: (1) For the valid dynamic feature points determined in step S5, record their current frame coordinates as... The coordinates of the matching point in the previous frame are The inverse transformation of the affine transformation matrix estimated in step S5 is then performed. Mapping the matching points from the previous frame to the viewpoint of the current frame yields the desired position assuming the object is stationary: The local motion residual vector is defined as the difference between the actual observed coordinates and the desired position in the current frame: The residual vector contains the rigid body translation, rotation, non-rigid body deformation, and observation noise of the entire object, and will be used as the input feature for clustering.

[0048] (2) Extract the local motion residual vector of each feature point To quantify the similarity of the motion patterns of two points, the Mahalanobis distance is defined as follows: In the formula, Representing feature points i and feature points j Measurement of motion differences between them and Representing points respectively i and points j The local motion residual vector, Denotes the inverse of the covariance matrix; take , The variance of the motion residuals of all static feature points is estimated and used as a unified noise benchmark. The square of this Mahalanobis distance... It approximately follows a chi-square distribution with 2 degrees of freedom. Based on the properties of the chi-square distribution, at a significance level... (Strict, used for initial clustering) and (Relaxed, (used for subsequent cluster merging) to determine two levels of thresholds and When the difference in motion between two points is If the values ​​are less than the corresponding threshold, the motion patterns of the two are determined to be the same.

[0049] (3) Initial cluster generation. For any point among the effective dynamic feature points obtained in step S5... Let the set of points in its ε-neighborhood be denoted as .when When (MinPts is set to 5 in this embodiment), the Huber loss function is used to robustly estimate the dominant motion direction in the neighborhood. Filter points with the same direction of movement .like ,but With the core point, For the initial cluster, record the reference motion direction of the cluster. and space center .

[0050] (4) Cluster expansion. Starting from the core point within the cluster, ... satisfying in the neighborhood Points are included in the current cluster, while core points are recursively expanded, and statistics are updated incrementally.

[0051] (5) Cluster merging. For spatially adjacent ( And their motions are similar. The clusters are merged. This makes the merger criteria more lenient. After the merger, points that are insufficient will be excluded. The noise clusters are used to obtain locally motion-consistent subclusters.

[0052] In one specific embodiment of this example, in step S6, for each of the locally motion-consistent subclusters, 3D motion decoupling is performed using depth information and known camera motion to separate the object's own motion components from the mixed observation. 3D coordinate compensation is then performed on the separated feature points to restore them to quasi-static observation points. This is specifically achieved through the following steps: (1) Decoupling of the object's own motion The subclusters obtained in step S5 satisfy local motion consistency between short-time frames and can be approximated as local rigid bodies. For the first subcluster with consistent local motion... Each feature point is determined based on its pixel coordinates in the current frame. and corresponding depth values via camera intrinsic parameter matrix Back-projecting to the current frame camera coordinate system, the three-dimensional observation point of the feature point in the current frame camera coordinate system is obtained, as shown in equation (12). Then, the three-dimensional point corresponding to the matching point of the feature point in the previous frame is obtained by ORB descriptor matching. .

[0053] Suppose that the inter-frame camera motion is provided by IMU pre-integration or initial pose estimation. , take the previous frame's 3D points Transform to the current frame coordinate system to obtain the static desired position under pure camera motion. The expression is: In the formula, Represents the rotation matrix. This represents the translation vector.

[0054] This point represents the position a static point should occupy under pure camera motion. Actual observation. With static expected position The difference between them reflects the independent rigid body motion of the object in the current camera coordinate system, denoted as . That is, the object's own motion will further transform the static desired point projected by the camera motion to the actual observed position in the current frame. This relationship is constructed as a motion decoupling objective function, which is solved in a closed form using singular value decomposition: This problem is solved using a closed-form method with singular value decomposition (SVD), directly providing an optimal estimate of the object's motion. An analytical solution is obtained without iteration, ensuring real-time processing of the front-end. In a preferred embodiment of this invention, only when the number of valid points within the cluster... The calculation is performed in real time, and those with insufficient points are marked as noise and not processed.

[0055] (2) Three-dimensional motion compensation After obtaining the object's own motion, its inverse transformation is used to separate the object's motion components from the mixed observations. The three-dimensional coordinates of the object when it is assumed to be stationary are then calculated, as expressed in the following expression: In the formula, This represents the compensated three-dimensional coordinates, i.e., the quasi-static point. It represents the amount of translation of the object itself. It indicates the amount of rotation of the object itself.

[0056] The compensated quasi-static observation point is obtained by reprojecting the image onto the image plane using the camera projection function. : Where the projection function .

[0057] In one specific embodiment of this example, step S7 adaptively adjusts the participation of the quasi-static observation points in pose estimation based on the number of feature points in the static point set in the current frame. The specific steps are as follows: (1) Calculation of variance of motion residuals within clusters Calculate the variance of the intra-cluster motion residuals for each locally motion-consistent sub-cluster. Taking the k-th dynamic sub-cluster as an example, the expression is: In the formula, Indicates the first sub-cluster i The motion residual vector of each feature point This represents the average motion residual vector of all feature points within the sub-cluster. Indicates the first k Each independent sub-cluster This represents the total number of feature points within the sub-cluster; The smaller the value, the more consistent the cluster's motion patterns, and the more reliable the compensation.

[0058] (2) Determining reuse weights The reuse weights of each cluster's compensated observations are jointly determined by geometric consistency and semantic prior, expressed as: In the formula, The semantic confidence factor is determined based on the category of the moving target corresponding to the local motion consistent sub-cluster. Rigid targets are assigned a higher value, while non-rigid targets are appropriately reduced. This is the normalization coefficient.

[0059] This reused weight function has adaptive decay characteristics: hour Compensation observations are used to participate in pose estimation with an intensity close to that of static points; As the weight increases, the weight decreases smoothly; in extreme cases... The system degenerates into approximate elimination.

[0060] In the tracking thread, the participation of the quasi-static observation points in pose estimation is adaptively adjusted based on the number of static feature points in the current frame. When the number of static feature points is sufficient, the final set of static points is used first for pose estimation, and the quasi-static observation points do not participate in the pose estimation optimization function. When the number of static feature points is lower than a preset threshold, the weights are reused. As the scaling factor of the information matrix or the weight of the robust kernel function in the pose estimation optimization function, the quasi-static observation point is used to realize weak constraint access, provide supplementary constraints for the tracking thread, prevent front-end degradation due to insufficient static points, and improve the robustness of the system in extreme dynamic scenarios.

[0061] In one specific implementation of this embodiment, tests were conducted on high-dynamic sequences of the TUM RGB-D dataset. Based on the ORB-SLAM3 framework and integrating the aforementioned dynamic feature compensation and weak constraint reuse modules, the absolute trajectory error (ATE) localization accuracy and robustness were significantly improved, with its fr3 / walking... xyz Sequences and fr3 / walking halfsphere The ATE trajectory precision of the sequence is as follows: Figure 2 , Figure 3 As shown.

[0062] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions conceived without inventive effort 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 defined in the claims.

[0063] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.

Claims

1. A visual SLAM method based on dynamic feature compensation and weak constraint reuse, characterized in that, include: S1: Acquire two adjacent frames of images, extract feature points and match them to obtain matching feature point pairs; S2: Use IMU pre-integration to obtain relative pose information between two adjacent frames, determine the motion mode of the current frame based on the translational modulus in the relative pose information, and adaptively select the fundamental matrix model or homography matrix model as the initial geometric model according to the motion mode. S3: Based on the initial geometric model, calculate the geometric distance error of each matching feature point, and perform adaptive dynamic and static classification on the distance error distribution of all feature points to obtain a preliminary static point set and a dynamic candidate point set; S4: Using the initial static point set as a seed, the geometric model is re-estimated and iteratively refined using the RANSAC algorithm to eliminate the initial error of the IMU and obtain the final static point set and dynamic candidate point set. S5: Based on the dynamic candidate point set, cross-validation is performed by combining semantic prior information and pixel-level motion foreground to select feature points in the real motion target area as effective dynamic feature points, and the effective dynamic feature points are clustered by motion pattern to be divided into several local motion consistent sub-clusters. S6: For each of the local motion consistent subclusters, use depth information to decouple the three-dimensional motion from the known camera motion, separate the object's own motion component from the mixed observation, perform three-dimensional coordinate compensation on the separated feature points, and restore them to quasi-static observation points. S7: Based on the number of feature points in the final static point set in the current frame, adaptively adjust the participation of the quasi-static observation points in pose estimation; when the number of static feature points is sufficient, the final static point set is used first for pose estimation, and the quasi-static observation points do not participate in the pose estimation optimization function; when the number of static feature points is lower than a preset threshold, the quasi-static observation points are included as weak constraints in the pose estimation optimization function to provide supplementary constraints for the tracking thread.

2. The method according to claim 1, characterized in that, The initial geometric model selection process in step S2 includes: Relative rotation between two adjacent frames is obtained using IMU pre-integration. Peaceful relocation And calculate the translational modulus: ; The motion mode is determined based on the translational modulus length. It was determined that the camera exhibited significant translational movement, and the fundamental matrix model was chosen as the initial geometric model, expressed as: ;like To determine that the camera is in an approximately pure rotational state, a homography matrix model is selected as the initial geometric model, expressed as: ; in, Represents the camera intrinsic parameter matrix. This represents an antisymmetric matrix operator constructed from three-dimensional vectors. This indicates the preset translation modulus threshold. Represents the rotation matrix. This represents the translation vector.

3. The method according to claim 1, characterized in that, Step S3, which involves adaptive dynamic and static classification of the distance error distribution of all feature points to obtain a preliminary static point set and a dynamic candidate point set, specifically includes: A two-component Gaussian mixture model is used to model the distance error distribution, and the expression is: ,in, Indicates geometric distance error. , These represent the combined weights of the two components. , Let represent the mean and variance of each Gaussian component, respectively. Represents the Gaussian distribution function; The model parameters are estimated iteratively using the expectation-maximization algorithm. Based on the Bayesian minimum error rate criterion, the optimal decision boundary is solved using the model parameters to generate an adaptive classification threshold. The boundary satisfies: ; Feature points with a distance error greater than the adaptive classification threshold are marked as dynamic candidate points, and the rest are marked as preliminary static points, thus obtaining a preliminary static point set and a dynamic candidate point set.

4. The method according to claim 1, characterized in that, Step S5, which involves cross-validating semantic prior information with pixel-level moving foreground information based on the dynamic candidate point set to filter feature points within the real moving target region, specifically includes: S51: Utilize the final static point set to estimate the affine transformation matrix, eliminate the background shift caused by camera motion, and generate a compensated image aligned with the reference frame; S52: Extract a pixel-level motion foreground mask from the compensated image and obtain the bounding rectangular region of the motion foreground mask. D' ; S53: Perform target detection on the current frame, obtain semantic target detection boxes, match the semantic target detection boxes with the motion foreground mask to filter out the real moving target regions; S54: The dynamic candidate points that fall within the actual moving target area are taken as effective dynamic feature points.

5. The method according to claim 4, characterized in that, In step S52, the PBAS algorithm is used to extract the pixel-level moving foreground mask; in step S53, target detection is implemented using YOLO series neural networks; the calculation method for the overlap rate matching is as follows: Calculate each YOLO object detection box Rectangular region circumscribed by the motion foreground mask D' Overlap rate: Where S() represents the area function of the region; if Greater than the preset overlap rate threshold δ If the target is detected, the region corresponding to the semantic target detection box is determined to be a real moving target region; otherwise, it is discarded as static background.

6. The method according to claim 1, characterized in that, Step S5, which involves clustering the effective dynamic feature points into motion pattern subclusters, specifically includes: The local motion residual vector of each effective dynamic feature point is extracted using the inverse transformation of the affine transformation matrix, expressed as: ,in, Indicates the actual observed coordinates of the current frame. This indicates the coordinates of the matching point in the previous frame. A Represents the affine transformation matrix; Based on the local motion residual vector, a motion difference metric is defined, and a density clustering algorithm based on motion consistency constraints is run to divide the effective dynamic feature points into several locally motion-consistent subclusters.

7. The method according to claim 6, characterized in that, The density clustering algorithm based on motion consistency constraints divides the effective dynamic feature points into several locally motion-consistent sub-clusters, specifically including: The similarity of the motion residuals between two points is quantified using Mahalanobis distance: ,in, Representing feature points i and feature points j The difference in motion scores between them and Representing points respectively i and points j The local motion residual vector, The inverse matrix of the covariance matrix; Based on the properties of the chi-square distribution, at the first significance level Determine the initial clustering threshold At the second significance level Determine the cluster merging threshold ,in ; Perform initial clustering and cluster expansion based on the initial clustering threshold, and use the cluster merging threshold to merge spatially adjacent and similarly moving clusters, remove noise points, and obtain locally consistent sub-clusters.

8. The method according to claim 1, characterized in that, Step S6 specifically includes the following processes: S61: For the first in a locally motion-consistent subcluster i Each feature point is determined based on its pixel coordinates in the current frame. and corresponding depth values Using the camera intrinsic parameter matrix Perform a back-projection transformation to map the feature point to the current frame camera coordinate system, thus obtaining the 3D observation point of the feature point in the current frame camera coordinate system. And obtain the 3D point corresponding to the matching point of the feature point in the previous frame through ORB descriptor matching. ; S62: Rotation matrix of inter-frame camera motion provided by IMU pre-integration or initial pose estimation. Translation vector , take the previous frame's 3D points Transform to the current frame coordinate system to obtain the static desired position under pure camera motion. The expression is: ,in, Represents the rotation matrix. Represents the translation vector; S63: Constructing the objective function for motion decoupling The rigid body motion parameters of the object itself are solved by singular value decomposition in a closed form. S64: By using the inverse transformation of the object's own motion parameters, the motion components of the object are separated from the mixed observations, and the three-dimensional coordinates are calculated assuming the object is stationary. ,in, This represents the compensated three-dimensional coordinates, i.e., the quasi-static point. It represents the amount of translation of the object itself. This represents the object's own rotation; and is then reprojected onto the image plane using a camera projection function to obtain the compensated quasi-static observation point. .

9. The method according to claim 1, characterized in that, Before incorporating the quasi-static observation point as a weak constraint into the pose estimation optimization function in step S7, a joint weight calculation step is also included, specifically: Calculate the variance of the intra-cluster motion residuals for each locally motion-consistent sub-cluster. Taking the k-th dynamic sub-cluster as an example, the expression is: ,in, Indicates the first sub-cluster i The motion residual vector of each feature point This represents the average motion residual vector of all feature points within the sub-cluster. Describes the k-th independent sub-cluster. This represents the total number of feature points within the sub-cluster; The reuse weights of compensated observations are determined based on geometric consistency and semantic priors: ,in, The semantic confidence factor is determined based on the category of the moving target corresponding to the local motion-consistent subcluster. These are the normalization coefficients; reuse weights The information matrix scaling factor or robust kernel function weight of the quasi-static observation point in the pose estimation optimization function is used to achieve weak constraint access.

Images