A backend optimization method based on multi-modal feature association

Abstract

The application discloses a kind of back-end optimization methods based on multi-modal feature association, can increase the accuracy and robustness of positioning in the simultaneous localization and mapping system under the use of multi-modal feature, its thought is to build the association model of feature, and the association relationship is combined with back-end optimization, reduce system cumulative error, carry out pose optimization.The method first gives the mathematical model of the spatial geometric relationship between features, then calculates the feature residual term corresponding to different features, finally adjusts the confidence weight of the above error term in the back-end using the feature association degree and carries out pose optimization.The application adds the re-projection error containing association relationship to the back-end optimization, can improve the accuracy and robustness of SLAM system pose solution, so as to meet the more extensive application scenarios of robot.

Description

Technical Field

[0001] This invention belongs to the field of image and graphics, and specifically relates to a backend optimization method based on multimodal feature association. Background Technology

[0002] In recent years, intelligent robots have become an important research direction in academia and industry. To make robots more intelligent and autonomous, they must improve their ability to recognize unknown environments. Simultaneously, Simultaneous Positioning and Mapping (SLAM) technology is a key technology for building 3D scene maps and for machine self-localization, and has been successfully applied to products such as unmanned vehicles, drones, delivery robots, and intelligent cleaning robots.

[0003] Furthermore, researchers found that highly complex features are more stable than point features and are better at reconstructing realistic scenes, especially in low-texture, structured indoor scenes. However, as feature complexity increases, the system becomes more reliant on the accuracy of feature associations. Therefore, improving the robustness of data associations is crucial for reducing the accumulated error of visual SLAM and optimizing the overall system performance.

[0004] To address the above issues, many studies have focused on improving feature extraction methods to ensure the reliability of features extracted by the system's front end. Among commonly used features such as points, lines, and surfaces, planar features are often extracted from dense point clouds of RGB-D images or 3D LiDAR. However, how to obtain accurate planar point clouds in a binocular SLAM system remains to be studied. Considering the complexity of real-world scenes, the accuracy of features extracted by the system will inevitably have some error. Therefore, when calculating reprojection error, there is usually a lack of suitable accuracy confidence parameters between different types of features and between different individual features within the same type. Summary of the Invention

[0005] To address the aforementioned issues, this invention discloses a backend optimization method based on multimodal feature association to optimize SLAM systems using multimodal features, thereby improving the accuracy and robustness of SLAM systems.

[0006] To achieve the above objectives, the technical solution of the present invention is as follows:

[0007] A backend optimization method based on multimodal feature association is used for robust localization of SLAM systems using multimodal features, comprising the following steps:

[0008] (1) Construct a correlation model for multimodal features;

[0009] (2) Calculate the characteristic residuals of point, line, and surface features;

[0010] (3) Calculate the reprojection error of line and surface features by combining the feature association model;

[0011] (4) Use the feature residuals and reprojection errors to build a back-end optimization model for pose optimization.

[0012] In step (1), the association model of multimodal features is constructed, and the specific steps are as follows:

[0013] (1.1) Match the extracted point, line, and polygon features with landmarks in the map; after matching, the landmark features in the map include point features P = {p i}, Line feature L={l j} and planar feature G={π k};

[0014] (1.2) Associate different types of features; associate point features with line features and planar features, associate line features with planar features, and finally output the set S of the number of points associated with the line features. l {n1,n2,…,n j}, the set S of point and line features associated with planar features gp {np1,np2,…,np k}、S gl {nl1,nl2,…,nl k}

[0015] In step (2), the feature residuals of points, lines, and surfaces are calculated. The specific steps are as follows:

[0016] (2.1) Calculate the residual term e of the point characteristics p Based on the matching relationship, the observed values ​​of the point features are obtained. The error equation for a single observation of the point features is e. p Defined as:

[0017] e p (p w ,T cw )=‖z-ρ(T cw p w )‖

[0018] in p is the pixel coordinate of the observation point. w It is a point feature in the world coordinate system, T cw ρ is the camera pose, and ρ is the projection equation that projects 3D points onto the pixel plane.

[0019] (2.2) Calculate the residual term e of the line characteristic l For the error equation e of a single observation of a line feature l Defined as:

[0020] e l (lw ,T cw )=‖l vρ (T cw p w )‖

[0021] Among them l v Let l be a unit orthogonal vector representing the line characteristic. w For line characteristics in the world coordinate system, p w As the endpoint, T cw ρ is the camera pose, and ρ is the projection equation that projects 3D points onto the pixel plane.

[0022] (2.3) Calculate the residual term e of the surface feature π The error equation for a single observation of a surface feature is defined as follows:

[0023] e π (π w ,T cw )=‖q(π)-q(T cw -T π w )‖

[0024] Where q(·) represents the hyperparameter transformation, and π is the observed value of the current plane. w For planar features in the world coordinate system, T cw It refers to the camera pose.

[0025] In step (3), the reprojection error of line and surface features is calculated using the feature association model. The specific steps are as follows:

[0026] (3.1) Calculate the reprojection error E of the line feature l Based on the number of associated points S of the line feature set l Define the residual weight λ of the linear characteristic portion. lj for:

[0027]

[0028] Where 0 < λ lj <2,n j For line feature l j The number of associated feature points, n max and n min These represent the maximum and minimum values ​​of the number of feature points associated with the line feature, respectively. Combined with the residual weight λ... lj Reprojection error E l Defined as:

[0029]

[0030] in, It is a feature of the world coordinate system lines. It represents the camera pose, with the subscript ∑. jm In this context, j is the index of the line feature, m is the index of the pose matrix, and l corresponds to the summation term. j T m The subscripts are used to substitute the associated line features and feature matrices and sum them to obtain the overall cost function;

[0031] (3.2) Calculate the reprojection error E of the surface features π Based on the set S of point and line features associated with surface features gp and S gl Define the residual weight λ of the linear characteristic portion. πk for:

[0032]

[0033] Where np k and nl k They are respectively related to the planar feature π k The number of associated point features and line features, np max and np min Let nl be the maximum and minimum number of point features associated with the surface feature, respectively. max and nl min These represent the maximum and minimum values ​​of the number of line features associated with the surface features, respectively. Combined with the residual weight λ... πk Reprojection error E π Defined as:

[0034]

[0035] in It is a planar feature of the world coordinate system. It represents the camera pose, with the subscript ∑. km In this context, k is the index of the surface feature, m is the index of the pose matrix, and π corresponds to the summation term. k T m The subscripts are used to substitute the associated surface features and feature matrices and sum them to obtain the overall cost function.

[0036] In step (4), a back-end optimization model is constructed using the feature residual term and reprojection error to optimize the pose. The specific steps are as follows:

[0037] (4.1) Combining the characteristic residual term and the reprojection error, the back-end optimization equation of the system can be defined as:

[0038]

[0039] Where e p E is the residual term for point features. l E represents the reprojection error of the line feature.π The reprojection error is for surface features. Let i be the i-th point in the world coordinate system. Let ∑ be the transformation matrix from the i-th world coordinate system to the camera coordinate system. im In this context, i represents the index of the point feature, m represents the index of the pose matrix, and p corresponds to the summation term. i T m subscript;

[0040] (4.2) During tracking, the camera pose is optimized by processing keyframes and performing local BA, while new features are added to the map.

[0041] (4.3) When a loop is detected in the system (reaching a previously visited place), the local BA is stopped and the global BA is executed. All camera poses and point, line and planar landmarks are corrected according to the current observation results, the accumulated error is eliminated, and the key frame poses and global map are updated.

[0042] The beneficial effects of this invention are:

[0043] To address the issue that SLAM systems using multimodal features are highly dependent on the accuracy of feature association, this invention proposes a backend optimization method based on multimodal feature association to improve the robustness of data association in SLAM systems. This method constructs association relationships between different features based on the spatial geometric relationships between features; subsequently, it combines these association relationships with backend optimization, using confidence weights to adjust reprojection errors to improve the system's robustness and accuracy. This method can improve the robustness of multimodal feature data association, provide better localization capabilities, and can be widely applied in scenarios such as autonomous driving and indoor navigation. Attached Figure Description

[0044] Figure 1 This is a system architecture diagram of a backend optimization method based on multimodal feature association;

[0045] Figure 2 This is a flowchart of a backend optimization method based on multimodal feature association. Detailed Implementation

[0046] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0047] like Figure 1 The diagram shown is a schematic representation of the method flow of the present invention.

[0048] Step S1: Construct a correlation model for multimodal features. This specifically includes:

[0049] S1.1 Match the extracted point, line, and polygon features with landmarks in the map; after matching, the landmark features in the map include point features P = {p i}, Line feature L={l j} and planar feature G={π k};

[0050] S1.2. Associate different types of features; associate point features with line features and planar features, associate line features with planar features, and finally output the set S of the number of points associated with the line features. l The set S of point and line features associated with planar features gp S gl ;

[0051] in

[0052] S l ={n1,n2,…,n j}

[0053] S gp ={np1,np2,…,np k}

[0054] S gl ={nl1,nl2,…,nl k}

[0055] n j For line feature l j The number of associated feature points, n pk and n lk The planar features π k The number of associated point features and line features.

[0056] Step S2: Calculate the feature residuals of point, line, and surface features. Specifically, this includes:

[0057] S2.1 Calculate the residual term e of the point characteristics p The residual term of a point feature is defined as its reprojection error. The observed values ​​of the point features are obtained based on the matching relationship. The error equation for a single observation of a point feature is defined as follows:

[0058] e p (p w ,T cw )=‖z-ρ(T cw p w )‖

[0059] in p is the pixel coordinate of the observation point. w It is a point feature in the world coordinate system, T cw ρ is the camera pose, and ρ is the projection equation that projects 3D points onto the pixel plane.

[0060] S2.2 Calculate the residual term e of the line characteristic l This method uses the endpoint format to define the line feature as l = (p s T ,p e T ) T , where p s and p e Let n represent the start and end points of the line feature, respectively, and define the direction vector of the line feature as n. l Based on the characteristic endpoint p of the line s and p e Obtain the unit orthogonal vector l of the line feature v for:

[0061]

[0062] residual e of linear characteristics l Defined as the actual line characteristic endpoint p s and p e To observation line l v The distance for any endpoint p w The error equation for a single observation of a line feature can be written as:

[0063] e l (l w ,T cw )=‖l v ρ(T cw p w )‖

[0064] Among them l v p is a unit orthogonal vector representing the line characteristic. w As the endpoint, l w For line features in the world coordinate system, T cw ρ is the camera pose, and ρ is the projection equation that projects 3D points onto the pixel plane.

[0065] S2.3, Calculate the residual term e of the surface characteristics π To avoid hyperparameter problems, this method uses the plane π = (n π T ,d π ) T Represented as the observation plane q(π) with minimal parameterization:

[0066]

[0067] Where π is the observed value of the current plane, n π =(n x ,n y ,nz ) TT Let n be a unit vector, representing the normal phasor of the plane, and its cross product is the direction vector n of the intersection line. l We obtain, i.e., n π =n l1 ×n l2 , With ψ=sin -1 n z These are the azimuth and elevation angles of the normal, respectively. ψ∈(-π,π); d π Let be the distance from the origin to the plane. The error equation for a single observation of a surface feature is defined as:

[0068] e π (π w ,T cw )=‖q(π)-q(T cw -T π w )‖

[0069] Where q(·) represents the hyperparameter transformation, and π is the observed value of the current plane. w For planar features in the world coordinate system, T cw It refers to the camera pose.

[0070] Step S3: Calculate the reprojection error of line and surface features using the feature association model. Specifically, this includes:

[0071] S3.1, Calculate the reprojection error E of the line feature. l Based on the number of associated points S of the line feature set l {n1,n2,…,n j Considering that the number of associations is positively correlated with the importance of features, the residual weight λ of the linear feature portion is defined. lj for:

[0072]

[0073] Where 0 < λ lj <2,n j For line feature l j The number of associated feature points, n max and n min These represent the maximum and minimum values ​​of the number of feature points associated with the line feature, respectively. Combined with the residual weight λ... lj Reprojection error E l Defined as:

[0074]

[0075] in, It is a feature of the world coordinate system lines. It represents the camera pose, with the subscript ∑. jm In this context, j is the index of the line feature, m is the index of the pose matrix, and l corresponds to the summation term. j T m The subscripts are used to substitute the associated line features and feature matrices and sum them to obtain the overall cost function;

[0076] S3.2, Calculation of reprojection error E for surface features π Based on the set S of point and line features associated with surface features gp {np1,np2,…,np k} and S gl {nl1,nl2,…,nl k Considering that the number of associations is positively correlated with the importance of features, the residual weight λ of the linear feature portion is defined. πk for:

[0077]

[0078] Where np k and nl k They are respectively related to the planar feature π k The number of associated point features and line features, np max and np min npl represents the maximum and minimum number of point features associated with the surface feature, respectively. max and nl min These represent the maximum and minimum values ​​of the number of line features associated with the surface features, respectively. Combined with the residual weight λ... πk Reprojection error E π Defined as:

[0079]

[0080] in It is a surface feature of the world coordinate system. It represents the camera pose, with the subscript ∑. km In this context, k is the index of the surface feature, m is the index of the pose matrix, and π corresponds to the summation term. k T m The subscripts are used to substitute the associated surface features and feature matrices and sum them to obtain the overall cost function.

[0081] Step S4: Construct a backend optimization model using feature residuals and reprojection errors to optimize pose. Specifically, this includes:

[0082] S4.1. Combining the relationships between point, line, and surface features, structured features are simultaneously incorporated into the backend optimization. Thus, the backend optimization equation of the system can be defined as:

[0083]

[0084] Where e p E is the residual term for point features. l E represents the reprojection error of the line feature. π The reprojection error is for surface features. Let i be the i-th point in the world coordinate system. Let ∑ be the transformation matrix from the i-th world coordinate system to the camera coordinate system. im In this context, i represents the index of the point feature, m represents the index of the pose matrix, and p corresponds to the summation term. i T m subscript;

[0085] S4.2 During tracking, the camera pose is optimized by processing keyframes and performing local BA, while new features are added to the map.

[0086] S4.3 When a loop is detected in the system (reaching a previously visited location), the local BA is stopped and the global BA is executed. All camera poses and point, line, and planar landmarks are corrected based on the current observation results to eliminate accumulated errors and update the keyframe poses and global map.

[0087] It should be noted that the above content merely illustrates the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. A backend optimization method based on multimodal feature association for robust localization of SLAM systems using multimodal features, characterized in that: Includes the following steps: (1) Construct a correlation model for multimodal features; (2) Calculate the characteristic residuals of points, lines, and surfaces; (3) Calculate the reprojection error of line and surface features by combining the feature association model; (4) Construct a back-end optimization model using feature residuals and reprojection errors to optimize pose; The specific steps are as follows: (4.1) Combining the characteristic residual term and the reprojection error, the back-end optimization equation of the system is defined as: ； in The residual term for point features, This represents the reprojection error of the line feature. The reprojection error is for surface features. The first in the world coordinate system One point, For the first Transformation matrices from world coordinate system to camera coordinate system, subscript middle The point feature number, The numbers represent the positions of the pose matrix, corresponding to the terms in the summation. subscript; (4.2) During tracking, the camera pose is optimized by processing keyframes and performing local BA, while new features are added to the map; (4.3) When a loop is detected in the system, the local BA is stopped and the global BA is executed. All camera poses and points, lines and planar landmarks will be corrected according to the current observation results to eliminate accumulated errors and update the key frame poses and global map.

2. The backend optimization method based on multimodal feature association according to claim 1, characterized in that, In step (1), the association model of multimodal features is constructed, and the specific steps are as follows: (1.1) Match the extracted point, line, and polygon features with landmarks in the map; after matching, the landmark features in the map include point features. Line features and planar features ; (1.2) Associate different types of features; associate point features with line features and planar features, associate line features with planar features, and finally output the set of numbers of points associated with line features. The set of point and line features associated with planar features , .

3. The backend optimization method based on multimodal feature association according to claim 1, characterized in that, In step (2), the feature residuals of points, lines, and surfaces are calculated. The specific steps are as follows: (2.1) Calculate the residual term of the point characteristics Based on the matching relationship, the observed values ​​of the point features are obtained, and the error equation for a single observation of the point features is... Defined as: ； in These are the pixel coordinates of the observation point. It is a point feature in the world coordinate system. It is the camera pose. It is the projection equation that projects 3D points onto the pixel plane; (2.2) Calculate the residual term of the line characteristic Error equation for a single observation of a line feature Defined as: ； in , is a unit orthogonal vector representing the line characteristic. Line features in the world coordinate system It is a point feature in the world coordinate system. It is the camera pose. It is the projection equation that projects 3D points onto the pixel plane; (2.3) Calculate the residual term of the surface feature The error equation for a single observation of a surface feature is defined as follows: ； in This represents the hyperparameter transformation. For the current observation values ​​on the plane, Planar features in the world coordinate system It refers to the camera pose.

4. The backend optimization method based on multimodal feature association according to claim 1, characterized in that, In step (3), the reprojection error of line and surface features is calculated using the feature association model. The specific steps are as follows: (3.1) Calculate the reprojection error of the line feature ; Based on the number of associated points of the line feature set Define the residual weights of the linear characteristic portion. for: ； in , For line features The number of associated feature points and These represent the maximum and minimum values ​​of the number of feature points associated with the line feature, respectively; combined with residual weights. Reprojection error Defined as: ； in, It is a feature of the world coordinate system lines. It is the camera pose, subscript middle It is the number of the line feature. These are the numbers in the pose matrix, corresponding to the terms in the summation. The subscripts are used to substitute the associated line features and feature matrices and sum them to obtain the overall cost function; (3.2) Calculate the reprojection error of surface features The set of point and line features associated with surface features. and Define the residual weights of the linear characteristic portion. for: ； in and respectively with planar features The number of associated point features and line features. and These represent the maximum and minimum values ​​of the number of point features associated with the surface feature, respectively. and These represent the maximum and minimum values ​​of the number of line features associated with the surface features; combined with residual weights. Reprojection error Defined as: ； in It is a planar feature of the world coordinate system. It is the camera pose, subscript middle It is the number of the surface feature. These are the numbers in the pose matrix, corresponding to the terms in the summation. The subscripts are used to substitute the associated surface features and feature matrices and sum them to obtain the overall cost function.

Images