CN104732518B - An Improved Method of PTAM Based on Ground Features of Intelligent Robot - Google Patents

Abstract

A kind of PTAM improved methods based on intelligent robot terrain surface specifications, first, complete parameter correction, this includes parameter definition and camera calibration；Then current environment texture information is obtained using camera, and builds four layers of Gaussian image pyramid, extract the characteristic information in present image with FAST Corner Detection Algorithms, the data correlation set up between Corner Feature obtains pose estimation model；In the initial drawing stage of map, obtain two key frames and camera is erected in mobile robot；In initialization procedure, mobile robot starts movement, while angle point information and setting up association in camera capture current scene；After the initialization for realizing sparse 3 D map, update key frame and set up characteristic point sub-pixel precision mapping relations with block matching method using polar curve search, accurately relocated with reference to pose estimation model realization camera.Match point is finally projected into space, current global context three-dimensional map is completed and creates.

Description

An Improved Method of PTAM Based on Ground Features of Intelligent Robot

technical field

The invention belongs to the field of robot vision and relates to an improved PTAM algorithm based on ground features.

Background technique

As the relationship between robots and humans is getting closer, the related technologies of intelligent robots have received great attention. Simultaneous Localization and Mapping (SLAM) of mobile robots is one of the most mainstream positioning technologies for intelligent mobile robots. It is actually a motion estimation problem, using the internal and external data acquired by the sensor to calculate the position of the mobile robot at a certain moment, and at the same time establish the map model it depends on. Vision-based SLAM belongs to the research category of visual measurement, because the visual sensor has its own unique advantages: small size, light weight, cheap price, easy installation, and the extracted external information is very rich. These advantages further promote the current research and application of vision-based SLAM. The application number is 201310678018.2, which provides a feature detection operator using SURF as a monocular visual SLAM. It innovates and improves in three aspects: the detection of interest points, the generation of SURF descriptors, and the matching of SURF points. The SURF feature matching method based on region growth completes the simultaneous positioning and map creation of the robot, so that in the process of SURF descriptor matching, a certain descriptor is only compared with the most likely matching descriptor, thus significantly reducing the single The number of comparisons in the visual SLAM problem improves the matching speed. However, this method still has the problems of not being able to establish a metric map and not being able to provide three-dimensional feature information, and the robustness and real-time performance of the algorithm need to be further improved.

The technique of creating an environment model from motion is a method to simultaneously solve the camera motion trajectory and the scene structure model. As a representative method of SFM, G. Klein et al. first proposed the concept of PTAM (Parallel Tracking and Mapping) in the field of augmented reality (AR) in 2007 to solve the problem of environment recognition in real time. However, the unimproved PTAM algorithm also has the problem of being unable to establish a metric map, and has strict restrictions on the movement of the camera.

Contents of the invention

In view of the above existing problems, the present invention proposes an improved PTAM algorithm based on ground features. On the basis of adding ground features, PTAM performs synchronous estimation on the current pose of the monocular camera and three-dimensional space points. While updating the detection feature points, the optimal local and global beam adjustment method is used to continuously update the poses of the camera and 3D feature points, completing high-precision positioning and map creation. This invention effectively solves the problem of the inability to create metric maps At the same time, it has greatly improved the real-time performance and 3D feature extraction, which is of great significance for solving related problems in robot navigation and positioning and 3D reconstruction.

The present invention adopts following technical scheme:

First, complete the parameter correction, which includes parameter definition and camera correction; then track the features in the key frame, and then realize the initialization of the pose, and then estimate the plane parameters on this basis; then use the camera to obtain the current environment texture information, And build a four-layer Gaussian image pyramid, use the FAST corner detection algorithm to extract the feature information in the current image, establish the data association between the corner features, and obtain the pose estimation model; in the initial drawing stage of the map, two key frames will be obtained The camera is set up on the mobile robot, and the initialization process starts after the camera and the ground are calibrated; during the initialization process, the mobile robot starts to move, and at the same time, the camera captures the corner information in the current scene and establishes an association, and the current pose is obtained by the pose estimation model Initial estimation, and then use the pose estimation method based on ground features to obtain an accurate estimate of the current pose; after realizing the initialization of the 3D sparse map, update the key frame and use the epipolar search and block matching method to establish the sub-pixel precision mapping relationship of feature points, Combined with the pose estimation model to achieve precise camera relocalization. Finally, the matching points are projected into the space to complete the creation of the 3D map of the current global environment. At the same time, the consistency optimization of the algorithm is completed by using global and local bundle adjustment.

Specifically include the following steps:

Step 1, parameter correction

Step 1.1, parameter definition

(1) Robot pose

The world coordinate system is established at the starting point of the mobile robot. During the movement of the robot, its pose can be expressed as:

It can be seen from the above formula that the pose of the mobile robot can be described by x=(x, y, θ), where x and y represent the translation of the mobile robot in the x-axis and y-axis directions respectively, and θ is the rotation angle of the mobile robot around the z-axis .

In the world coordinate system, the pose of the camera can be expressed as:

Among them, T _c ∈ SE(3), SE(3) is a special rigid body transformation group, R _c is a 3×3 rotation matrix, and t _c is a 3×1 translation matrix. The pose matrix establishes the corresponding relationship between the world coordinate system and the points p _c and p _w in the camera coordinate system, that is, p _c = T _c p _w . As shown in Figure 2, the camera is fixed on the mobile robot in this study, and there is a fixed rotation-translation relationship T _cr between the camera and the robot. During the moving process of the robot, if the pose increment of the robot is T _rs at a certain time interval, and correspondingly, the pose increment of the camera is T _cs , then:

T _rs = T _rc T _cs T _cr (3)

in,

(2) Ground plane calibration parameters

Fix the camera on the mobile robot, and place the calibration target flat on the ground. The camera establishes the pose relationship between the camera and the robot by acquiring image information with a calibrated target. T _cp ∈ SE(3) is the transformation matrix between the camera and the calibration target, R _cp and t _cp represent the rotation and translation components of T _cp respectively, and their inverse transformation It is assumed that the coordinate origin of the robot is consistent with the camera, and the x-axis of the robot is oriented in the same direction as the z-axis of the camera. In the target coordinate system, the x-direction vector of the robot can be represented by the first two components of the vector in the third row of _Tcp . At this time, the transformation relationship between the robot coordinate system and the target coordinate system can be expressed as:

Among them, γ is the rotation angle between the robot coordinate system and the target coordinate system, and (x _r , y _r ) is the coordinate of the robot origin in the target coordinate system, which can be determined by the translation vector of the camera and the target. Therefore, the transformation relationship between the robot and the camera can be expressed as T _cr = T _cp T _pr , According to the pose relationship between the camera and the target plane, the ground plane equation in the camera coordinate system can be obtained:

(0,0,1,0)T _pc p _c =0 (5)

Wherein, p _c is a three-dimensional point in the camera coordinate system, the ground plane parameter can be expressed as (n ^T ,d), and n=(0,0,1)R _pc , d=t _p (3).

Step 1.2, Camera Correction

Due to the manufacturing reasons of the lens, there will be distortion. The distortion is mainly divided into mirror distortion caused by the shape of the lens and tangential distortion caused by the camera assembly process. In this paper, the FOV model is used to correct the monocular camera, and the correction model is shown in Figure 3. This model is proposed by Devernay and Faugeras as a de-distortion method for wide-angle cameras. Its mathematical description is as follows:

In the formula, x _u is the normalized coordinate of the pixel, u _d is the pixel coordinate after distortion, ω is the distortion coefficient, r _d , r _u are the distances from the normalized coordinate before and after correction to the center of the coordinate system, respectively.

Using the above camera model, the image pixel coordinates are mapped to the normalized coordinate plane, and combined with the camera internal parameter matrix K, image distortion correction is realized, namely:

u＝Kx _u (7)

Step 2, initialization based on ground features

Step 2.1, Feature Tracking

FAST corner detection is a simple and fast corner detection algorithm proposed by Edward Rosten and Tom Drummond. This algorithm uses the difference in the gray value of adjacent pixels in a certain neighborhood (usually a circular area) of a pixel to determine whether the pixel is a corner point, as follows:

Among them, u _f is the candidate corner point, s is any point in the neighborhood of the candidate corner point, c(u _f ) represents the neighborhood of u _f , I(x) is the gray value of the image at x, if N exceeds the threshold condition, The candidate corner point at x is the corner point. The candidate point is the corner point.

Before the camera tracks the thread, the system uses the camera to obtain the current environment texture information, and constructs a four-layer Gaussian image pyramid, uses the FAST corner detection algorithm, combines the block search method to establish the data association between corner features, and uses the projection relationship to predict 3D The corresponding relationship between the point and the feature point in the current frame, and search in a fixed area near the position to obtain an accurate matching relationship.

Step 2.2, pose initialization

During the movement of the robot, the ground information is obtained through the camera, and the corner features are tracked using the FAST algorithm. According to the above data association information, the homography relationship of plane points can be established, namely:

π(Hu _r )=u _p (9)

Among them, π(p) is the projection function, p=(x,y,z), π(p)=(x/z,y/z,1), u _r , u _p are the reference frame and the current frame Corresponding feature points. According to the camera translation T _c and the ground plane equation parameters (n ^T ,d), the homography relationship of plane points can be established:

H＝KT _c (I|n _d ) ^T K ^-1 (10)

Wherein, n _d =n/d, K is the camera intrinsic parameter matrix. Substituting Equation (3) into Equation (10), the homography relationship between the pose of the robot and the ground features can be described as:

H＝KT _cr T _rs T _rc (I n _d )TK ^-1 (11)

Using the homography relationship of the above image feature points, the pose estimation model can be established:

Among them, r(x)=π(H(x)u _ref )-u _p is the homography relationship error function, that is, the projection error function, and H(x) is the homography matrix. According to the small displacement assumption, the first-order Taylor expansion of the error function r is expressed as:

At this time, the extreme value condition of the energy function is:

which is:

in, The pose estimation of the mobile robot can be obtained by iteratively solving the above formula. In order to further improve the positioning accuracy, this paper introduces robust weights to build a pose estimation model based on M-estimation. At this time, the energy function is expressed as:

Among them, w(x) is the robust Tukey weighting function. At this time, the extreme value condition of the energy function is:

Step 2.3, Planar Parameter Estimation

The robot uses the homography relationship based on ground features to realize robot positioning, but due to the influence of non-ground features, it is difficult to ensure the positioning accuracy, so the present invention adopts a fast non-ground feature removal method. The normal estimation model is constructed according to the cross product of parallel vectors. Experiments show that the ideal cross product condition cannot be satisfied due to certain errors in corner point detection, and the fitness of the normal estimation model is poor. The projection error r is used as the judgment basis to determine the non-ground feature information. However, as the distance between non-ground features and observation points decreases, the discrimination of this method decreases significantly. Compared with the above method, the present invention uses the homography matrix to directly derive the normal estimation model on the basis of the initial estimation of the pose, and judges whether the corner point is ground feature information by obtaining the difference between it and the parameters of the ground plane equation. According to the plane homography relation, the estimation model of the plane parameter n _d can be established:

Wherein, r(n _d )=π(H(n _d )u _ref )-u _p is the corresponding projection error function. Similar to the solution process of pose estimation, on the basis of the small displacement assumption, the solution model of formula (18) can be established according to the first-order Taylor formula of the error function.

At this time, the plane parameters can be obtained by iterative formula (18):

in,

Step 3, pose estimation

The main function of the pose estimation module is to complete the real-time update of the camera pose. Its specific implementation process is as follows. After the initialization work is completed, the pose estimation model is established according to the projection error, and its mathematical description is as follows:

where e _j is the projection error, and is the Tukey dual-weight objective function, σ _T is the unbiased estimate of the standard deviation of feature point matching, μ is the camera pose six-tuple description vector.

For the failures in tracking, a feature observation ratio is proposed to evaluate the tracking quality of each frame image. If the ratio is lower than the set threshold, the tracking effect is poor, and the tracking will continue, but the system will not update the frame to the map; if the feature observation ratio is lower than the set threshold in the tracking of multiple frames of images In the case of , it can be considered as a "follow-up" situation, and then the tracking process is initialized. Through the above steps, the precise estimation of the pose is realized, and the tracking process can be carried out normally.

Step 4, map creation

Step 4.1, map initialization

The system uses the ground features and standard stereo camera algorithm model to establish the current environment initialization map. In the initialization process of the map, using the FAST corner point matching relationship in the image, combined with the RANSAC algorithm, using the initial pose estimation method based on ground features, using the triangulation method to calculate the three-dimensional coordinates of the current feature point, and determining the starting point as the global The origin of the world coordinate system to complete the initialization of the 3D sparse map.

In the initial drawing stage of the map, the specific process of obtaining two key frames is as follows. The camera is set up on the mobile robot, and the initialization process starts after the camera and the ground are calibrated. In the initialization process, the mobile robot starts to move, and at the same time, the camera captures the corner information in the current scene and establishes an association, and obtains the initial estimation of the current pose by solving equation (12). When the initial estimate exceeds the set threshold d ₀ , use the pose estimation method based on ground features in 2.2 to obtain an accurate estimate of the current pose.

Step 4.2, keyframe update and epipolar search

When the map is initialized, it only contains two keyframes, and can only describe the adjacent small volume space. When the camera leaves the initial position, new keyframes and map features need to be added. If the number of image frames between the camera and the current key frame exceeds the threshold condition, the camera tracking effect is the best, and the distance between the camera and the nearest map key point is as small as possible, the process of adding key frames will be automatically executed. Due to real-time constraints, the tracking system may only measure a subset of features visible in the frame, so the mapping thread needs to replan and measure the remaining map features.

In the update of the key frame, the system will first perform Shi-Tomas evaluation on all the corner points in the newly added key frame to obtain the current corner point information with salient features, and the salient points near the successfully observed feature points will be discarded , the remaining salient points will be used as candidate map nodes. Since the new map node requires known depth information, which cannot be obtained only by a single frame image, it is necessary to select the nearest key frame as the second view, and use epipolar search and block matching methods to establish the sub-pixel precision mapping relationship of feature points. In the second view, the zero-mean SSD method is adopted, combined with the pose estimation model to achieve precise camera relocalization. Finally, the matched points are projected into space to generate a 3D map of the current global environment.

Step 4.3, Optimization

The optimal local and global LM (Levenberg-Marquardt) bundle adjustment algorithm is used to optimize the global consistency of the current map to improve robustness. The mathematical description of the cluster adjustment algorithm is:

Among them, σ _ji is the unbiased estimate of the matching standard deviation of FAST feature points in the i-th keyframe, μ _i is the pose six-tuple description vector of the i-th keyframe, and p _i is a point in the global map.

The stable cost function in (21) is made extremely small by continuous bundle adjustment. Global clustering adjusts the poses of all keyframes and the positions of map points, and at the same time utilizes the inherent sparsity in the structure-motion problem to make the complexity of the overall matrix decomposition present a cubic-level reduction, which is reduced by O((N+M) ³ ) to O(N ³ ), but in most estimation cases is constrained by the generated outer point vector product, which increases significantly as the map grows.

Aiming at the above problems, local bundle adjustment is introduced, here only the subset poses of the key frames are adjusted, and the objective function at this time becomes:

X is a series of keyframes to be adjusted, Y is a fixed frame set, and Z is a subset of map points. The set X is composed of the latest keyframe and the 4 closest keyframes in the map, and all the visible points in the map of the keyframe form the set Z, and finally Y contains all the keys obtained from the measurement data in Z frame. As a result, local bundle adjustment optimizes the pose of the latest, most recent keyframe, updating all points in the map, while also significantly reducing the complexity to no more than O(NM).

Compared with the original PTAM algorithm, the present invention adds ground features to improve it, and realizes the following advantages:

(1) The initialization process is improved, which effectively removes the strict restrictions on camera movement imposed by the PTAM algorithm.

(2) It can effectively establish a relatively accurate metric map and provide a reliable guarantee for further environmental measurement.

Description of drawings

Fig. 1 is the flowchart of the method involved in the present invention;

Figure 2 Camera correction model;

Figure 3 Schematic diagram of planar parameter calibration.

detailed description

Below in conjunction with accompanying drawing, the patent of the present invention is described in further detail.

The PTAM improved algorithm flow chart based on ground features is shown in Figure (1), which specifically includes the following steps:

Step 1, parameter correction

Step 1.1, parameter definition

The robot pose representation is constructed from the relationship between the robot coordinate system and the world coordinate system, and the ground plane calibration parameters are determined by the pose relationship between the camera and the target plane.

Step 1.2, Camera Correction

The FOV model is used to correct the monocular camera, and the image pixel coordinates are mapped to the normalized coordinate plane. At the same time, the image distortion correction is realized by combining the camera internal parameter matrix K.

Step 2, initialization based on ground features

Step 2.1, Feature Tracking

Obtain environmental texture information, extract features, and establish corner feature associations.

Step 2.2, pose initialization

Establish a homography relationship and a pose initialization estimation model to realize pose initialization.

Step 2.3, Planar Parameter Estimation

Step 3, pose estimation

After the initialization is completed, the initialization pose estimation of the camera is realized according to the pose estimation model based on ground features. Furthermore, PTAM uses the epipolar search method to establish the sub-pixel precision matching relationship of the corner points in the image pyramid according to the initial pose of the camera, and reintroduces it into the pose estimation model to achieve precise camera repositioning.

Step 4, map creation

Step 4.1, map initialization

The system uses the standard stereo camera algorithm model to establish the current environment initialization map

Step 4.2, keyframe update and epipolar search

Add a new key frame and use the epipolar search and block matching methods to establish the sub-pixel precision mapping relationship of feature points, and combine the pose estimation model to achieve precise camera relocation. Finally, the matching points are projected into the space to complete the creation of the 3D map of the current global environment.

Step 4.3, Optimization

Consistency optimization of maps is achieved using global and local bundle adjustments.

Claims (1)

1. A kind of PTAM improvement method based on intelligent robot ground feature, it is characterized in that: First, complete the parameter correction, which includes parameter definition and camera correction; then track the features in the key frame, and then realize the initialization of the pose, and then estimate the plane parameters on this basis; then use the camera to obtain the current environment texture information, And build a four-layer Gaussian image pyramid, use the FAST corner detection algorithm to extract the feature information in the current image, establish the data association between the corner features, and obtain the pose estimation model; in the initial drawing stage of the map, two key frames will be obtained The camera is set up on the mobile robot, and the initialization process starts after the camera and the ground are calibrated; during the initialization process, the mobile robot starts to move, and at the same time, the camera captures the corner information in the current scene and establishes an association, and the current pose is obtained by the pose estimation model Initial estimation, and then use the pose estimation method based on ground features to obtain an accurate estimate of the current pose; after realizing the initialization of the 3D sparse map, update the key frame and use the epipolar search and block matching method to establish the sub-pixel precision mapping relationship of feature points, Combined with the pose estimation model to achieve precise camera relocation; finally, the matching points are projected into space to complete the creation of a 3D map of the current global environment; at the same time, the consistency optimization of the algorithm is completed by using global and local cluster adjustments; Specifically include the following steps: Step 1, parameter correction Step 1.1, parameter definition (1) Robot pose The world coordinate system is established at the starting point of the mobile robot. During the movement of the robot, its pose is expressed as: <mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mi>r</mi> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <msub> <mi>R</mi> <mi>r</mi> </msub> </mtd> <mtd> <msub> <mi>t</mi> <mi>r</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>R</mi> <mi>r</mi> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mrow> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <msub> <mi>t</mi> <mi>r</mi> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mi>x</mi> </mtd> </mtr> <mtr> <mtd> <mi>y</mi> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> It can be seen from the above formula that the pose of the mobile robot is described by x=(x, y, θ), where x and y represent the translation of the mobile robot in the x-axis and y-axis directions respectively, and θ is the rotation angle of the mobile robot around the z-axis ; In the world coordinate system, the pose of the camera is expressed as: <mrow> <msub> <mi>T</mi> <mi>c</mi> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <msub> <mi>R</mi> <mi>c</mi> </msub> </mtd> <mtd> <msub> <mi>t</mi> <mi>c</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> Among them, T _c ∈ SE(3), SE(3) is a special rigid body transformation group, R _c is a 3×3 rotation matrix, t _c is a 3×1 translation matrix; the pose matrix establishes the world coordinate system and the camera coordinate Corresponding relationship between points p _c and p _w under the system, that is, p _c = T _c p _w ; in this method, the camera is fixed on the mobile robot, and there is a fixed rotation-translation relationship T _cr between the camera and the robot; during the movement of the robot, If the pose increment of the robot in a certain time interval is T _rs , and correspondingly, the pose increment of the camera is T _cs , then: T _rs = T _rc T _cs T _cr (3) in, (2) Ground plane calibration parameters Fix the camera on the mobile robot, and place the calibration target flat on the ground; the camera establishes the pose relationship between the camera and the robot by acquiring image information with the calibration target; T _cp ∈ SE(3) is the transformation between the camera and the calibration target Matrix, R _cp , t _cp represent the rotation and translation components of T _cp respectively, and its inverse transformation It is assumed that the coordinate origin of the robot is consistent with the camera, and the x-axis of the robot is oriented in the same direction as the z-axis of the camera; in the target coordinate system, the x-direction vector of the robot is represented by the first two components of the third line vector of T _cp ; When , the transformation relationship between the robot coordinate system and the target coordinate system is expressed as: <mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>r</mi> </msub> <mo>=</mo> <msub> <mi>T</mi> <mrow> <mi>r</mi> <mi>p</mi> </mrow> </msub> <msub> <mi>p</mi> <mi>p</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mrow> <mi>r</mi> <mi>p</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mrow> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>x</mi> <mi>r</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>y</mi> <mi>r</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow> Among them, γ is the rotation angle between the robot coordinate system and the target coordinate system, (x _r , y _r ) is the coordinate of the robot origin in the target coordinate system, which is determined by the translation vector between the camera and the target; therefore, the distance between the robot and the camera The transformation relation is expressed as T _cr =T _cp T _pr , According to the pose relationship between the camera and the target plane, the ground plane equation in the camera coordinate system is obtained: (0,0,1,0)T _pc p _c =0 (5) Among them, p _c is a three-dimensional point in the camera coordinate system, the ground plane parameter is expressed as (n ^T ,d), and n=(0,0,1)R _pc , d=t _p (3); Step 1.2, Camera Correction There will be distortion due to the manufacturing of the lens, and the distortion is mainly divided into mirror distortion caused by the lens shape and tangential distortion caused by the camera assembly process; this method uses the FOV model to correct the monocular camera; the model is A de-distortion method for wide-angle cameras proposed by Devernay and Faugeras, whose mathematical description is as follows: <mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>u</mi> <mi>d</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mn>0</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>f</mi> <mi>u</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>f</mi> <mi>v</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mfrac> <msub> <mi>r</mi> <mi>d</mi> </msub> <msub> <mi>r</mi> <mi>u</mi> </msub> </mfrac> <msub> <mi>x</mi> <mi>u</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>r</mi> <mi>d</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&amp;omega;</mi> </mfrac> <mi>arctan</mi> <mrow> <mo>(</mo> <mn>2</mn> <msub> <mi>r</mi> <mi>u</mi> </msub> <mi>tan</mi> <mfrac> <mi>&amp;omega;</mi> <mn>2</mn> </mfrac> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>r</mi> <mi>u</mi> </msub> <mo>=</mo> <mfrac> <mrow> <mi>tan</mi> <mrow> <mo>(</mo> <msub> <mi>r</mi> <mi>d</mi> </msub> <mi>&amp;omega;</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mn>2</mn> <mi>tan</mi> <mfrac> <mi>&amp;omega;</mi> <mn>2</mn> </mfrac> </mrow> </mfrac> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow> In the formula, x _u is the normalized coordinate of the pixel, u _d is the pixel coordinate after distortion, ω is the distortion coefficient, r _d and r _u are the distances from the normalized coordinate before and after correction to the center of the coordinate system; Using the camera model, the image pixel coordinates are mapped to the normalized coordinate plane, and combined with the camera internal parameter matrix K, the image distortion correction is realized, namely: u＝Kx _u (7) Step 2, initialization based on ground features Step 2.1, Feature Tracking FAST corner detection is a simple and fast corner detection algorithm proposed by Edward Rosten and Tom Drummond; this algorithm uses the difference in the gray value of adjacent pixels in a certain neighborhood of a pixel to determine whether the pixel is a corner , as follows: <mrow> <mi>N</mi> <mo>=</mo> <munder> <mi>&amp;Sigma;</mi> <mrow> <mi>s</mi> <mo>&amp;Element;</mo> <mi>c</mi> <mrow> <mo>(</mo> <msub> <mi>u</mi> <mi>f</mi> </msub> <mo>)</mo> </mrow> </mrow> </munder> <mo>|</mo> <mi>I</mi> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>I</mi> <mrow> <mo>(</mo> <msub> <mi>u</mi> <mi>f</mi> </msub> <mo>)</mo> </mrow> <mo>|</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow> Among them, u _f is the candidate corner point, s is any point in the neighborhood of the candidate corner point, c(u _f ) represents the neighborhood of u _f , I(s) is the gray value of the image at s, if N exceeds the threshold condition, The candidate corner point at s is the corner point; Before the camera tracks the thread, the system uses the camera to obtain the current environment texture information, and constructs a four-layer Gaussian image pyramid, uses the FAST corner detection algorithm, combines the block search method to establish the data association between corner features, and uses the projection relationship to predict 3D The corresponding relationship between the point and the feature point in the current frame, and search in a fixed area near the position to obtain an accurate matching relationship; Step 2.2, pose initialization During the moving process of the robot, the ground information is obtained through the camera, and the corner point features are tracked using the FAST algorithm; according to the above data association information, the homography relationship of the plane points is established, namely: π(Hu _r )=u _p (9) Among them, π(p) is the projection function, p=(x,y,z), π(p)=(x/z,y/z,1), u _r , u _p are the reference frame and the current frame Corresponding to the feature point; according to the camera translation T _c and the ground plane equation parameters (n ^T , d), the homography relationship of the plane point is established: H＝KT _c (I|n _d ) ^T K ^-1 (10) Among them, n _d =n/d, K is the internal parameter matrix of the camera; Substituting Equation (3) into Equation (10), the relationship between the pose of the robot and the ground feature homography is described as: H＝KT _cr T _rs T _rc (I n _d )TK ^-1 (11) Using the homography relationship of the above image feature points, a pose estimation model is established: <mrow> <mtable> <mtr> <mtd> <mrow> <mi>x</mi> <mo>=</mo> <mi>arg</mi> <munder> <mi>min</mi> <mi>x</mi> </munder> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <munder> <mi>&amp;Sigma;</mi> <mi>p</mi> </munder> <mo>|</mo> <mo>|</mo> <mi>r</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow> Among them, r(x)=π(H(x)u _ref )-u _p is the homography relationship error function, that is, the projection error function, and H(x) is the homography matrix; according to the small displacement assumption, the error function r The first-order Taylor expansion of is expressed as: <mrow> <mi>r</mi> <mrow> <mo>(</mo> <mi>&amp;xi;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>r</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <mrow> <mo>&amp;part;</mo> <mi>r</mi> </mrow> <mrow> <mo>&amp;part;</mo> <mi>&amp;xi;</mi> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <mi>&amp;xi;</mi> <mo>=</mo> <mn>0</mn> </mrow> </msub> <mi>&amp;xi;</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow> At this time, the extreme value condition of the energy function is: <mrow> <mfrac> <mrow> <mo>&amp;part;</mo> <mi>f</mi> </mrow> <mrow> <mo>&amp;part;</mo> <mi>&amp;xi;</mi> </mrow> </mfrac> <mo>=</mo> <msup> <mi>&amp;Sigma;J</mi> <mi>T</mi> </msup> <mi>r</mi> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow> which is: <mrow> <mtable> <mtr> <mtd> <mrow> <munder> <mi>&amp;Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>r</mi> <mo>(</mo> <mn>0</mn> <mo>)</mo> <mo>+</mo> <mi>J</mi> <mi>&amp;xi;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;xi;</mi> <mo>=</mo> <mo>-</mo> <msup> <mrow> <mo>(</mo> <munder> <mi>&amp;Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mi>J</mi> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <munder> <mi>&amp;Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mi>r</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow> in, The pose estimation of the mobile robot is obtained by iteratively solving the above formula; in order to further improve the positioning accuracy, robust weights are introduced to build a pose estimation model based on M-estimation. At this time, the energy function is expressed as: <mrow> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <munder> <mo>&amp;Sigma;</mo> <mi>p</mi> </munder> <mi>w</mi> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mo>|</mo> <mo>|</mo> <mi>r</mi> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow> Among them, w(r) is the robust Tukey weighting function. At this time, the extreme value condition of the energy function is: <mrow> <mi>&amp;xi;</mi> <mo>=</mo> <mo>-</mo> <msup> <mrow> <mo>(</mo> <munder> <mi>&amp;Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mi>w</mi> <mo>(</mo> <mi>r</mi> <mo>)</mo> <mi>J</mi> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <munder> <mi>&amp;Sigma;</mi> <mi>p</mi> </munder> <msup> <mi>J</mi> <mi>T</mi> </msup> <mi>w</mi> <mrow> <mo>(</mo> <mi>r</mi> <mo>)</mo> </mrow> <mi>r</mi> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow> Step 2.3, Planar Parameter Estimation The robot uses the homography relationship based on ground features to realize robot positioning, but due to the influence of non-ground features, it is difficult to ensure the positioning accuracy, so a fast non-ground feature removal method is adopted; the normal direction estimation model is constructed according to the parallel vector cross product method ;Due to the error in corner detection, the ideal cross product condition cannot be satisfied, and the fitness of the normal estimation model is poor; the projection error r is used as the judgment basis to determine the non-ground feature information; but as the distance between the non-ground feature and the observation point , the discrimination of this method is significantly reduced; compared with the above method, on the basis of the initial estimation of the pose, the normal estimation model is directly derived by using the homography matrix, and the difference between the angle and the parameters of the ground plane equation is obtained to determine the angle Whether the point is ground feature information; according to the plane homography relational formula, the estimation model of the plane parameter n _d is established: <mrow> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <munder> <mo>&amp;Sigma;</mo> <mi>p</mi> </munder> <mo>|</mo> <mo>|</mo> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>)</mo> </mrow> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow> Among them, r(n _d )=π(H(n _d )u _ref )-u _p is the corresponding projection error function; similar to the pose estimation solution process, based on the small displacement assumption, according to the first-order Taylor of the error function The solution model of formula establishment formula (18); At this time, the plane parameters are obtained by iterative formula (18): <mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>=</mo> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>+</mo> <mover> <mi>n</mi> <mo>^</mo> </mover> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mover> <mi>n</mi> <mo>^</mo> </mover> <mo>=</mo> <mo>-</mo> <msup> <mrow> <mo>(</mo> <munder> <mo>&amp;Sigma;</mo> <mi>p</mi> </munder> <msubsup> <mi>J</mi> <mi>n</mi> <mi>T</mi> </msubsup> <msub> <mi>J</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <munder> <mo>&amp;Sigma;</mo> <mi>p</mi> </munder> <msubsup> <mi>J</mi> <mi>n</mi> <mi>T</mi> </msubsup> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>n</mi> <mi>d</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow> in, Step 3, pose estimation The main function of the pose estimation module is to complete the real-time update of the camera pose; the specific implementation process is as follows; after the initialization is completed, the pose estimation model is established according to the projection error, and its mathematical description is as follows: <mrow> <mi>&amp;xi;</mi> <mo>=</mo> <munder> <mi>argmin</mi> <mi>&amp;xi;</mi> </munder> <mi>&amp;Sigma;</mi> <mi>O</mi> <mi>b</mi> <mi>j</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>|</mo> <msub> <mi>e</mi> <mi>j</mi> </msub> <mo>|</mo> </mrow> <msub> <mi>&amp;sigma;</mi> <mi>j</mi> </msub> </mfrac> <mo>,</mo> <msub> <mi>&amp;sigma;</mi> <mi>T</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow> where e _j is the projection error, and is the Tukey dual-weight objective function, σ _T is the unbiased estimate of the standard deviation of feature point matching, μ is the camera pose six-tuple description vector; For the failures in tracking, the feature observation ratio is proposed to evaluate the tracking quality of each frame image; if the ratio is lower than the set threshold, the tracking effect is poor, and the tracking will continue, but the system will not update the frame to the map In the middle; if the feature observation ratio is lower than the set threshold in the tracking of multiple frames of images, it is considered to be a "follow-up" situation, and the tracking process is initialized; the accuracy of the pose is achieved through the above steps It is estimated that the tracking process can be carried out normally; Step 4, map creation Step 4.1, map initialization The system uses the ground features and the standard stereo camera algorithm model to establish the current environment initialization map; in the map initialization process, using the FAST corner point matching relationship in the image, combined with the RANSAC algorithm, using the initial pose estimation method based on ground features, using triangulation The measurement method calculates the three-dimensional coordinates of the current feature point, and determines the starting point as the origin of the global world coordinate system, and completes the initialization of the three-dimensional sparse map; In the initial drawing stage of the map, the specific process of obtaining two key frames is as follows. The camera is set up on the mobile robot, and the initialization process starts after the camera and the ground are calibrated; during the initialization process, the mobile robot starts to move, and the camera captures the current Corner information in the scene is associated and the initial estimate of the current pose is obtained by solving equation (12); when the initial estimate exceeds the set threshold d ₀ , the pose estimation method based on ground features in 2.2 is used to obtain the current pose accurate estimate; Step 4.2, keyframe update and epipolar search When the map is initialized, it only contains two keyframes, and can only describe the adjacent small volume space. When the camera leaves the initial position, it is necessary to add new keyframes and map features; if the image frame between the camera and the current keyframe The number exceeds the threshold condition, the camera tracking effect is the best, and the distance between the camera and the nearest map key point is as small as possible, and the process of adding key frames will be performed automatically; due to real-time constraints, the tracking system may only measure the visual features in the frame subset, so the map thread needs to replan and measure the remaining map features; In the update of the key frame, the system will first perform Shi-Tomas evaluation on all the corner points in the newly added key frame to obtain the current corner point information with salient features, and the salient points near the successfully observed feature points will be discarded , the retained salient points will be used as candidate map nodes; since the new map nodes require known depth information, which cannot be obtained only by a single frame image, it is necessary to select the nearest key frame as the second view, using the epipolar line The search and block matching method establishes the sub-pixel precision mapping relationship of feature points; in the second view, the zero-mean SSD method is used, combined with the pose estimation model to achieve precise camera relocation; finally, the matching points are projected into space to generate the current global environment 3D map; Step 4.3, Optimization The optimal local and global LM cluster adjustment algorithm is used to optimize the global consistency of the current map to improve robustness; the mathematical description of the cluster adjustment algorithm is: <mrow> <mo>{</mo> <mo>{</mo> <msub> <mi>&amp;mu;</mi> <mn>2</mn> </msub> <mo>...</mo> <msub> <mi>&amp;mu;</mi> <mi>N</mi> </msub> <mo>}</mo> <mo>,</mo> <mo>{</mo> <msub> <msup> <mi>p</mi> <mo>&amp;prime;</mo> </msup> <mn>1</mn> </msub> <mo>...</mo> <msub> <msup> <mi>p</mi> <mo>&amp;prime;</mo> </msup> <mi>M</mi> </msub> <mo>}</mo> <mo>}</mo> <mo>=</mo> <munder> <mrow> <mi>arg</mi> <mi>min</mi> </mrow> <mrow> <mo>{</mo> <mo>{</mo> <mi>&amp;mu;</mi> <mo>}</mo> <mo>,</mo> <mo>{</mo> <mi>p</mi> <mo>}</mo> <mo>}</mo> </mrow> </munder> <munder> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>&amp;Element;</mo> <mi>X</mi> <mo>&amp;Element;</mo> <mi>Y</mi> </mrow> </munder> <munder> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>&amp;Element;</mo> <msub> <mi>S</mi> <mi>i</mi> </msub> </mrow> </munder> <mi>O</mi> <mi>b</mi> <mi>j</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>|</mo> <msub> <mi>e</mi> <mrow> <mi>j</mi> <mi>i</mi> </mrow> </msub> <mo>|</mo> </mrow> <msub> <mi>&amp;sigma;</mi> <mrow> <mi>j</mi> <mi>i</mi> </mrow> </msub> </mfrac> <mo>,</mo> <msub> <mi>&amp;sigma;</mi> <mi>T</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow> Among them, σ _ji is the unbiased estimate of the matching standard deviation of FAST feature points in the i-th key frame, μ _i represents the pose six-tuple description vector of the i-th key frame, and p _i is a point in the global map; Through continuous bundle adjustment, the stable cost function in formula (21) is minimized; the global bundle adjusts the poses of all key frames and the positions of map points, and at the same time utilizes the inherent sparsity in the structure-motion problem to make the overall matrix decomposition The complexity shows a cubic level of decline, from O((N+M) ³ ) to O(N ³ ), but in most estimation cases it is constrained by the generated outer point vector product. As the map grows, This calculation will increase significantly; Aiming at the above problems, local bundle adjustment is introduced, here only the subset poses of the key frames are adjusted, and the objective function at this time becomes: <mrow> <mo>{</mo> <mo>{</mo> <msub> <mi>&amp;mu;</mi> <mi>&amp;chi;</mi> </msub> <mo>&amp;Element;</mo> <mi>X</mi> <mo>}</mo> <mo>,</mo> <mo>{</mo> <msub> <msup> <mi>p</mi> <mo>&amp;prime;</mo> </msup> <mi>z</mi> </msub> <mo>&amp;Element;</mo> <mi>Z</mi> <mo>}</mo> <mo>}</mo> <mo>=</mo> <munder> <mi>argmin</mi> <mrow> <mo>{</mo> <mo>{</mo> <mi>&amp;mu;</mi> <mo>}</mo> <mo>,</mo> <mo>{</mo> <mi>p</mi> <mo>}</mo> <mo>}</mo> </mrow> </munder> <munder> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>&amp;Element;</mo> <mi>X</mi> <mo>&amp;Element;</mo> <mi>Y</mi> </mrow> </munder> <munder> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>&amp;Element;</mo> <mi>Z</mi> <mo>&amp;cap;</mo> <msub> <mi>S</mi> <mi>i</mi> </msub> </mrow> </munder> <mi>O</mi> <mi>b</mi> <mi>j</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow> X is a series of keyframes to be adjusted, Y is a fixed frame set, and Z is a subset of map points; the set X is composed of the latest keyframe and the 4 closest keyframes in the map, and all keyframes in the map The visible points form a set Z, and finally Y contains all the key frames obtained from the measurement data in Z; thus, the local cluster adjustment optimizes the pose of the latest and most recent key frames, and updates all points in the map. At the same time, the complexity is also significantly reduced, not exceeding O(NM).