A bidirectional deep visual-inertial pose estimation method combined with multi-line lidar

By combining the tight coupling and loose coupling methods of multi-line lidar and visual data, combined with IMU data, and using factor graph optimization technology, the problems of low positioning accuracy and poor robustness in the existing technology are solved, and the problem of low positioning accuracy and poor robustness in different scenarios is realized. High-precision position estimation and robustness enhancement, suitable for autonomous driving of unmanned vehicles.

CN114966734BActive Publication Date: 2024-11-08HANGZHOU YUJIASHAN INTELLIGENT CONTROL TECHNOLOGY CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210479056.4
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-04-28
Publication Date
2024-11-08
Estimated Expiration
2042-04-28

AI Technical Summary

Technical Problem

The prior art has low positioning accuracy and poor robustness in indoor and tunnel environments, making it difficult to adapt to different scenario needs, especially when the GPS signal is lacking.

Method used

Using a two-way depth visual inertial pose estimation method combined with multiline lidar, the pose estimation and closed-loop detection are performed through the tight coupling and loose coupling of the lidar point cloud and the visual image, using factor graph optimization technology and combining IMU data. Improve positioning accuracy and robustness.

Benefits of technology

It realizes high-precision pose estimation under the absence of GPS signal, improves robustness, and is suitable for different scenarios, especially in environments with poor lighting and missing textures, and enhances the positioning ability of autonomous driving of unmanned vehicles.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114966734B_ABST
    Figure CN114966734B_ABST
Patent Text Reader

Abstract

The present invention discloses a bidirectional deep visual inertial pose estimation method combined with a multi-line laser radar, comprising: after the bidirectional deep visual image uses the laser radar point cloud to restore the depth information of the feature point, it is tightly coupled with the IMU data respectively to obtain two pose estimates, and the two pose estimates are weightedly fused to obtain a visual odometer; the visual odometer or IMU estimation result is used as a priori pose estimation, and the prior pose estimation is iteratively optimized in combination with the laser point line residual to obtain a laser odometer; the bidirectional deep visual image and the laser radar point cloud are respectively closed-loop tested, and if the closed-loop verification is valid, the relative pose transformation relationship between the laser key frames corresponding to the current moment and the closed-loop moment is used as a closed-loop constraint; the laser odometer constraint, the visual odometer constraint and the closed-loop constraint are used as constraint items, and factor graph optimization is performed to obtain the optimal pose estimation. The present invention has high-precision and robust pose estimation in scenes such as poor lighting and texture loss.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the field of mobile robot technology and multi-sensor fusion positioning, and more specifically, relates to a bidirectional deep visual-inertial pose estimation method combined with a multi-line laser radar. Background Art

[0002] Traditional outdoor positioning methods are mainly based on global position signals. In urban environments, the positioning accuracy of GPS can reach 5-10m, which meets the requirements of rough positioning, but it is far from enough for mobile robots that require high-precision posture, such as unmanned car autonomous driving.

[0003] GPS is prone to signal loss in heavily obstructed environments such as indoors and tunnels, and cannot provide stable positioning information, and its robustness is not high. In the absence of global position signals such as GPS and Beidou or when the signals are weak, the robot's posture estimation needs to be achieved with the help of sensors such as lidar, cameras, and IMU.

[0004] Visual SLAM is prone to tracking failure in scenes with poor lighting conditions and no textures due to the small number of effective features detected. At present, the fusion of vision and IMU is a common way to improve the robustness of the algorithm. IMU can provide short-term pose estimation when vision fails, but if the robot is in uniform motion or pure rotation for a long time, the positioning accuracy and robustness of the robot will decrease. Laser SLAM technology is mature, the map construction is clear and intuitive, and the positioning accuracy is high. It is currently widely used in the field of positioning and navigation, but it is prone to failure in unstructured environments such as tunnels, corridors, and open spaces. Both laser SLAM and visual SLAM have their advantages and disadvantages, and the positioning accuracy and robustness need to be improved to meet the needs of different scenarios.

[0005] It can be seen that the existing technology has the technical problems of low positioning accuracy, poor robustness and difficulty in adapting to the application requirements of different scenarios. Based on the advantages and disadvantages of various sensors, the present invention integrates multiple sensors for SLAM, which is a feasible research direction. Summary of the invention

[0006] In response to the above defects or improvement needs of the prior art, the present invention provides a bidirectional deep visual-inertial pose estimation method combined with a multi-line lidar, thereby solving the technical problems of the prior art such as low positioning accuracy, poor robustness and difficulty in adapting to application requirements of different scenarios.

[0007] To achieve the above object, according to one aspect of the present invention, a bidirectional deep visual-inertial pose estimation method combined with a multi-line laser radar is provided, comprising:

[0008] After using the lidar point cloud to restore the depth information of the feature points in the bidirectional deep visual image, the bidirectional deep visual image is tightly coupled with the IMU data to obtain two pose estimates, and the two pose estimates are weightedly fused to obtain the visual odometer.

[0009] Extract feature points from the laser radar point cloud, construct laser point line residuals, use the visual odometer or IMU estimation results as prior pose estimation, combine the prior pose estimation with the laser point line residuals, construct the Gauss-Newton equation, iteratively optimize the Gauss-Newton equation, and obtain the laser odometer;

[0010] Perform closed-loop detection on the bidirectional depth vision image and the LiDAR point cloud respectively, and verify all closed-loop detection results using LiDAR point cloud registration. If the closed-loop verification is valid, the relative position transformation relationship between the laser keyframes corresponding to the current moment and the closed-loop moment is used as the closed-loop constraint.

[0011] The pose transformation relationship between the visual odometry corresponding to adjacent visual depth image key frames is taken as the visual odometry constraint, and the pose transformation relationship corresponding to the laser odometry between adjacent laser key frames is taken as the laser odometry constraint. The laser odometry constraint, visual odometry constraint and closed-loop constraint are taken as constraint items, and factor graph optimization is performed to obtain the optimal pose estimation.

[0012] Furthermore, the verification of the closed-loop detection result includes:

[0013] Perform closed-loop detection on the bidirectional deep vision image, obtain two timestamps of the current moment and the closed-loop moment, search for two laser key frames corresponding to the two timestamps in the historical laser key frame queue, construct local feature point clouds for the two laser key frames respectively, and perform point cloud registration on the two local feature point clouds. If the registration is successful, the visual closed-loop is valid;

[0014] Local feature point clouds are constructed for the laser key frame corresponding to the current moment in the closed-loop detection of the laser radar point cloud and the laser key frame corresponding to the closed-loop moment, and point cloud registration is performed on the two local feature point clouds. If the registration is successful, the laser closed-loop is valid.

[0015] Furthermore, the closed-loop visual keyframes detected by the bidirectional depth vision image closed-loop meet the requirements of temporal consistency check and spatial consistency check. The temporal consistency check means that the interval between the current closed-loop moment and the previous closed-loop moment is greater than a preset time threshold, and the spatial consistency check means that when the closed-loop occurs, the distance between the current depth vision image keyframe and the closed-loop visual keyframe is less than a preset distance.

[0016] Furthermore, the closed-loop detection includes:

[0017] For each directional deep vision image in the bidirectional deep vision image, the similarity between the deep vision image and the word bag is calculated, and the deep vision image with the greatest similarity is used as a closed-loop visual key frame;

[0018] Get the positions of all historical laser keyframes, build a 3D Kd-tree, and search in the Kd-tree according to the current laser keyframe pose at regular intervals to find the nearest neighbor frame to the current laser keyframe; if the position distance between the current laser keyframe and the neighbor frame is less than the set threshold, then the neighbor frame is used as the laser keyframe corresponding to the closed-loop moment.

[0019] Furthermore, the depth information of the feature points in the bidirectional depth vision image is restored by:

[0020] A local point cloud set is constructed by dedistorting the lidar point cloud, and the feature points in the bidirectional depth vision image are restored by finding matching depth information in the local point cloud set. If matching points are missing, the depth information of the depth camera that collects the bidirectional depth vision image is used for restoration.

[0021] Furthermore, the local point cloud set is constructed in the following manner:

[0022] The dedistorted LiDAR point clouds are stacked up, the LiDAR point clouds in the adjacent time range are retained, and the old LiDAR point clouds are discarded to obtain a local point cloud set. The adjacent time range is dynamically updated according to the translation increment and rotation increment:

[0023] TimeSet=Tmax*(1-(μ*deltaA / Amax+η*deltaP / Pmax))

[0024] Among them, TimeSet is the time length for maintaining the local point cloud set, that is, the adjacent time range, Tmax is the maximum value of TimeSet, μ and η are the proportional coefficients of the rotation increment deltaA and the translation increment deItaP, respectively, and Amax and Pmax are the maximum values ​​of the angle increment and the rotation increment, respectively.

[0025] Furthermore, the method of iterative optimization combined with laser point line residual is:

[0026] The curvature of the laser point is calculated by the square of the sum of the distance deviations between the five previous and next points and the current point, and the laser feature points, including corner points and plane points, are extracted according to the curvature of the laser point.

[0027] Corner feature map and plane feature map maintained by laser keyframe based on sliding window;

[0028] For the corner point corresponding to the current laser frame, the k-nearest neighbor search method is used to find the five corner points adjacent to the point in the corner point feature map; if the distances between these five points and the reference point are all less than the threshold and are approximately collinear, the corner point is considered to match the feature map;

[0029] For the plane point corresponding to the current laser frame, find the 5 closest points in the plane feature map; if the distances between these 5 points and the reference point are all less than the threshold and are approximately coplanar, the plane point is considered to match the plane feature map;

[0030] For the two types of matched feature points, the distances from the feature points to the target straight line features and the target plane features are used as observation values ​​to establish the Gauss-Newton equation, and the relatively high-frequency visual odometry or IMU odometry is used as a priori estimation. The laser odometry is obtained by minimizing the distance from the detected line and surface features to the feature map.

[0031] Furthermore, the calculation of the visual odometer includes:

[0032] After using the lidar point cloud to restore the depth information of the feature points in the bidirectional depth vision image, the bidirectional depth vision images are each jointly optimized by the visual reprojection error and the IMU measurement error, and two pose estimates are obtained based on the sliding window optimization method, and the visual odometry is obtained by weighted fusion.

[0033] Furthermore, the factor graph optimization further includes:

[0034] An IMU constraint is constructed based on the IMU data between the current laser key frame and the previous laser key frame. The IMU constraint includes an IMU pre-integration factor and an IMU bias factor, wherein the IMU pre-integration factor is constructed by the acceleration and angular velocity data of the IMU, and the IMU bias factor is constructed by the IMU bias. The laser odometer constraint, the visual odometer constraint, the closed-loop constraint and the IMU constraint are used as constraint items, and factor graph optimization is performed to obtain the optimal pose estimation and bias of the current laser key frame.

[0035] Furthermore, the method further comprises:

[0036] The IMU data after the current laser key frame moment is re-integrated with the bias after factor graph optimization, and then the IMU estimation result at each moment after the current laser key frame moment is obtained, and the IMU estimation result is used as the initial value of the visual odometry pose estimation.

[0037] The IMU estimation result is the robot posture estimation result at the IMU frequency.

[0038] According to another aspect of the present invention, there is provided a bidirectional depth visual-inertial pose estimation system combined with a multi-line laser radar, comprising: a dual depth camera, a multi-line laser radar, an IMU sensor and a processor;

[0039] The dual depth camera is used to collect bidirectional depth vision images;

[0040] The multi-line laser radar is used to collect laser radar point clouds;

[0041] The IMU sensor is used to collect IMU data;

[0042] The processor communicates with the dual depth camera, multi-line laser radar, and IMU sensor, including:

[0043] The visual odometry estimator is used to restore the depth information of feature points in the bidirectional deep visual image using the lidar point cloud. The bidirectional deep visual image is then optimized by jointly optimizing the visual reprojection error and the IMU measurement error, and two pose estimates are obtained based on the sliding window optimization method. The visual odometry is then obtained by weighted fusion.

[0044] Laser odometry estimator, used to extract feature points from the laser radar point cloud, construct laser point line residuals, use the visual odometry or IMU estimation results as prior pose estimation, combine the prior pose estimation with the laser point line residuals, construct the Gauss-Newton equation, iteratively optimize the Gauss-Newton equation, and obtain the laser odometry;

[0045] The closed-loop detection module is used to perform closed-loop detection on the bidirectional depth vision image and the lidar point cloud respectively, and verify all closed-loop detection results using the lidar point cloud registration. If the closed-loop verification is valid, the relative position transformation relationship between the laser keyframes corresponding to the current moment and the closed-loop moment is used as the closed-loop constraint;

[0046] The factor graph optimization module is used to use the pose transformation relationship between the visual odometry corresponding to adjacent visual depth image key frames as the visual odometry constraint, the pose transformation relationship corresponding to the laser odometry between adjacent laser key frames as the laser odometry constraint, and the laser odometry constraint, visual odometry constraint and closed-loop constraint as constraint items to perform factor graph optimization and obtain the optimal pose estimation.

[0047] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects compared with the prior art:

[0048] (1) The present invention uses bidirectional deep vision images for pose estimation to avoid closed-loop missed detection due to limited visual angles and single-view feature loss. The depth information of feature points in bidirectional deep vision images is restored using laser radar point clouds. Laser radar and vision are tightly coupled at the data level. The depth information of the laser radar point cloud is used to assist in restoring the depth of visual feature points. In addition, the visual odometer or IMU estimation result is used as a priori pose estimation to calculate the laser odometer, which is loosely coupled. The loosely coupled method has the advantages of simple structure, low computational cost and strong scalability. The prior pose estimation has two options to share the risk of failure. The present invention combines tight coupling and loose coupling to ensure positioning accuracy and has a risk avoidance strategy. It can accurately locate even in the absence of global position signals such as GPS, GNSS, and Beidou. The present invention combines vision, laser and closed-loop data for factor graph optimization, and adopts multi-data fusion. It still has high-precision and robust pose estimation in scenes such as poor lighting and texture loss, and achieves accurate positioning. It can be applied to autonomous driving of unmanned vehicles.

[0049] (2) The present invention combines bidirectional depth vision and multi-line laser radar for closed-loop detection. After the visual closed-loop detection is successful, secondary confirmation is performed through laser radar point cloud registration to eliminate repeated closed-loop detection and false detection existing in the vision, thereby improving the accuracy and recall rate of closed-loop detection.

[0050] (3) To improve the overall efficiency of the algorithm, the selection of visual closed-loop frames needs to be checked for temporal consistency and spatial consistency to avoid the situation where the front and rear cameras detect closed loops at the same time or detect closed loops continuously in a short period of time. When the forward or backward visual features are missing and it is not convenient to perform closed loop detection, the image from another perspective can play a compensatory role, thereby optimizing the key frame pose with the help of closed loops.

[0051] (4) The laser radar will also perform laser closed-loop detection based on the odometer neighbor detection. The present invention closes the loop of vision and laser, ensures the validity of the closed-loop keyframe, and verifies the validity of the closed-loop, thereby accurately detecting the historical closed-loop and optimizing the closed-loop, providing a globally consistent and smooth point cloud map for subsequent positioning methods under known maps.

[0052] (5) The present invention integrates bidirectional depth vision with multi-line laser radar at the data level, dynamically updates the local point cloud set, and associates the visual feature points with the laser point cloud for depth recovery, thereby improving the accuracy and computational efficiency of depth recovery of visual feature points. The present invention dynamically updates the adjacent time range according to the translation increment and rotation increment, thereby changing the density of the point cloud set.

[0053] (6) As the system runs longer, if the poses and features of all key frames are considered at the same time, the computational efficiency will gradually decrease and even fail to run stably in real time. In order to save the optimization calculation of a large number of feature points, the present invention constructs odometer constraints based on the pose transformation relationship between key frames. Through factor graph joint optimization, a high-precision and robust pose estimation is obtained.

[0054] (7) The bidirectional deep vision images are each optimized through the joint optimization of the visual reprojection error and the IMU measurement error, and two pose estimates are obtained based on the sliding window optimization method. The visual odometry is then obtained by weighted fusion to avoid the loss of a single visual feature or tracking failure, which would lead to a reduction in the system positioning accuracy and reliability.

[0055] (8) The present invention introduces IMU constraints in the factor graph optimization process, and optimizes and updates the pose and IMU bias by combining laser odometer constraints, visual odometer constraints, closed-loop constraints, and IMU constraints. Optimizing the IMU bias can suppress the drift problem existing in pre-integration.

[0056] (9) The present invention re-performs IMU pre-integration on the IMU data after the current laser key frame moment with the bias amount after factor graph optimization. The improvement of IMU pre-integration accuracy can provide a more accurate initial value estimate for the subsequent laser odometer. The IMU estimation result is then used as the initial value of the pose estimation of the visual odometer, and then the new visual odometer and IMU estimation results are used to calculate the laser odometer, and vision and laser limit the deviation for the IMU. Under the condition of bias limitation, the odometer of the IMU is very accurate, which further expands the applicable scenarios of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 is a flow chart of a bidirectional deep visual-inertial pose estimation method combined with a multi-line laser radar provided in an embodiment of the present invention;

[0058] Figure 2 This is a core architecture diagram of a bidirectional deep visual-inertial pose estimation system combined with a multi-line laser radar provided in an embodiment of the present invention;

[0059] Figure 3 (a) is a dense map of three-dimensional point clouds formed by the KITTI dataset provided in an embodiment of the present invention;

[0060] Figure 3 (b) is a dense three-dimensional point cloud map formed by the M2DGR data set provided by an embodiment of the present invention;

[0061] Figure 4 (a) is a state diagram before closed-loop correction provided by an embodiment of the present invention;

[0062] Figure 4(b) is a state diagram after closed-loop correction provided by an embodiment of the present invention;

[0063] Figure 5 is a robot data flow diagram provided by an embodiment of the present invention;

[0064] Figure 6 This is a ROS system node diagram provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0065] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0066] The present invention provides a bidirectional deep visual inertial pose estimation method combined with a multi-line laser radar, which can be applied in urban streets, corridors, indoor environments, etc. to achieve high-precision pose estimation, and still has high robustness in environments with poor lighting conditions. The purpose of the present invention is to improve the accuracy and robustness of robot pose estimation in the absence of global position signals such as GPS and Beidou.

[0067] In the absence of global position signals, the present invention provides a pose estimation method based on multi-line laser radar, bidirectional depth vision and IMU fusion. The overall process of the method is as follows: Figure 1 As shown, including:

[0068] After using the lidar point cloud to restore the depth information of the feature points in the bidirectional deep visual image, the bidirectional deep visual image is tightly coupled with the IMU data to obtain two pose estimates, and the two pose estimates are weightedly fused to obtain the visual odometer.

[0069] Extract feature points from the laser radar point cloud, construct laser point line residuals, use the visual odometer or IMU estimation results as prior pose estimation, combine the prior pose estimation with the laser point line residuals, construct the Gauss-Newton equation, iteratively optimize the Gauss-Newton equation, and obtain the laser odometer;

[0070] Perform closed-loop detection on the bidirectional depth vision image and the LiDAR point cloud respectively, and verify all closed-loop detection results using LiDAR point cloud registration. If the closed-loop verification is valid, the relative position transformation relationship between the laser keyframes corresponding to the current moment and the closed-loop moment is used as the closed-loop constraint.

[0071] The pose transformation relationship between the visual odometry corresponding to adjacent visual depth image key frames is taken as the visual odometry constraint, and the pose transformation relationship corresponding to the laser odometry between adjacent laser key frames is taken as the laser odometry constraint. The laser odometry constraint, visual odometry constraint and closed-loop constraint are taken as constraint items, and factor graph optimization is performed to obtain the optimal pose estimation.

[0072] like Figure 2 As shown, a bidirectional deep visual inertial pose estimation system combined with a multi-line laser radar comprises: a dual-depth camera, a multi-line laser radar, an IMU sensor and a processor;

[0073] The dual depth camera is used to collect bidirectional depth vision images;

[0074] The multi-line laser radar is used to collect laser radar point clouds;

[0075] The IMU sensor is used to collect IMU data;

[0076] The processor communicates with the dual depth camera, multi-line laser radar, and IMU sensor, including:

[0077] The visual odometry estimator is used to restore the depth information of feature points in the bidirectional deep visual image using the lidar point cloud, and then tightly couple the bidirectional deep visual image with the IMU data to obtain two pose estimates, and then weightedly fuse the two pose estimates to obtain the visual odometry.

[0078] Laser odometry estimator, used to extract feature points from the laser radar point cloud, construct laser point line residuals, use the visual odometry or IMU estimation results as prior pose estimation, combine the prior pose estimation with the laser point line residuals, construct the Gauss-Newton equation, iteratively optimize the Gauss-Newton equation, and obtain the laser odometry;

[0079] The closed-loop detection module is used to perform closed-loop detection on the bidirectional depth vision image and the lidar point cloud respectively, and verify all closed-loop detection results using the lidar point cloud registration. If the closed-loop verification is valid, the relative position transformation relationship between the laser keyframes corresponding to the current moment and the closed-loop moment is used as the closed-loop constraint;

[0080] The factor graph optimization module is used to use the pose transformation relationship between the visual odometry corresponding to adjacent visual depth image key frames as the visual odometry constraint, the pose transformation relationship corresponding to the laser odometry between adjacent laser key frames as the laser odometry constraint, and the laser odometry constraint, visual odometry constraint and closed-loop constraint as constraint items to perform factor graph optimization and obtain the optimal pose estimation.

[0081] Specifically, the visual odometry is calculated as follows:

[0082] A local point cloud set is constructed by dedistorting the lidar point cloud, and the feature points in the bidirectional depth vision image are restored by finding matching depth information in the local point cloud set. If matching points are missing, the depth information of the depth camera that collects the bidirectional depth vision image is used for restoration.

[0083] The LiDAR point cloud is combined with the IMU for dedistortion to obtain the dedistorted LiDAR point cloud.

[0084] The local point cloud set is constructed in the following way:

[0085] The dedistorted LiDAR point clouds are stacked up, the LiDAR point clouds in the adjacent time range are retained, and the old LiDAR point clouds are discarded to obtain a local point cloud set. The adjacent time range is dynamically updated according to the translation increment and rotation increment to change the density of the point cloud set. The calculation formula of the adjacent time range is:

[0086] TimeSet=Tmax*(1-(μ*deltaA / Amax+η*deltaP / Pmax))

[0087] Among them, TimeSet is the time length for maintaining the local point cloud set, that is, the adjacent time range, Tmax is the maximum value of TimeSet, μ and η are the proportional coefficients of the rotation increment deltaA and the translation increment deltaP, respectively, and Amax and Pmax are the maximum values ​​of the angle increment and the rotation increment, respectively.

[0088] The bidirectional deep vision front end performs adaptive mean histogram processing on the input image to improve the contrast and clarity of the image, and then extracts Harris corner features. The KLT optical flow tracking algorithm is used to track the feature points of the previous frame, find matching points on the frame, and record the status of the feature points, so as to eliminate the feature points that failed to track and update the number of times the feature points were tracked. According to the number of times the feature points are tracked, the feature points with the most tracked times are retained as much as possible, and the feature points on the entire image are homogenized. By setting the minimum distance between adjacent feature points, the feature points in the texture-rich area of ​​the image are prevented from being too dense. Due to the elimination and homogenization of feature points, as well as the reduction in the number of feature points in the current frame during motion, it is necessary to perform additional extraction based on the number of feature points to increase the number of feature points to meet the basic requirements of the subsequent algorithm. At the same time, the basic matrix is ​​calculated based on the feature points tracked in two adjacent frames, and the abnormal points are eliminated by the RANSAC algorithm.

[0089] The bidirectional deep vision images are each optimized through the joint optimization of the visual reprojection error and the IMU measurement error, and two pose estimates are obtained based on the sliding window optimization method. The visual odometry is then obtained by weighted fusion to avoid the loss of a single visual feature or tracking failure, which may lead to a reduction in the positioning accuracy and reliability of the system. The visual odometry can be used as the initial value of the laser registration to improve the registration accuracy.

[0090] The bidirectional depth vision image includes a forward depth vision image and a backward depth vision image. The depth information of the feature points in the forward depth vision image and the backward depth vision image are restored by the following steps, taking the forward depth vision image and the backward depth vision image as an example:

[0091] (1) Dynamically maintain the local point cloud set and change the density of the point cloud set according to the rotation increment and translation increment of the mobile platform;

[0092] (2) Convert the local point cloud set to the camera coordinate system, and project the feature points in the forward depth vision image and the lidar point cloud onto a unit sphere centered on the forward depth camera;

[0093] (3) For each feature point in the forward depth vision image, a 2D kd tree is searched based on the polar coordinates of the feature point in the forward depth vision image to obtain the three lidar point clouds closest to the feature point on the sphere;

[0094] (4) If the maximum distance between the three lidar point clouds is greater than 1.5 m, it is considered that there is no information in the local point cloud set that matches the feature points in the forward depth vision image, and it is necessary to use monocular triangulation or binocular stereo matching methods to restore the depth information, otherwise jump to step (5);

[0095] (5) Combine the three lidar point clouds and obtain the depth value of the feature point through interpolation calculation.

[0096] Since the system continuously collects sensor data during operation, if the sensor measurements at all times are considered, the amount of calculation required for data preprocessing and optimization is too large. In order to reduce the amount of calculation, the image key frame data is processed based on the sliding window optimization method, and only the key frames within the window are considered during the optimization process. In essence, the visual inertial tightly coupled nonlinear optimization problem is to find the Jacobian matrix and covariance matrix of the vision and IMU pre-integration. As shown in formula (1), the state vector to be optimized involved in this problem includes the state of the IMU in the sliding window, including PVQ (position, velocity, rotation), Bias, and the external parameters from the camera to the IMU and the inverse depth of the landmark points.

[0097]

[0098] In the above formula, xi Represents the IMU state corresponding to the i-th frame image, and Represents position, direction and speed respectively, b a and b g Denote the bias of the accelerometer and gyroscope, λ m represents the inverse depth of the mth 3D feature point, n represents the starting index of the IMU state in the sliding window, N represents the number of key frames contained in the window, m represents the starting index of the midpoint feature state variable in the window, and M represents the number of feature points in the window. Represents the external parameters from camera to IMU.

[0099] The nonlinear optimization problem is solved by minimizing the sum of multiple errors. The cost function that needs to be constructed for system optimization is shown in formula (2):

[0100]

[0101] in, It represents the IMU residual term, represents the visual observation residual term, and ρ is the robust kernel function. p , H P} is a marginalized prior constraint item, which can retain the influence of inter-frame constraints on the current state.

[0102] The calculation of laser odometry includes:

[0103] The extraction of laser feature points is based on the curvature of the laser point. The curvature of each laser point is calculated for the dedistorted laser point cloud. As shown in formula (3), the curvature of the laser point is calculated by the square of the sum of the distance deviations between the five previous and next points and the current point. If the points near the laser point are approximately on the same sphere, the calculated curvature should be close to 0.

[0104]

[0105] Similar to the extraction of visual feature points, the laser feature points also need to be homogenized, and the laser point cloud is divided into several segments. Each sub-segment of the laser point cloud takes a maximum of 30 corner points. The selection of corner points gives priority to points with large curvature values, while non-corner points are considered to be plane points. The corner points and plane points are added to the corner point and plane point cloud collections respectively, and the plane point cloud collection is downsampled. Finally, the point cloud information of the current laser frame after feature extraction is published on the ROS platform, which contains laser point cloud, initial pose, feature point cloud and other data. The point cloud information will be subscribed and used in the backend of the algorithm.

[0106] The laser odometry only uses the laser radar keyframes, and the rest of the frames are ignored. The map for laser frame matching uses a feature map maintained by the laser keyframe based on a sliding window to reduce computational complexity. After point cloud dedistortion and point cloud feature extraction, the laser odometry is obtained by minimizing the distance from the detected line and surface features to the feature map with a relatively high-frequency visual odometry or IMU estimation as the prior estimate. Among them, the prior estimate gives higher priority to the visual odometry. The visual inertial subsystem is prone to failure due to insufficient number of tracked feature points in environments such as fast movement, poor lighting conditions, and texture loss. When a single visual sensor fails, the other module can still work normally, thereby correctly estimating the IMU Bias to avoid Bias estimation errors affecting the pose estimation of the entire system. The conditions for determining the failure of the visual sensor are too few tracked feature points, the Bias exceeds the threshold, the rotation increment, or the vehicle speed is too fast. When the failure condition is met, the visual initialization is re-performed. When both the front and rear bidirectional deep vision systems fail, the laser odometry will not use the visual odometry as a priori when estimating.

[0107] The i-th laser keyframe is expressed as in is the corner feature, is a plane feature. Local feature map M i The poses of the first n key frames {T i-n , ..., T i} and the features of the first n+1 key frames {K i-n , ..., K i}, where M i Includes corner feature map and planar feature maps When the latest radar key frame K i+1 When it arrives, match the frame with the local feature map M i , construct point-line and point-surface constraints. By minimizing the total constraints, the pose T of the latest frame is obtained. i+1 .

[0108] Before optimization, the feature points corresponding to the lidar keyframe are converted to the coordinate system of the feature map and defined as is the coordinate of the i-th feature point in the k+1-th laser key frame in the feature map coordinate system. The optimization method adopts the Gauss-Newton method, and its iterative method is shown in formula (4). The residual equation is constructed based on the point-line and point-surface residuals for optimization.

[0109]

[0110] Among them, f is the residual equation, J is the Jacobian matrix of the residual term relative to the state variable to be optimized, and ΔP is the increment of the variable to be optimized, including the translation and rotation parts.

[0111] Before using the Gauss-Newton method to solve, it is necessary to construct the residual equation. Extract corner features separately and corner feature maps Matching features, and plane features and planar feature maps Matching features and constructing residual constraints.

[0112] For the corner feature corresponding to the current laser frame, the PCL library's k-nearest neighbor search method is used to search for corner features on the map. Find the five corner points adjacent to the point in the map. If the distances between these five points and the reference point are all less than the threshold and are approximately collinear, then the corner point feature is considered to be consistent with the map. Matching. The residual calculation of the corner point is essentially to find the distance from the point to the corresponding feature line on the feature map, as shown in formula (5):

[0113]

[0114] in, and are two points on the feature line corresponding to the corner feature and the feature map.

[0115] For the plane feature corresponding to the current laser frame, Find the nearest 5 points in . If the distances between these 5 points and the reference point are all less than the threshold and they are approximately coplanar, then the plane feature is considered to be consistent with the map. Matching. The residual calculation of the plane feature point is essentially to find the distance from the point to the target plane feature on the feature map, as shown in formula (6):

[0116]

[0117] For the two types of matched feature points, the Jacobian matrix is ​​calculated. The distances from the feature points to the target straight line features and the target plane features are used as observation values. The Gauss-Newton equation is established and the current pose is iteratively optimized according to formula (4), thereby obtaining the laser odometer after laser calibration.

[0118] Factor graph optimization includes:

[0119] As the system runs longer, if the poses and features of all keyframes are considered at the same time, the computational efficiency will gradually decrease and may even fail to run stably in real time. In order to save the optimization calculation of a large number of feature points, the odometry constraints are constructed based on the pose transformation relationship between the keyframes. For adjacent laser keyframes, the laser odometry factor (i.e., laser odometry constraint) is constructed based on the pose transformation relationship between them; for adjacent visual keyframes, the visual odometry factor (i.e., visual odometry constraint) is constructed based on the pose transformation relationship between them; for closed-loop keyframes, the pose transformation relationship between the laser keyframe at the current moment and the laser keyframe at the closed-loop moment is obtained through ICP registration, and the closed-loop factor (closed-loop constraint) is constructed based on this.

[0120] Furthermore, the odometry factor can be uniformly defined as:

[0121]

[0122] In the above formula, and represents the relative position relationship between the i-th and j-th key frames, r ij Represents the odometry factor.

[0123] The objective function of the entire optimization problem is as follows:

[0124]

[0125] In the above formula, T represents the historical keyframe pose set, O, V and L represent the laser odometry factor set, visual odometry factor set and closed-loop factor set respectively, and h represents the Huber function that limits the error to be too large, which is used to avoid false detection of closed-loop and cause excessive impact on pose optimization.

[0126] Furthermore, the above problem is solved based on the iSAM2 optimizer to obtain the optimized laser keyframe pose. According to the IMU data between the current laser keyframe and the previous laser keyframe, the IMU pre-integration factor and IMU bias factor are calculated and added, wherein the IMU pre-integration factor is constructed by the acceleration and angular velocity data of the IMU, and the IMU bias factor is constructed by the IMU bias, further optimizing the state of the current laser keyframe, including the current frame pose, velocity and IMU bias. Specifically, the factor graph is optimized by the iSAM2 optimizer in the open source nonlinear optimization library Gtsam to update the robot pose and bias. Optimizing the IMU bias can suppress the drift problem existing in pre-integration.

[0127] Furthermore, for the optimized laser odometry, the IMU data after the current frame time is re-integrated with the latest IMU bias. The improvement of IMU pre-integration accuracy can provide a more accurate initial value estimate for the laser odometry. For the IMU data after the corresponding moment of the optimized laser odometry, based on the state and bias corresponding to the moment, according to the IMU pre-integration amount, the state of the IMU at each moment is calculated, and the IMU odometry at each moment (i.e., the IMU estimation result) is obtained, thereby realizing the parallel estimation of the visual odometry, laser odometry, and IMU odometry. The IMU odometry is used as the initial value of the pose estimation of the visual odometry, and the visual odometry and the IMU odometry are used as the matching initial values ​​of the laser odometry, while the vision and laser limit the deviation of the IMU. Under the condition of bias limitation, the odometry of the IMU is very accurate.

[0128] On the basis of obtaining high-precision pose, the feature point cloud of the laser keyframe is registered to the environment map to build a 3D scene map of the laser data. Taking the KITTI 05 sequence and the M2DGR street 04 sequence dataset as examples, the map construction effect is as follows: Figure 3 (a) and Figure 3 As shown in (b), the present invention can establish a globally consistent and smooth point cloud map that truly reflects the three-dimensional structure of the actual scene.

[0129] Closed-loop detection includes:

[0130] Visual loop closure detection uses the DBoW2 method. When a new image keyframe is added, the Brief feature descriptor is used to describe all feature points of the image. Since the number of Harris corner points extracted by the visual front end is small, 500 FAST feature points need to be extracted and the Brief descriptor is calculated for loop closure detection. The similarity score between the current image keyframe and the bag of words is calculated, and the closed-loop visual keyframe is returned based on the similarity. The current frame is matched with the candidate closed-loop keyframe through the Brief descriptor, and the outliers are removed using the RANSAC method. When the number of matching point pairs exceeds the threshold, the closed-loop candidate frame is considered valid.

[0131] The visual part uses dual cameras to detect closed loops simultaneously. In order to improve the overall operating efficiency of the algorithm, the selection of visual closed loop frames requires time consistency checks and spatial consistency checks to avoid the situation where the front and rear cameras detect closed loops at the same time or detect closed loops continuously in a short period of time. The time consistency check means that the interval between the current visual closed loop time and the last closed loop time must be greater than the set time threshold, and the spatial consistency check means that when a closed loop occurs, the distance between the current image key frame and the closed loop frame must be less than the set distance.

[0132] In actual scenarios, when the lack of forward or backward visual features makes it difficult to perform closed-loop detection, a camera from another perspective can make up for it, thereby optimizing the historical keyframe pose with the help of closed loops. When vision successfully detects a closed loop, the information containing the current image frame and the closed-loop frame timestamp is published in the form of a ROS Topic. The laser inertial subsystem performs secondary confirmation of the laser closed loop based on the closed-loop timestamp information published by vision.

[0133] Perform closed-loop detection on the bidirectional deep vision image, obtain two timestamps of the current moment and the closed-loop moment, search for two laser key frames corresponding to the two timestamps in the historical laser key frame queue, construct local feature point clouds for the two laser key frames respectively, and perform point cloud registration on the two local feature point clouds. If the registration is successful, the visual closed-loop is valid;

[0134] Local feature point clouds are constructed for the laser key frame corresponding to the current moment in the closed-loop detection of the laser radar point cloud and the laser key frame corresponding to the closed-loop moment, and point cloud registration is performed on the two local feature point clouds. If the registration is successful, the laser closed-loop is valid.

[0135] In order to improve the efficiency of point cloud registration, the two local point clouds are subjected to ICP point cloud registration. When the ICP iteration converges and the matching score meets the threshold requirement, the closed loop is considered valid. Figure 4 (a) is a state diagram before closed-loop correction provided by an embodiment of the present invention. Figure 4 (b) is a state diagram after closed-loop correction provided by an embodiment of the present invention, which illustrates that the closed-loop correction effect of the present invention is good.

[0136] In an embodiment of the present invention, experiments are conducted on the public benchmark datasets KITTI and M2DGR sequence datasets to verify the effect of laser radar on the secondary verification of visual closed loop. The closed loop detection of bidirectional deep vision is verified based on laser point cloud registration. The results are shown in Table 1. The recorded data only contains data when the visual closed loop detection is successful. The format of the data elements in the table is: (id1-id2), where id1 and id2 represent the ID numbers of the laser key frame and the closed loop laser key frame corresponding to the visual closed loop information, respectively.

[0137] Table 1. Closed-loop confirmation of laser vision combination

[0138]

[0139] In Table 1, after the laser point cloud registration for secondary confirmation, the closed-loop failure reasons corresponding to No. 1 and No. 3 are that both the visual closed-loop and the laser closed-loop were successfully detected near the current position, and there is no need to repeat the calculation of the closed-loop factor. The failure reason corresponding to No. 2 in the table is that the score of the point cloud registration is greater than the set closed-loop threshold, so it is necessary to discard this type of closed-loop information and regard it as a closed-loop false detection. Based on the verification results, it can be concluded that vision has closed-loop false detections in some scenes, and in the lidar closed-loop inspection, point cloud registration can be used to effectively eliminate such false detections and improve the accuracy and recall of closed-loop detection.

[0140] like Figure 5 As shown, a measuring device installed on a mobile robot includes a multi-line laser radar, a dual-depth camera and an IMU sensor; the sensor is installed on a double-layer support frame, the bottom of the support frame is fixed on the robot mobile platform, and a 24V to 220V inverter is fixed on the side of the platform; the motor driver of the robot chassis adopts an AC servo motor driver, the upper layer adopts a high-performance laptop as a computing platform, the laser radar is connected through the network port, the camera is connected through the USB port, and the lower layer adopts an embedded control board as the main control board, which communicates with the upper platform through the serial port.

[0141] Specifically, a measuring device installed on a mobile robot includes: a Leishen 32-line laser radar, two front and rear ZED2 cameras (with built-in IMU), a high-performance computing platform and a main control board. The sensor is installed on a double-layer support frame, the bottom of the support frame is fixed on the mobile platform, and a 24V to 220V inverter is fixed on the side of the platform. The motor driver of the robot chassis uses Panasonic's AC servo motor driver, the upper layer uses a high-performance laptop as a computing platform, connected to the laser radar through the network port, and the USB port is connected to the ZED2 camera. The lower layer uses an embedded control board as the main control board, which communicates with the upper platform through the serial port.

[0142] The ZED2 binocular depth camera is produced by Stereo labs in the United States, with high-definition images and high frame rates. The camera has built-in sensors such as high-precision IMU, magnetometer and barometer, with a 6-axis attitude position accuracy of 1mm and a direction accuracy of 0.1°. It is widely used in application scenarios such as target detection, scene reconstruction, visual positioning and human-computer interaction. The laser radar uses the Leishen 32-line laser radar C32-151A, with a total of 32 laser channels, a ranging range of 100 to 200m, a ranging accuracy of 3cm, and a single echo data rate of 650,000 points per second. The vertical field of view of the laser radar is -16° to 15°, the vertical angular resolution is 1°, the horizontal field of view is 360°, and the horizontal angular resolution is 0.18°. In this embodiment, the camera image output frequency is set to 20Hz, the IMU output frequency is 400Hz, and the laser radar output frequency is 10Hz. Among them, the ZED2 camera and the laser radar are both driven by ROS to obtain their images and point cloud data. The high-performance laptop is equipped with an Intel i7-10870H processor, 16G memory and RTX 2060 graphics card. The embedded control board is designed and developed based on stm32f103 and has functions such as PWM output, IO acquisition, timer and serial communication.

[0143] The overall software architecture is developed based on the robot operating system ROS. The high-performance computing platform is installed with the Ubuntu 18.04 system and the Melodic version of ROS, which are compatible with each other and use C++ as the main programming language. The ROS system is a robot operating system, such as Figure 6 As shown in the figure, the ROS node diagram of the software system includes visual feature extraction and tracking nodes, visual odometry nodes, visual closed-loop detection nodes, laser point cloud dedistortion nodes, laser point cloud feature extraction nodes, IMU pre-integration nodes and back-end optimization nodes, which are used for visual feature extraction and tracking, visual odometry estimation, visual closed-loop detection, laser point cloud dedistortion, laser point cloud feature extraction, IMU pre-integration and factor graph optimization respectively.

[0144] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A bidirectional deep visual-inertial pose estimation method combined with a multi-line laser radar, characterized in that: include: After using the lidar point cloud to restore the depth information of the feature points in the bidirectional deep visual image, the bidirectional deep visual image is tightly coupled with the IMU data to obtain two pose estimates, and the two pose estimates are weightedly fused to obtain the visual odometer. Extract feature points from the laser radar point cloud, construct laser point line residuals, use the visual odometer or IMU estimation results as prior pose estimation, combine the prior pose estimation with the laser point line residuals, construct the Gauss-Newton equation, iteratively optimize the Gauss-Newton equation, and obtain the laser odometer; Perform closed-loop detection on the bidirectional depth vision image and the LiDAR point cloud respectively, and verify all closed-loop detection results using LiDAR point cloud registration. If the closed-loop verification is valid, the relative position transformation relationship between the laser keyframes corresponding to the current moment and the closed-loop moment is used as the closed-loop constraint. The pose transformation relationship between the visual odometry corresponding to adjacent visual depth image key frames is taken as the visual odometry constraint, and the pose transformation relationship corresponding to the laser odometry between adjacent laser key frames is taken as the laser odometry constraint. The laser odometry constraint, visual odometry constraint and closed-loop constraint are taken as constraint items, and factor graph optimization is performed to obtain the optimal pose estimation.

2. The method for bidirectional deep visual-inertial pose estimation combined with a multi-line laser radar as claimed in claim 1, characterized in that: The verification of the closed-loop detection result includes: Perform closed-loop detection on the bidirectional deep vision image, obtain two timestamps of the current moment and the closed-loop moment, search for two laser key frames corresponding to the two timestamps in the historical laser key frame queue, construct local feature point clouds for the two laser key frames respectively, and perform point cloud registration on the two local feature point clouds. If the registration is successful, the visual closed-loop is valid; Local feature point clouds are constructed for the laser key frame corresponding to the current moment in the closed-loop detection of the laser radar point cloud and the laser key frame corresponding to the closed-loop moment, and point cloud registration is performed on the two local feature point clouds. If the registration is successful, the laser closed-loop is valid.

3. A bidirectional deep visual-inertial pose estimation method combined with a multi-line laser radar as described in claim 1 or 2, characterized in that: The closed-loop visual keyframes detected by the bidirectional deep vision image closed-loop meet the requirements of temporal consistency check and spatial consistency check. The temporal consistency check means that the interval between the current closed-loop moment and the previous closed-loop moment is greater than a preset time threshold. The spatial consistency check means that when the closed-loop occurs, the distance between the current deep vision image keyframe and the closed-loop visual keyframe is less than a preset distance.

4. A bidirectional deep visual-inertial pose estimation method combined with a multi-line laser radar as described in claim 1 or 2, characterized in that: The closed-loop detection includes: For each directional deep vision image in the bidirectional deep vision image, the similarity between the deep vision image and the word bag is calculated, and the deep vision image with the greatest similarity is used as a closed-loop visual key frame; Get the positions of all historical laser keyframes, build a 3D Kd-tree, and search in the Kd-tree according to the current laser keyframe pose at regular intervals to find the nearest neighbor frame to the current laser keyframe; if the position distance between the current laser keyframe and the neighbor frame is less than the set threshold, then the neighbor frame is used as the laser keyframe corresponding to the closed-loop moment.

5. A bidirectional deep visual-inertial pose estimation method combined with a multi-line laser radar as described in claim 1 or 2, characterized in that: The depth information of the feature points in the bidirectional depth vision image is restored by: A local point cloud set is constructed by dedistorting the lidar point cloud, and the feature points in the bidirectional depth vision image are restored by finding matching depth information in the local point cloud set. If matching points are missing, the depth information of the depth camera that collects the bidirectional depth vision image is used for restoration.

6. The method for bidirectional deep visual-inertial pose estimation combined with a multi-line laser radar as claimed in claim 5, characterized in that: The local point cloud set is constructed in the following way: The dedistorted LiDAR point clouds are stacked up, the LiDAR point clouds in the adjacent time range are retained, and the old LiDAR point clouds are discarded to obtain a local point cloud set. The adjacent time range is dynamically updated according to the translation increment and rotation increment: TimeSet=Tmax*(1-(μ*deltaA / Amax+η*deltaP / Pmax)) Among them, TimeSet is the time length for maintaining the local point cloud set, that is, the adjacent time range, Tmax is the maximum value of TimeSet, μ and η are the proportional coefficients of the rotation increment deltaA and the translation increment deltaP, respectively, and Amax and Pmax are the maximum values ​​of the angle increment and the rotation increment, respectively.

7. A bidirectional deep visual-inertial pose estimation method combined with a multi-line laser radar as described in claim 1 or 2, characterized in that: The visual odometer is obtained by: After using the lidar point cloud to restore the depth information of the feature points in the bidirectional depth vision image, the bidirectional depth vision images are each jointly optimized by the visual reprojection error and the IMU measurement error, and two pose estimates are obtained based on the sliding window optimization method, and the visual odometry is obtained by weighted fusion.

8. A bidirectional deep visual-inertial pose estimation method combined with a multi-line laser radar as claimed in claim 1 or 2, characterized in that: The factor graph optimization further includes: An IMU constraint is constructed based on the IMU data between the current laser key frame and the previous laser key frame. The IMU constraint includes an IMU pre-integration factor and an IMU bias factor, wherein the IMU pre-integration factor is constructed by the acceleration and angular velocity data of the IMU, and the IMU bias factor is constructed by the IMU bias. The laser odometer constraint, the visual odometer constraint, the closed-loop constraint and the IMU constraint are used as constraint items, and factor graph optimization is performed to obtain the optimal pose estimation and bias of the current laser key frame.

9. The method for bidirectional deep visual-inertial pose estimation combined with a multi-line laser radar as claimed in claim 8, characterized in that: The method further comprises: The IMU data after the current laser key frame moment is re-integrated with the bias after factor graph optimization, and then the IMU estimation result at each moment after the current laser key frame moment is obtained, and the IMU estimation result is used as the initial value of the visual odometry pose estimation.

10. A bidirectional deep visual-inertial pose estimation system combined with a multi-line laser radar, characterized in that: include: Dual depth cameras, multi-line lidar, IMU sensor and processor; The dual depth camera is used to collect bidirectional depth vision images; The multi-line laser radar is used to collect laser radar point clouds; The IMU sensor is used to collect IMU data; The processor comprises: The visual odometry estimator is used to restore the depth information of feature points in the bidirectional deep visual image using the lidar point cloud. The bidirectional deep visual image is then optimized by jointly optimizing the visual reprojection error and the IMU measurement error, and two pose estimates are obtained based on the sliding window optimization method. The visual odometry is then obtained by weighted fusion. Laser odometry estimator, used to extract feature points from the laser radar point cloud, construct laser point line residuals, use the visual odometry or IMU estimation results as prior pose estimation, combine the prior pose estimation with the laser point line residuals, construct the Gauss-Newton equation, iteratively optimize the Gauss-Newton equation, and obtain the laser odometry; The closed-loop detection module is used to perform closed-loop detection on the bidirectional depth vision image and the lidar point cloud respectively, and verify all closed-loop detection results using the lidar point cloud registration. If the closed-loop verification is valid, the relative position transformation relationship between the laser keyframes corresponding to the current moment and the closed-loop moment is used as the closed-loop constraint; The factor graph optimization module is used to use the pose transformation relationship between the visual odometry corresponding to adjacent visual depth image key frames as the visual odometry constraint, the pose transformation relationship corresponding to the laser odometry between adjacent laser key frames as the laser odometry constraint, and the laser odometry constraint, visual odometry constraint and closed-loop constraint as constraint items to perform factor graph optimization and obtain the optimal pose estimation.