Solid state lidar-camera tight coupling pose estimation method

Abstract

The application provides a solid-state laser radar-camera tightly coupled pose estimation method, step one: performing scale invariant feature transform (SIFT) feature point detection and descriptor extraction on two adjacent image frames of a camera; step two: projecting non-repeated scanning point clouds of two adjacent solid-state laser radars on corresponding image frames to obtain depth maps, and then converting the depth maps into three-dimensional matching points; step three: performing a RANSAC algorithm based on rigid transformation on the three-dimensional matching points to obtain adjacent camera poses and calculate adjacent solid-state laser radar poses; step four: calculating the curvature of the solid-state laser radar point clouds, and respectively constructing a point-to-plane iterative closest point (ICP) algorithm and a point-to-line ICP laser radar factor according to the division of planar points and edge points according to the curvature; step five: extracting 2D registration points from the image to construct a camera factor based on epipolar geometry; step six: fusing laser radar optimization factors and camera optimization factors, and performing factor graph optimization to finally obtain fine solid-state laser radar pose estimation.

Description

Technical Field

[0001] This invention relates to solid-state lidar pose estimation, belonging to the field of robot state estimation. It has broad application prospects in 3D reconstruction, VR real estate viewing, SLAM, autonomous driving, and robotics. Background Technology

[0002] LiDAR is divided into two categories: traditional mechanical LiDAR (represented by Velodyne) and emerging solid-state LiDAR (represented by Livox). Mechanical LiDAR is expensive (a 32-line LiDAR costs hundreds of thousands of yuan), requiring a mechanical rotating structure to rotate the laser emitter, resulting in a ring-shaped point cloud. Solid-state LiDAR, on the other hand, is relatively inexpensive (the Livox Horizon costs only 8,000 yuan). It obtains a non-repeating point cloud by rotating the laser emitter through an internal optical prism, and after integration, a dense point cloud covering the field of view can be obtained. This invention focuses on pose estimation technology for solid-state LiDAR, which belongs to a subclass of LiDAR pose estimation. Furthermore, this invention is a tightly coupled pose estimation method between solid-state LiDAR and camera, that is, it tightly utilizes camera image information and LiDAR point cloud information for pose estimation, which is significantly different from the loosely coupled method of estimating camera pose and LiDAR pose separately and then fusing them.

[0003] LiDAR pose estimation technology is a technique for estimating the position and attitude of a LiDAR system. It can also be understood as LiDAR point cloud registration technology or LiDAR mapping technology, and it has many important applications in various industries. For example, autonomous vehicles pre-build high-precision maps (point cloud maps) of the region, and then use single-frame point cloud data from the onboard LiDAR to match the high-precision map to obtain the vehicle's pose (see reference: Egger P, Borges PVK, CattG, et al. PoseMap: Lifelong, Multi-Environment 3D LiDAR Localization[C] / / 2018IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS).IEEE,2018.). For example, the temporal point cloud data frames of mobile lidar can be correlated to create a map of the scene (see reference: Zhang J, Singh S. LOAM: Lidar odometry and mapping in real-time[C] / / Robotics: Science and Systems.2014,2(9):1-9.). This technology plays an important role in remote sensing mapping, 3D reconstruction, digital twins and other fields.

[0004] There are three commonly used methods for lidar pose estimation: 1. The ICP (Iterative Closest Point) method. The ICP method is an algorithm that alternately finds the nearest point pair, eliminates point pairs with large distances, and finds the optimal rigid body transformation based on the matching point pair. It belongs to the EM (Expectation Maximum) optimization algorithm category. Researchers have developed point-to-plane ICP algorithms (see reference: Besl PJ, McKay N D. Method for registration of 3-D shapes[C] / / Sensorfusion IV: control paradigms and data structures.Spie,1992,1611:586-606.) based on the classic point-to-point ICP algorithm (see reference: Low K L. Linear least-squares optimization for point-to-plane icp surface registration[J].Chapel Hill,University of North Carolina,2004,4(10):1-3.), surface-to-surface ICP (see reference: Segal A,Haehnel D,Thrun S. Generalized-icp[C] / / Robotics:science and systems.2009,2(4):435.), and voxel-based surface-to-surface ICP (see reference: Koide K,Yokozuka M,Oishi S,etal.Voxelized icp for fast and accurate3d point cloud registration[C] / / 2021IEEE International Conference on Robotics and Automation(ICRA).IEEE,2021:11054-11059.) algorithm.2. The method based on NDT (Normal Distributions Transform) (see reference: Biber P, Strabeer W. The normal distributions transform: A new approach to laserscan matching[C] / / Proceedings 2003 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat.No.03CH37453).IEEE,2003,3:2743-2748.) involves the following steps: First, the reference point cloud is divided into voxels of a specified size. Then, a Gaussian distribution is fitted to each voxel (the mean and covariance matrix of the Gaussian distribution are calculated). Next, the pose of the target point cloud is optimized using nonlinear least squares to maximize the sum of the probability densities of the target point cloud falling into the distribution of the reference point cloud after rigid body transformation. 3. The LOAM (Lidar Odometry and Mapping) based method is a commonly used approach in LiDAR-based SLAM systems. The specific steps involve calculating the Laplacian operator on the point cloud generated by scanning, and then classifying the point cloud into planar points and edge points based on the calculation results. Next, pose optimization is performed using surface-to-surface ICP and point-to-line ICP.Further developments in LOAM systems have incorporated loop closure detection, such as Lego-LOAM (see: Shan T, Englot B. Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain [C] / / 2018 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 4758-4765.). Improvements have also been made to address the characteristics of new solid-state lidars, such as Livox-LOAM (see: Lin J, Zhang F. Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV [C] / / 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020: 3126-3131.).

[0005] A detailed comparison of the three methods is shown in the table below. ICP and NDT algorithms do not have requirements regarding the source of the point cloud, while the LOAM-based algorithm requires the point cloud to be obtained from LiDAR scanning. In terms of speed, ICP is slow because it requires frequent nearest neighbor calculations; the NDT method pre-divides the point cloud into several voxels and fits it with a Gaussian distribution, thus eliminating the need for nearest neighbor calculations and making it fast; LOAM, due to its point cloud classification and downsampling, significantly reduces computational load and is relatively fast. Regarding memory usage, both ICP and NDT algorithms require high memory consumption, while the LOAM-based algorithm, due to its preprocessing of the point cloud, requires less memory for optimization. Furthermore, in terms of initial value dependence, the ICP algorithm is highly dependent on initial values; poor initial values ​​can easily cause the ICP algorithm to get trapped in local minima, resulting in low robustness; the NDT algorithm, however, can be optimized even with poor initial values ​​and has better robustness; the LOAM-based method falls between ICP and NDT due to its algorithmic limitations.

[0006]

[0007]

[0008] Table 1 Comparison of commonly used LiDAR pose estimation algorithms.

[0009] The above methods are pose estimation methods that only use the lidar point cloud modes. In recent years, there have been many methods that fuse lidar and camera for pose estimation. For example, R2-Live combines lidar, camera, and inertial measurement unit for sequential pose estimation (see: Lin J, Zheng C, Xu W, et al. R2-LIVE: A Robust, Real-Time, LiDAR-Inertial-Visual Tightly-Coupled State Estimator and Mapping[J]. IEEE Robotics and Automation Letters, 2021, 6(4): 7469-7476.). Similar work includes LVI-SAM (see: Shan T, Englot B, Ratti C, et al. Lvi-sam: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping[C] / / 2021IEEE internationalconference on robotics and automation(ICRA). IEEE, 2021: 5692-5698.).

[0010] However, the above methods are all based on nonlinear least squares optimization algorithms, and the optimization objective function is non-convex. When the initial pose is not good enough, it is easy to converge to a local minimum. Therefore, the above methods are difficult to solve the problem of large pose difference of LiDAR. The above methods are often used to solve the pose estimation problem of sequence data (SLAM task) with small pose difference. Summary of the Invention

[0011] 1. Purpose: To address the above-mentioned problems, the purpose of this invention is to provide a method that can simultaneously solve the pose estimation problem of solid-state lidar under both large and small pose conditions.

[0012] 2. Technical Solution: To achieve this objective, the technical solution of this invention requires the use of image information, meaning the sensor needs to have both a camera and a solid-state LiDAR mode. A coarse-to-fine pose estimation strategy is then employed, with the overall process divided into two steps: coarse estimation and fine estimation. In the coarse estimation step, 3D matching points are obtained through image feature matching and depth map generation by projecting point clouds. Then, a coarse LiDAR pose is obtained through Random Sample Consensus (RANSAC). In the fine estimation step, factor map optimization is performed by combining LiDAR factors and camera factors to obtain a more accurate solid-state LiDAR pose. The technical approach of this invention is mainly reflected in the following three aspects:

[0013] 1) Design a coarse-to-fine registration process. For large pose estimation problems, a rough pose can be estimated first, and then fine optimization can be performed based on it.

[0014] 2) In the coarse estimation stage, image information is combined. Two-dimensional matching points are obtained using image descriptors. The dense point cloud obtained by non-repeating scanning of solid-state lidar is projected onto the image to obtain a depth map. Then, the depth map is mapped into three-dimensional matching points. Finally, the coarse pose is obtained through rigid body transformation RANSAC.

[0015] 3) In the fine estimation stage, image information is incorporated. Simultaneously, lidar factors and epipolar-constrained camera factors are constructed and fused for lidar-camera tightly coupled factor graph optimization.

[0016] This invention relates to a tightly coupled pose estimation method for a solid-state lidar-camera system, the specific steps of which are as follows:

[0017] Step 1: Perform Scale-invariant feature transform (SIFT) feature point detection and descriptor extraction on two adjacent frames of the camera image, and then perform feature point matching to obtain two-dimensional feature points for image matching.

[0018] Step 2: Project the non-repeating scan point clouds of two adjacent solid-state LiDAR frames onto the corresponding image frames to obtain a depth map. Then, use a depth map completion algorithm to obtain a dense depth map, so that the two-dimensional matching points of the image can be transformed into three-dimensional matching points through the depth map.

[0019] Step 3: Apply the Random Sample Consensus (RANSAC) algorithm based on rigid body transformation to the 3D matching points to obtain the poses of adjacent cameras. Calculate the poses of adjacent solid-state lidar units using the pose constraints formed by the cameras and solid-state lidar units, i.e., a rough pose estimate.

[0020] Step 4: Compute the curvature of the solid-state lidar points, and construct the Iterative Closest Point (ICP) algorithm from point to surface and the ICP lidar factor from point to line based on the curvature to divide the planar points and edge points.

[0021] Step 5: Extract 2D registration points from the image to construct camera factors based on epipolar geometry.

[0022] Step 6: Fuse the LiDAR optimization factor and the camera optimization factor, and perform factor graph optimization to finally obtain a refined solid-state LiDAR pose estimate.

[0023] Output: Registered solid-state LiDAR point cloud, solid-state LiDAR pose.

[0024] Specifically, step one is as follows:

[0025] 1.1: Perform SIFT feature point detection and extract descriptors from adjacent images.

[0026] 1.2: A brute-force matcher is used for coarse matching, and a model based on the homography matrix is ​​used to remove outliers in the coarse matching, thereby obtaining the two-dimensional registration points of adjacent frames of the final image.

[0027] Step two is as follows:

[0028] 2.1: Project the point cloud onto the visible light image, and assign the depth information of the point cloud along the camera's z-axis to the nearest neighbor pixel to obtain the depth image. At this time, the depth image still contains many undefined points.

[0029] 2.2: A depth image completion algorithm based on morphological operations is adopted to complete the depth image. Specifically, morphological closing, dilation, median filtering, and Gaussian filtering operations are used alternately. After the depth image is completed, the entire depth image has a depth definition, and all two-dimensional matching points can be transformed into three-dimensional matching points through back projection. The specific steps are as follows: assuming the homogeneous coordinates of the matching pixels in the image are x = [u, v, 1], the solution method for its corresponding three-dimensional coordinates in the camera coordinate system is shown in Equation (1).

[0030]

[0031] Where K rgb D rgb These are the intrinsic parameters and distortion coefficients of the visible light camera, respectively, which have been pre-calibrated and are known values.

[0032] in Represents the distortion coefficient D on the camera normalization plane. rgbDistortion correction is performed on pixel p, and z is the depth at pixel [u, v] in the visible light image. Since the completed depth map I has already been obtained... depth The depth of each pixel is defined as z = I. depth (u, v), therefore, according to equation (1), all two-dimensional matching points can be transformed into three-dimensional matching points.

[0033] Step three is as follows:

[0034] 3.1: Use the Random Consensus Sampling Algorithm Based on Rigid Body Transformation (RANSAC) to remove mismatched points from 3D matching point pairs, obtaining a coarse estimate of the pose of adjacent cameras. Step 2 outputs a set S of N pairs of 3D matching points. M ={(s k d k )} k＝1，2，...，N , where (s k d k ) represents the k-th corresponding point s in the previous frame image. k The k-th corresponding point d in the current frame image k The point pairs formed. From S M T rounds of random sampling are performed, and m pairs of points are randomly sampled using a uniform distribution in each round. Then, the rigid body transformation [R] is calculated using the m pairs of points. i |t i ], where R i Let t represent the rotation matrix calculated in the i-th round of sampling. i This represents the translation vector calculated in the i-th round of sampling. Using this rigid body transformation, apply it to all s... k Perform the transformation to obtain the transformed point s′ k =R i ×s k +t i If s′ k With d k The distance is less than the distance threshold ∈ d If an interior point is found, it is considered an interior point. The number of interior points obtained from each round of sampling is counted, and the rigid body transformation with the highest number of interior points is output as the optimal rigid body transformation, i.e., a rough estimate of the inter-frame relative pose of the camera.

[0035] 3.2: The RANSAC algorithm based on rigid body transformation provides a coarse estimate of the inter-frame relative pose of the output camera. according to Figure 2 The relationship between lidar and camera pose constraints, and a rough estimation of the relative pose between lidar frames. The calculation method is shown in equation (2). Where T cam→Lidar This refers to the pose of the lidar relative to the camera, which has been calibrated in advance and is a known value; Refers to T cam→LidarThe inverse represents the pose of the camera relative to the lidar.

[0036]

[0037] That is, this step uses the pose constraint formed by the solid-state lidar and the camera to transform the coarse estimated pose of the camera into the coarse estimated pose of the lidar.

[0038] Step four is as follows:

[0039] 4.1: First, calculate any point P using equation (3). i Spatial curvature at P i ). Among them, S nn Represented by lidar point P i For the set of indicators of the three points before and after the center, |S nn | Represents set S nn The number of elements in ||P i ||2 represents point P i The 2-norm, j takes all over S nn All elements, S j For the corresponding neighborhood point.

[0040]

[0041] After calculating the curvature at each point using equation (3), a threshold ∈ is set. c Divide the point cloud into a set of edge points E k With the set of points H in the plane k .

[0042] 4.2: Extracting the set E of edge points between adjacent frames of the lidar radar k E k+1 and the set of plane points H k H k+1 Then, the following lidar factors can be constructed. remember The objective function for optimization is shown in equation (4).

[0043]

[0044] in This represents the lidar pose in the k-th frame. This represents the LiDAR pose in the (k+1)th frame. This represents the relative pose between lidar frames. (d) e (e, E) k ) represents the distance of edge point e from E k The distance d from the nearest edge in the middle h (h,H k) represents the distance H from point h in the plane. k The objective function is the distance to the nearest plane in the middle. This optimization objective function is equivalent to a combination of the point-to-line ICP algorithm and the point-to-plane ICP algorithm.

[0045] Step five is as follows:

[0046] 5.1: First, SIFT feature points are used for image matching, and the RANSAC algorithm is used to eliminate incorrect matches, resulting in a normalized set of corresponding plane points that satisfy epipolar geometry constraints. in These represent the m-th corresponding points of the k-th frame and the (k+1)-th frame, respectively.

[0047] 5.2: According to Figure 2 The solid-state lidar pose constraint relationship and camera inter-frame pose are shown. It can be calculated from equation (5).

[0048]

[0049] Remember T c→l :=T Cam→Lidar This represents the extrinsic parameters of the lidar relative to the camera, which have been pre-calibrated. Using the above extrinsic parameter relationships, the following camera factor is constructed. The optimization objective function is shown in equation (6).

[0050]

[0051] Where trace represents the trace of the matrix, and essential(·) is the expression for the pose. The loss function maps to the essential matrix E := t^R, where t^ represents the skew matrix with respect to vector t. The purpose of this loss function is to constrain the matching points between the k-th frame and the (k+1)-th image to satisfy epipolar geometric constraints. For example... Figure 3 As shown, O k is the kth frame image I k The camera's optical center, O k+1 It is the (k+1)th frame image I k+1 The camera's optical center, O k With O k+1 The lines connecting the graph intersect at point e. k and e k+1 P is a common 3D coordinate point captured by two cameras, and its projection onto I. k and I k+1 The image coordinates on are respectively and l k For e k and The line of intersection is called The corresponding epipolar line, similarly... k+1 for The corresponding polar line. Therefore, formula (6) expresses the meaning of the matching point. Must The corresponding polar line l k superior.

[0052] Step six is ​​as follows:

[0053] 6.1: LiDAR Factor With camera factor The optimized objective function obtained by fusion is shown in equation (7).

[0054]

[0055] Multiple frames can be optimized simultaneously, or only two adjacent frames can be optimized. Furthermore, the initial pose of this optimization problem is a rough estimate of the inter-frame relative pose of the lidar, obtained from step three.

[0056] 6.2: Factor plot as shown Figure 4 As shown, the Levenberg-Marquardt (LM) optimization algorithm is used for factor graph optimization, and the optimization result is a fine-grained inter-frame relative pose estimation for the final LiDAR.

[0057] A solid-state lidar-camera tightly coupled pose estimation method is proposed. The basic system structure and workflow of this method are as follows: Figure 1 As shown, its features include:

[0058] A coarse-to-fine pose estimation strategy.

[0059] The coarse pose estimation step is used to give a rough estimate of the solid-state lidar pose as the initial value for the fine estimation step.

[0060] The fine pose estimation step is used to fine-tune the pose, thereby obtaining a better pose estimate.

[0061] For input LiDAR point cloud and image data with significant pose differences, the coarse pose estimation step incorporates image information for feature point matching. The point cloud is then projected onto the image to obtain a depth map, which is then incomplete. Finally, rigid body transformation RANSAC is used to estimate the camera and LiDAR poses. In the fine pose estimation step, image information is again incorporated. A camera factor based on epipolar constraints is constructed and tightly coupled with the LiDAR factor for optimization. The initial optimization value is the coarse pose estimate output from the coarse pose estimation step, ultimately yielding the fine pose estimate and registered point cloud.

[0062] 3. Advantages and Efficacy: This invention proposes a solid-state LiDAR-camera tightly coupled pose estimation method, which consists of a coarse pose estimation step and a fine pose estimation step. The coarse pose estimation step incorporates image and depth map information to obtain a rough pose estimate. Then, the fine pose estimation step further optimizes the pose to obtain a better estimate. Compared with other pose estimation algorithms, this algorithm can solve the LiDAR pose estimation problem under large field of view and large pose, and has broad application value and prospects in fields such as 3D reconstruction, VR real estate viewing, autonomous driving, and robotics. Attached Figure Description

[0063] Figure 1 This is a flowchart of the algorithm for a solid-state lidar-camera tightly coupled pose estimation method proposed in this invention.

[0064] Figure 2 This is a schematic diagram of the LiDAR-camera pose transformation model and pose constraints.

[0065] Figure 3 This is a schematic diagram of the epipolar geometric constraints of an image.

[0066] Figure 4 This is a graph of the tight coupling nonlinear optimization factors between lidar and camera.

[0067] Figure 5(a) shows the point cloud of the previous lidar frame.

[0068] Figure 5(b) shows the lidar point cloud of the current frame.

[0069] Figure 5(c) is the previous visible light image.

[0070] Figure 5(d) is the visible light image of the current frame.

[0071] Figure 6(a) is a side view of the initial point cloud overlay result, which is the effect of directly overlaying two frames of point clouds.

[0072] Figure 6(b) is a top view of the initial point cloud overlay result, which is the effect of directly overlaying two frames of point clouds.

[0073] Figure 7 This is a schematic diagram of image feature point matching results based on SIFT.

[0074] Figure 8(a) is the original depth map of the previous frame.

[0075] Figure 8(b) is the original depth map of the current frame.

[0076] Figure 8(c) shows the depth map completion effect of the previous frame.

[0077] Figure 8(d) shows the depth map completion effect of the current frame.

[0078] Figure 9(a) is a front view of the coarse pose estimation effect of solid-state lidar; the circles in the figure represent areas where the matching is poor.

[0079] Figure 9(b) is a top view of the rough pose estimation effect of solid-state lidar; the circles in the figure represent areas where the matching is poor.

[0080] Figure 10(a) is a front view of the fine pose estimation effect of solid-state lidar.

[0081] Figure 10(b) is a top view of the fine pose estimation effect of solid-state lidar.

[0082] Figure 11(a) is a main view of the running effect of the comparison method Iterative Nearest Neighbor (ICP).

[0083] Figure 11(b) is a top view of the running effect of the comparative method Iterative Nearest Neighbor (ICP).

[0084] Figure 11(c) is a main view of the running effect of the comparison method General Iterative Nearest Neighbor (GICP).

[0085] Figure 11(d) is a top view of the running effect of the comparison method General Iterative Nearest Neighbor (GICP).

[0086] Figure 11(e) is a front view of the running effect of the comparison method Normal Distribution Transformation (NDT).

[0087] Figure 11(f) is a top view of the running effect of the comparison method Normal Distribution Transform (NDT).

[0088] Figure 11(g) is a front view of the operational results of the comparison method LiDAR odometry and mapping (LOAM).

[0089] Figure 11(h) is a top view of the operational results of the comparison method, lidar odometer, and mapping (LOAM).

[0090] Figure 11(i) is a front view of the running effect of the solid-state lidar-camera tightly coupled pose estimation method proposed in this paper.

[0091] Figure 11(j) is a top view of the running effect of the solid-state lidar-camera tightly coupled pose estimation method proposed in this paper. Detailed Implementation

[0092] To better understand the technical solution of the present invention, the embodiments of the present invention will be further described below with reference to the accompanying drawings.

[0093] This invention is a solid-state lidar-camera tightly coupled pose estimation method, and its algorithm flowchart is as follows: Figure 1As shown. The algorithm's input consists of solid-state LiDAR point clouds and camera images corresponding to different frames. Input examples are shown in Figures 5(a)-(d). Here, only for VR house viewing applications, two frames of indoor data are used as examples. Figures 5(a) and 5(b) are the point clouds of the previous and current frames. For ease of display, the LiDAR point clouds are projected onto the images to obtain colored point clouds; Figures 5(c) and 5(d) are the images corresponding to the previous and current frames. The effect of directly superimposing the two frame point clouds is shown in Figures 6(a) and 6(b). It can be seen that the two frame point clouds are not registered and have a large pose difference. First, the camera intrinsic parameters K are calibrated in advance. rgb and camera distortion coefficient D rgb In this example Camera distortion coefficient D rgb = [-0.227, 0.244, 4.52 × 10] -5 8.65×10 -4 ,0]. Then calibrate the lidar pose relative to the camera T. cam→lidar In this example At this time, K rgb D rgb T cam→lidar All are input into the algorithm as known quantities.

[0094] The specific implementation steps for each part are as follows:

[0095] Step 1: Perform SIFT feature point detection and descriptor extraction on the previous and current frames of the input example, then perform feature point matching, and use the RANSAC algorithm based on homography transform to remove outliers, obtaining two-dimensional feature points for image matching. The effect of matching two-dimensional feature points is as follows: Figure 7 As shown.

[0096] Step 2: Project the non-repeating scan point clouds of two adjacent solid-state LiDAR frames onto the corresponding image frames to obtain depth maps. Figure 8(a) shows the depth map effect of the previous frame, and Figure 8(b) shows the depth map effect of the current frame. Then, a dense depth map is obtained through a depth map completion algorithm. The completion effects of the previous frame and the current frame are shown in Figure 8(c) and Figure 8(d), respectively. Furthermore, the two-dimensional matching points of the image can be transformed into three-dimensional matching points through the depth map. The specific calculation method is shown in Formula 1.

[0097] Step 3: Perform the RANSAC algorithm based on rigid body transformation on the 3D matching points to obtain the poses of adjacent cameras. The rigid body transformation RANSAC algorithm has 30 sampling rounds T = 30, and the number of sampling point pairs in each round M = 5. The inlier determination threshold is ∈ d =0.1m. The pose of adjacent solid-state lidar units is calculated using pose constraints formed by the camera and solid-state lidar, i.e., a rough estimate of the pose is obtained. The point cloud registration results with a rough estimate of the pose are shown in Figures 9(a) and 9(b). It can be seen that compared with the initial poses in Figures 6(a) and 6(b), Figures 9(a) and 9(b) achieve a better point cloud registration result. However, Figures 9(a) and 9(b) still cannot achieve a perfect match. The circles in Figure 9 represent areas where the matching is poor.

[0098] Step 4: Set the curvature threshold ∈ using formula (3) to calculate the curvature of the solid-state lidar point cloud computing. c =0.5, and construct point-to-plane ICP and point-to-line ICP lidar factors according to the curvature division of plane points and edge points respectively.

[0099] Step 5: Extract 2D registration points from the image, and then use the pose constraints shown in Equation (5) to construct camera factors based on epipolar geometry.

[0100] Step Six: Fuse the LiDAR optimization factor and the camera optimization factor, and perform factor graph optimization. The initial optimization value is... Finally, a refined solid-state lidar pose estimation is obtained. The results of fine pose estimation are shown in Figure 10(a) and Figure 10(b).

[0101] Output: Registered solid-state LiDAR point cloud, solid-state LiDAR pose.

[0102] The results of comparison with other LiDAR pose estimation algorithms are shown in Figures 11(a)-(j). Figures 11(a) and 11(b) are the front and top views of the running effect of the comparative method Iterative Nearest Neighbor (ICP), respectively; Figures 11(c) and 11(d) are the front and top views of the running effect of the comparative method General Iterative Nearest Neighbor (GICP), respectively; Figures 11(e) and 11(f) are the front and top views of the running effect of the comparative method Orthogonal Distribution Transform (NDT), respectively; and Figures 11(g) and 11(h) are the front and top views of the running effect of the comparative method LiDAR Odometry and Mapping (LOAM), respectively. It can be seen that classic LiDAR pose estimation methods such as ICP, GICP, NDT, and LOAM cannot solve the LiDAR pose estimation problem under large pose conditions, and all converge to a misalignment error, resulting in unsatisfactory final point cloud fusion results. Figures 11(i) and 11(j) are the front view and top view of the operation effect of the solid-state lidar-camera tightly coupled pose estimation method of the present invention, respectively. As can be seen, the present invention can solve the lidar pose estimation problem under large pose conditions very well, thus proving the superiority of the present invention in terms of effect.

[0103] The solid-state lidar-camera tightly coupled pose estimation method proposed in this invention can effectively solve the problem of inter-frame pose estimation of solid-state lidar under large pose conditions. Solid-state lidar pose estimation has many applications. After accurate pose estimation, the inter-frame point clouds can be registered and fused, and then applied to applications such as VR house viewing, 3D reconstruction, and autonomous driving.

Claims

1. A solid-state lidar-camera tightly coupled pose estimation method, characterized in that, The specific steps are as follows: Step 1: Perform Scale Invariant Feature Transform (SIFT) feature point detection and descriptor extraction on two adjacent frames of the camera image, and then perform feature point matching to obtain two-dimensional feature points for image matching; Step 2: Project the non-repeating scan point clouds of two adjacent solid-state LiDAR frames onto the corresponding image frames to obtain a depth map; then use the depth map completion algorithm to obtain a dense depth map, and then the two-dimensional matching points of the image are transformed into three-dimensional matching points through the depth map; Step 3: Perform the RANSAC algorithm based on rigid body transformation on the 3D matching points to obtain the poses of adjacent cameras; calculate the poses of adjacent solid-state lidars through the pose constraints formed by the cameras and solid-state lidars, i.e., roughly estimate the poses. Step 4: Compute the curvature of the solid-state lidar points, and construct the point-to-surface iterative nearest point algorithm (ICP) and the point-to-line ICP lidar factor based on the curvature to divide the planar points and edge points respectively; Step 5: Extract 2D registration points from the image to construct camera factors based on epipolar geometry; Step 6: Fuse the LiDAR optimization factor and the camera optimization factor, and perform factor graph optimization to finally obtain a refined solid-state LiDAR pose estimate; Output: Registered solid-state LiDAR point cloud, solid-state LiDAR pose.

2. The solid-state lidar-camera tightly coupled pose estimation method according to claim 1, characterized in that: Step one is described in detail as follows: 1.1: Perform SIFT feature point detection and extract descriptors from adjacent images; 1.2: A brute-force matcher is used for coarse matching, and a model based on the homography matrix is ​​used to remove outliers in the coarse matching, thereby obtaining the two-dimensional registration points of adjacent frames of the final image.

3. The solid-state lidar-camera tightly coupled pose estimation method according to claim 1, characterized in that: Step two is described in detail below: 2.1: Project the point cloud onto the visible light image, and assign the depth information of the point cloud along the camera's z-axis to the nearest neighbor pixel to obtain the depth image. At this time, the depth image still contains many undefined points. 2.2: A depth map completion algorithm based on morphological operations is adopted to complete the depth map, and morphological closing operation, dilation operation, median filtering and Gaussian filtering operation are used alternately. After the depth map is completed, the entire depth map has a depth definition, and then all two-dimensional matching points can be transformed into three-dimensional matching points through back projection. The specific steps are as follows: Let the homogeneous coordinates of the matching pixels in the image be x=[u,v,1], then the solution method for its corresponding three-dimensional coordinates in the camera coordinate system is shown in Equation (1). Where K rgb D rgb These are the intrinsic parameters and distortion coefficients of the visible light camera, respectively, which have been pre-calibrated and are known values; among them... Represents the distortion coefficient D on the camera normalization plane. rgb Distortion correction is performed on pixel p, and z is the depth at pixel [u,v] in the visible light image; since the completed depth map I has already been obtained... depth The depth of each pixel is defined as z = I. depth (u,v), therefore, according to equation (1), all two-dimensional matching points can be transformed into three-dimensional matching points.

4. The solid-state lidar-camera tightly coupled pose estimation method according to claim 1, characterized in that: Step three is described in detail below: 3.1: The Random Consensus Sampling Algorithm (RANSAC) based on rigid body transformation is used to remove mismatched points in the 3D matching point pairs, obtaining a coarse estimate of the pose of adjacent cameras; Step 2 outputs a set S of N pairs of 3D matching points. M ={(s k ,d k )} k＝1,2,..,N , where (s k ,d k ) represents the k-th corresponding point s in the previous frame image. k The k-th corresponding point d in the current frame image k The point pairs formed; from S M The system randomly samples T rounds from the data, and in each round, it randomly samples m pairs of points using a uniform distribution; then, it uses the m pairs of points to calculate the rigid body transformation [R]. i |t i ]; where R i Let t represent the rotation matrix calculated in the i-th round of sampling. i Represents the translation vector calculated in the i-th round of sampling; Using this rigid body transformation for all s k Perform the transformation to obtain the transformed point s′ k =R i ×s k +t i If s′ k With d k The distance is less than the distance threshold ε d If the point is an interior point, it is considered an interior point. The number of interior points obtained from each round of sampling is counted, and the rigid body transformation with the highest number of interior points is output as the optimal rigid body transformation, i.e., a rough estimate of the inter-frame relative pose of the camera. 3.2: The RANSAC algorithm based on rigid body transformation provides a coarse estimate of the inter-frame relative pose of the output camera. Based on the pose constraint relationship between the lidar and the camera, a rough estimate of the inter-frame relative pose of the lidar is performed. The calculation method is shown in equation (2); where T cam→Lidar This refers to the pose of the lidar relative to the camera, which has been calibrated in advance and is a known value; Refers to T cam→Lidar The inverse represents the camera's pose relative to the lidar; The pose constraint formed by the solid-state lidar and the camera is used to transform the coarse estimated pose of the camera into a coarse estimated pose of the lidar.

5. The solid-state lidar-camera tightly coupled pose estimation method according to claim 1, characterized in that: Step four is described in detail below: 4.1: First, calculate any point P using equation (3). i Spatial curvature at P i ); where S nn Represented by lidar point P i For the set of indicators of the three points before and after the center, |S nn | Represents set S nn The number of elements in ||P i ||2 represents point P i The 2-norm, j takes all over S nn All elements, S j For the corresponding neighborhood points; After calculating the curvature at each point using equation (3), a threshold ε is set. c Divide the point cloud into a set of edge points E k With the set of points H in the plane k ; 4.2: Extracting the set E of edge points between adjacent frames of the lidar radar k E k+1 and the set of plane points H k H k+1 Then, the following lidar factors are constructed. remember The objective function for optimization is shown in equation (4); in This represents the lidar pose in the k-th frame. This represents the LiDAR pose in the (k+1)th frame. Represents the relative pose between lidar frames; Represents the distance between edge point e and E k The distance to the nearest edge in the middle. Represents the distance H from point h in the plane. k The distance to the nearest plane in the middle; this optimization objective function is equivalent to a combination of the point-to-line ICP algorithm and the point-to-plane ICP algorithm.

6. The solid-state lidar-camera tightly coupled pose estimation method according to claim 1, characterized in that: Step five is described in detail below: 5.1: First, SIFT feature points are used for image matching, and the RANSAC algorithm is used to eliminate incorrect matches, resulting in a normalized set of corresponding plane points that satisfy epipolar geometry constraints. in These represent the m-th corresponding points of the k-th frame and the (k+1)-th frame, respectively; 5.2: Based on the pose constraints of the solid-state lidar, the inter-frame pose of the camera. Calculated from equation (5); Remember T c→l :=T Cam→Lidar , representing the extrinsic parameters of the lidar relative to the camera, which have been pre-calibrated. The extrinsic parameter relationship is constructed as follows: camera factor The objective function for optimization is shown in equation (6); Where trace represents the trace of the matrix, and essential(·) is the expression for the pose. The function maps to the essential matrix E:=t^R, where t^ represents the antisymmetric matrix with respect to vector t; the objective function is to constrain the matching points of the k-th frame and the k+1-th image to satisfy the epipolar geometric constraint relationship; O k It is the k-th frame image I k The camera's optical center, O k+1 It is the (k+1)th frame image I k+1 The camera's optical center, O k With O k+1 The lines connecting the graph intersect at point e. k and e k+1 P is a common 3D coordinate point captured by two cameras, and its projection onto I. k and I k+1 The image coordinates on are respectively and l k For e k and The line of intersection is called The corresponding polar lines, similarly l k+1 for The corresponding polar line; then formula (6) means the matching point. Must The corresponding polar line l k superior.

7. The solid-state lidar-camera tightly coupled pose estimation method according to claim 1, characterized in that: Step six is ​​described in detail below: 6.1: LiDAR Factor With camera factor The optimized objective function obtained by fusion is shown in equation (7); Simultaneously optimize multiple frames, or optimize only two adjacent frames; in addition, the initial pose of this optimization problem is a rough estimate of the inter-frame relative pose of the lidar input in step three. 6.2: Factor graph optimization is performed using the LM optimization algorithm, and the optimization result is a fine inter-frame relative pose estimation for the final LiDAR.

Images