A method for fast robust 3d reconstruction based on multi-sensor tight coupling

Abstract

A kind of method of fast robust three-dimensional reconstruction based on multi-sensor tight coupling, it includes laser radar with built-in IMU, visible light camera, infrared camera, MCU, microcomputer, battery and display.The present application has the beneficial effects that: the method mainly fuses laser radar SLAM subsystem and visual SLAM subsystem and infrared SLAM subsystem organically through tight coupling algorithm, realizes the tight coupling system of four sensors of IMU, laser radar, visible light camera, infrared camera, obtains the gray of point by the way of patch pyramid and the bilinear interpolation of point after the optimization of radar, then calculates the photometric error of reference patch and current frame, to obtain the optimized twice optimized pose, finally the optimized pose is used as the initial pose of infrared to optimize the pose of infrared image, finally realizes the triple optimization of attitude, so as to guarantee the robustness, stability of system, and can adjust the working condition of visual SLAM, laser SLAM and infrared SLAM three systems according to actual environment.

Description

Technical Field

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

[0002] SLAM technology primarily involves sensors such as cameras, lidar, and IMUs. Tightly coupled algorithms enable stable and robust system operation. The technology is categorized into three tightly coupled subsystems: visual SLAM, laser SLAM, and infrared SLAM. Multi-sensor fusion leverages the strengths of various sensors 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 and cameras) to achieve multi-sensor fusion and improve the accuracy and robustness of positioning and map construction. Its main advantages: IMU is autonomous and continuous, and can continuously provide motion information without relying on 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; its adaptability to dynamic environments is limited, and it needs to work together with other sensors to improve performance.

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

[0005] The principle of visual SLAM technology is to use a camera as the primary sensor, capture images of the environment, extract feature points (such as corners and edges), and then perform feature matching between frames to construct the motion trajectories of the feature points. The motion trajectories of these feature points can be used to estimate the camera's position and pose, namely its extrinsic parameters. Furthermore, based on the estimated camera pose, a three-dimensional map can be constructed and optimized to improve its accuracy and robustness. Its main advantages are: it does not rely on external sensors, and can achieve positioning and mapping solely through the camera; it is suitable for a variety of indoor and outdoor scenarios 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; and it has high real-time requirements, requiring efficient algorithms and hardware support.

[0006] Infrared SLAM uses infrared sensors for environmental perception, acquiring thermal radiation information from the environment. The image is then subjected to noise reduction filtering and feature extraction and matching, resulting in an infrared odometry system. Its main advantages include its ability to handle low-light, textureless, and dusty environments, which are challenging for traditional visual sensors. Compared to visible light cameras, infrared cameras can sense the thermal radiation signatures of objects, providing stable image input even at night or in low-light conditions. However, it still has the following drawbacks: infrared images suffer from intensity drift, leading to periodic interruptions and non-uniformity in image transmission; and they also suffer from low contrast and high noise. Summary of the Invention

[0007] The purpose of the present invention is to address the shortcomings and defects in the prior art and provide a method for fast and robust 3D reconstruction based on multi-sensor tight coupling. The method mainly integrates the lidar SLAM subsystem, the visual SLAM subsystem and the infrared SLAM subsystem through a tight coupling algorithm to realize a tight coupling system of four sensors: IMU, lidar, visible light camera and infrared camera. The grayscale of the radar-optimized points is obtained by patch pyramid and bilinear interpolation of the points, and then the photometric error between the reference patch and the current frame is calculated to obtain a twice-optimized pose. Finally, the optimized pose is used as the infrared initial pose to optimize the pose of the infrared image, and finally the triple optimization of the pose is achieved. In this way, the robustness and stability of the system can be guaranteed, and the working conditions of the three systems of visual SLAM, laser SLAM and infrared SLAM can be adjusted according to the actual environment.

[0008] To achieve the above-mentioned purpose, the present invention adopts the following technical solutions: a method for fast and robust three-dimensional reconstruction based on tight coupling of multiple sensors, which is based on a fast and robust three-dimensional reconstruction system based on tight coupling of multiple sensors, the system including a laser radar with a 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 a built-in IMU, the visible light camera and the infrared camera receive the trigger signal, they start to collect data. After collecting the data, 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, it reads the data from the ROS communication buffer for processing, and the battery is used for power supply, and the content is displayed on the display. The microcomputer processes the data specifically as follows: according to the received data, first forward propagates the built-in IMU of the laser radar, and then backpropagates the data reorganized according to the camera time to remove motion distortion;

[0009] Then solve the distance from the point in the current frame to its adjacent plane and perform Kalman filtering to update the map. For mature planes, select the 50 nearest lidar points 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 the visual update, select the points with the smallest depth and even distribution, and divide the current frame image into a grid of 30*30 pixels. If the grid cell 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 addition position by more than 40 pixels, add the new patch to the existing visual map point. Update the map points, patches, and patch normals, select and process visible light images from the same perspective, and then project the map points scanned by the current frame onto the current frame after removing singular points according to ray projection. Match from near to far and update the visible light visual pose by minimizing the photometric error. Finally, apply the odometry obtained in the previous step to the infrared data, so that after the infrared data is grayscale equalized to remove noise in 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 point, the odometry is output and the map points are further updated. After the odometry is updated, the infrared image and the 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.

[0010] Furthermore, the most important thing in the entire 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 in the form of grid to voxel. Specifically, after pre-calibration in the entire 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 the candidate points selected from the point cloud from being unevenly distributed, the pixel with the highest gradient inside the grid where there is no map point or the distribution is uneven is found as the map point, thereby deriving the physical connection between different modalities.

[0011] Furthermore, the system data processing flow is as follows: first, the IMU's prior data and point cloud are used to perform the first update to obtain a relatively accurate pose and obtain the 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 pose update in the second step and further optimize the map points; the third step is to optimize the pose of the infrared image and the map points optimized in the second step, and obtain the optimal pose by extracting the edges of the infrared image and performing edge matching with the help of map points.

[0012] A method for fast and robust 3D reconstruction based on tight coupling of multiple sensors is characterized by including the following specific scheme: 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 error state iterative Kalman filtering. In the point cloud update stage, the IMU data is first forward propagated according to formulas 1.1 and 1.2 to obtain the prior attitude and its covariance, and then backpropagated according to formula 1.3 to remove point cloud distortion. 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 composition is Is the error state The Jacobian matrix, F W is the state error with respect to the noise W 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 current frame point cloud. It converts the point cloud from the radar coordinate system to the IMU coordinate system. 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, surface features are used instead of corner features in the entire point cloud map to speed up the processing. The point cloud of the newly scanned frame is projected to a less accurate 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 according to formula 1.4 to form the optimization term: 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 nearest plane after dedistortion, which is theoretically zero; for visible light image data, the map points generated by the radar point cloud data are used to perform a multi-layer pyramid direct method on the visible light image from coarse to fine to further align the images, and the grayscale residual of the newly added image and the reference image patch is obtained in this process according to formula 1.5: Among them, I i is the grayscale value of the current frame pixel, π(*) 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 the 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 according to formula 1.6, r T (x k ,Tp k )=I k π( T R G T P k + T t G )-Ak π(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 grayscale value of the reference edge, r T (x k ,Tp k ) is the grayscale residual between the reference edge and the current frame edge, which should theoretically be zero. Then, the residuals are optimized in sequence using the LM method according to formula 1.7 until the system error state reaches a minimum or the number of iterations reaches a maximum. During this process, the Kalman gain is continuously updated according to formula 1.8 and the system error state according to formula 1.9: 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 a measurement of the system, is the expected state of the computational system 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 according to formula 1.10, followed by map update and the next round of data processing: in, The system status of the last iteration That is the optimal state, The covariance matrix corresponding to the optimal state.

[0013] After adopting the above technical scheme, the beneficial effects of the present invention are as follows: this method mainly organically integrates the lidar SLAM subsystem and the visual SLAM subsystem with the infrared SLAM subsystem through a tight coupling algorithm, realizing a tight coupling system of four sensors including IMU, lidar, visible light camera and infrared camera, obtaining the grayscale of the radar-optimized points through a patch pyramid and bilinear interpolation of the points, and then calculating the photometric error between the reference patch and the current frame to obtain the twice-optimized pose, and finally using the optimized pose as the infrared initial pose to optimize the pose of the infrared image, ultimately achieving triple optimization of the pose, 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

[0014] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. 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 any creative work.

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

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

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

[0018] Figure 4 It is a top view of the experimental verification in the present invention.

[0019] Figure 5 It is a front view of the experimental verification in the present invention. DETAILED DESCRIPTION

[0020] The technical solution adopted in this specific embodiment is: Figure 1 As shown in FIG, a method for fast and robust 3D reconstruction based on tight coupling of multiple sensors is provided. The method is based on a fast and robust 3D reconstruction system based on tight coupling of multiple sensors. The system includes a lidar with a built-in IMU, a visible light camera, an infrared camera, an MCU, a microcomputer, a battery, and a display.

[0021] The microcomputer distributes the clock information to the MCU, which then sends a trigger signal to the lidar through the serial port, and to the visible light camera and infrared camera. When the lidar (with built-in IMU), visible light camera and infrared camera receive the trigger signal, they start collecting 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.

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

[0023] Based on 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;

[0024] Then solve the distance from the point in the current frame to its adjacent plane and perform Kalman filtering to update the map. For mature planes, select the 50 nearest lidar points 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 the visual update, select the points with the smallest depth and even distribution, and divide the current frame image into a grid of 30*30 pixels. If the grid unit contains the projected visual map point, 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 addition position by more than 40 pixels, add the new patch to the existing visual map point. Finally, update the map point, patch, and patch normal vector. Select and process the visible light image under the same perspective, and then project the map points scanned by the current frame onto the current frame after removing singular points according to ray projection. Match from near to far and update the visible light visual pose by minimizing the photometric error.

[0025] Finally, the odometry obtained from the previous step is applied to the infrared data. After grayscale equalization to remove noise from the image and edge extraction, the infrared data 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 weighted to color the point cloud, and finally the voxel map is updated.

[0026] For more specific instructions, see Figure 3As shown in the figure, the most important thing in the whole system is the connection and processing flow between different modal data. 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 near point cloud is found within 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 where there is no map point or the distribution is uneven is found as the map point. In this way, the physical connection between different modalities is obtained, and the specific processing flow of the system data is as follows:

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

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

[0029] 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.

[0030] See 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 solutions:

[0031] The system update sequence 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 according to formulas 1.1 and 1.2 to obtain the prior attitude and its covariance, and then backpropagated according to formula 1.3 to remove point cloud distortion;

[0032]

[0033] 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 composition is Is the error state The Jacobian matrix, F W is the state error with respect to the noise W i The Jacobian matrix of is the covariance matrix of the state error, Q is the process noise of the IMU;

[0034]

[0035] 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 current frame point cloud. It converts the point cloud from the radar coordinate system to the IMU coordinate system. is the position of the point cloud at the moment of measurement;

[0036] Next, we use the data from various sensors to estimate the pose. First, we use the IMU's prior pose to process the point cloud. Specifically, we use surface features instead of corner features in the entire point cloud map to speed up processing. The point cloud of the newly scanned frame is projected to a less accurate position in the world coordinate system using the IMU's prior pose. Then, according to formula 1.4, the distance from these points to the plane formed by the nearest neighbor points is calculated to form the optimization term:

[0037]

[0038] 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 nearest neighbor plane after dedistortion, which is theoretically zero;

[0039] 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 the visible light image, and further image alignment is performed from coarse to fine. In this process, the grayscale residual of the newly added image and the reference image patch is obtained according to formula 1.5:

[0040]

[0041] Among them, I i is the grayscale value of the current frame pixel, π(*) 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;

[0042] For infrared image processing, the edge of the infrared image is detected by the 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 according to formula 1.6

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

[0044] 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 grayscale value of the reference edge, r T (x k ,Tp k ) is the grayscale residual between the reference edge and the current frame edge, which should be zero in theory;

[0045] Then, according to formula 1.7, the LM method residuals are optimized in sequence until the system error state reaches the minimum value or the number of iterations reaches the maximum value. During this process, the Kalman gain is continuously updated according to formula 1.8 and the system error state according to formula 1.9:

[0046]

[0047] 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;

[0048]

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

[0050]

[0051] in, is a measurement of the system, is the expected state of the computational system at the κ+1th iteration, is the desired state of the system,

[0052] After the optimization is completed, the system state and its corresponding covariance are updated according to formula 1.10, followed by map update and the next round of data processing:

[0053]

[0054] in, The system status of the last iteration That is the optimal state, The covariance matrix corresponding to the optimal state.

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

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

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

[0058] See 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.

[0059] The above description is only used to illustrate the technical solution of the present invention and is not intended 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 is based on a fast and robust 3D reconstruction system with tight coupling of multiple sensors, which includes a lidar 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, which then sends trigger signals to the lidar, visible light camera, and infrared camera through the serial port. When the lidar, visible light camera, and infrared camera with the built-in IMU receive the trigger signal, they start collecting 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 data timestamp 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. The microcomputer processes the data specifically as follows: Based on 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 solve the distance from the point in the current frame to its adjacent plane and perform Kalman filtering to update the map. For mature planes, select the 50 nearest lidar points 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 the visual update, select the points with the smallest depth and even distribution, and divide the current frame image into a grid of 30*30 pixels. If the grid unit contains the projected visual map point, 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 addition position by more than 40 pixels, add the new patch to the existing visual map point. Finally, update the map point, patch, and patch normal vector. Select and process the visible light image under the same perspective, and then project the map points scanned by the current frame onto the current frame after removing singular points according to ray projection. Match from near to far and update the visible light visual pose by minimizing the photometric error. Finally, the odometry obtained from the previous step is applied to the infrared data. After grayscale equalization to remove noise from the image and edge extraction, the infrared data 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 weighted to color the point cloud, and finally the voxel map is updated.

2. 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 entire system is the connection and processing flow between different modal data. Regarding the connection between data, the infrared image, visible light image and point cloud are connected in the form of grid to voxel. Specifically, after pre-calibration in the entire system, the infrared and visible light images are divided into 30*30 pixel grids, and then the nearby point cloud is found within the corresponding voxel range within the grid as the candidate point of the map point. To prevent the candidate points selected from the point cloud from being unevenly distributed, the pixel with the highest gradient within the grid where there is no map point or the distribution is uneven is found as the map point, thereby deriving the physical connection between different modalities.

3. The method for fast and robust 3D reconstruction based on multi-sensor tight coupling according to claim 1, characterized in that: The specific process of processing system data is as follows: 1) First, the IMU prior data and point cloud are used to perform the first step of updating to obtain a relatively accurate pose and obtain the map point and the corresponding normal vector; 2) Then, the map points selected from the visible light image and the point cloud are updated using 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.

4. A method for fast and robust 3D reconstruction based on tight coupling of multiple sensors, characterized in that: It includes the following specific plans: The system update sequence 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 according to formulas 1.1 and 1.2 to obtain the prior attitude and its covariance, and then backpropagated according to formula 1.3 to remove point cloud distortion; 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 composition is Is the error state The Jacobian matrix, F W is the state error with respect to the noise W 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 current frame point cloud; It converts the point cloud from the radar coordinate system to the IMU coordinate system. is the position of the point cloud at the moment of measurement; Next, we use the data from various sensors to estimate the pose. First, we use the IMU's prior pose to process the point cloud. Specifically, we use surface features instead of corner features in the entire point cloud map to speed up processing. The point cloud of the newly scanned frame is projected to a less accurate position in the world coordinate system using the IMU's prior pose. Then, according to formula 1.4, the distance from these points to the plane formed by the nearest neighbor points is calculated to form the optimization term: 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 nearest neighbor 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 the visible light image, and further image alignment is performed from coarse to fine. In this process, the grayscale residual of the newly added image and the reference image patch is obtained according to formula 1.5: Among them, I i is the grayscale value of the current frame pixel, π(*) 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 is theoretically zero; For infrared image processing, the edge of the infrared image is detected by the 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 according to 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 grayscale value of the reference edge, r T (x k ,Tp k ) is the grayscale residual between the reference edge and the current frame edge, which is theoretically zero; Then, according to formula 1.7, the LM method residuals are optimized in sequence until the system error state reaches the minimum value or the number of iterations reaches the maximum value. During this process, the Kalman gain is continuously updated according to formula 1.8 and the system error state according to formula 1.9: 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 a measurement of the system, is the expected state of the computational system 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 according to formula 1.10, followed by map update and the next round of data processing: in, The system status of the last iteration That is the optimal state, The covariance matrix corresponding to the optimal state.

Images