CN107680133A - A kind of mobile robot visual SLAM methods based on improvement closed loop detection algorithm - Google Patents

Abstract

The present invention claims a mobile robot visual SLAM method based on an improved closed-loop detection algorithm, including steps: S1, using Zhang Dingyou's calibration method to calibrate Kinect; S2, performing ORB feature extraction on the acquired RGB image, and using FLANN for feature matching ; S3, delete the wrong match; obtain the spatial coordinates of the matching point, and then use the PnP algorithm to estimate the inter-frame pose transformation (R, t); S4, perform unstructured iterative optimization on the pose transformation obtained by PnP. S5, the image frame is preprocessed, the image is described by the bag of visual words, and an improved similarity score matching method is used for image matching to obtain candidate closed loops; the correct closed loop is screened out. S6, using the graph optimization method with cluster adjustment as the core to optimize the pose and landmarks, and obtain more accurate camera poses and landmarks through continuous iterative optimization. The present invention can obtain more accurate pose estimation and better three-dimensional reconstruction effect in indoor environment.

Description

A Visual SLAM Method for Mobile Robots Based on Improved Closed-Loop Detection Algorithm

technical field

The invention belongs to the field of mobile robot navigation, in particular to a vision-based synchronous positioning and map construction method.

Background technique

Simultaneous Location and Mapping (SLAM for short) is to use the sensors carried by the robot to obtain the information of the environment in which it is located to estimate the pose of the robot and build an environmental map. It is the key technology for the robot to move completely autonomously. At the same time, visual SALM is still the key to the current VR technology, and has broad application prospects in the future virtual reality industry.

Vision sensors can obtain rich image information similar to the information perceived by the human eye. These rich environmental information provide more details for the construction of environmental maps, which can achieve better map construction effects. At present, great progress has been made in visual SLAM. In 2007, Klein and others proposed PTAM, which realized the parallelization of tracking and mapping, and was the first solution to use nonlinear optimization as the backend; Endres proposed RGBD-SLAM based on depth cameras in 2014, which integrated In 2015, Raul Mur-Artal, etc., based on PTAM architecture, added the functions of map initialization and closed-loop detection, and optimized the selection of key frames. Good results have been achieved in processing speed and map accuracy.

Although in the past few years, visual SLAM has achieved good processing speed and map accuracy, but due to its strict requirements on processing speed and map accuracy, there is still a lot of room for improvement. The cumulative error caused by the long-term operation of the mobile robot will cause the inconsistency between positioning and map construction, and the introduction of closed-loop detection can reduce the cumulative error. However, the traditional closed-loop detection algorithm is not efficient and has low accuracy in detecting closed-loop. To this end, an improved closed-loop detection algorithm is introduced to improve the accuracy and efficiency of closed-loop detection. At the same time, in order to reduce the system error, a nonlinear optimization method is used to locally optimize the front-end estimated pose, and then globally optimize the closed-loop information and the front-end estimated information. Therefore, the proposed mobile robot visual synchronization positioning and map construction method based on the improved closed-loop detection algorithm can meet the real-time requirements, and can obtain more accurate pose estimation and better 3D reconstruction.

Contents of the invention

The present invention aims to solve the above problems of the prior art. A mobile robot visual SLAM method based on an improved loop-closed detection algorithm for more accurate pose estimation is proposed. The technical scheme of the present invention is as follows: a kind of mobile robot visual SLAM method based on improved closed-loop detection algorithm, it comprises the following steps:

S1. Use Zhang Dingyou's calibration method to calibrate Kinect to obtain the camera's internal parameters;

S2, carry out ORB feature extraction to the acquired RGB image, and adopt FLANN (fast approximate nearest neighbor) algorithm to carry out feature matching, including correct matching result and wrong matching result;

S3. For the mismatching result of step S2, use RANSAC (random sampling consistency) to delete the mismatching; obtain the spatial coordinates of the matching point through the Kinect camera model, and then use the PnP (n-point perspective problem) algorithm to estimate the inter-frame pose transformation ( R,t);

S4. Perform unstructured iterative optimization on the pose transformation calculated by the PnP algorithm in step S3;

S5. By preprocessing the image frame obtained in step S2, the image is described through the bag of visual words, and an improved similarity score matching method is used for image matching, and the matching obtained is more in line with the actual similarity between images, thereby Obtain candidate closed loops, and at the same time, use time continuity and spatial consistency to filter out the correct closed loops;

S6. Use the graph optimization method with bundle adjustment BA as the core to optimize the pose and landmarks, and obtain more accurate camera poses and landmarks through continuous iterative optimization.

Further, the ORB feature extraction step in step S2 is:

First, FAST corner point extraction is performed on the RGB image, and the ORB feature points are invariant to scale and rotation by using the image pyramid and gray-scale centroid method;

Again, the binary description of the surrounding image of the feature points extracted in the previous step is performed through BRIEF.

Further, in the step S3, the spatial coordinates of the matching point are obtained through the Kinect camera model, and the method for solving the spatial coordinates is:

z _o = z

If Kinect obtains the depth value z corresponding to the pixel coordinates (u, v) in the image, then the space coordinates (x _o , y _o , z _o ) can be obtained according to the above Kinect camera model.

Further, the step of optimizing the pose transformation obtained by PnP in step S4 is:

First, with the camera pose as the optimization variable, an optimization problem is constructed by minimizing the reprojection error;

Then, use the PnP value as the initial value in the original PnP function, and then call g2o for optimization.

Further, the step of detecting the candidate closed loop in step S5 is:

First, preprocess the acquired image, and use the following similarity calculation function to obtain the image similarity score. When the score is greater than the set threshold, the current frame is discarded; if not, the current frame represents a new scene and is used to participate in the closed loop detection;

Among them, V _l represents the weighted vector of the previous frame image, V _c represents the weighted vector of the current frame, and the H value represents the image similarity.

Next, use the hierarchical K-means clustering algorithm to describe the image; then use the following improved single-node scoring function to match the image similarity score;

Using the method of top-down similarity increment can avoid repeating cumulative similarity, and define the l-layer similarity score increment as:

Define the matching kernel as:

Among them, η _l represents the matching strength coefficient of the lth layer.

Further, the function of the improved similarity score is defined as:

Based on the new single-node similarity score function proposed by the above formula, the similarity score of the first layer is:

in, Indicates the score of the image X on the i-th node of the l-th layer of the tree, Indicates the score of image Y on the i-th node of the l-th layer of the tree.

Further, using the graph optimization method with bundle adjustment BA as the core to optimize the pose and landmarks specifically includes: First, the overall cost function of constructing the pose and landmarks, that is, the objective function is:

e=z-h(ξ,p)

Among them, ξ is the Lie algebra corresponding to the camera pose, that is, the extrinsic parameters (R, t), p is the three-dimensional coordinate point of the landmark, z is the pixel coordinate data observed by Kinect, e is the observation error, and z _ij represents the position ξ _i The pixel coordinate data generated by observing the landmark p _j at the place;

Then solve this least squares method, defining the independent variables as all variables to be optimized:

x=[ξ ₁ ,...,ξ _m ,p ₁ ,...,p _n ]

According to the idea of nonlinear optimization, the optimal solution of the cost function is found by constantly looking for the descending direction △x; when an increment is given to the independent variable, the objective function becomes:

Among them, F _ij and E _ij represent the partial derivative of the camera pose and the partial derivative of the landmark position in the current state, respectively;

Finally, the following incremental equation is solved:

H△x=g, g is equivalent to a constant.

in,

H＝J ^T J+λI

J=[F E], matrices E and F represent the derivative of the overall objective function to the overall variable.

Advantage of the present invention and beneficial effect are as follows:

The invention provides a mobile robot visual SLAM method based on an improved closed-loop detection algorithm, which uses ORB features to improve the speed of feature extraction and matching, and improves the real-time performance of the system; and improves the accuracy of closed-loop detection through the improved closed-loop detection algorithm to avoid It solves the problem of perception ambiguity and reduces the cumulative error of the system; and through the local optimization of the pose and the global optimization of the entire system data, the error caused by the system noise is reduced, so more accurate pose estimation and better Good 3D reconstruction effect.

Description of drawings

Fig. 1 is a flow chart of the mobile robot visual SLAM method based on the improved closed-loop detection algorithm of the present invention.

detailed description

The technical solutions in the embodiments of the present invention will be described clearly and in detail below with reference to the drawings in the embodiments of the present invention. The described embodiments are only some of the embodiments of the invention.

The technical scheme that the present invention solves the problems of the technologies described above is:

As shown in Figure 1, the present invention provides a kind of mobile robot visual SLAM method based on improved closed-loop detection algorithm, it is characterized in that, comprises the following steps:

S1, use Zhang Dingyou calibration method to calibrate Kinect; correct camera distortion and obtain camera internal parameters; Zhang Dingyou calibration method is a calibration method that only needs to use a printed checkerboard to obtain camera internal parameters.

S2, using opencv to extract ORB features from RGB images, the obtained ORB features are robust and time-consuming, which is very suitable for real-time SLAM; the Fast Approximate Nearest Neighbor (FLANN) algorithm is suitable for situations with a large number of matching points, and can effectively Complete feature matching.

S3, for the wrong matching situation, use random sampling consistency (RANSAC) to delete the wrong matching; obtain the spatial coordinates of the matching point through the Kinect camera model, and then use the PnP algorithm to estimate the inter-frame pose transformation (R, t); space coordinate solution method for:

z _o = z

If Kinect obtains the depth value z corresponding to the pixel coordinates (u, v) in the image, then the space coordinates (x _o , y _o , z _o ) can be obtained according to the above Kinect camera model.

S4, in order to provide a more stable and accurate pose estimation for the back-end global optimization, unstructured iterative optimization is performed on the pose transformation obtained by PnP. The steps to optimize the pose transformation obtained by PnP are:

First, with the camera pose as the optimization variable, an optimization problem is constructed by minimizing the reprojection error;

Then, use the PnP value as the initial value in the original PnP function, and then call g2o for optimization.

S5, first, preprocess the acquired image; use the following similarity calculation function to obtain the image similarity score, when the score is greater than the set threshold, the current frame is discarded; if not, the current frame represents a new scene, used for Participate in closed-loop detection; among them, the threshold in preprocessing should be reasonably set according to the quality of the acquired image and the image acquisition rate, because the smaller the threshold, the more image frames will be deleted, and the higher the accuracy of acquiring new scenes. However, if the setting is too low, when the robot returns to the area it has been to, it will not be able to detect the closed loop, so the threshold should be set reasonably according to the situation.

Next, the hierarchical K-means clustering algorithm is used to describe the image; the following improved single-node scoring function is used for similarity score matching.

The function of the improved similarity score is defined as:

Based on the new single-node similarity score function proposed by the above formula, the similarity score of the first layer is:

Using the method of top-down similarity increment can avoid repeating cumulative similarity, and define the l-layer similarity score increment as:

Define the matching kernel as:

Among them, η _l represents the matching strength coefficient of the lth layer.

By comparing the similarity score with the set threshold, the candidate closed loops are screened out; then the correct closed loops are screened out of the candidate closed loops by using time continuity and spatial consistency.

S6. First, in order to optimize the pose and landmark points calculated by the visual odometry, the overall cost function (also called the objective function) is constructed as:

e=z-h(ξ,p)

Among them, ξ is the Lie algebra corresponding to the camera pose, that is, the extrinsic parameters (R, t), p is the three-dimensional coordinate point of the landmark, z is the pixel coordinate data observed by Kinect, e is the observation error, and z _ij represents the position ξ _i The pixel coordinate data generated by observing the landmark p _j at the place.

Then solve this least squares method, defining the independent variables as all variables to be optimized:

x=[ξ ₁ ,...,ξ _m ,p ₁ ,...,p _n ]

According to the idea of nonlinear optimization, the optimal solution of the cost function is found by constantly looking for the descending direction △x; when an increment is given to the independent variable, the objective function becomes:

Among them, F _ij and E _ij represent the partial derivative of the camera pose and the partial derivative of the landmark position in the current state, respectively.

Finally, the following incremental equation is solved:

H△x=g

in,

H＝J ^T J+λI

J＝[F E]

Finally, the convergence value x that satisfies the state variable of the objective equation is obtained through multiple iterations, and the optimized pose and landmark can be obtained.

The above embodiments should be understood as only for illustrating the present invention but not for limiting the protection scope of the present invention. After reading the contents of the present invention, skilled persons can make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims (7)

1. a mobile robot visual SLAM method based on improved closed-loop detection algorithm, is characterized in that, comprises the following steps: S1. Use Zhang Dingyou's calibration method to calibrate Kinect to obtain the camera's internal parameters; S2. Perform ORB feature extraction on the acquired RGB image, and use the FLANN fast approximate nearest neighbor algorithm to perform feature matching, including correct matching results and incorrect matching results; S3. For the mismatching result of step S2, use RANSAC random sampling consistency to delete the mismatching; obtain the spatial coordinates of the matching point through the Kinect camera model, and then use the n-point perspective problem algorithm PnP to estimate the inter-frame pose transformation (R, t) ; S4. Perform unstructured iterative optimization on the pose transformation calculated by the PnP algorithm in step S3; S5. By preprocessing the image frame obtained in step S2, the image is described through the bag of visual words, and an improved similarity score matching method is used for image matching, and the matching obtained is more in line with the actual similarity between images, thereby Obtain candidate closed loops, and at the same time, use time continuity and spatial consistency to filter out the correct closed loops; S6. Use the graph optimization method with bundle adjustment BA as the core to optimize the pose and landmarks, and obtain more accurate camera poses and landmarks through continuous iterative optimization. 2. mobile robot vision synchronous positioning and map construction method according to claim 1, is characterized in that, the ORB feature extraction step in the step S2 is: First, FAST corner point extraction is performed on the RGB image, and the ORB feature points are invariant to scale and rotation by using the image pyramid and gray-scale centroid method; Again, the binary description of the surrounding image of the feature points extracted in the previous step is performed through BRIEF. 3. mobile robot vision synchronous location and map construction method according to claim 2, it is characterized in that, in described step S3, obtain matching point spatial coordinates by Kinect camera model, spatial coordinate solution method is: <mrow><msub><mi>x</mi><mi>o</mi></msub><mo>=</mo><mfrac><mi>z</mi><msub><mi>f</mi><mi>x</mi></msub></mfrac><mrow><mo>(</mo><mi>u</mi><mo>-</mo><msub><mi>c</mi><mi>x</mi></msub><mo>)</mo></mrow></mrow> <mrow><msub><mi>y</mi><mi>o</mi></msub><mo>=</mo><mfrac><mi>z</mi><msub><mi>f</mi><mi>y</mi></msub></mfrac><mrow><mo>(</mo><mi>v</mi><mo>-</mo><msub><mi>c</mi><mi>y</mi></msub><mo>)</mo></mrow></mrow> z _o = z If Kinect obtains the depth value z corresponding to the pixel coordinates (u, v) in the image, then the space coordinates (x _o , y _o , z _o ) can be obtained according to the above Kinect camera model. 4. a kind of mobile robot visual SLAM method based on improved closed-loop detection algorithm according to claim 3, is characterized in that, the step of optimizing the pose transformation that PnP obtains in step S4 is: First, with the camera pose as the optimization variable, an optimization problem is constructed by minimizing the reprojection error; Then, use the PnP value as the initial value in the original PnP function, and then call g2o for optimization. 5. a kind of mobile robot visual SLAM method based on improved closed-loop detection algorithm according to claim 4, is characterized in that, the detection step of described step S5 candidate closed-loop is: First, preprocess the acquired image, and use the following similarity calculation function to obtain the image similarity score. When the score is greater than the set threshold, the current frame is discarded; if not, the current frame represents a new scene and is used to participate in the closed loop detection; <mrow><mi>H</mi><mo>=</mo><mfrac><mrow><mo>(</mo><msub><mi>V</mi><mi>l</mi></msub><mo>,</mo><msub><mi>V</mi><mi>c</mi></msub><mo>)</mo></mrow><mrow><mo>|</mo><mo>|</mo><msub><mi>V</mi><mi>l</mi></msub><mo>|</mo><mo>|</mo><mo>|</mo><mo>|</mo><msub><mi>V</mi><mi>c</mi></msub><mo>|</mo><mo>|</mo></mrow></mfrac><mo>,</mo></mrow> Wherein, V ₁ represents the weighted vector of the previous frame image, V _c represents the weighted vector of the current frame, and the H value represents the image similarity; Next, use the hierarchical K-means clustering algorithm to describe the image; then use the following improved single-node scoring function to match the image similarity score; Using the method of top-down similarity increment can avoid repeating cumulative similarity, and define the l-layer similarity score increment as: <mrow><msup><mi>&amp;Delta;S</mi><mi>l</mi></msup><mrow><mo>(</mo><mi>X</mi><mo>,</mo><mi>Y</mi><mo>)</mo></mrow><mo>=</mo><mfenced open = "{" close = ""><mtable><mtr><mtd><mrow><msup><mi>S</mi><mi>l</mi></msup><mrow><mo>(</mo><mi>X</mi><mo>,</mo><mi>Y</mi><mo>)</mo></mrow><mo>-</mo><msup><mi>S</mi><mrow><mi>l</mi><mo>+</mo><mn>1</mn></mrow></msup><mrow><mo>(</mo><mi>X</mi><mo>,</mo><mi>Y</mi><mo>)</mo></mrow><mo>,</mo></mrow></mtd><mtd><mrow><mn>1</mn><mo>&amp;le;</mo><mi>l</mi><mo>&lt;</mo><mi>d</mi></mrow></mtd></mtr><mtr><mtd><mrow><msup><mi>S</mi><mi>d</mi></msup><mrow><mo>(</mo><mi>X</mi><mo>,</mo><mi>Y</mi><mo>)</mo></mrow><mo>,</mo></mrow></mtd><mtd><mrow><mi>l</mi><mo>=</mo><mi>d</mi></mrow></mtd></mtr></mtable></mfenced></mrow> Define the matching kernel as: <mrow><mi>K</mi><mrow><mo>(</mo><mi>X</mi><mo>,</mo><mi>Y</mi><mo>)</mo></mrow><mo>=</mo><munderover><mo>&amp;Sigma;</mo><mrow><mi>l</mi><mo>=</mo><mn>1</mn></mrow><mi>d</mi></munderover><msub><mi>&amp;eta;</mi><mi>l</mi></msub><msup><mi>&amp;Delta;S</mi><mi>l</mi></msup><mrow><mo>(</mo><mi>X</mi><mo>,</mo><mi>Y</mi><mo>)</mo></mrow></mrow> Among them, η _l represents the matching strength coefficient of the lth layer. 6. a kind of mobile robot visual SLAM method based on improved closed-loop detection algorithm according to claim 5, is characterized in that, the function definition of the similarity score of described improvement is: <mrow><msubsup><mi>S</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>X</mi><mo>,</mo><mi>Y</mi><mo>)</mo></mrow><mo>=</mo><mi>min</mi><mo>{</mo><msubsup><mi>&amp;omega;</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>X</mi><mo>)</mo></mrow><mo>,</mo><msubsup><mi>&amp;omega;</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>Y</mi><mo>)</mo></mrow><mo>}</mo><mo>+</mo><mfrac><mn>1</mn><mrow><mn>1</mn><mo>+</mo><mo>|</mo><msubsup><mi>&amp;omega;</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>X</mi><mo>)</mo></mrow><mo>-</mo><msubsup><mi>&amp;omega;</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>Y</mi><mo>)</mo></mrow><mo>|</mo></mrow></mfrac></mrow> Based on the new single-node similarity score function proposed by the above formula, the similarity score of the first layer is: <mrow><msup><mi>S</mi><mi>l</mi></msup><mrow><mo>(</mo><mi>X</mi><mo>,</mo><mi>Y</mi><mo>)</mo></mrow><mo>=</mo><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><msup><mi>k</mi><mi>l</mi></msup></munderover><msubsup><mi>S</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>X</mi><mo>,</mo><mi>Y</mi><mo>)</mo></mrow><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><msup><mi>k</mi><mi>l</mi></msup></munderover><mi>min</mi><mo>{</mo><msubsup><mi>&amp;omega;</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>X</mi><mo>)</mo></mrow><mo>,</mo><msubsup><mi>&amp;omega;</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>Y</mi><mo>)</mo></mrow><mo>}</mo><mo>+</mo><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><msup><mi>k</mi><mi>l</mi></msup></munderover><mfrac><mn>1</mn><mrow><mn>1</mn><mo>+</mo><mo>|</mo><msubsup><mi>&amp;omega;</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>X</mi><mo>)</mo></mrow><mo>-</mo><msubsup><mi>&amp;omega;</mi><mi>i</mi><mi>l</mi></msubsup><mrow><mo>(</mo><mi>Y</mi><mo>)</mo></mrow><mo>|</mo></mrow></mfrac></mrow> in, Indicates the score of the image X on the i-th node of the l-th layer of the tree, Indicates the score of image Y on the i-th node of the l-th layer of the tree. 7. A kind of mobile robot visual SLAM method based on improved closed-loop detection algorithm according to claim 5 or 6, it is characterized in that, adopting the graph optimization method with cluster adjustment BA as the core to optimize pose and landmark specifically includes : First, the overall cost function for constructing the pose and landmarks, that is, the objective function is: <mrow><mfrac><mn>1</mn><mn>2</mn></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>m</mi></munderover><munderover><mo>&amp;Sigma;</mo><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mi>n</mi></munderover><mo>|</mo><mo>|</mo><msub><mi>e</mi><mrow><mi>i</mi><mi>j</mi></mrow></msub><mo>|</mo><msup><mo>|</mo><mn>2</mn></msup><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>m</mi></munderover><munderover><mo>&amp;Sigma;</mo><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mi>n</mi></munderover><mo>|</mo><mo>|</mo><msub><mi>z</mi><mrow><mi>i</mi><mi>j</mi></mrow></msub><mo>-</mo><mi>h</mi><mrow><mo>(</mo><msub><mi>&amp;xi;</mi><mi>i</mi></msub><mo>,</mo><msub><mi>p</mi><mi>j</mi></msub><mo>)</mo></mrow><mo>|</mo><msup><mo>|</mo><mn>2</mn></msup></mrow> e=z-h(ξ,p) Among them, ξ is the Lie algebra corresponding to the camera pose, that is, the extrinsic parameters (R, t), p is the three-dimensional coordinate point of the landmark, z is the pixel coordinate data observed by Kinect, e is the observation error, and z _ij represents the position ξ _i The pixel coordinate data generated by observing the landmark p _j at the place; Then solve this least squares method, defining the independent variables as all variables to be optimized: x=[ξ ₁ ,...,ξ _m ,p ₁ ,...,p _n ] According to the idea of nonlinear optimization, the optimal solution of the cost function is found by constantly looking for the descending direction △x; when an increment is given to the independent variable, the objective function becomes: <mrow><mfrac><mn>1</mn><mn>2</mn></mfrac><mo>|</mo><mo>|</mo><mi>f</mi><mrow><mo>(</mo><mi>x</mi><mo>+</mo><mi>&amp;Delta;</mi><mi>x</mi><mo>)</mo></mrow><mo>|</mo><msup><mo>|</mo><mn>2</mn></msup><mo>&amp;ap;</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mi>m</mi></munderover><munderover><mo>&amp;Sigma;</mo><mrow><mi>j</mi><mo>=</mo><mn>1</mn></mrow><mi>n</mi></munderover><mo>|</mo><mo>|</mo><msub><mi>e</mi><mrow><mi>i</mi><mi>j</mi></mrow></msub><mo>+</mo><msub><mi>F</mi><mrow><mi>i</mi><mi>j</mi></mrow></msub><msub><mi>&amp;Delta;&amp;xi;</mi><mi>i</mi></msub><mo>+</mo><msub><mi>E</mi><mrow><mi>i</mi><mi>j</mi></mrow></msub><msub><mi>&amp;Delta;p</mi><mi>j</mi></msub><mo>|</mo><msup><mo>|</mo><mn>2</mn></msup></mrow> Among them, F _ij and E _ij represent the partial derivative of the camera pose and the partial derivative of the landmark position in the current state, respectively; Finally, the following incremental equation is solved: H△x=g, g is equivalent to a constant. in, H＝J ^T J+λI J=[F E], matrices E and F represent the derivative of the overall objective function to the overall variable.