Rapid robust three-dimensional reconstruction method based on multi-sensor tight coupling

Abstract

The invention discloses a rapid robust three-dimensional reconstruction method based on multi-sensor tight coupling. A laser radar with a built-in IMU, a visible light camera, an infrared camera, an MCU, a microcomputer, a battery and a display are included. The method has the beneficial effects that the laser radar SLAM subsystem, the visual SLAM subsystem and the infrared SLAM subsystem are organically fused mainly through a tight coupling algorithm, so that a tight coupling system of four sensors including an IMU, a laser radar, a visible light camera and an infrared camera is realized; according to the method, the gray level of a point after radar optimization is obtained through a patch pyramid and a point-to-point bilinear interpolation mode, then the luminosity error of a reference patch and a current frame is calculated, the pose after two times of optimization is obtained, and finally the pose after optimization is used as an infrared initial pose to optimize the pose of an infrared image. And finally, triple optimization of the attitude is realized, so that the robustness and stability of the system can be ensured, and the working conditions of the visual SLAM system, the laser SLAM system and the infrared SLAM system can be adjusted according to the actual environment.

Description

Technical Field

[0001] The invention relates to a multi-sensor fusion method, and in particular to a fast and robust three-dimensional reconstruction method based on multi-sensor tight coupling. Background Art

[0002] SLAM technology mainly includes some sensors such as cameras, lidar, IMU, etc., and the tight coupling algorithm can well achieve stable and robust operation of the system. The technology is mainly divided into three subsystems of visual SLAM, laser SLAM and infrared SLAM. Through multi-sensor fusion technology, the advantages of various sensors can be fully utilized to improve the overall performance and robustness of the SLAM system.

[0003] Regarding the principle of inertial (inertial measurement unit IMU) technology: Inertial SLAM uses an inertial measurement unit (IMU) to provide acceleration and angular velocity information about the robot's movement. By integrating this information, the robot's position information can be obtained. IMU is usually used in combination with other sensors (such as lidar, camera) to achieve multi-sensor fusion and improve the accuracy and robustness of positioning and map construction. Its main advantages: IMU is autonomous and continuous, can continuously provide motion information, and is independent of the external environment; when used in combination with other sensors, it can complement each other's shortcomings and improve the overall performance of the system. However, it still has the following shortcomings: The integration process of IMU will introduce cumulative errors, which need to be fused and corrected with other sensor data; the adaptability to dynamic environments is limited, and it needs to work in conjunction with other sensors to improve performance.

[0004] Regarding the principle of radar SLAM (lidar) technology: radar SLAM uses lidar as the main sensor to detect targets and measure distances by emitting laser beams. Lidar can provide high-precision, high-resolution distance and angle information, which is used to build a three-dimensional point cloud map of the environment. In the SLAM process, the lidar data is processed to extract feature points in the environment, and maps and positioning robots are built through feature matching and pose estimation. Its main advantages: lidar has high ranging accuracy, strong environmental adaptability, and is not affected by light; it can provide rich three-dimensional environmental information, which is conducive to building high-precision maps; it is suitable for various complex environments indoors and outdoors, including dynamic scenes. However, it still has the following shortcomings: the cost of lidar is relatively high, which may limit its popularity in some low-cost applications; the data processing volume is large, and there are certain requirements for computing resources.

[0005] Regarding the principle of visual SLAM technology: using the camera as the main sensor, by capturing images in the environment and extracting feature points (such as corners, edges, etc.) in the image, and then matching features between different frames to construct the motion trajectory of the feature points. Using the motion trajectory of these feature points, the position and posture of the camera, that is, the external parameters of the camera, can be estimated. Furthermore, based on the estimated camera posture, a three-dimensional map can be constructed, and the map can be optimized to improve the accuracy and robustness of the map. Its main advantages are: it does not rely on external sensors, and positioning and map construction can be achieved only through the camera; it is suitable for a variety of indoor and outdoor scenes and has broad application prospects; it can construct dense three-dimensional maps and provide rich environmental information. However, it still has the following shortcomings: it is sensitive to environmental factors such as lighting and texture, and its robustness needs to be improved; its performance is limited in large-scale, complex and dynamic scenes; it has high real-time requirements and requires efficient algorithms and hardware support.

[0006] For infrared SLAM: infrared sensors are used for environmental perception to obtain thermal radiation information in the environment, and then the image is subjected to noise reduction filtering and feature extraction and matching to obtain an infrared odometer. Its main advantages are: it can cope with low-light, texture-free, dusty and other environments that traditional visual sensors cannot cope with; compared with visible light cameras, infrared cameras can sense the thermal radiation characteristics of objects, so they can still provide stable image input at night or in low light conditions. However, it still has the following shortcomings: infrared images face intensity drift, resulting in periodic transmission interruptions and non-uniformity problems; infrared images have low contrast and high noise. Summary of the invention

[0007] The purpose of the present invention is to provide a method for fast and robust 3D reconstruction based on tight coupling of multiple sensors in view of the shortcomings and defects in the prior art. The method mainly integrates the laser radar SLAM subsystem, the visual SLAM subsystem and the infrared SLAM subsystem organically through a tight coupling algorithm, realizes a tight coupling system of four sensors including IMU, laser radar, visible light camera and infrared camera, obtains the grayscale of the radar-optimized points through a patch pyramid and bilinear interpolation of the points, and then calculates the photometric error between the reference patch and the current frame, thereby obtaining a twice-optimized pose, and finally uses the optimized pose as the infrared initial pose to optimize the pose of the infrared image, finally realizing triple optimization of the pose, thereby ensuring the robustness and stability of the system, and adjusting the working conditions of the three systems including visual SLAM, laser SLAM and infrared SLAM according to the actual environment.

[0008] To achieve the above-mentioned purpose, the present invention adopts the following technical scheme: a method for fast and robust three-dimensional reconstruction based on tight coupling of multiple sensors, which includes a laser radar with built-in IMU, a visible light camera, an infrared camera, an MCU, a microcomputer, a battery and a display: the microcomputer distributes the clock information to the MCU, and then the MCU sends a trigger signal to the laser radar through the serial port, and sends a trigger signal to the visible light camera and the infrared camera. When the laser radar (with built-in IMU), the visible light camera and the infrared camera receive the trigger signal, they start to collect data. After the data is collected, the collection time and the data are continuously transmitted to the microcomputer through the ROS communication mechanism. The microcomputer first frames the data according to the timestamp of the data and then performs subsequent processing. After the data is processed, the data is read from the ROS communication buffer for processing, and the battery is used for power supply, and the content is displayed on the display.

[0009] Furthermore, the microcomputer processes the data specifically as follows: first, forward propagation is performed on the built-in IMU of the laser radar according to the received data, and then reverse propagation is performed on the data reorganized by the laser radar according to the camera time to remove motion distortion;

[0010] Next, the distance from the point in the current frame to its adjacent plane is solved and Kalman filtering is performed to update the map. For mature planes, the 50 nearest lidar points are selected as candidates for visual map point generation. For immature planes, all lidar points are candidates and the points with the largest gradient in the current frame image are selected as candidate points. After visual update, the points with the smallest depth and uniform distribution are selected, and the current frame image is divided into a grid of 30*30 pixels. If the grid cell contains a visible map point projected here, and more than 20 frames have passed since the last patch was added, or its pixel position in the current frame deviates from the last patch added position by more than 40 pixels, the new patch is added to the existing visual map point. Finally, Update the map points, patches, and the normal vectors of patches. Select and process the visible light image under the same viewing angle. Then, project the map points scanned by the current frame onto the current frame after removing the singular points according to the ray projection. Match from near to far and update the visible light visual pose by minimizing the photometric error. Finally, apply the odometer obtained by the previous step optimization to the infrared data. After the infrared data is subjected to grayscale equalization to remove noise in the image and edge extraction, it is further optimized using the visible light visual pose corresponding to the current time. When the infrared alignment reaches the optimal value, output the odometer and further update the map points. After updating the odometer, color the point cloud with the infrared image and the visible light image of the current frame in a weighted manner, and finally update the voxel map.

[0011] Furthermore, the most important thing in the whole system is the connection and processing flow between different modal data. As for the connection between data, the infrared image, visible light image and point cloud are connected through the grid-to-voxel form. Specifically, after pre-calibration in the whole system, the infrared and visible light images are divided into 30*30 pixel grids, and then the nearby point clouds are found within the corresponding voxel range inside the grid as candidate points for map points. In order to prevent uneven distribution of candidate points selected from the point cloud, the pixel with the highest gradient inside the grid where there is no map point or uneven distribution is found as the map point, thereby obtaining the physical connection between different modalities.

[0012] Furthermore, the processing flow of the system data is specifically as follows: first, the IMU's prior data and point cloud are used to perform the first step of updating to obtain a relatively accurate posture and obtain map points and corresponding normal vectors; then the visible light image and the map points selected in the point cloud are updated through the multi-layer pyramid direct method to obtain a more accurate posture update in the second step and further optimize the map points; the third step is to optimize the posture of the infrared image and the map points optimized in the second step, by extracting the edges of the infrared image and performing edge matching with the help of map points, so as to obtain the optimal posture.

[0013] A method for fast and robust 3D reconstruction based on tight coupling of multiple sensors, characterized in that it includes the following specific schemes: It is mainly divided into three parts according to the system update order: point-to-plane point cloud update, multi-layer pyramid visual direct method, DoG edge extraction and point-to-plane ICP edge matching. The main idea is to construct and optimize the system state based on the error state iterative Kalman filter. In the point cloud update stage, the IMU data is first forward propagated to obtain the prior attitude and its covariance (Formula 1.1 and Formula 1.2), and then back-propagated to remove the point cloud distortion (Formula 1.3);

[0014]

[0015] in, is the error state of the system, mainly determined by the actual measurement x by the IMU i+1 and real location The residual consists of Is the error state The Jacobian matrix, F ω is the state error with respect to the noise ω i The Jacobian matrix of is the covariance matrix of the state error, Q is the process noise of the IMU; in, is the position of the radar point after distortion, It is the dedistortion transformation matrix from the attitude at the moment of radar point measurement to the attitude at the end of the IMU corresponding to the point cloud of this frame. It converts the point cloud from the radar coordinate system to the IMU coordinate system. is the position of the point cloud at the time of measurement; the next step is to use the data from each sensor to estimate the pose. First, the point cloud is processed using the prior pose of the IMU. Specifically, the surface features are used instead of the corner features in the entire point cloud map to speed up the processing. The point cloud of the new scan frame is projected to an inaccurate position in the world coordinate system through the prior pose of the IMU, and then the distance from these points to the plane formed by the nearest neighbor points is calculated to form the optimization term (Formula 1.4): in, is the normal vector of the plane formed by the neighboring points, It is the transformation from IMU coordinate system to world coordinate system. It is the conversion from radar coordinate system to IMU coordinate system, Lp j is the point cloud position after distortion removal, q j is a point on the neighboring plane, r l (x k ,Lp j ) is the residual from the radar point to the neighboring plane after dedistortion, which is theoretically zero; for visible light image data, the map points generated by radar point cloud data are used to perform multi-layer pyramid direct method on the visible light image from coarse to fine to further align the image, and in this process, the grayscale residual of the newly added image and the reference image patch is obtained (Formula 1.5): Among them, I i is the grayscale value of the pixel in the current frame, π(*) is the pinhole projection model of the visible light camera, is the coordinate transformation matrix from the IMU coordinate system to the visible light camera, is the transformation matrix from the world coordinate system to the IMU coordinate system, G p i is the pixel sampled by the camera, A i is the transformation matrix from the reference patch to the camera coordinate system, Q i is the pixel of the reference patch, r c (x k ,Cp i ) is the grayscale residual between the current frame image and the reference frame image, which should be zero in theory. For infrared image processing, the edge of the infrared image is detected by Gaussian difference processing method, and then the edge matching is performed using the more accurate posture obtained in the previous step. In the matching process, the grayscale residual between the first added infrared image and the reference image is calculated (Formula 1.6), r T (x k ,Tp k )=I k π(T R G T P k + T t G )-A k π(u′)#(1.6), where, I k is the infrared grayscale value of the current frame, π(*) is the pinhole projection model of the infrared camera, T R G is the rotation from the world coordinate system to the infrared camera coordinate system, T P k is the pixel sampled by the infrared camera in the world coordinate system, T t G is the translation from the world coordinate system to the infrared camera coordinate system, A k is the transformation matrix from the reference edge to the edge of the current frame, u′ is the gray value of the reference edge, r T (x k ,Tp k ) is the grayscale residual between the reference edge and the edge of the current frame, which should be zero in theory; then the residual is optimized in turn through the LM method (Formula 1.7) until the system error state reaches the minimum value or the number of iterations reaches the maximum value. In this process, the Kalman gain (Formula 1.8) and the system error state (Formula 1.9) are continuously updated: in, is the least squares term of the IMU, is the covariance matrix of the IMU. is the least squares term of the lidar, Σ l is the covariance matrix of the radar measurements, is the least squares term of the visible light camera, Σ c is the covariance matrix of the visible light camera measurements, is the least squares term of the infrared camera, Σ T is the covariance matrix of the infrared camera measurements; Where K is the Kalman gain, H κ is the Jacobian matrix of the iterative process measurement, and R is the measurement noise matrix; Among them, z κ is the measurement of the system, is the system state calculated at the κ+1th iteration, is the expected state of the system. After the optimization is completed, the system state and its corresponding covariance are updated (Formula 1.10), followed by map update and the next round of data processing: in, System status at the last iteration That is the optimal state, The covariance matrix corresponding to the optimal state.

[0016] After adopting the above technical scheme, the beneficial effects of the present invention are as follows: the method mainly integrates the laser radar SLAM subsystem and the visual SLAM subsystem with the infrared SLAM subsystem organically through a tightly coupled algorithm, realizes a tightly coupled system of four sensors including IMU, laser radar, visible light camera and infrared camera, obtains the grayscale of the radar-optimized points through a patch pyramid and bilinear interpolation of the points, and then calculates the photometric error between the reference patch and the current frame, thereby obtaining the posture after twice optimization, and finally uses the optimized posture as the infrared initial posture to optimize the posture of the infrared image, and finally realizes triple optimization of the posture, which can ensure the robustness and stability of the system, and can adjust the working conditions of the three systems of visual SLAM, laser SLAM and infrared SLAM according to the actual environment. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0018] Figure 1 It is a hardware structure diagram of the present invention.

[0019] Figure 2 It is a system block diagram of the present invention.

[0020] Figure 3 It is the multimodal data connection diagram in the present invention.

[0021] Figure 4 It is a top view verified by experiments in the present invention.

[0022] Figure 5 It is a front view verified by experiments in the present invention. DETAILED DESCRIPTION

[0023] The technical solution adopted in this specific implementation is: Figure 1 As shown, a method for fast and robust 3D reconstruction based on tight coupling of multiple sensors is characterized in that it includes a laser radar with a built-in IMU, a visible light camera, an infrared camera, an MCU, a microcomputer, a battery and a display:

[0024] The microcomputer distributes the clock information to the MCU, and then the MCU sends a trigger signal to the laser radar through the serial port, and sends a trigger signal to the visible light camera and infrared camera. When the laser radar (built-in IMU), visible light camera and infrared camera receive the trigger signal, they start to collect data. After collecting the data, the collection time and data are continuously transmitted to the microcomputer through the ros communication mechanism. The microcomputer first frames the data according to the timestamp of the data and then performs subsequent processing. After the data is processed, it is read from the ros communication buffer for processing. The battery is used for power supply and the content is displayed on the display.

[0025] For more specific instructions, see Figure 2 As shown, the microcomputer processes data specifically as follows:

[0026] According to the received data, the built-in IMU of the lidar is first forward propagated, and then the data reorganized by the lidar according to the camera time is back-propagated to remove motion distortion;

[0027] Then, the distance from the point in the current frame to its adjacent plane is solved and Kalman filtering is performed to update the map. For mature planes, the nearest 50 lidar points are selected as candidates for visual map point generation. For immature planes, all lidar points are candidates and the points with the largest gradient in the current frame image are selected as candidate points. After visual update, the points with the smallest depth and uniform distribution are selected, and the current frame image is divided into a grid of 30*30 pixels. If the grid unit contains the projected visual map point here, and more than 20 frames have passed since the last patch was added, or its pixel position in the current frame deviates from the last patch added position by more than 40 pixels, the new patch is added to the existing visual map point. Finally, the map point, patch and patch normal vector are updated, the visible light image under the same viewing angle is selected and processed, and then the map points scanned by the current frame are projected onto the current frame after removing the singular points according to the ray projection, matching is performed from near to far and the visible light visual pose is updated by minimizing the photometric error.

[0028] Finally, the odometry obtained from the previous step is applied to the infrared data. After the infrared data is grayscale equalized to remove noise from the image and edge extracted, it is further optimized using the visible light visual pose corresponding to the current time. When the infrared alignment reaches the optimal value, the odometry is output and the map points are further updated. After the odometry is updated, the infrared image and visible light image of the current frame are used to color the point cloud in a weighted manner, and finally the voxel map is updated.

[0029] For more specific instructions, see Figure 3As shown, the most important thing in the whole system is the connection and processing flow between different modal data. For the connection between data, infrared images, visible light images and point clouds are connected through grid to voxel form. Specifically, after pre-calibration in the whole system, infrared and visible light images are divided into 30*30 pixel grids, and then the near point cloud is found in the corresponding voxel range inside the grid as the candidate point of the map point. In order to prevent the uneven distribution of candidate points selected from the point cloud, the pixel with the highest gradient inside the grid is found as the map point in the grid without map points or uneven distribution. The physical connection between different modes is obtained, and the processing flow of system data is as follows:

[0030] 1) First, the IMU prior data and point cloud are used to perform the first step of updating to obtain a more accurate pose and map point and corresponding normal vector;

[0031] 2) Then, the map points selected from the visible light image and the point cloud are updated through the multi-layer pyramid direct method to obtain a more accurate pose update in the second step and further optimize the map points;

[0032] 3) The third step is to optimize the pose of the infrared image and the map points optimized in the second step. The optimal pose is obtained by extracting the edges of the infrared image and performing edge matching with the help of map points.

[0033] See also Figure 2 As shown, a method for fast and robust 3D reconstruction based on tight coupling of multiple sensors is characterized in that it includes the following specific schemes:

[0034] According to the system update sequence, it is mainly divided into three parts: point-to-plane point cloud update, multi-layer pyramid visual direct method, DoG edge extraction and point-to-plane ICP edge matching. The main idea is to construct and optimize the system state based on the error state iterative Kalman filter. In the point cloud update stage, the IMU data is first forward propagated to obtain the prior attitude and its covariance (Formula 1.1 and Formula 1.2), and then back-propagated to remove point cloud distortion (Formula 1.3);

[0035]

[0036] in, is the error state of the system, mainly determined by the actual measurement x by the IMU i+1 and real location The residual consists of Is the error state The Jacobian matrix, F ω is the state error with respect to the noise ω i The Jacobian matrix of is the covariance matrix of the state error, Q is the process noise of the IMU;

[0037]

[0038] in, is the position of the radar point after distortion, It is the dedistortion transformation matrix from the attitude at the moment of radar point measurement to the attitude at the end of the IMU corresponding to the point cloud of this frame. It converts the point cloud from the radar coordinate system to the IMU coordinate system. is the position of the point cloud at the time of measurement;

[0039] Next, we use the data from each sensor to estimate the pose. First, we use the prior pose of the IMU to process the point cloud. Specifically, we use surface features instead of corner features in the entire point cloud map to speed up the processing. The point cloud of the new scan frame is projected to an inaccurate position in the world coordinate system through the prior pose of the IMU. Then, the distance from these points to the plane formed by the nearest neighbor points is calculated to form the optimization term (Formula 1.4):

[0040]

[0041] in, is the normal vector of the plane formed by the neighboring points, It is the transformation from IMU coordinate system to world coordinate system. It is the conversion from radar coordinate system to IMU coordinate system, Lp j is the point cloud position after distortion removal, q j is a point on the neighboring plane, r l (x k ,Lp j ) is the residual error from the radar point to the neighboring plane after dedistortion, which is theoretically zero;

[0042] For processing visible light image data, specifically, the map points generated by radar point cloud data are used to perform multi-layer pyramid direct method on visible light images from coarse to fine to further align the images, and in this process, the grayscale residual of the newly added image and the reference image patch is obtained (Formula 1.5):

[0043]

[0044] Among them, I i is the grayscale value of the pixel in the current frame, π(*) is the pinhole projection model of the visible light camera, is the coordinate transformation matrix from the IMU coordinate system to the visible light camera, is the transformation matrix from the world coordinate system to the IMU coordinate system, G p i is the pixel sampled by the camera, Ai is the transformation matrix from the reference patch to the camera coordinate system, Q i is the pixel of the reference patch, r c (x k ,Cp i ) is the grayscale residual between the current frame image and the reference frame image, which should be zero in theory;

[0045] For infrared image processing, the edge of the infrared image is detected by Gaussian difference processing method, and then the edge matching is performed using the more accurate posture obtained in the previous step. During the matching process, the grayscale residual between the first added infrared image and the reference image is calculated (Formula 1.6)

[0046] r T (x k ,Tp k )=I k π( T R G T P k + T t G )-A k π(u′)# (1.6)

[0047] Among them, I k is the infrared grayscale value of the current frame, π(*) is the pinhole projection model of the infrared camera, T R G is the rotation from the world coordinate system to the infrared camera coordinate system, T P k is the pixel sampled by the infrared camera in the world coordinate system, T t G is the translation from the world coordinate system to the infrared camera coordinate system, A k is the transformation matrix from the reference edge to the edge of the current frame, u′ is the gray value of the reference edge, r T (x k ,Tp k ) is the grayscale residual between the reference edge and the edge of the current frame, which should be zero in theory;

[0048] Then the residuals are optimized in turn through the LM method (Formula 1.7) until the system error state reaches the minimum value or the number of iterations reaches the maximum value. In this process, the Kalman gain (Formula 1.8) and the system error state (Formula 1.9) are continuously updated:

[0049]

[0050] in, is the least squares term of the IMU, is the covariance matrix of the IMU. is the least squares term of the lidar, Σ l is the covariance matrix of the radar measurements, is the least squares term of the visible light camera, Σ c is the covariance matrix of the visible light camera measurements, is the least squares term of the infrared camera, Σ T is the covariance matrix of the infrared camera measurements;

[0051]

[0052] Where K is the Kalman gain, H κ is the Jacobian matrix of the iterative process measurement, and R is the measurement noise matrix;

[0053]

[0054] Among them, z κ is the measurement of the system, is the system state calculated at the κ+1th iteration, is the desired state of the system,

[0055] After the optimization is completed, the system state and its corresponding covariance are updated (Formula 1.10), followed by map update and the next round of data processing:

[0056]

[0057] in, System status at the last iteration That is the optimal state, The covariance matrix corresponding to the optimal state.

[0058] The following are the relevant experiments of the present invention

[0059] After completing the construction of the multi-sensor system, a handheld device scanning and mapping experiment was carried out. The mapping site was a small forest next to the teaching building. The height of the forest was about 9 meters, and the forest data was collected from the 4th floor of the teaching building downwards.

[0060] See also Figure 4 As shown, it can be seen that there is no noise in the gaps and edges of the woods. From the coloring effects of the visible light image and the infrared image, it can be seen that after three pose optimizations, the constructed and colored map has a high accuracy.

[0061] See also Figure 5 As shown, it can be seen that the people under the woods are more obvious after coloring, and the edges of the cement floor next to the woods are very obvious after coloring.

[0062] The above description is only used to illustrate the technical solution of the present invention rather than to limit it. Other modifications or equivalent substitutions made to the technical solution of the present invention by ordinary technicians in this field should be included in the scope of the claims of the present invention as long as they do not depart from the spirit and scope of the technical solution of the present invention.

Claims

1. A method for fast and robust 3D reconstruction based on tight coupling of multiple sensors, characterized by: It includes a laser radar with built-in IMU, a visible light camera, an infrared camera, an MCU, a microcomputer, a battery and a display: The microcomputer distributes the clock information to the MCU, and then the MCU sends a trigger signal to the laser radar through the serial port, and sends a trigger signal to the visible light camera and infrared camera. When the laser radar (built-in IMU), visible light camera and infrared camera receive the trigger signal, they start to collect data. After collecting the data, the collection time and data are continuously transmitted to the microcomputer through the ros communication mechanism. The microcomputer first frames the data according to the timestamp of the data and then performs subsequent processing. After the data is processed, it is read from the ros communication buffer for processing. The battery is used for power supply and the content is displayed on the display.

2. The method for fast and robust 3D reconstruction based on multi-sensor tight coupling according to claim 1, characterized in that: The microcomputer processes data specifically as follows: According to the received data, the built-in IMU of the lidar is first forward propagated, and then the data reorganized by the lidar according to the camera time is back-propagated to remove motion distortion; Then, the distance from the point in the current frame to its adjacent plane is solved and Kalman filtering is performed to update the map. For mature planes, the nearest 50 lidar points are selected as candidates for visual map point generation. For immature planes, all lidar points are candidates and the points with the largest gradient in the current frame image are selected as candidate points. After visual update, the points with the smallest depth and uniform distribution are selected, and the current frame image is divided into a grid of 30*30 pixels. If the grid unit contains the projected visual map point here, and more than 20 frames have passed since the last patch was added, or its pixel position in the current frame deviates from the last patch added position by more than 40 pixels, the new patch is added to the existing visual map point. Finally, the map point, patch and patch normal vector are updated, the visible light image under the same viewing angle is selected and processed, and then the map points scanned by the current frame are projected onto the current frame after removing the singular points according to the ray projection, matching is performed from near to far and the visible light visual pose is updated by minimizing the photometric error. Finally, the odometry obtained from the previous step is applied to the infrared data. After the infrared data is grayscale equalized to remove noise from the image and edge extracted, it is further optimized using the visible light visual pose corresponding to the current time. When the infrared alignment reaches the optimal value, the odometry is output and the map points are further updated. After the odometry is updated, the infrared image and visible light image of the current frame are used to color the point cloud in a weighted manner, and finally the voxel map is updated.

3. The method for fast and robust 3D reconstruction based on multi-sensor tight coupling according to claim 1, characterized in that: The most important thing in the whole system is the connection and processing flow between different modal data. As for the connection between data, infrared images, visible light images and point clouds are connected through grid-to-voxel form. Specifically, after pre-calibration in the whole system, infrared and visible light images are divided into 30*30 pixel grids, and then the nearby point clouds are found within the corresponding voxel range inside the grid as candidate points for map points. In order to prevent uneven distribution of candidate points selected from the point cloud, the pixels with the highest gradient inside the grid where there are no map points or the grid is unevenly distributed are found as map points, thereby obtaining the physical connection between different modalities.

4. The method for fast and robust 3D reconstruction based on multi-sensor tight coupling according to claim 1, characterized in that: The processing flow of the system data is specifically as follows: 1) First, the IMU prior data and point cloud are used to perform the first step of updating to obtain a more accurate pose and map point and corresponding normal vector; 2) Then, the map points selected from the visible light image and the point cloud are updated through the multi-layer pyramid direct method to obtain a more accurate pose update in the second step and further optimize the map points; 3) The third step is to optimize the pose of the infrared image and the map points optimized in the second step. The optimal pose is obtained by extracting the edges of the infrared image and performing edge matching with the help of map points.

5. A method for fast and robust three-dimensional reconstruction based on tight coupling of multiple sensors, characterized in that: It includes the following specific plans: According to the system update sequence, it is mainly divided into three parts: point-to-plane point cloud update, multi-layer pyramid visual direct method, DoG edge extraction and point-to-plane ICP edge matching. The main idea is to construct and optimize the system state based on the error state iterative Kalman filter. In the point cloud update stage, the IMU data is first forward propagated to obtain the prior attitude and its covariance (Formula 1.1 and Formula 1.2), and then back-propagated to remove point cloud distortion (Formula 1.3); in, is the error state of the system, mainly determined by the actual measurement x by the IMU i+1 and real location The residual consists of Is the error state The Jacobian matrix, F ω is the state error with respect to the noise ω i The Jacobian matrix of is the covariance matrix of the state error, Q is the process noise of the IMU; in, is the position of the radar point after distortion, It is the dedistortion transformation matrix from the attitude at the moment of radar point measurement to the attitude at the end of the IMU corresponding to the point cloud of this frame. It converts the point cloud from the radar coordinate system to the IMU coordinate system. is the position of the point cloud at the time of measurement; Next, we use the data from each sensor to estimate the pose. First, we use the prior pose of the IMU to process the point cloud. Specifically, we use surface features instead of corner features in the entire point cloud map to speed up the processing. The point cloud of the new scan frame is projected to an inaccurate position in the world coordinate system through the prior pose of the IMU. Then, the distance from these points to the plane formed by the nearest neighbor points is calculated to form the optimization term (Formula 1.4): in, is the normal vector of the plane formed by the neighboring points, It is the transformation from IMU coordinate system to world coordinate system. It is the conversion from radar coordinate system to IMU coordinate system, Lp j is the point cloud position after distortion removal, q j is a point on the neighboring plane, r l (x k ,Lp j ) is the residual error from the radar point to the neighboring plane after dedistortion, which is theoretically zero; For processing visible light image data, specifically, the map points generated by radar point cloud data are used to perform multi-layer pyramid direct method on visible light images from coarse to fine to further align the images, and in this process, the grayscale residual of the newly added image and the reference image patch is obtained (Formula 1.5): Among them, I i is the grayscale value of the pixel in the current frame, π(*) is the pinhole projection model of the visible light camera, is the coordinate transformation matrix from the IMU coordinate system to the visible light camera, is the transformation matrix from the world coordinate system to the IMU coordinate system, G p i is the pixel sampled by the camera, A i is the transformation matrix from the reference patch to the camera coordinate system, Q i is the pixel of the reference patch, r c (x k ,Cp i ) is the grayscale residual between the current frame image and the reference frame image, which should be zero in theory; For infrared image processing, the edge of the infrared image is detected by Gaussian difference processing method, and then the edge matching is performed using the more accurate posture obtained in the previous step. During the matching process, the grayscale residual between the first added infrared image and the reference image is calculated (Formula 1.6) r T ( x k ,Tp k )=I k π( T R G T P k + T t G )-TO k π(u′)# 1.6) Among them, I k is the infrared grayscale value of the current frame, π(*) is the pinhole projection model of the infrared camera, T R G is the rotation from the world coordinate system to the infrared camera coordinate system, T P k is the pixel sampled by the infrared camera in the world coordinate system, T t G is the translation from the world coordinate system to the infrared camera coordinate system, A k is the transformation matrix from the reference edge to the edge of the current frame, u′ is the gray value of the reference edge, r T (x k ,Tp k ) is the grayscale residual between the reference edge and the edge of the current frame, which should be zero in theory; Then the residuals are optimized in turn through the LM method (Formula 1.7) until the system error state reaches the minimum value or the number of iterations reaches the maximum value. In this process, the Kalman gain (Formula 1.8) and the system error state (Formula 1.9) are continuously updated: in, is the least squares term of the IMU, is the covariance matrix of the IMU. is the least squares term of the lidar, Σ l is the covariance matrix of the radar measurements, is the least squares term of the visible light camera, Σ c is the covariance matrix of the visible light camera measurements, is the least squares term of the infrared camera, Σ T is the covariance matrix of the infrared camera measurements; Where K is the Kalman gain, H κ is the Jacobian matrix of the iterative process measurement, and R is the measurement noise matrix; in, is the measurement of the system, is the system state calculated at the κ+1th iteration, is the desired state of the system, After the optimization is completed, the system state and its corresponding covariance are updated (Formula 1.10), followed by map update and the next round of data processing: in, System status at the last iteration That is the optimal state, The covariance matrix corresponding to the optimal state.

Images