A data-adaptive iterative reweighted robust kernel based visual slam backend optimization method

Abstract

The application belongs to the field of robots and computer vision, and particularly relates to a visual SLAM backend optimization technology based on data self-adaption and iterative reweighted robust kernel, which comprises the following steps: S1: a front-end data reading and processing module: a robot carries a camera to collect images, and performs feature extraction and feature matching on the images to generate initial camera poses and map points; S2: a backend optimization module: according to the initial camera poses and map points generated by the front end, a median absolute deviation in a self-adaption strategy is calculated, a robustness parameter is calculated, and then is brought into a target function, the target function is simplified by using an iterative reweighting strategy, and finally, the optimal camera poses and map points are solved by using an L-M algorithm; S3: a loop detection module: whether the robot reaches a previous position is detected to detect a loop, and information is transmitted to the backend for processing; and S4: a map construction module: an environmental map is constructed according to the optimized camera poses and map points.

Description

Technical Field

[0001] This invention relates to the fields of robotics and computer vision, specifically to a visual SLAM backend optimization technique based on data-adaptive iterative reweighted robust kernel, used to improve the accuracy and robustness of VSLAM systems in complex scenes. Background Technology

[0002] Visual Simultaneous Localization and Mapping (VSLAM) is a core technology for robot navigation and environmental perception, its development benefiting from its widespread application in mobile robots, autonomous vehicles, and virtual / augmented reality (VR / AR). VSLAM uses image data acquired by cameras to estimate camera pose in real time and construct 3D environmental maps, providing crucial support for the perception and decision-making of autonomous mobile devices in complex environments. With the rapid development of artificial intelligence and machine learning technologies, VSLAM research and applications have made significant progress, bringing unprecedented possibilities to robotics and intelligent systems. However, despite numerous technological breakthroughs, the complexity of the real world still poses serious challenges to VSLAM. Dynamic changes, noise interference, missing data, and the presence of outliers in the real environment significantly affect the performance of VSLAM systems. Especially in dynamic scenes, such as densely populated city streets, rapidly changing indoor environments with varying lighting conditions, or areas with sparse texture features, VSLAM systems often struggle to maintain accurate environmental mapping and efficient pose estimation. These problems not only affect the system's accuracy and robustness but also limit its widespread deployment in complex and changing scenarios.

[0003] To improve the mapping accuracy of Visual Simultaneous Localization and Mapping (VSLAM) systems, backend optimization is an indispensable step. Backend optimization processes the initial pose and map information obtained from the frontend, reducing error accumulation and ensuring the global consistency of the system output. In the early stages of VSLAM technology development, backend optimization typically employed filtering-based methods, such as Extended Kalman Filter (EKF) and Particle Filter. These methods performed well in low-dimensional and small-scale environments, but their application gradually limited due to their sensitivity to outliers and noise, and the difficulty in maintaining global consistency. Especially in large-scale scenes or complex environments, these filters often fail to meet practical requirements.

[0004] In recent years, nonlinear optimization methods have gradually become a core technology in advanced VSLAM frameworks, with representative systems including ORB-SLAM2 and ORB-SLAM3. These frameworks effectively improve the robustness and accuracy of the system by modeling the localization and mapping problem as a nonlinear minimization problem, using reprojection error as the cost function objective, and combining optimization algorithms such as Levenberg-Marquardt (LM) and Gauss-Newton. This graph-based optimization approach can not only handle large-scale data but also achieve global consistency through loop detection, making it the mainstream solution for current VSLAM systems.

[0005] To further enhance the adaptability and robustness of the system in different scenarios, advanced VSLAM frameworks, including ORB-SLAM3 and CCM-SLAM, have introduced robust kernel functions in the backend optimization. Robust kernel functions effectively improve system performance in dynamic or complex environments by suppressing the influence of outliers on the optimization results. Common kernel functions such as the Huber kernel, Cauchy kernel, and Tukey kernel achieve optimization through fixed robust parameters. While this fixed-parameter design performs well in some scenarios, it often struggles to cope with different noise patterns and outlier distributions in diverse real-world scenarios.

[0006] Especially with increasing scene complexity and dynamic changes, selecting a suitable robust kernel function and its parameters becomes a significant challenge. On the one hand, the types and applicability of existing robust kernel functions are limited, making it difficult to cover all possible application scenarios; on the other hand, the multi-parameter coupling problem significantly increases the complexity and difficulty of parameter tuning, thus limiting the practical application of robust kernel functions. For example, when dealing with non-Gaussian noise or complex dynamic scenes, a robust kernel with fixed parameters may not achieve a balance between accuracy and robustness. Therefore, finding a method to adaptively adjust the parameters of a robust kernel function according to different scenarios is of great significance to existing backend optimization research. Summary of the Invention

[0007] To address the aforementioned problems, this invention provides a visual SLAM backend optimization method based on a data-adaptive iteratively reweighted robust kernel-based approach (DA-IRRK) for back-end optimization in visual SLAM.

[0008] This invention adopts the following technical solution: a visual SLAM backend optimization method based on data adaptive iterative reweighted robust kernel, comprising:

[0009] S1: Front-end data reading and processing module: Acquires images through the camera mounted on the robot, performs feature extraction and feature matching on the images, and generates initial camera pose and map points;

[0010] S2: Backend Optimization Module: After this module is enabled, to reduce computing resources and achieve real-time operation of the robot system, it selects several representative frames from the acquired images as keyframes for optimization. Simultaneously, during SLAM operation, if no loop closure is detected, the keyframes of the local map (local keyframes) are optimized (i.e., local optimization). After a loop closure is detected, the keyframes of the entire map (global keyframes) are optimized (i.e., global optimization). This module first calculates the median absolute deviation (MAD) in the adaptive strategy based on the initial camera pose and map points generated by the front-end data reading and processing module. The robustness parameter obtained through the MAD is then fed back into the constructed objective function. An iterative reweighting strategy is used to simplify the objective function, and finally, the Levenberg-Marquardt (LM) algorithm is used to solve for the optimal camera pose and map points. The keyframes refer to images with clear pixels after removing duplicates.

[0011] S3: Loop Detection Module: Detects loops based on whether the robot has reached the starting position. If a loop is detected, the information will be passed to the backend optimization module for processing.

[0012] S4: Map Building Module: After optimization, the required environment map is built based on the optimal camera pose and map points.

[0013] The front-end data reading and processing module described in step S1 includes the following steps:

[0014] S11: Acquire image data from the environment, obtaining input through a camera (such as a monocular, binocular, or RGB-D camera);

[0015] S12: Perform feature extraction and filtering on the input image, extracting key points and descriptors to reduce noise interference;

[0016] S13: Use geometric constraints to perform feature matching, eliminate incorrect matches, and generate initial camera pose and map points.

[0017] The backend optimization module in step S2 includes the following steps:

[0018] S21: Calculate the median absolute deviation (MAD) of the adaptive strategy based on the initial camera pose and map points generated by the front-end data reading and processing module, and then calculate the robustness parameter in the robust kernel function through the median absolute deviation to achieve adaptive adjustment of the robustness parameter;

[0019] S22: Construct the objective function, bring the robustness parameters back into the objective function, and simplify it through an iterative reweighting strategy;

[0020] S23: Optimize the pose and map points of the current frame using the Levenberg-Marquardt (LM) algorithm until the reprojection error converges or reaches a preset threshold.

[0021] Step S21 includes:

[0022] S211: Calculate the current reprojection error based on the initial camera pose and map points generated by the front end.

[0023] e ij (χ)=z ij -f ij (χ)

[0024] The above formula represents the χ of a 3D map point in a real environment. mj The observation z in the i-th frame of the two-dimensional image from the camera ij The projection value f calculated on the corresponding two-dimensional image ij The difference between (·), where χ mj This refers to the *i*-th 3D map point in the real environment projected onto the *m*-th frame of the 2D image, where *m* ranges from 1 to *i*; the observed value *z* ij Represented in pixel coordinates, corresponding to χ mj The projection in the i-th frame; where i ranges from 1 to N, N is the number of image frames captured by the camera; j ranges from 1 to M, where M is the number of map points visible from the position in the i-th frame;

[0025] S212: Calculate the median absolute deviation (MAD) and the corresponding robustness parameter τ based on the reprojection error obtained from the current initial camera pose and map points.

[0026]

[0027] In the formula, σ MAD (e ij (χ)) represents the adaptive threshold, and median(x) represents the median of the dataset consisting of reprojection errors. v Let represent the v-th reprojection error value in the set, MAD(x) be the median of the absolute values ​​of the deviations between the reprojection error data and their medians, and C represent the scaling factor. It is σ MAD (e ij The quartile coefficients of τ are given by T, which is the vector form of τ.

[0028] Step S22 includes:

[0029] Taking the Huber kernel function as an example, we first know the form of the Huber kernel function and its first derivative function as follows:

[0030]

[0031] Where τ represents the robustness parameter and e represents the error.

[0032] When using the Huber kernel function to solve the visual SLAM backend optimization problem, the objective function is constructed as follows:

[0033]

[0034] The derivative of the objective function is as follows:

[0035]

[0036] in, Defined as l(e) ij The first derivative of (χ)), l′(e ij (χ) represents the influence function. This represents the weighting function.

[0037] Step S23 includes:

[0038] In the Levenberg-Marquardt algorithm, the projection equation f ij (χ) First, perform a Taylor expansion:

[0039]

[0040] Where Δχ represents the increment of camera pose and map points, and J is... Matrix form;

[0041] Then, substituting these into the reprojection error formula and the derivative formula of the objective function:

[0042]

[0043] Finally, the incremental matrix form of camera pose and map points is obtained, and the optimal solution for camera pose and map points is obtained through iterative updates:

[0044] Below is the matrix form of the increment Δχ in the LM algorithm:

[0045] Δχ=(J T WJ+λ·diag(J T WJ)) -1 J T WΔE

[0046] Where W is the weight function ωij (Δχ) in matrix form, where λ is the Lagrange multiplier and ΔE is Δe. ij In matrix form.

[0047] Step S3, the loop closure detection, includes the following steps:

[0048] S31: While performing S13, loop closure detection is performed to determine whether the current frame forms a loop with the image frame acquired at the previous starting position.

[0049] S32: If a loop closure is detected, all camera poses and map points are input into the S2 backend optimization module for optimization to reduce drift error.

[0050] Compared with the prior art, the present invention has the following beneficial effects:

[0051] 1. This invention proposes a Data Adaptive Iterative Reweighted Robust Kernel (DA-IRRK) method for backend optimization in VSLAM. In this method, the kernel function is used as the objective function of the backend optimization problem. The robust parameters in the kernel function are adaptively updated based on the reprojection error reflected by the median absolute deviation (MAD). The proposed method exhibits high robustness in different scenarios. Furthermore, this method solves the constructed backend optimization problem through an iterative reweighted update process.

[0052] 2. The method proposed in this invention has been implemented in various VSLAM frameworks, including ORB-SLAM3, JORB-SLAM, and CCM-SLAM, to demonstrate its effectiveness in visual-only SLAM, multi-sensor fusion SLAM, and collaborative SLAM. The method has been tested on indoor and outdoor datasets and compared with other robust kernel methods and the advanced multi-convex combined maximum correlation entropy criterion (MCMCC) method.

[0053] 3. This invention analyzes the performance differences between this method and other methods from the perspective of reprojection error statistics, providing new insights into the application of adaptability in VSLAM backend optimization problems. Attached Figure Description

[0054] Figure 1 This is a flowchart of the present invention;

[0055] Figure 2 This is a schematic diagram of the reprojection process of the present invention;

[0056] Figure 3 This is a schematic diagram showing the relationship between the reprojection error and the robust parameter τ in the DA-IRRK method of the present invention in EuRoC_V101 and EuRoC_V203.

[0057] Figure 4 This is a schematic diagram showing the relationship between the reprojection error of the present invention and the robust parameter τ in the Huber method in EuRoC_V101 and EuRoC_V203.

[0058] Figure 5 This is a schematic diagram showing the relationship between the reprojection error and the robust parameter τ in the DA-IRRK method of the present invention in KITTI_01 and KITTI_09;

[0059] Figure 6 This is a schematic diagram showing the relationship between the reprojection error of the present invention and the robust parameter τ in the Huber method in KITTI_01 and KITTI_09;

[0060] Figure 7 This is a schematic diagram of the reprojection error distribution of global keyframes in EuRoC of the present invention;

[0061] Figure 8 This is a schematic diagram of the reprojection error distribution of local keyframes at three different time points in the EuRoC of this invention;

[0062] Figure 9 This is a schematic diagram of the reprojection error distribution of global keyframes in KITTI of the present invention;

[0063] Figure 10 This is a schematic diagram of the reprojection error distribution of local keyframes at three different time points in the KITTI of this invention;

[0064] Figure 11 This is a schematic diagram showing the relationship between the reprojection error and the robust parameter τ in the DA-IRRK method of the present invention in EuRoC_V201 and EuRoC_V202.

[0065] Figure 12 This is a schematic diagram showing the relationship between the reprojection error of the present invention and the robust parameter τ in the Huber method in EuRoC_V201 and EuRoC_V202;

[0066] Figure 13 This is a schematic diagram showing the relationship between the reprojection error of the present invention and the robust parameter τ in the DA-IRRK method in KITTI_06 and KITTI_07.

[0067] Figure 14 This is a schematic diagram showing the relationship between the reprojection error of the present invention and the robust parameter τ in the Huber method in KITTI_06 and KITTI_07.

[0068] Figure 15This is a schematic diagram of the local mapping reprojection error distribution of sequences 06 and 07 in JORB-SLAM of the present invention;

[0069] Figure 16 A framework diagram of the optimization method described in this invention. Detailed Implementation

[0070] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0071] like Figure 1 The diagram shows a general SLAM framework. This invention provides a visual SLAM backend optimization method based on a data-adaptive iterative reweighted robust kernel. First, images are acquired through the first two modules. Then, the images are preprocessed in the front-end module. The initial camera pose and map points are then passed to the back-end module. Finally, the updated camera pose and map points are passed to the mapping module to obtain the required map. This invention mainly involves (1) calculating the median absolute deviation (MAD) of the adaptive strategy based on the initial camera pose and map points generated by the front-end data reading and processing module, and calculating the robustness parameter in the robust kernel function through the median absolute deviation to achieve adaptive adjustment of the robustness parameter; (2) constructing a target function, bringing the robustness parameter back into the target function, and simplifying it through an iterative reweighting strategy; (3) optimizing the pose and map points of the current frame through the Levenberg-Marquardt algorithm until the incremental convergence of the camera pose and map points or the preset threshold is reached. The specific steps are as follows:

[0072] A visual SLAM backend optimization method based on data-adaptive iterative reweighted robust kernel includes:

[0073] S1: Front-end data reading and processing module: Acquires images through the camera mounted on the robot, performs feature extraction and feature matching on the images, and generates initial camera pose and map points;

[0074] S11: Real-time acquisition of environmental image data via a camera (such as a monocular, binocular, or RGB-D camera). The input image can include continuous frame data from a static or dynamic environment.

[0075] S12: Perform feature extraction and filtering on the input image, extracting key points and descriptors to reduce noise interference;

[0076] Step S12 includes:

[0077] S121: Feature detection: Detecting salient key points in an image using algorithms (such as ORB or SIFT);

[0078] S122: Descriptor calculation: Generates the corresponding descriptor based on the feature point for feature matching;

[0079] S123: Filtering and denoising: Eliminate low-quality feature points, reduce the impact of noise on matching, and enhance adaptability to dynamic scenes.

[0080] S13: Use geometric constraints (such as the fundamental matrix or homography matrix) to perform feature matching, eliminate incorrect matches, and generate initial camera pose and map points.

[0081] Step S13 includes:

[0082] S131: Matching point pair generation: Finding potentially corresponding feature point pairs between images through descriptor matching;

[0083] S132: Mismatch Removal: Combining the RANSAC algorithm, incorrect matching point pairs are removed, while high-confidence feature point pairs are retained;

[0084] S133: Preliminary pose estimation: Calculate the camera pose transformation of the current frame relative to the reference frame based on high-quality matched point pairs, and generate preliminary sparse map points by combining disparity information.

[0085] S2: Backend optimization module: Based on the reprojection error distribution (environmental characteristics), calculate the median absolute deviation (MAD) and construct a function that minimizes the reprojection error cost. The Huber kernel function is used as an example for explanation below.

[0086] S21: Calculate the current reprojection error based on the initial camera pose and map points obtained in S1, such as... Figure 2 The diagram shown is a schematic diagram of the reprojection error principle.

[0087] e ij (χ)=z ij -f ij (χ)

[0088] The above formula represents the χ of a 3D map point in a real environment. mj The observation z in the i-th frame of the two-dimensional image from the camera ij The projection value f calculated on the corresponding two-dimensional image ij The difference between (·), where χ mj This refers to the *i*-th 3D map point in the real environment projected onto the *m*-th frame of the 2D image, where *m* ranges from 1 to *i*; the observed value *z* ij Represented in pixel coordinates, corresponding to χ mjThe projection in the i-th frame; where i ranges from 1 to N, N is the number of image frames captured by the camera; j ranges from 1 to M, M is the number of map points visible from the position of the i-th frame.

[0089] The robustness parameter τ is calculated using the Median Absolute Deviation (MAD) strategy, such as... Figure 3 5, 11, and 13 are the robustness parameter τ values ​​obtained by the adaptive algorithm proposed in this invention on different datasets, respectively. Figure 4 6, 12, and 14 are the robustness parameter τ values ​​obtained by the Huber kernel function on different datasets, respectively.

[0090]

[0091] In the formula, σ MAD (e ij (χ)) represents the adaptive threshold, and median(x) represents the median of the dataset consisting of reprojection errors. v Let represent the v-th reprojection error value in the set. MAD(x) is the median of the absolute values ​​of the deviations between the reprojection error data and their medians. C represents the scaling factor, with an efficiency of 95% according to the normal model. Typically, C is set to 1.345. It is σ MAD (e ij The quartile coefficients of τ are given by T, which is the vector form of τ.

[0092] S22: Solve the objective function iteratively through a reweighted update process;

[0093] Taking the Huber kernel function as an example, we first know the form of the Huber kernel function and its first derivative function as follows:

[0094]

[0095] Where τ represents the robustness parameter and e represents the error.

[0096] When using the Huber kernel function to solve the visual SLAM backend optimization problem, the objective function is constructed as follows:

[0097]

[0098] In general SLAM algorithms, to optimize reprojection error, the objective function is usually minimized, i.e., the reprojection error is minimized. This requires differentiating the objective function, which takes the following form:

[0099]

[0100] in, Defined as l(e) ij The first derivative of (χ)), l′(e ij (χ) represents the influence function. This represents the weighting function.

[0101] After simplifying the objective function using the aforementioned iterative reweighting strategy, the objective function can be further optimized using the Levenberg-Marquardt (LM) algorithm.

[0102] S23: Optimize the pose and map points of the current frame using the LM algorithm until the reprojection error converges or reaches a preset threshold.

[0103] In the LM algorithm, the projection equation f ij (χ) First, perform a Taylor expansion.

[0104]

[0105] Where Δχ represents the updated camera pose and map points, and J is... Matrix form;

[0106] Then substitute these values ​​into the reprojection error formula and the derivative formula of the objective function.

[0107]

[0108] Finally, the incremental matrix form of camera pose and map points is obtained, and the optimal solution for camera pose and map points is obtained through iterative updates:

[0109] Below is the increment in the LM algorithm. Δ Matrix form of χ:

[0110] Δχ=(J T WJ+λ·diag(J T WJ)) -1 J T WΔE

[0111] Where W is the weight function ω ij (Δχ) in matrix form, where λ is the Lagrange multiplier and ΔE is Δe. ij In matrix form.

[0112] S3: Loop Detection Module: Detects loops based on whether the robot has reached the starting position. If a loop is detected, the information will be passed to the backend optimization module for processing.

[0113] S31: While performing feature matching in S13, the loop closure detection module is activated to determine in real time whether the current frame forms a loop with the image frame acquired at the starting position. By comparing the image features or pose similarity between the current frame and the image frame acquired at the starting position, a loop closure is considered to be established when the similarity exceeds a preset threshold.

[0114] S32: Once a loop closure is detected, the camera pose information and map point information are transmitted to the S2 backend optimization module. The optimization method is then used to optimize the pose and map points of all image frames, reducing trajectory drift errors and ensuring consistency between pose and map points. During the optimization process, the pose is adjusted based on an objective function constructed using an adaptive robust kernel function, thereby enhancing the system's robustness.

[0115] S4: Map Building Module: After optimization, the required environment map is built based on the optimal camera pose and map points.

[0116] This invention uses the evaluation tool EVO to evaluate the obtained trajectory, and the evaluation index is the root mean square error (RMSE) corresponding to the absolute pose error (APE). The proposed algorithm is compared with other robust function-based methods (Huber, MCMCC, Cauchy, Tukey) using the EuRoC and KITTI datasets. In addition, to verify the generality of the proposed method, it is validated in different SLAM frameworks (ORB-SLAM3, JORB-SLAM, CCM-SLAM) and multi-sensor combinations (vision + inertial navigation).

[0117] The specific experimental procedure includes the following steps:

[0118] (1) The proposed MAD adaptive strategy is implanted into the Huber kernel function to update the robustness parameters in real time, and then the objective function is solved by using the iterative reweighting strategy.

[0119] (2) The proposed method was tested on the visual SLAM framework ORB-SLAM3 using the EuRoC dataset and the KITTI dataset. The experimental results are as follows:

[0120] Table 1 shows the experimental results on the EuRoC dataset.

[0121]

[0122] Table 2 shows the experimental results on the KITTI dataset.

[0123]

[0124] Table 3 shows the experimental results of multi-sensor combination (vision + inertial navigation) on the EuRoC dataset.

[0125]

[0126] Table 4. Experimental results in JORB-SLAM

[0127]

[0128] Table 5 Experimental results in CCM-SLAM

[0129]

[0130] By comparing experimental results, the proposed method outperforms other algorithms in most cases, demonstrating good robustness, whether in indoor and outdoor scenes with a pure visual framework, or in various modes such as multi-sensor combinations and different visual SLAM frameworks. During optimization, to reduce computational resources and achieve real-time operation, keyframes are selected as optimization targets after image acquisition. Simultaneously, during SLAM operation, if no loop closure is detected, keyframes of the local map (local keyframes) are optimized (local optimization); if a loop closure is detected, keyframes of the entire map (global keyframes) are optimized (global optimization). Therefore, this invention analyzes the reprojection error distribution of global and local keyframes on the KITTI and EuRoC datasets to gain a deeper understanding of the root causes of different experimental results for various algorithms, such as... Figure 7 This is the reprojection error distribution of global keyframes in the EuRoC dataset, such as... Figure 8 This is the reprojection error distribution of local keyframes in the EuRoC dataset, such as... Figure 9 It is the reprojection error of global keyframes in the KITTI dataset, such as Figure 10 It is the reprojection error distribution of local keyframes in the KITTI dataset.

Claims

1. A data-adaptive based iterative re-weighted robust kernel visual SLAM backend optimization method, characterized in that, Comprise: S1: front-end data reading and processing module: image is collected through the camera carried by the robot, and feature extraction, feature matching, initial camera pose and map points are generated; S2: back-end optimization module: after collecting images, several representative frames in the image frame are selected as key frames as optimization objects, at the same time, if no loop is detected during the running of SLAM, the key frames of the local map, that is, the local key frames, are optimized, and after detecting the loop, the key frames of the whole map, that is, the global key frames, are optimized; the module first calculates the median absolute deviation MAD in the adaptive strategy according to the initial camera pose and map points generated by the front-end data reading and processing module, then brings the robustness parameter obtained by the median absolute deviation MAD back to the constructed target function, simplifies the target function by using the iterative reweighting strategy, and finally solves the optimal camera pose and map points by using the Levenberg-Marquardt algorithm; S3: loop detection module: according to whether the robot reaches the previous position, the loop is detected, and if the loop is detected, the information is transmitted to the back-end optimization module for processing; S4: map construction module: after optimization, the required environment map is constructed according to the optimal camera pose and map points.

2. The method of claim 1, wherein, The step S1 comprises: S11: collecting image data in the environment, acquiring input through the camera; S12: feature extraction and filtering processing are performed on the input image, key points and descriptors are extracted to reduce noise interference; S13: feature matching is performed by using geometric constraints, false matching is eliminated, and initial camera pose and map points are generated.

3. The method of claim 1, wherein, The step S2 comprises: S21: the median absolute deviation MAD of the adaptive strategy is calculated according to the initial camera pose and map points generated by the front-end data reading and processing module, then the robustness parameter in the robust kernel function is calculated through the median absolute deviation, and adaptive adjustment of the robustness parameter is realized; S22: the target function is constructed, the robustness parameter is brought back to the target function, and simplification is performed through the iterative reweighting strategy; S23: the pose and map points of the current frame are optimized through the Levenberg-Marquardt algorithm until the increments of the camera pose and map points converge or reach the preset threshold.

4. The method of claim 3, wherein, The step S21 comprises: S211: the current re-projection error is calculated according to the initial camera pose and map points generated by the front end: e ij (x) = z ij -f ij (x) The above formula represents the difference between the observed value z mj on the i-th frame of the two-dimensional image of the camera and the corresponding projection value f ij calculated on the two-dimensional image, where χ ij is the j-th three-dimensional map point in the real environment projected onto the m-th frame of the two-dimensional image, and m ranges from 1 to i; the observed value z mj is expressed in pixel coordinates, and f ij is the projection of χ mj in the i-th frame; where i ranges from 1 to N, N is the number of image frames captured by the camera; j ranges from 1 to M, M is the number of map points visible from the i-th frame. S212: the value of the median absolute deviation MAD and the corresponding robustness parameter tau are calculated according to the re-projection error obtained by the initial camera pose and map points; MAD(x) = median(|x v -median(x)|) T = cσ MAD (e ij (χ)) where σ MAD (e ij (χ)) represents an adaptive threshold, median(x) represents the median of the data set consisting of the re-projection errors, x v vth re-projection error value in the set, MAD(x) refers to the median of the absolute values of the deviations between the re-projection error data and their median, c represents a scaling factor, is the quartile coefficient of σ MAD (e ij (χ)), and T is the vector form of τ.

5. The method of claim 4, wherein, The step S22 comprises: Taking Huber kernel function as an example, the Huber kernel function and its first derivative function are as follows: Wherein, tau represents the robustness parameter, e represents the error, When Huber kernel function is used to solve the back-end optimization problem of visual SLAM, the constructed target function is as follows: The derivative of the target function is as follows: wherein is defined as the first derivative of l(e ij (χ)) with respect to e ij (χ)) represents an influence function, represents a weight function.

6. The method of claim 5, wherein, The step S23 comprises: In the Levenberg-Marquardt algorithm, the projection equation f ij (x) First, a Taylor expansion is made: where Δχ represents the delta camera pose and map point, J is in matrix form; Then substitute the re-projection error formula and the derivative formula of the target function into the formula: e ij (x) = z ij -f ij (x) = z ij -f ij (x k )+f ij (x k )-f ij (x) ≈Δe ij -ΣJ·Δχ Finally, the incremental matrix form of camera pose and map points is obtained, and the optimal solution of camera pose and map points is obtained by iterative updating: The following is the matrix form of the increment Δχ in the L-M algorithm: Δχ = (J T WJ+λ·diag(J T WJ)) -1 J T WΔE where W is the weight function ω ij the matrix form of (Δχ), λ is the Lagrange multiplier, ΔE is Δe ij the matrix form of.

7. The method of claim 2, wherein, The step S3 includes: S31: Simultaneously with S13, loop detection is performed to determine whether the current image frame constitutes a loop with the image frame collected at the starting position; S32: If a loop is detected, the camera pose and map points of the global key frame are input to the S2 back-end optimization module for optimization to reduce drift error.

Images