Online extrinsic parameter optimization method based on VIO and LIO

Abstract

The application relates to an online external parameter optimization method based on VIO and LIO, which comprises the following steps: registering IMU pre-integration and camera motion recovery to obtain self pose estimation of the camera; taking the IMU pre-integration as prior information of an NDT algorithm of a laser radar, inputting the prior information and laser radar data into the NDT algorithm to obtain self pose estimation of the laser radar; and establishing a transformation relationship among the self pose estimation of the camera, the self pose estimation of the laser radar and external parameters, determining a cost function containing the external parameters, the self pose estimation of the camera and the self pose estimation of the laser radar according to the transformation relationship, and minimizing the cost function to obtain final external parameters. The application combines IMU pre-integration, makes the pose estimation more accurate, and makes the optimization process easier to converge when the final external parameters are obtained by minimizing the cost function, so that the robustness and accuracy of external parameter estimation can be improved.

Description

Technical Field

[0001] This invention relates to the field of multi-sensor information fusion technology, and in particular to an online extrinsic parameter optimization method based on VIO and LIO. Background Technology

[0002] In the field of multi-sensor information fusion, when using cameras and LiDAR for fusion, matching 2D images with 3D point clouds is crucial, relying on accurate coordinate system transformation, i.e., extrinsic parameters. Therefore, the accuracy of extrinsic parameters has a significant impact on the robustness and accuracy of information fusion. Through online extrinsic parameter calibration, SLAM (Simultaneous Localization and Mapping) systems can adjust the relative positions and attitude relationships between sensors in real time, thereby obtaining more accurate perception data and improving the overall system performance.

[0003] Traditional offline extrinsic calibration methods, such as the checkerboard calibration method, require multiple matching steps and are prone to drift over time, lacking accuracy and robustness. In large-scale environments, if there are errors in the rotation relationship, significant measurement errors will occur due to the long measurement distance of the lidar. Furthermore, it requires the preparation of corner targets that can be detected by both the camera and the lidar, placing high demands on the environment.

[0004] Online automatic extrinsic parameter calibration methods can effectively overcome the shortcomings of traditional offline extrinsic parameter calibration methods. Existing online automatic extrinsic parameter calibration methods use cameras and LiDAR for extrinsic parameter calibration. In any scene without any pre-defined features, the corresponding extrinsic parameters can be derived numerically from the respective modes of the camera and LiDAR, and then optimized iteratively to obtain the final extrinsic parameters. However, existing online automatic extrinsic parameter calibration methods struggle to obtain accurate convergence conditions if only a single rotational motion is applied. Furthermore, minimal rotational motion can lead to the cumulative propagation of errors, causing a decrease in the accuracy of camera motion estimation. Additionally, the poor robustness of LiDAR pose estimation in adverse environments such as rain and snow further reduces the robustness of extrinsic parameter estimation. Summary of the Invention

[0005] Therefore, it is necessary to provide an online extrinsic parameter optimization method based on VIO and LIO to address the above-mentioned technical problems. This method incorporates IMU pre-integration, which makes pose estimation more accurate and the optimization process more likely to converge when minimizing the cost function to obtain the final extrinsic parameters. This improves the robustness and accuracy of extrinsic parameter estimation.

[0006] This invention provides an online extrinsic parameter optimization method based on VIO and LIO, comprising the following steps:

[0007] The camera's own pose estimate is obtained by registering the IMU pre-integration with the camera motion recovery.

[0008] IMU pre-integration is used as prior information for the NDT algorithm of LiDAR. The prior information and LiDAR data are input into the NDT algorithm to obtain the LiDAR's own pose estimation.

[0009] Establish the transformation relationship between the camera's self-pose estimation, the lidar's self-pose estimation, and the extrinsic parameters, and determine the cost function that includes the extrinsic parameters, the camera's self-pose estimation, and the lidar's self-pose estimation based on the transformation relationship. Minimize the cost function to obtain the final extrinsic parameters.

[0010] In one embodiment, registering IMU pre-integration with camera motion recovery includes:

[0011] Pre-integration is performed on the acceleration and angular velocity information of the IMU to obtain the IMU pre-integration;

[0012] A sliding window containing a fixed number of camera frames is established, and visual motion recovery processing is performed on the sliding window;

[0013] In the sliding window, IMU pre-integration is registered with camera motion recovery.

[0014] In one embodiment, using IMU pre-integration as prior information for the NDT algorithm of the lidar, and inputting the prior information into the NDT algorithm to obtain the lidar's own pose estimation includes:

[0015] Create a point cloud object, read the point cloud data collected by the LiDAR into the point cloud object, and then filter the point cloud data collected by the LiDAR.

[0016] The point clouds of two adjacent frames after filtering and the prior information of pose transformation provided by IMU pre-integration are input into the NDT algorithm to obtain the self-pose estimation of the lidar.

[0017] In one embodiment, registering IMU pre-integration with camera motion recovery within a sliding window includes:

[0018] Define the reprojection error of visual inertia and calculate the Jacobian matrix of the reprojection error;

[0019] The reprojection error and Jacobian matrix are optimized using the g2o library, where the number of optimization factors is kept constant to the number of states in the sliding window.

[0020] In one embodiment, the visual inertial reprojection error is:

[0021]

[0022] In the formula, c represents the keyframe in the sliding window. Let l be the pixel position of feature point l in the i-th and j-th frames of the camera. This represents the rotation of the IMU in the i-th frame relative to the world coordinate system. This represents the translation transformation of the IMU in the i-th frame relative to the world coordinate system. This represents the 3D point coordinates of the l-th group of feature points in the j-th camera frame. π represents the projected 3D point coordinates of the l-th group of feature points captured by the camera in the i-th frame in the j-th frame. c -1 This means projecting pixels onto the camera's unit sphere. For the state variables to be optimized, For the external parameters of the camera and IMU, d0~d m The depth of feature points in groups 0 to m. and This represents the result of IMU pre-integration. Let be the rotation matrix for transforming the world coordinate system to the IMU coordinate system of the j-th frame. This is the extrinsic rotation matrix from the IMU to the camera.

[0023] In one embodiment, the Jacobian matrix is:

[0024]

[0025] In the formula, m is the number of feature points in the variable to be optimized.

[0026] In one embodiment, the transformation relationship between the camera's self-pose estimation, the lidar's self-pose estimation, and the extrinsic parameters is as follows:

[0027] POS L =R POS C +t

[0028] In the formula, POS L and POS C These represent the positions of the lidar and camera in the world coordinate system at the same moment.

[0029] In one embodiment, the cost function, which includes extrinsic parameters, camera pose estimation, and lidar pose estimation, is:

[0030]

[0031] In the formula, POS Ci It is the position of the camera at time i, obtained by rotating the camera from its initial origin coordinates through a rotation matrix pre-integrated from the visual SFM and IMU. Translation vector The transformation yields, and For the accurate translation vector and rotation matrix between lidar frames, and R and t are the accurate translation vector and rotation matrix between camera frames, respectively, and the rotation and translation amounts of the final extrinsic parameters.

[0032] In one embodiment, the final extrinsic parameter is

[0033]

[0034] The beneficial effects of this invention are as follows: The self-pose estimation of the camera and the self-pose estimation of the lidar are both combined with IMU pre-integration, which makes the pose estimation more accurate and can ensure the robustness of extrinsic parameter estimation under non-ideal conditions. Furthermore, IMU pre-integration can provide initial values ​​of pose changes for key frames of the camera and lidar respectively, making the optimization process easier to converge when minimizing the cost function to obtain the final extrinsic parameters. It can obtain accurate convergence conditions, improve the convergence speed of the calculation, and thus improve the real-time performance of online extrinsic parameter optimization. Attached Figure Description

[0035] Figure 1 This is one of the flowcharts illustrating the online extrinsic parameter optimization method based on VIO and LIO provided in this embodiment of the invention;

[0036] Figure 2 This is a flowchart illustrating an online extrinsic parameter optimization method based on VIO and LIO provided in another embodiment of the present invention;

[0037] Figure 3 This is a schematic diagram of the process of registering IMU pre-integration and camera motion recovery in a sliding window according to an embodiment of the present invention;

[0038] Figure 4 This is a schematic diagram of the self-pose estimation process of a lidar provided in an embodiment of the present invention;

[0039] Figure 5 This is a schematic diagram of radius filtering provided in an embodiment of the present invention;

[0040] Figure 6 This is a flowchart illustrating how the NDT algorithm, provided in this embodiment of the invention, obtains the final pose estimate of a lidar. Detailed Implementation

[0041] The following is a description of some relevant terms used in this invention.

[0042] VIO (Visual-Inertial Odometry), also known as visual inertial system (VINS), is an algorithm that implements SLAM by fusing camera and IMU data. Depending on the fusion framework, it is divided into loosely coupled and tightly coupled systems.

[0043] LIO (Lidar-Inertial Odometry) is a radar inertial odometry system that can construct maps and output odometry data based on measurement data from radar and IMU.

[0044] Sensor extrinsic parameters refer to the position and orientation parameters of a sensor relative to a specific reference coordinate system. Position and orientation parameters describe the sensor's position, orientation, or attitude in three-dimensional space, and affect the position and orientation of the sensor's measurement data in the global coordinate system.

[0045] An IMU is a sensor device that integrates an accelerometer and a gyroscope. It can measure dynamic information such as an object's acceleration, angular velocity, and attitude.

[0046] LiDAR (Light Detection and Ranging) is a sensor technology that uses laser beams to measure the distance, shape, and contour of a target object. A typical LiDAR system includes a laser transmitter, receiver, clock, and computing devices.

[0047] A camera is a device used to capture visual information and is commonly used in fields such as photography, video recording, computer vision, and machine vision.

[0048] The NDT (Normal Distribution Transform) algorithm is the algorithm published in the paper "The Normal Distributions Transform: A New Approach to Laser Scan Matching".

[0049] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0050] In one embodiment, such as Figure 1 As shown, Figure 1 This is one of the flowcharts illustrating the online extrinsic parameter optimization method based on VIO and LIO provided in this embodiment of the invention. The online extrinsic parameter optimization method based on VIO and LIO of the present invention includes the following steps:

[0051] S101. Register the IMU pre-integration with the camera motion recovery to obtain the camera's own pose estimate.

[0052] S102. Using IMU pre-integration as prior information for the NDT algorithm of the lidar, the prior information and lidar data are input into the NDT algorithm to obtain the lidar's own pose estimation.

[0053] In this embodiment, both the camera's self-pose estimation and the LiDAR's self-pose estimation are obtained by combining IMU pre-integration, which makes the pose estimation more accurate and can ensure the robustness of extrinsic parameter estimation under non-ideal environments.

[0054] S103. Establish the transformation relationship between the camera's self-pose estimation, the lidar's self-pose estimation, and the extrinsic parameters, and determine the cost function that includes the extrinsic parameters, the camera's self-pose estimation, and the lidar's self-pose estimation based on the transformation relationship. Minimize the cost function to obtain the final extrinsic parameters.

[0055] IMU pre-integration can provide initial values ​​for pose changes for keyframes of the camera and radar respectively. Information from multiple sensors is more accurate than that from a single sensor, making the optimization process easier to converge when minimizing the cost function to obtain the final extrinsic parameters. It can obtain accurate convergence conditions, improve the convergence speed of the calculation, and thus improve the real-time performance of online extrinsic parameter optimization.

[0056] In one embodiment, such as Figure 2 As shown, Figure 2 This is a flowchart illustrating an online extrinsic parameter optimization method based on VIO and LIO provided in another embodiment of the present invention. This embodiment involves how to register IMU pre-integration with camera motion recovery. Based on the above embodiment, step S101 includes:

[0057] S201. Perform pre-integration processing on the acceleration and angular velocity information of the IMU to obtain the IMU pre-integration.

[0058] Specifically, IMU pre-integration includes the IMU position, velocity, and rotation pre-integration between two frames.

[0059] It should be noted that the process of pre-integrating the acceleration and angular velocity information of the IMU to obtain the IMU pre-integral is well known to those skilled in the art and will not be elaborated here.

[0060] S202. Establish a sliding window containing a fixed number of camera frames and perform visual motion recovery processing on the sliding window.

[0061] Specifically, motion recovery processing, also known as motion recovery optimization, involves setting up a sliding window containing a fixed number of camera frames (15 frames) during the initial pose recovery process. This window is used to accommodate camera keyframes with sufficient feature points (more than 100) and sufficient parallax between adjacent camera frames (more than 20 pixels).

[0062] When processing images captured by the camera begins, Shi-Tomasi corner detection is performed on incoming camera frames. For every two consecutive frames with more than 100 feature points, KLT sparse optical flow tracing is used for preliminary camera motion recovery, resulting in the pairing of frame i and frame i+1. During KLT sparse optical flow tracing, feature points with the same characteristics are matched to obtain the displacement vector of the feature point pair in frame i and frame i+1. This vector can be used to calculate the pixel position difference (i.e., disparity) of the same feature in the two frames. The pixel position difference corresponding to the k-th feature pair is d. k (k = 1, 2, 3, 4...), there are m sets of features. The final average disparity between two adjacent frames is calculated as follows:

[0063]

[0064] when Both of the above frames enter the optimized sliding window described above. The next step, motion recovery optimization, will then proceed.

[0065] Visual Sfm (Structure from Motion) is performed within a sliding window to recover the initial camera pose. A camera frame with more than 30 tracked feature points and a disparity greater than 20 pixels compared to the last frame is searched. Once found, the motion transformations R and t of the corresponding camera positions in the two frames are recovered using the five-point essential matrix initialization method. If no suitable camera frame is found, the last frame is retained, the first frame is discarded, and the process continues to wait for the next keyframe. An arbitrary scale factor is set, and all landmarks observed in the two frames are triangulated. The Perspective-n-Point (PnP) algorithm is then used to estimate the poses of all other frames within the sliding window. Assuming the initial pose transformations in the i-th and j-th frames are...

[0066] S203. In the sliding window, register the IMU pre-integration with the camera motion recovery.

[0067] like Figure 3 As shown, Figure 3 This is a schematic diagram of the process of registering IMU pre-integration and camera motion recovery in a sliding window according to an embodiment of the present invention. Registering IMU pre-integration and camera motion recovery in a sliding window includes the following steps:

[0068] S301. Define the reprojection error of visual inertia and calculate the Jacobian matrix of the reprojection error.

[0069] In the sliding window, IMU pre-integration is registered with camera motion recovery, i.e., global BA (Bundle Adjustment) reprojection error optimization is performed within the sliding window to optimize the pose of all frames. Specifically:

[0070] When each camera keyframe enters the sliding window, a nearby IMU frame is selected as the IMU pre-integration keyframe, which is then used as the sampling frame for subsequent IMU pre-integration. This is to facilitate subsequent visual and inertial alignment to eliminate scale factors. The above IMU pre-integration settings yield the following results: These represent the rotation and translation transformations of the i-th frame IMU relative to the world coordinate system, respectively, and are used in the residual calculation for subsequent reprojection error optimization.

[0071] In this embodiment, the target camera residual is obtained by subtracting the projected 3D points of the l-th group of feature points captured by the i-th frame camera from the 3D points of the l-th group of feature points captured by the j-th frame camera (here, it is required that the l-th group of feature points are co-viewed in the i-th and j-th camera frames). The state variable to be optimized is set as follows: For the extrinsic parameters of the camera and IMU, initial values ​​for offline extrinsic parameter calibration are provided for initial optimization. During the nonlinear optimization process, more accurate camera and IMU extrinsic parameters, d0~d, will be obtained through iteration. m The depth of feature points in groups 0 to m is the scale factor that can eliminate translation blur. The residual cost function for inertial and visual registration is defined as follows:

[0072]

[0073] In the formula, ρ represents the camera keyframes within the sliding window, and ρ is the robust kernel function used to reduce the impact of mismatches on the optimization results. In the cost function... The pixel error covariance matrix representing the l-th group of feature points in frames i and j is used to characterize the uncertainty of observation. Its initial value is typically set to 1.5 × I. 2×2 During optimization, the pixel error covariance matrix will propagate to the error covariance matrix of the projected 3D points. The specific propagation method is as follows:

[0074]

[0075] The covariance matrix of the above 3D point projections (i.e., the measurement covariance matrix) will be used as a weight in the reprojection error optimization to improve the accuracy of parameter iteration. ρ is defined as follows, e 2 For the residual function:

[0076]

[0077] Visual residuals are defined as follows:

[0078]

[0079] In the formula, c represents the keyframe in the sliding window. Let l be the pixel position of feature point l in the i-th and j-th frames of the camera. This represents the rotation of the IMU in the i-th frame relative to the world coordinate system. This represents the translation transformation of the IMU in the i-th frame relative to the world coordinate system. This represents the 3D point coordinates of the l-th group of feature points in the j-th camera frame. π represents the projected 3D point coordinates of the l-th group of feature points captured by the camera in the i-th frame in the j-th frame. c -1 This means projecting pixels onto the camera's unit sphere. For the state variables to be optimized, For the external parameters of the camera and IMU, d0~d m The depth of feature points in groups 0 to m. and This represents the result of IMU pre-integration. Let be the rotation matrix for transforming the world coordinate system to the IMU coordinate system of the j-th frame. This is the extrinsic rotation matrix from the IMU to the camera.

[0080] The visual residuals correspond to the residuals of the state variables as follows:

[0081]

[0082]

[0083] in, This is the result of IMU pre-integration, which is performed in a separate IMU pre-integration thread. Once the pre-integration process is complete, the parameter values ​​are passed to the vision recovery thread. Optimization processes are executed in two separate threads to improve operational efficiency.

[0084] The Jacobian matrix required for optimization based on the above residuals is as follows:

[0085]

[0086] In the formula, m is the number of feature points in the variable to be optimized.

[0087] S302. The reprojection error and Jacobian matrix are put into the g2o library for optimization. During the optimization process, the number of optimization factors is kept equal to the number of states in the sliding window.

[0088] Specifically, in this embodiment, when optimizing the reprojection error and the Jacobian matrix using the g2o library, the Jacobian matrix combined with the inverse matrix of the 3D projection point covariance matrix (i.e., the information matrix) is set as the constraint edge for g2o optimization. After optimizing and obtaining the accurate depth of the 3D points seen by the camera in frames i and j, the accurate 3D point positions and pixel positions in frames i and j are known. At this point, PnP is performed again to solve the camera frame transformation pose. Finally, we obtain the accurate translation vector between camera keyframes after eliminating the translation scale factor. With rotation matrix That is, the camera's own pose estimation.

[0089] In one embodiment, such as Figure 4 As shown, Figure 4 This is a schematic diagram of the self-pose estimation process of a lidar provided in an embodiment of the present invention. This embodiment involves how to use IMU pre-integration as prior information for the lidar's NDT algorithm, and input the prior information into the NDT algorithm to obtain the lidar's self-pose estimation. Based on the above embodiment, step S102 includes the following steps:

[0090] S401. Create a point cloud object, read the point cloud data collected by the LiDAR into the point cloud object, and then filter the point cloud data collected by the LiDAR.

[0091] Specifically, firstly, a point cloud object is created, and the point cloud data collected by the LiDAR is read into the point cloud object. Ground point cloud filtering is then performed. A ground height threshold is set as the height of the radar above the ground. Points below the threshold are marked as ground points. The least squares method is used to fit several ground points into a planar model of the ground. All points below the ground model are removed to obtain the non-ground point cloud. For example... Figure 5 As shown, Figure 5 This is a schematic diagram of radius filtering provided in an embodiment of the present invention. During filtering, a radius filter is set, and the density of neighboring points around each point is calculated to determine whether it is an outlier; if it is an outlier, it is removed. A filtering radius of 30cm is set around each point, and a point cloud count threshold of 15 is set. That is, if the number of other point clouds within a 30cm radius around a point cloud is less than 15, then this point is an outlier. In the x-axis direction, the range of the collected point clouds is set between 0.3m and 40m (the maximum distance for LiDAR point cloud collection is 45m) to remove the point clouds of the platform equipment itself and inaccurate point clouds at greater distances.

[0092] S402. Input the point cloud of the two adjacent frames after filtering and the prior information of pose transformation provided by IMU pre-integration into the NDT algorithm to obtain the self-pose estimation of the lidar.

[0093] In this embodiment, as Figure 6As shown, Figure 6 This is a flowchart illustrating the NDT algorithm provided in this embodiment of the invention for obtaining the final pose estimate of a LiDAR sensor. Specifically, step S402 of the NDT algorithm involves first rasterizing the point cloud to be registered, dividing it into grids of a specified size, constructing a probability distribution function for each grid using a normal distribution, and then optimizing and solving for the optimal transformation parameters to maximize the probability density distribution of the source point cloud, thereby achieving the best matching between the two point clouds. This ultimately yields a more accurate translation vector between LiDAR frames. With rotation matrix That is, the radar's own pose estimation.

[0094] In one embodiment, the positional relationship between the camera's self-pose estimation and the lidar's self-pose estimation can be linked by extrinsic parameters. The established transformation relationship between the camera's self-pose estimation, the lidar's self-pose estimation, and the extrinsic parameters is as follows:

[0095] POS L =R POS C +t

[0096] In the formula, POS L and POS C These represent the positions of the lidar and camera in the world coordinate system at the same moment.

[0097] The LiDAR and camera have the following transition relationships from frame i to frame j:

[0098]

[0099] When selecting frames i to j from the LiDAR and camera, in addition to the aforementioned criteria for keyframe selection for each, we impose a timestamp alignment constraint. Specifically, only frames i differing by no more than 0.01 seconds in time can be selected as keyframes to ensure motion consistency. From the above formula, we can derive the residual formula including extrinsic parameters, which we define as...

[0100]

[0101] In the formula, POS Ci It is the position of the camera at time i, obtained by rotating the camera from its initial origin coordinates through a rotation matrix pre-integrated from the visual SFM and IMU. Translation vector The transformation yields, and For the accurate translation vector and rotation matrix between lidar frames, and R and t are the accurate translation vector and rotation matrix between camera frames, respectively, and the rotation and translation amounts of the final extrinsic parameters.

[0102] The Jacobian matrix of the residuals with respect to state variables R and t is:

[0103]

[0104] Therefore, we can obtain the extrinsic parameters R,t by solving the following least squares problem:

[0105]

[0106] The online extrinsic parameter optimization method based on VIO and LIO in this embodiment combines IMU pre-integration, which makes pose estimation more accurate and the optimization process more likely to converge when minimizing the cost function to obtain the final extrinsic parameters, thereby improving the robustness and accuracy of extrinsic parameter estimation.

[0107] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. An online extrinsic parameter optimization method based on VIO and LIO, characterized in that, Includes the following steps: The camera's own pose estimate is obtained by registering the IMU pre-integration with the camera motion recovery. IMU pre-integration is used as prior information for the NDT algorithm of lidar. The prior information and lidar data are input into the NDT algorithm to obtain the lidar's own pose estimation. Establish the transformation relationship between the camera's self-pose estimation, the lidar's self-pose estimation, and the extrinsic parameters, and determine the cost function containing the extrinsic parameters, the camera's self-pose estimation, and the lidar's self-pose estimation based on the transformation relationship. Minimize the cost function to obtain the final extrinsic parameters. Registration of IMU pre-integration with camera motion recovery includes: Pre-integration is performed on the acceleration and angular velocity information of the IMU to obtain the IMU pre-integration; A sliding window containing a fixed number of camera frames is established, and visual motion recovery processing is performed on the sliding window; In the sliding window, IMU pre-integration is registered with camera motion recovery; Registering IMU pre-integration with camera motion recovery within the sliding window includes: Define the reprojection error of visual inertia and calculate the Jacobian matrix of the reprojection error; The reprojection error and the Jacobian matrix are put into the g2o library for optimization, wherein the optimization process keeps the number of optimization factors equal to the number of states in the sliding window. The reprojection error of visual inertia is ， In the formula, denoted by , representing the reprojection error due to visual inertia; 'c' represents the keyframe in the sliding window. For feature points The pixel positions in the i-th and j-th frames of the camera. This represents the rotation of the IMU in the i-th frame relative to the world coordinate system. and The result of IMU pre-integration, This represents the translation transformation of the IMU in the i-th frame relative to the world coordinate system. This represents the translation transformation of the IMU in the j-th frame relative to the world coordinate system. Representing the The 3D point coordinates of the group of feature points in the j-th camera frame. Represents the i-th frame captured by the camera. The projected 3D point coordinates of the group of feature points in the j-th camera frame. This means projecting pixels onto the camera's unit sphere. For the state variables to be optimized, For the external parameters of the camera and IMU, The depth of feature points in groups 0 to m. Let be the rotation matrix for transforming the world coordinate system to the IMU coordinate system of the j-th frame. This is the extrinsic rotation matrix from the IMU to the camera; The cost function, which includes extrinsic parameters, camera pose estimation, and lidar pose estimation, is: ， In the formula, The camera was in the first The position at each frame time is determined by the rotation matrix obtained from the initial coordinates of the camera origin through the pre-integration of the visual SFM and IMU. Translation vector The transformation yields, and For the accurate translation vector and rotation matrix between lidar frames, and For the accurate translation vector and rotation matrix between camera frames, This represents the rotation and translation of the final external parameters.

2. The online extrinsic parameter optimization method based on VIO and LIO according to claim 1, characterized in that, Using IMU pre-integration as prior information for the NDT algorithm of the lidar, and inputting the prior information into the NDT algorithm to obtain the lidar's own pose estimation includes: Create a point cloud object, read the point cloud data collected by the lidar into the point cloud object, and then filter the point cloud data collected by the lidar. The point clouds of two adjacent frames after filtering, as well as the prior information of pose transformation provided by the IMU pre-integration, are input into the NDT algorithm to obtain the self-pose estimation of the lidar.

3. The online extrinsic parameter optimization method based on VIO and LIO according to claim 1, characterized in that, The Jacobian matrix is: ， In the formula, m is the number of feature points in the variable to be optimized.

4. The online extrinsic parameter optimization method based on VIO and LIO according to claim 1, characterized in that, The relationship between the camera's self-pose estimation, the lidar's self-pose estimation, and the transformation of extrinsic parameters is as follows: ， In the formula, and These represent the positions of the lidar and camera in the world coordinate system at the same moment.

5. The online extrinsic parameter optimization method based on VIO and LIO according to claim 1, characterized in that, The final extrinsic parameter is 。

Images