Method for generating texture information point cloud map based on panoramic camera and lidar fusion

Abstract

The invention discloses a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar. Firstly, after the IMU posture is obtained by fusing and solving the laser point cloud data of each frame and the IMU data of each frame acquired in real time through an iterative error extended Kalman filter algorithm, the laser point cloud data is spliced ​​to generate a point cloud map according to the laser radar posture obtained by calculating the IMU posture and the laser radar external parameters and the IMU external parameters; then, after calibrating a binocular fisheye camera, the laser point cloud data is projected onto a pinhole camera model image after correcting the binocular fisheye image to obtain projection points; the laser point cloud data is classified according to the projection points to obtain laser point pairs; visual mileage is estimated according to the laser point pairs and the laser radar posture to obtain the optimal state information of the visual odometer; finally, the laser point cloud is colored using a Bayesian formula to obtain a point cloud map with texture information.

Description

Technical Field

[0001] The present invention relates to the field of point cloud mapping, and in particular to a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar. Background Art

[0002] As a high-precision measuring instrument, three-dimensional LiDAR can provide geometric features of the surrounding environment and is less affected by environmental factors such as light. It is often used in fields such as unmanned vehicles, drones, and virtual reality. The use of LiDAR can quickly and accurately obtain depth information of the surrounding environment. The use of LiDAR in conjunction with SLAM technology can well construct a three-dimensional map of the environment, which can assist in surveying and mapping, geological disaster prediction and other outdoor work, greatly improving the safety and efficiency of outdoor operations. However, LiDAR often obtains the intensity information of the surrounding environment rather than texture information, and the final point cloud map has less texture information. Therefore, cameras are often used to color the point cloud map so that the map can obtain color texture information and more intuitively reflect the surrounding environment information;

[0003] In traditional point cloud mapping methods, point cloud coloring usually uses a SLAM system, which mainly uses a monocular camera for coloring. A monocular camera can only color one side of the environment at a time, such as Fig.10 , Fig.11 , Fig.12 and Fig.13 As shown in Figure 2, color assignment using the SLAM system is not efficient, and the monocular camera provides fewer constraints, resulting in relatively low robustness of the SLAM system. Summary of the invention

[0004] The purpose of the present invention is to provide a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a lidar, which solves the above-mentioned technical problems pointed out in the prior art.

[0005] The present invention proposes a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar, comprising the following steps:

[0006] After the IMU pose is obtained by fusing and solving the laser point cloud data of each frame and the IMU data of each frame acquired in real time through the iterative error extended Kalman filter algorithm, the laser point cloud data is spliced ​​to generate a point cloud map according to the laser radar pose calculated according to the IMU pose and the laser radar external parameters and the IMU external parameters;

[0007] Calibrate the binocular fisheye camera using a camera distortion model according to each frame of the binocular fisheye image acquired in real time to obtain the internal parameters of the binocular fisheye camera and the external parameters of the binocular fisheye camera;

[0008] After correcting the binocular fisheye image according to the internal parameters of the binocular fisheye camera to obtain a pinhole camera model image, the laser point cloud data is projected to obtain a projection point corresponding to each laser point cloud data; the laser point cloud data is classified according to the projection point to obtain a laser point pair;

[0009] Performing visual odometer estimation based on the laser point pair and the laser radar pose to obtain optimal state information of the visual odometer;

[0010] Coloring each of the laser point cloud data in the point cloud map according to the optimal state information and the pinhole camera model image to obtain color information corresponding to each of the laser point cloud data;

[0011] The color information corresponding to the laser point cloud data is:

[0012]

[0013]

[0014]

[0015] In the formula, and c s Indicates the projection points of the map point cloud before and after coloring;

[0016] γ s represents the observation projection point;

[0017] as well as Respectively represent the covariance of map points before and after coloring and the covariance of observed projection points;

[0018] σ s represents the change in covariance over time.

[0019] Preferably, the binocular fisheye camera includes a left fisheye camera and a right fisheye camera;

[0020] Preferably, the internal parameter of the binocular fisheye camera is a camera distortion parameter;

[0021] Preferably, the external parameter of the binocular fisheye camera is the coordinate transformation matrix of the camera

[0022] Preferably, the laser point cloud data of each frame and the IMU data of each frame acquired in real time are fused and solved by an iterative error extended Kalman filter algorithm to obtain the IMU pose, including the following steps:

[0023] Based on the IMU data, the laser point cloud data are respectively subjected to motion distortion removal operations to obtain a plurality of target laser point cloud data; and a target laser point cloud data set is constructed based on all the target laser point cloud data;

[0024] According to the target laser point cloud data set, a point cloud data set corresponding to each frame is obtained; the point cloud data set corresponding to each frame is traversed, and according to the coordinates of each target laser point cloud data, four reference plane points corresponding to the shortest distance in the plane distance are calculated to construct a reference plane; and the center point pj of the reference plane is obtained; and a total laser point cloud residual constraint r is constructed for each plane point pk in the current frame k;

[0025] The total laser point cloud residual constraint r is expressed as:

[0026] r=u T (p k -p j );

[0027] In the formula, u T represents the normal vector of the fitted plane;

[0028] p k is a plane point in the current frame k;

[0029] p j is the center point of the reference plane.

[0030] Based on the first IMU data of the current frame k and the second IMU data of the next frame k+1, an integral solution is performed to obtain the IMU pose information in the k+1th frame.

[0031] Preferably, the IMU posture information of the k+1th frame includes the IMU posture of the k+1th frame W R Ik+1 , IMU speed of the k+1th frame w v k+1 , IMU position of the k+1th frame w p k+1 ;

[0032] Among them, the IMU posture of the k+1th frame W R Ik+1 , IMU speed of the k+1th frame w v k+1 , IMU position of the k+1th frame w p k+1 Respectively expressed as:

[0033]

[0034]

[0035]

[0036] In the formula, Represents the IMU attitude information of the k+1 frame; Represents the IMU attitude information of k frames; b represents the angular velocity information of the k-frame IMU; gk Represents the gyroscope random walk error information; Represents the gyroscope measurement white noise error information; w v k+1 Represents the IMU velocity information of the k+1th frame; w v k Represents the IMU velocity information of the kth frame; w g represents IMU gravity acceleration information; Represents the k-frame IMU acceleration measurement value; represents the random walk noise of the IMU accelerometer; represents the accelerometer white noise of frame k; w p k Represents the IMU position information of frame k;

[0037] Preferably, the plane distance is the distance from the plane point pk in the current frame k to each plane point in the plane laser point cloud set in the previous frame k-1;

[0038] Preferably, the point cloud data set includes a planar laser point cloud set;

[0039] The plane laser point cloud data set includes a plurality of plane points; the plane points are target laser point cloud data in the target laser point cloud data set;

[0040] Preferably, the laser radar posture calculated according to the IMU posture and the laser radar external parameters and the IMU external parameters includes the following steps:

[0041] According to the state x of the kth frame k Perform a priori deduction to obtain the state x of the k+1th frame k+1 ; and according to the state quantity x of the kth frame k and the state quantity x of the k+1th frame k+1 Calculate the state error δx of the kth frame k ;

[0042] The state quantity x of the kth frame i for:

[0043]

[0044] In the formula, Represents the IMU attitude information of the kth frame, Represents the IMU position information of frame k, G V k T Represents the IMU velocity information of the kth frame, Respectively represent the random walk error information of the gyroscope and acceleration in the kth frame, G g T Represents gravitational acceleration information; They represent the transformation matrix from the IMU coordinate system to the camera coordinate system and the sensor acquisition time difference, respectively, and φ represents the camera intrinsic parameter f x 、f y 、c x and c y ;

[0045] The state quantity x of the k+1th frame k+1 for:

[0046]

[0047] in,

[0048]

[0049]

[0050] The state error δx of the kth frame k for:

[0051]

[0052] According to the state error δx of the kth frame i Obtain the state prior value of the k+1th frame by calculating the state recursion equation According to the state prior value of the k+1th frame Calculate the prior covariance matrix

[0053] The state prior value of the k+1th frame for:

[0054]

[0055] in, Represents the prior value of the k-th frame state;

[0056] The prior covariance matrix for:

[0057]

[0058] Where Q represents the noise covariance matrix, and represents the state recursion matrix;

[0059] According to the state recursion equation and the prior covariance matrix Calculate the a posteriori state error according to the measurement equation;

[0060] The calculation method of the posterior state error is:

[0061]

[0062] Wherein, H is the state observation matrix; the state observation matrix is:

[0063]

[0064] According to the prior covariance matrix Calculate and obtain the measurement covariance matrix R;

[0065] The measurement covariance matrix is:

[0066]

[0067] According to the state observation matrix H, the measurement covariance matrix R and the prior covariance matrix Calculate and obtain the Kalman filter gain;

[0068] The Kalman filter gain is:

[0069] K=(H T R -1 H+P -1 ) -1 H T R -1 ;

[0070] The Kalman filter gain is used to update the posterior state matrix of the k+1th frame. And based on the posterior state matrix of the k+1th frame The covariance matrix is ​​updated to obtain a posterior covariance matrix, the first iteration is completed and an iteration counter is set, and the iteration counter is incremented by one; and the above operation is repeated to record the iteration number;

[0071] The posterior state matrix of the k+1th frame for:

[0072]

[0073] The posterior covariance matrix is:

[0074]

[0075] According to the posterior state matrix of the k+1th frame and the state prior value of the k+1th frame Calculate and obtain the amount of change E; and obtain the laser radar posture by repeating the above steps according to the preset maximum threshold value E' of the amount of change and the maximum threshold value of the number of iterations.

[0076] Preferably, the step of classifying the laser point cloud data according to the projection points to obtain laser point pairs comprises the following steps:

[0077] Determine the projection point p k Whether the coordinate is negative or at the edge of the pinhole camera model image;

[0078] If yes, the laser point cloud data is filtered out; if no, the laser point cloud data is divided into left 3D laser point pair ζ l Align with right 3D laser point r ; and according to the left 3d laser point to ζ l Align with right 3D laser point r The corresponding projection point p k Obtain the projection point p corresponding to the next frame through optical flow tracking k .

[0079] Preferably, the projection point p k Including the left fisheye camera projection point p kl With the right fisheye camera projection point p kr ;

[0080] Among them, the left fisheye camera projection point p kl for:

[0081]

[0082] The right fisheye camera projection point p kr for:

[0083]

[0084] In the formula, Indicates the projection point of the left fisheye camera; represents the projection point of the right fisheye camera; K is the camera internal parameter; a T b Represents the coordinate system transformation matrix from the a coordinate system to the b coordinate system; P k is the laser point cloud data in the point cloud map; n is the normalization factor;

[0085] Preferably, performing visual odometer estimation based on the laser point pair and the laser radar pose to obtain optimal state information of the visual odometer includes the following steps:

[0086] Obtaining a reprojection error by calculation according to the laser point pair;

[0087] The reprojection error is:

[0088] In the formula, I T c Represents the coordinate system transformation matrix from IMU to the left fisheye camera and the right fisheye camera;

[0089] Based on the reprojection error, the reprojection error is used as observation, and an iterative error Kalman filter is used to estimate the visual odometer to obtain the optimal state information of the visual odometer.

[0090] Compared with the prior art, the embodiments of the present invention have at least the following technical advantages:

[0091] From the analysis of the above-mentioned method for generating texture information point cloud map based on the fusion of panoramic camera and laser radar provided by the present invention, it can be known that in specific application, the laser point cloud data of each frame is first collected in real time by using the laser radar, and the IMU data corresponding to each frame is collected by using the gyroscope; the laser point cloud data and IMU data of each frame are fused and solved by the iterative error extended Kalman filter algorithm to obtain the IMU posture, and the iterative error extended Kalman filter algorithm can ensure that the posture solution speed is fast and the result of the posture solution is accurate; then the IMU data is solved by the laser radar external parameters and the IMU external parameters to obtain the laser radar posture corresponding to each frame, and the laser point cloud data is spliced ​​according to the laser radar posture to obtain a point cloud map; so that the obtained point cloud map is more accurate;

[0092] Further obtain the binocular fisheye image corresponding to each frame, calibrate the binocular fisheye camera according to the binocular fisheye image, and obtain the camera internal parameters (i.e., camera distortion parameters) and the camera coordinate transformation matrix, which can provide a basis for the subsequent color processing of the point cloud map;

[0093] After correcting the binocular fisheye image according to the camera internal parameters to obtain the pinhole camera model image, the laser point cloud data in the point cloud map is projected into the pinhole camera model image to obtain a 2D image (that is, the 3D laser point cloud data is projected into the pinhole camera model image to obtain the 2D pixel point (i.e., the projection point) corresponding to each 3D laser point cloud data, thereby obtaining the entire 2D image from all 2D pixel points); the laser point cloud data is classified into laser point pairs according to the projection position of the 2D pixel point, and the projection point of the next frame is obtained by optical flow according to the position of the projection point;

[0094] Visual mileage estimation is further performed based on the laser point pairs and the laser radar pose to obtain the optimal state of the fisheye camera, ensuring that each frame of laser point cloud data in the point cloud map is within the shooting range of the fisheye camera, thereby improving the subsequent coloring effect;

[0095] Finally, the Bayesian formula is used to color the laser point cloud to obtain a point cloud map with texture information. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0097] Figure 1 A schematic diagram of the operation steps of a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar provided in an embodiment of the present invention;

[0098] Figure 2 A color point cloud image and a corresponding trajectory simulation schematic diagram of a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar provided in an embodiment of the present invention;

[0099] Figure 3 A schematic diagram of a local coloring effect simulation method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar provided in an embodiment of the present invention;

[0100] Figure 4 A schematic diagram of a local coloring effect simulation method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar provided in an embodiment of the present invention;

[0101] Figure 5 A schematic diagram of a local coloring effect simulation method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar provided in an embodiment of the present invention;

[0102] Figure 6 A schematic diagram of the operation steps for obtaining the IMU posture in a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar provided in an embodiment of the present invention;

[0103] Figure 7 A schematic diagram of the operation steps for obtaining the laser radar posture in a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar provided in an embodiment of the present invention;

[0104] Figure 8A schematic diagram of the operation steps for obtaining laser point pairs in a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar provided in an embodiment of the present invention;

[0105] Fig. 9 A schematic diagram of the operation steps for obtaining optimal state information of a visual odometer in a method for generating a texture information point cloud map based on fusion of a panoramic camera and a laser radar provided in an embodiment of the present invention;

[0106] Fig.10 A schematic diagram of simulating the coloring effect of point clouds in the prior art;

[0107] Fig.11 It is a schematic diagram of simulating the local coloring effect in the prior art;

[0108] Fig.12 It is a schematic diagram of simulating the local coloring effect in the prior art;

[0109] Fig.13 The figure is a schematic diagram of simulating the local coloring effect in the prior art. DETAILED DESCRIPTION

[0110] The technical solution of the present invention will be described clearly and completely below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0111] The present invention is further described in detail below through specific embodiments in conjunction with the accompanying drawings.

[0112] like Figure 1 As shown, the present invention proposes a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar, comprising the following steps:

[0113] Step S10: using the laser radar to acquire laser point cloud data of each frame in real time, and using the IMU to acquire IMU data of each frame in real time; acquiring the laser radar external parameter of the laser radar and the IMU external parameter of the IMU; fusing and solving the laser point cloud data and the IMU data through an iterative error extended Kalman filter (ESIKF) algorithm to obtain the IMU pose; calculating the laser radar pose according to the IMU pose and the laser radar external parameter and the IMU external parameter; splicing the laser point cloud data according to the laser radar pose to generate a point cloud map;

[0114] It should be noted that the technical solution adopted in the above-mentioned embodiment of the present application, after fusing and solving the laser point cloud data and the IMU data by using the iterative error extended Kalman filter algorithm to obtain the IMU pose, transfers the laser point cloud data to the world coordinate system based on the IMU pose and the laser radar external parameter and the IMU external parameter obtained by the fusion solution to generate a point cloud map;

[0115] The above-mentioned iterative error extended Kalman filter algorithm is a commonly used posture solution algorithm, which has a fast posture solution speed and accurate posture solution results;

[0116] The above-mentioned IMU external parameters refer to the transformation matrix from the IMU data coordinate system to the laser point cloud data coordinate system, which can be obtained through calibration through an open source algorithm.

[0117] Step S20: Acquire the binocular fisheye image corresponding to each frame in real time; according to the binocular fisheye image, use the camera distortion model to calibrate the binocular fisheye camera to obtain the camera distortion parameters (the camera distortion parameters are the internal parameters of the fisheye camera), and calibrate the external parameters between the left fisheye camera and the right fisheye camera to obtain the camera coordinate transformation matrix

[0118] The binocular fisheye camera includes a left fisheye camera and a right fisheye camera;

[0119] It should be noted that there are many camera distortion calibration models that can be selected. Since this technology is mainly aimed at panoramic cameras, the MEI model is the best;

[0120] The above calibration uses the open source algorithm Kalibr algorithm to calibrate the internal and external parameters of the camera.

[0121] Step S30: after correcting the binocular fisheye image according to the camera distortion parameter to obtain a pinhole camera model image, projecting the laser point cloud data to obtain a projection point corresponding to each laser point cloud data; classifying the laser point cloud data according to the projection point to obtain a laser point pair, and obtaining all projection points corresponding to the next frame of the binocular fisheye image according to the projection point;

[0122] Step S40: performing visual odometer estimation according to the laser point pair and the laser radar posture to obtain optimal state information of the visual odometer;

[0123] Step S50: coloring each of the laser point cloud data in the point cloud map according to the optimal state information and the pinhole camera model image to obtain color information corresponding to each of the laser point cloud data;

[0124] The color information corresponding to the laser point cloud data is:

[0125]

[0126]

[0127]

[0128] In the formula, and c s Represents the map point cloud pixel before and after the update, γ s represents the observed pixel, as well as Respectively represent the map point covariance and observed pixel covariance before and after the update; σ s represents the covariance change with time, and σ in the embodiment of the present application s Select 0.1 based on experience.

[0129] It should be noted that in the above embodiment of the present application, the laser point cloud data is projected to obtain a 2D image in step S30, wherein the left fisheye camera is first projected, and then the RGB value is calculated by bilinear interpolation according to the projected pixel coordinates, and then the corresponding point cloud color parameters are updated; the point cloud color is updated using the Bayesian formula, such as Figure 2 , Figure 3 , Figure 4 and Figure 5 As shown, a point cloud map containing texture information is obtained;

[0130] In the above embodiment of the present application, since the residual of the binocular camera is added to the VIO system (i.e., the system for the above visual mileage estimation operation), the constraints finally obtained are more accurate, and the robustness of the coloring system is improved to a certain extent compared with the previous one. At the same time, the binocular fisheye camera is used for coloring, which improves the coloring effect to a certain extent.

[0131] The above-mentioned application first uses the laser radar to collect the laser point cloud data of each frame in real time, and uses the gyroscope to collect the IMU data corresponding to each frame; the laser point cloud data of each frame and the IMU data are fused and solved by the iterative error extended Kalman filter algorithm to obtain the IMU posture. The iterative error extended Kalman filter algorithm can ensure that the posture solution speed is fast and the posture solution result is accurate; then the IMU data is solved by the laser radar external parameters and the IMU external parameters to obtain the laser radar posture corresponding to each frame, and the laser point cloud data is spliced ​​according to the laser radar posture to obtain a point cloud map; so that the obtained point cloud map is more accurate;

[0132] Further obtain the binocular fisheye image corresponding to each frame, calibrate the binocular fisheye camera according to the binocular fisheye image, and obtain the camera internal parameters (i.e., camera distortion parameters) and the camera coordinate transformation matrix, which can provide a basis for the subsequent color processing of the point cloud map;

[0133] After correcting the binocular fisheye image according to the camera internal parameters to obtain the pinhole camera model image, the laser point cloud data in the point cloud map is projected into the pinhole camera model image to obtain a 2D image (that is, the 3D laser point cloud data is projected into the pinhole camera model image to obtain the 2D pixel point (i.e., the projection point) corresponding to each 3D laser point cloud data, thereby obtaining the entire 2D image from all 2D pixel points); the laser point cloud data is classified into laser point pairs according to the projection position of the 2D pixel point, and the projection point of the next frame is obtained by optical flow according to the position of the projection point;

[0134] Visual mileage estimation is further performed based on the laser point pairs and the laser radar pose to obtain the optimal state of the fisheye camera, ensuring that each frame of laser point cloud data in the point cloud map is within the shooting range of the fisheye camera, thereby improving the subsequent coloring effect;

[0135] Finally, the Bayesian formula is used to color the laser point cloud to obtain a point cloud map with texture information.

[0136] Specifically, Figure 6 As shown, in step S10, the IMU pose is obtained by fusing the laser point cloud data and the IMU data through an iterative error extended Kalman filter algorithm (ESIKF), including the following steps:

[0137] Step S11: performing motion distortion removal operations on the laser point cloud data based on the IMU data to obtain a plurality of target laser point cloud data; and constructing a target laser point cloud data set based on all the target laser point cloud data;

[0138] Step S12: according to the target laser point cloud data set, obtain the point cloud data set corresponding to each frame; traverse the point cloud data set corresponding to each frame, and according to the coordinates of each target laser point cloud data (the coordinates of the target laser point cloud data usually refer to 3D coordinates, and the method of calculating the distance using 3D coordinates is a prior art, which will not be repeated in this application), calculate and obtain the plane distance from the plane point pk in the current frame k to each plane point in the plane laser point cloud set in the previous frame k-1; obtain the four reference plane points (the plane points in the previous frame k-1) corresponding to the shortest distance in the plane distance to construct a reference plane; and obtain the center point pj of the reference plane; construct a total laser point cloud residual constraint r for each plane point pk in the current frame k;

[0139] The total laser point cloud residual constraint r is expressed as:

[0140] r=u T (p k -p j );

[0141] In the formula, u T represents the normal vector of the fitting plane, which is obtained by fitting the above four reference plane points (the four reference plane points are the four plane points in the previous frame k-1); k is a plane point in the current frame k; p j is the center point of the four reference plane points; the final calculated r is point p k The distance to the reference plane (i.e. the above-mentioned total laser point cloud residual constraint r).

[0142] The point cloud data set includes a plane laser point cloud set; further, a plurality of plane points (or target laser point cloud data) located at the edge of the plane corresponding to each frame in the plane laser point cloud set constitute an edge laser point cloud set;

[0143] The plane laser point cloud data set includes a plurality of plane points; the plane points are target laser point cloud data in the target laser point cloud data set;

[0144] The edge laser point cloud set includes a plurality of edge points; the edge points are target laser point cloud data with edge properties (edge ​​properties refer to being at the edge of the plane corresponding to each frame) in the target laser point cloud data set;

[0145] Step S13: performing integral calculation based on the first IMU data of the current frame k and the second IMU data of the next frame k+1 to obtain the IMU pose information of the k+1th frame;

[0146] The IMU state information of the k+1th frame includes the IMU attitude information of the k+1th frame W R Ik+1 , IMU velocity information of the k+1th frame w v k+1 , IMU position information of the k+1th frame w p k+1 ;

[0147] Among them, the IMU attitude information of the k+1th frame W R Ik+1 , IMU velocity information of the k+1th frame w v k+1 , IMU position information of the k+1th frame w p k+1 Respectively expressed as:

[0148]

[0149]

[0150]

[0151] In the formula, Represents the IMU attitude information of the k+1 frame; Represents the IMU attitude information of k frames; Represents the angular velocity information of k frames of IMU; Represents the gyroscope random walk error information; Indicates the white noise error information measured by the gyroscope (the random walk error and white noise error will be initially calibrated using the open source algorithm imu_utils, and the algorithm will be subsequently optimized); w v k+1 Represents the IMU velocity information of the k+1th frame; w v k Represents the IMU velocity information of the kth frame; w g represents IMU gravity acceleration information; Represents the k-frame IMU acceleration measurement value; represents the random walk noise of the IMU accelerometer; represents the accelerometer white noise of frame k; w p k Represents the IMU position information of frame k;

[0152] It should be noted that the above-mentioned embodiment of the present application first performs a motion distortion removal operation on the laser point cloud data to obtain the target laser point cloud data. The laser point cloud data is obtained by scanning the object with a laser radar and measuring the time and intensity of the reflected light. However, in the case of object movement or sensor movement, due to the relative movement during the scanning process, motion distortion will be introduced, causing the point cloud data to deviate from the actual position or deform. This distortion will affect subsequent point cloud processing tasks, such as mapping, positioning, target recognition, etc. Therefore, performing a motion distortion removal operation on the laser point cloud data can improve the positioning accuracy of the laser point cloud data by the laser radar, improve the mapping quality, and reduce the computational complexity of subsequent algorithms.

[0153] Then, according to the coordinates of the target laser radar point cloud data of each frame, the plane points in each frame are analyzed, and the four plane points with the shortest distance from the plane points of the previous frame (i.e., the above-mentioned reference plane points) are calculated respectively by using the plane points of the current frame, so as to obtain the reference plane and the center point of the reference plane, and then the total laser point cloud residual constraint r is constructed for the plane points of the current frame, which is used to measure the quality and accuracy of the matching;

[0154] Further calculate the IMU pose information of the next frame based on the IMU data of the current frame and the IMU data of the next frame;

[0155] The k-th frame mentioned above is the laser point cloud data collected at the k-th moment. This is because in the fields of 3D reconstruction, visual SLAM (Simultaneous Localization and Mapping), autonomous driving, etc., continuous frames are usually used to represent point cloud data at different times to capture the movement and changes of the environment; each frame corresponds to the point cloud data at a certain moment, which can be used to establish environmental models, perform positioning and map construction, and other tasks.

[0156] Specifically, Figure 7 As shown, in step S10, the laser radar posture calculated according to the IMU posture and the laser radar external parameter and the IMU external parameter includes the following operation steps:

[0157] Step S101: Obtain the state quantity x of the kth frame k ; According to the state quantity x of the kth frame k Perform a priori deduction to obtain the state x of the k+1th frame k+1 ; and according to the state quantity x of the kth frame k and the state quantity x of the k+1th frame k+1 Calculate the state error δx of the kth frame k ;

[0158] The state quantity x of the kth frame i for:

[0159]

[0160] In the formula, represents the IMU posture of the kth frame, represents the IMU position of frame k, represents the IMU velocity of the kth frame, Respectively represent the random walk errors of the gyroscope and acceleration in the kth frame, G g T represents the acceleration due to gravity, They represent the transformation matrix from the IMU coordinate system to the camera coordinate system and the sensor acquisition time difference, respectively, and φ represents the camera intrinsic parameter f x 、f y 、c x and c y ;

[0161] The state quantity x of the k+1th frame k+1 for:

[0162]

[0163] in,

[0164]

[0165]

[0166] The state error δx of the kth frame k for:

[0167]

[0168] It should be noted that the above embodiment performs a priori deduction of the state quantity of the current frame k to obtain the state quantity of the next frame k+1, thereby calculating the state error of the current frame k, which can be used as the difference between the actual observed data and the predicted state, and can also provide observation update values ​​for subsequent calculations.

[0169] Step S102: according to the state error δx of the kth frame i Obtain the state prior value of the k+1th frame by calculating the state recursion equation According to the state prior value of the k+1th frame Calculate the prior covariance matrix

[0170] The state prior value of the k+1th frame for:

[0171]

[0172] in, Represents the prior value of the k-th frame state;

[0173] The prior covariance matrix for:

[0174]

[0175] Where Q represents the noise covariance matrix, and represents the state recursion matrix;

[0176] It should be noted that in the above embodiment, the prior covariance matrix provides confidence information for the system state estimation, and the observed data and the prior estimation can be weighted during the state update process to obtain a more accurate a posteriori estimation.

[0177] Step S103: According to the state recursion equation and the prior covariance matrix Calculate the a posteriori state error according to the measurement equation;

[0178] The calculation method of the posterior state error is:

[0179]

[0180] in, (state observation matrix);

[0181] It should be noted that the above-mentioned embodiment of the present application can compare and fuse the observed data with the prior state by calculating the a posteriori state error, thereby obtaining a more accurate estimate of the current state of the system. The calculation of the a posteriori state error provides a mechanism to correct the prior state to a posteriori state that is closer to the true state.

[0182] Step S104: According to the prior covariance matrix Calculate and obtain the measurement covariance matrix R;

[0183] The measurement covariance matrix is:

[0184]

[0185] Step S105: Based on the state observation matrix H, the measurement covariance matrix R and the prior covariance matrix Calculate and obtain the Kalman filter gain;

[0186] The Kalman filter gain is:

[0187] K=(H T R -1 H+P -1 ) -1 H T R -1 ;

[0188] It should be noted that the Kalman filter gain is used to weight the measurement data and the prior estimate to obtain an updated a posteriori estimate. The calculation of the gain utilizes the characteristics of the observation model (state observation matrix) and measurement noise (measurement covariance matrix), as well as the uncertainty of the prior estimate (prior covariance matrix). Through the fusion of information, the Kalman filter gain can effectively combine the prior estimate and the observation data to provide a more accurate a posteriori estimate result.

[0189] Step S106: Using the Kalman filter gain to update the posterior state matrix of the k+1th frame And based on the posterior state matrix of the k+1th frame The covariance matrix is ​​updated to obtain a posterior covariance matrix, the first iteration is completed and an iteration counter is set, and the iteration counter is incremented by one; and the above operation is repeated to record the iteration number;

[0190] The posterior state matrix of the k+1th frame for:

[0191]

[0192] The posterior covariance matrix is:

[0193]

[0194] Step S107: Based on the posterior state matrix of the k+1th frame and the state prior value of the k+1th frame Calculate and obtain the amount of change E; preset the maximum threshold value E' of the amount of change and the maximum number of iterations; stop the iteration operation when the amount of change E is greater than or equal to the maximum threshold value E' of the amount of change or when the number of iterations reaches the maximum number of iterations, and obtain the posterior state matrix of the k+1th frame corresponding to the current number of iterations is the LiDAR pose;

[0195] It should be noted that the above application makes the delayed state matrix approach the real level through continuous iterative operations, and uses a preset maximum iteration number threshold to prevent over-coupling;

[0196] The above-mentioned embodiment of the present application obtains the final lidar posture through continuous iterative adjustment, and then uses the lidar posture to splice the laser point cloud data of each frame to obtain a point cloud map, which can provide a more accurate and complete point cloud map.

[0197] Specifically, Figure 8 As shown, in step S30, preferably, the laser point cloud data is classified according to the projection points to obtain laser point pairs, including the following operation steps:

[0198] Step S31: Determine the projection point p k Whether the coordinate is negative or at the edge of the pinhole camera model image;

[0199] Step S32: If yes, the laser point cloud data is filtered out; if no, the laser point cloud data is divided into left 3D laser point pair ζ l Align with right 3D laser point r ; and according to the left 3d laser point to ζ l Align with right 3D laser point r The corresponding projection point p k Obtain the projection point p corresponding to the next frame through optical flow tracking k .

[0200] Preferably, the projection point p k Including the left fisheye camera projection point p kl With the right fisheye camera projection point p kr ;

[0201] Among them, the left fisheye camera projection point p kl for:

[0202]

[0203] The right fisheye camera projection point p kr for:

[0204]

[0205] In the formula, Indicates the projection point of the left fisheye camera; represents the projection point of the right fisheye camera; K is the camera internal parameter; a T b Represents the coordinate system transformation matrix from the a coordinate system to the b coordinate system; P k is the laser point cloud data in the point cloud map; n is the normalization factor;

[0206] It should be noted that the above embodiment projects the 3D point (i.e., the laser point cloud data in the above point cloud map). If the pixel coordinates obtained when projecting to the left fisheye camera are negative or at the boundary, the projection to the other camera is attempted. If both cameras fail to project, the laser point is considered to be out of the camera field of view. Finally, the 3D laser point is divided into the left 3D laser point pair ζ l and right 3D laser point r Then l and r The 2D points in the image are tracked by optical flow to obtain the pixel coordinates of the corresponding feature points in the next frame (i.e., the projection point p k ).

[0207] Specifically, Fig. 9 As shown, in step S40, visual odometer estimation is performed according to the laser point pair and the laser radar posture to obtain optimal state information of the visual odometer, including the following operation steps:

[0208] Step S41: Calculating and obtaining a reprojection error according to the laser point pair;

[0209] The reprojection error is:

[0210] In the formula, I T c Represents the coordinate system transformation matrix from IMU to the left fisheye camera and the right fisheye camera;

[0211] It should be noted that, in the above-mentioned reprojection error calculation method of the embodiment of the present application, I T cAccording to the left fisheye camera and the right fisheye camera, if it is the left fisheye camera, it is the external parameter of the left fisheye camera, and if it is the right fisheye camera, it is the external parameter of the right fisheye camera;

[0212] For the convenience of calculation, the above formula is decomposed. First, the 3D point is converted from the world coordinate system G to the camera coordinate system c. The conversion method is as follows:

[0213]

[0214] In the above formula, c p According to the left and right cameras, it is expressed as c l and c R ; Ps is the sth laser point cloud data;

[0215] The visual residual can then be calculated:

[0216]

[0217] Similarly, according to the principle that the visual residual approaches 0, the residual linearization can be obtained:

[0218] (x k+1 is the actual state of the current device, followed by the state we estimate through the sensor)

[0219] in,

[0220]

[0221] Among them, for the case where the external parameters of the right fisheye camera and the left fisheye camera are different, the external parameter matrix of the IMU and the right fisheye camera can be known according to the coordinate transformation matrix:

[0222]

[0223] in, Represents the external parameter matrix of IMU and right fisheye camera, is the external parameter matrix of the left fisheye camera and the right fisheye camera, is the external parameter matrix of the left fisheye camera and IMU; because it is a panoramic camera, the external parameter matrix of the left fisheye camera and the right fisheye camera can be considered as:

[0224]

[0225] Step S42: taking the reprojection error as observation, performing iterative error Kalman filtering to estimate the visual odometer, and obtaining optimal state information of the visual odometer;

[0226] It should be noted that the above embodiment uses the Kalman filter gain calculation formula and the Kalman filter posterior formula to perform state update iteration to estimate the optimal state information.

[0227] The state after the reprojection error optimization is then subjected to a second round of visual optimization using photometric error. That is, the photometric residual is obtained by subtracting the color projected onto the image from the color of the map point. Similar optimization is performed on the reprojection error to estimate the optimal state information, which is the optimal posture state information of the fisheye camera. This ensures that each frame of laser point cloud data in the point cloud map is within the shooting range of the fisheye camera, thereby improving the subsequent coloring effect.

[0228] In summary, the present invention proposes a method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar. First, the laser radar is used to collect the laser point cloud data of each frame in real time, and the gyroscope is used to collect the IMU data corresponding to each frame.

[0229] Then, the laser point cloud data is subjected to motion distortion removal to obtain the target laser point cloud data, thereby improving the positioning accuracy of the laser point cloud data and the quality of mapping, while reducing the computational complexity of subsequent algorithms.

[0230] Then, according to the coordinates of the target laser radar point cloud data of each frame, the plane points in each frame are analyzed, and the four plane points with the shortest distance from the plane points of the previous frame (i.e., the above-mentioned reference plane points) are calculated respectively by using the plane points of the current frame, so as to obtain the reference plane and the center point of the reference plane, and then the total laser point cloud residual constraint r is constructed for the plane points of the current frame, which is used to measure the quality and accuracy of the matching;

[0231] Further calculate the IMU pose information of the next frame based on the IMU data of the current frame and the IMU data of the next frame;

[0232] The iterative error extended Kalman filter algorithm can ensure that the pose solution speed is fast and the pose solution result is accurate. Then, the IMU data is solved by the lidar external parameters and the IMU external parameters to obtain the lidar pose corresponding to each frame, and the laser point cloud data is spliced ​​according to the lidar pose to obtain a point cloud map, making the obtained point cloud map more accurate.

[0233] Further obtain the binocular fisheye image corresponding to each frame, calibrate the binocular fisheye camera according to the binocular fisheye image, and obtain the camera internal parameters (i.e., camera distortion parameters) and the camera coordinate transformation matrix, which can provide a basis for the subsequent color processing of the point cloud map;

[0234] After correcting the binocular fisheye image according to the camera internal parameters to obtain the pinhole camera model image, the laser point cloud data in the point cloud map is projected into the pinhole camera model image to obtain a 2D image (that is, the 3D laser point cloud data is projected into the pinhole camera model image to obtain the 2D pixel point (i.e., the projection point) corresponding to each 3D laser point cloud data, thereby obtaining the entire 2D image from all 2D pixel points); the laser point cloud data is classified into laser point pairs according to the projection position of the 2D pixel point, and the projection point of the next frame is obtained by optical flow according to the position of the projection point;

[0235] The Kalman filter gain calculation formula and the Kalman filter posterior formula are further used to iteratively estimate the optimal state information for state update.

[0236] Then the state after the reprojection error optimization is subjected to the second round of visual optimization, using the photometric error for optimization, that is, the photometric residual is obtained by subtracting the color projected on the image and the color of the map point, and the optimal state information is estimated by performing similar optimization as the reprojection error, which is the optimal posture state information of the fisheye camera, ensuring that each frame of laser point cloud data in the point cloud map is within the shooting range of the fisheye camera, improving the subsequent coloring effect

[0237] Finally, the Bayesian formula is used to color the laser point cloud to obtain a point cloud map with texture information.

[0238] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. A person skilled in the art may modify the technical solutions described in the above embodiments, or replace part or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar, characterized in that: The steps are as follows: After the IMU pose is obtained by fusing and solving the laser point cloud data of each frame and the IMU data of each frame acquired in real time through the iterative error extended Kalman filter algorithm, the laser point cloud data is spliced ​​to generate a point cloud map according to the laser radar pose calculated according to the IMU pose and the laser radar external parameters and the IMU external parameters; Calibrate the binocular fisheye camera using a camera distortion model according to each frame of the binocular fisheye image acquired in real time to obtain the internal parameters of the binocular fisheye camera and the external parameters of the binocular fisheye camera; After correcting the binocular fisheye image according to the internal parameters of the binocular fisheye camera to obtain a pinhole camera model image, the laser point cloud data is projected to obtain a projection point corresponding to each laser point cloud data; the laser point cloud data is classified according to the projection point to obtain a laser point pair; Performing visual odometer estimation based on the laser point pair and the laser radar pose to obtain optimal state information of the visual odometer; Coloring each of the laser point cloud data in the point cloud map according to the optimal state information and the pinhole camera model image to obtain color information corresponding to each of the laser point cloud data; The binocular fisheye camera includes a left fisheye camera and a right fisheye camera; the internal parameter of the binocular fisheye camera is a camera distortion parameter; the external parameter of the binocular fisheye camera is a coordinate transformation matrix of the camera; The laser point cloud data of each frame and the IMU data of each frame acquired in real time are fused and solved by iterative error extended Kalman filter algorithm to obtain the IMU posture, including the following steps: Based on the IMU data, the laser point cloud data are respectively subjected to motion distortion removal operations to obtain a plurality of target laser point cloud data; and a target laser point cloud data set is constructed based on all the target laser point cloud data; According to the target laser point cloud data set, a point cloud data set corresponding to each frame is obtained; the point cloud data set corresponding to each frame is traversed, and according to the coordinates of each target laser point cloud data, four reference plane points corresponding to the shortest distance in the plane distance are calculated to construct a reference plane; and the center point pj of the reference plane is obtained; and a total laser point cloud residual constraint r is constructed for each plane point pk in the current frame k; Perform integral calculation based on the first IMU data of the current frame k and the second IMU data of the next frame k+1 to obtain the IMU pose information at the k+1th frame; The laser radar posture calculated according to the IMU posture and the laser radar external parameters and the IMU external parameters includes the following steps: According to the state x of the kth frame k Perform a priori deduction to obtain the state x of the k+1th frame k+1 ; and according to the state quantity x of the kth frame k and the state quantity x of the k+1th frame k+1 Calculate the state error δx of the kth frame k ; According to the state error δx of the kth frame i Obtain the state prior value of the k+1th frame by calculating the state recursion equation According to the state prior value of the k+1th frame Calculate the prior covariance matrix According to the state recursion equation and the prior covariance matrix Calculate the a posteriori state error according to the measurement equation; According to the prior covariance matrix Calculate and obtain the measurement covariance matrix R; According to the state observation matrix H, the measurement covariance matrix R and the prior covariance matrix Calculate and obtain the Kalman filter gain; The Kalman filter gain is used to update the posterior state matrix of the k+1th frame. And based on the posterior state matrix of the k+1th frame The covariance matrix is ​​updated to obtain a posterior covariance matrix, the first iteration is completed and an iteration counter is set, and the iteration counter is incremented by one; and the above operation is repeated to record the iteration number; According to the posterior state matrix of the k+1th frame and the state prior value of the k+1th frame Calculate and obtain the amount of change E; and obtain the laser radar posture by repeating the above steps according to the preset maximum threshold value E' of the amount of change and the maximum threshold value of the number of iterations.

2. The method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar according to claim 1, characterized in that: The IMU posture information of the k+1th frame includes the IMU posture of the k+1th frame IMU velocity of the k+1th frame w v k+1 , IMU position of the k+1th frame w p k+1 .

3. The method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar according to claim 2, characterized in that: The plane distance is the distance from the plane point pk in the current frame k to each plane point in the plane laser point cloud set in the previous frame k-1.

4. The method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar according to claim 3 is characterized in that: The point cloud data set includes a planar laser point cloud set; The planar laser point cloud data set includes a plurality of planar points; the planar points are target laser point cloud data in the target laser point cloud data set.

5. The method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar according to claim 4, characterized in that: The step of classifying the laser point cloud data according to the projection points to obtain laser point pairs includes the following steps: Determine the projection point p k Whether the coordinate is negative or at the edge of the pinhole camera model image; If yes, the laser point cloud data is filtered out; if no, the laser point cloud data is divided into left 3D laser point pair ζ l Align with right 3D laser point r ; and according to the left 3d laser point to ζ l Align with right 3D laser point r The corresponding projection point p k Obtain the projection point p corresponding to the next frame through optical flow tracking k .

6. The method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar according to claim 5, characterized in that: The projection point p k Including the left fisheye camera projection point p kl With the right fisheye camera projection point p kr .

7. The method for generating a texture information point cloud map based on the fusion of a panoramic camera and a laser radar according to claim 6, characterized in that: Performing visual odometer estimation according to the laser point pair and the laser radar posture to obtain optimal state information of the visual odometer includes the following steps: Obtaining a reprojection error by calculation according to the laser point pair; Based on the reprojection error, the reprojection error is used as observation, and an iterative error Kalman filter is used to estimate the visual odometer to obtain the optimal state information of the visual odometer.

Images