Multi-sensor positioning and mapping method, device, computer equipment and medium

Abstract

The present application relates to a multi-sensor positioning and mapping method, device, computer equipment and medium, wherein the method includes: obtaining the spatial transformation relationship between different types of sensors, pre-integrating IMU data to obtain IMU prior pose, performing optical flow tracking on preset tracking points in the image collected by the camera to obtain optical flow tracking results; obtaining the initial pose of the laser radar and initializing the laser radar based on the IMU prior pose and spatial transformation relationship, obtaining tracking points with depth information based on the optical flow tracking results and the point cloud data collected by the laser radar after initialization; obtaining the camera pose based on the tracking points with depth information, performing loop detection and verification using the camera pose, and obtaining loop constraints; obtaining the optimal pose of each type of sensor in multi-sensor fusion based on the loop constraints, camera pose, IMU prior pose and laser radar initial pose. The entire scheme can achieve accurate multi-sensor positioning and mapping.

Description

Technical Field

[0001] The present application relates to the field of computer vision technology, and in particular to a multi-sensor positioning and mapping method, apparatus, computer equipment and medium. Background Art

[0002] With the rapid development of science and technology, technologies such as virtual reality (VR), augmented reality (AR), mixed reality (MR), autonomous driving, and robot navigation are gradually penetrating into every aspect of our lives. These technologies not only provide users with a more immersive experience, but also show great application potential in many fields such as industrial automation, architectural surveying and mapping, and cultural heritage protection. However, to achieve the widespread application of these technologies, a key technical challenge is how to achieve high-precision positioning and map construction of the scene.

[0003] Traditional positioning and mapping methods mainly rely on single sensors, such as GPS, LiDAR or cameras. However, these methods often have many limitations in practical applications. For example, GPS cannot work properly indoors or in signal-blocked areas; although LiDAR can provide high-precision distance information, it is greatly affected by ambient light and weather; cameras are easily affected by lighting conditions, obstructions and image noise, resulting in reduced positioning accuracy.

[0004] Therefore, how to provide an accurate positioning and mapping solution has become a technical problem that urgently needs to be solved. Summary of the invention

[0005] Based on this, it is necessary to provide an accurate multi-sensor positioning and mapping method, device, computer equipment, storage medium and computer program product to address the above technical problems.

[0006] In a first aspect, the present application provides a multi-sensor positioning and mapping method. The method comprises:

[0007] Obtaining spatial transformation relationships between different types of sensors, including lidar, IMU (Inertial Measurement Unit) and camera;

[0008] The IMU data is pre-integrated to obtain the IMU prior pose, and the preset tracking points in the image captured by the camera are optically tracked to obtain the optical flow tracking result;

[0009] According to the IMU prior pose and the spatial transformation relationship, the initial pose of the laser radar is obtained, and the laser radar is initialized, and the tracking points with depth information are obtained based on the optical flow tracking results and the point cloud data collected by the laser radar after the initialization;

[0010] Acquire a camera pose according to the tracking point with depth information, and use the camera pose to perform loop detection and verification to obtain loop constraints;

[0011] According to the loop constraint, the camera pose, the IMU prior pose and the lidar initial pose, the optimal pose of each type of sensor in multi-sensor fusion is obtained.

[0012] In one embodiment, obtaining the initial posture of the laser radar according to the IMU prior posture and the spatial transformation relationship, and initializing the laser radar, and obtaining the tracking points with depth information based on the optical flow tracking results and the point cloud data collected by the laser radar after initialization includes:

[0013] Obtaining an initial laser radar posture according to the IMU prior posture and the spatial transformation relationship;

[0014] Initializing the laser radar based on the initial position and posture of the laser radar, and obtaining point cloud data collected by the laser radar after initialization;

[0015] Based on the initial position of the laser radar and the spatial transformation relationship, the point cloud data is transformed into a camera coordinate system through external parameters between the laser radar and the camera to obtain point cloud depth information;

[0016] The point cloud depth information is bound to the tracking point determined by the optical flow tracking result to obtain the tracking point with depth information.

[0017] In one embodiment, obtaining the optimal pose of each type of sensor in multi-sensor fusion according to the loop constraint, the camera pose, the IMU prior pose, and the lidar initial pose includes:

[0018] Constructing an objective function of multi-sensor fusion according to the camera pose, the IMU prior pose and the lidar initial pose;

[0019] The objective function is iteratively optimized according to the loop constraint to obtain the optimal posture of each type of sensor in the multi-sensor fusion corresponding to minimizing the objective function.

[0020] In one embodiment, acquiring the camera pose according to the tracking point with depth information, performing loop detection and verification using the camera pose, and obtaining the loop constraint comprises:

[0021] Obtaining the camera pose according to the tracking points with depth information;

[0022] According to the preset threshold rules, the key frames in the images captured by the camera are filtered;

[0023] When a new key frame is added, the global descriptor of the key frame is extracted using a deep learning-based method;

[0024] Retrieving the global descriptor and the historical key frames to obtain a rough matching result;

[0025] Using a preset local feature extraction method to extract features from the rough matching result, and screening the best matching result;

[0026] The best matching result is verified using the camera pose, and if conditions are met, it is determined that a loop is detected and the loop constraint is recorded.

[0027] In one embodiment, acquiring the camera pose according to the tracking point having depth information comprises:

[0028] The camera position and posture are obtained according to the tracking points with depth information and by minimizing the reprojection error.

[0029] In one embodiment, acquiring the spatial transformation relationship between sensors of different types includes:

[0030] Use the Kalibr tool to calibrate the relative position and attitude between the laser radar, IMU and camera coordinate systems;

[0031] Based on the calibration results, the spatial transformation relationship between the lidar, IMU and camera is constructed.

[0032] In one embodiment, performing optical flow tracking on preset tracking points in an image captured by a camera to obtain an optical flow tracking result includes:

[0033] Get the image captured by the camera;

[0034] Convert the image captured by the camera into a grayscale image;

[0035] Performing Shi-Tomasi corner detection on the grayscale image to obtain corner feature points;

[0036] The corner feature points in the previous frame image are used as initial tracking points, and the current frame image is read;

[0037] The Lucas-Kanade optical flow algorithm is used to calculate the position change of the corner feature points in the previous frame in the current frame to obtain the optical flow tracking result.

[0038] In a second aspect, the present application also provides a multi-sensor positioning and mapping device. The device comprises:

[0039] A transformation relationship acquisition module is used to acquire the spatial transformation relationship between different types of sensors, wherein the different types of sensors include laser radar, IMU and camera;

[0040] The sensor data initial processing module is used to perform pre-integration processing on the IMU data to obtain the IMU prior pose, and perform optical flow tracking on the preset tracking points in the image collected by the camera to obtain the optical flow tracking result;

[0041] A tracking module, used to obtain an initial posture of the laser radar according to the IMU prior posture and the spatial transformation relationship, and initialize the laser radar, and obtain tracking points with depth information based on the optical flow tracking results and the point cloud data collected by the laser radar after initialization;

[0042] A loop detection module, used to obtain a camera pose according to the tracking point with depth information, and perform loop detection and verification using the camera pose to obtain loop constraints;

[0043] The optimization module is used to obtain the optimal pose of each type of sensor in multi-sensor fusion according to the loop constraint, the camera pose, the IMU prior pose and the lidar initial pose.

[0044] In a third aspect, the present application further provides a computer device. The computer device includes a memory and a processor, the memory stores a computer program, and the processor implements the following steps when executing the computer program:

[0045] Acquire spatial transformation relationships between different types of sensors, wherein the different types of sensors include lidar, IMU, and camera;

[0046] The IMU data is pre-integrated to obtain the IMU prior pose, and the preset tracking points in the image captured by the camera are optically tracked to obtain the optical flow tracking result;

[0047] According to the IMU prior pose and the spatial transformation relationship, the initial pose of the laser radar is obtained, and the laser radar is initialized, and the tracking points with depth information are obtained based on the optical flow tracking results and the point cloud data collected by the laser radar after the initialization;

[0048] Acquire a camera pose according to the tracking point with depth information, and use the camera pose to perform loop detection and verification to obtain loop constraints;

[0049] According to the loop constraint, the camera pose, the IMU prior pose and the lidar initial pose, the optimal pose of each type of sensor in multi-sensor fusion is obtained.

[0050] In a fourth aspect, the present application further provides a computer-readable storage medium. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the following steps are implemented:

[0051] Acquire spatial transformation relationships between different types of sensors, wherein the different types of sensors include lidar, IMU, and camera;

[0052] The IMU data is pre-integrated to obtain the IMU prior pose, and the preset tracking points in the image captured by the camera are optically tracked to obtain the optical flow tracking result;

[0053] According to the IMU prior pose and the spatial transformation relationship, the initial pose of the laser radar is obtained, and the laser radar is initialized, and the tracking points with depth information are obtained based on the optical flow tracking results and the point cloud data collected by the laser radar after the initialization;

[0054] Acquire a camera pose according to the tracking point with depth information, and use the camera pose to perform loop detection and verification to obtain loop constraints;

[0055] According to the loop constraint, the camera pose, the IMU prior pose and the lidar initial pose, the optimal pose of each type of sensor in multi-sensor fusion is obtained.

[0056] In a fifth aspect, the present application further provides a computer program product. The computer program product includes a computer program, and when the computer program is executed by a processor, the following steps are implemented:

[0057] Acquire spatial transformation relationships between different types of sensors, wherein the different types of sensors include lidar, IMU, and camera;

[0058] The IMU data is pre-integrated to obtain the IMU prior pose, and the preset tracking points in the image captured by the camera are optically tracked to obtain the optical flow tracking result;

[0059] According to the IMU prior pose and the spatial transformation relationship, the initial pose of the laser radar is obtained, and the laser radar is initialized, and the tracking points with depth information are obtained based on the optical flow tracking results and the point cloud data collected by the laser radar after the initialization;

[0060] Acquire a camera pose according to the tracking point with depth information, and use the camera pose to perform loop detection and verification to obtain loop constraints;

[0061] According to the loop constraint, the camera pose, the IMU prior pose and the lidar initial pose, the optimal pose of each type of sensor in multi-sensor fusion is obtained.

[0062] The above-mentioned multi-sensor positioning and mapping method, device, computer equipment, storage medium and computer program product obtain the spatial transformation relationship between different types of sensors, perform pre-integration processing on IMU data to obtain IMU prior pose, perform optical flow tracking on preset tracking points in the image collected by the camera to obtain optical flow tracking results; according to the IMU prior pose and spatial transformation relationship, obtain the initial pose of the laser radar and initialize the laser radar, and obtain the tracking points with depth information based on the optical flow tracking results and the point cloud data collected by the laser radar after initialization; obtain the camera pose based on the tracking points with depth information, use the camera pose to perform loop detection and verification, and obtain loop constraints; according to the loop constraints, camera pose, IMU prior pose and laser radar initial pose, obtain the optimal pose of each type of sensor in multi-sensor fusion. In the whole process, the sensor data collected by multiple types of sensors are fused, and optimization is performed based on the loop constraints during the fusion process, and finally the accurate optimal pose of each type of sensor is obtained, so that accurate multi-sensor positioning and mapping can be achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 A diagram of an application environment of a multi-sensor positioning and mapping method in one embodiment;

[0064] Figure 2 A schematic diagram of a process of a multi-sensor positioning and mapping method in one embodiment;

[0065] Figure 3 It is a schematic diagram of a flow chart of a multi-sensor positioning and mapping method in another embodiment;

[0066] Figure 4 is a schematic diagram of a sub-process of S400 in one embodiment;

[0067] Figure 5 It is a flowchart of a multi-sensor positioning and mapping method in a specific application example;

[0068] Figure 6 It is a structural block diagram of a multi-sensor positioning and mapping device in one embodiment;

[0069] Figure 7 FIG. 4 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION

[0070] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0071] The multi-sensor positioning and mapping method provided in the embodiments of the present application can be applied to Figure 1 In the application environment shown, the terminal 102 is connected to the control device 104, and the control device 104 is connected to various types of sensors, such as Figure 1 The terminal 102 sends a positioning and mapping request to the control device 104, and the control device 104 responds to the positioning and mapping request. The control device 104 obtains the spatial transformation relationship between different types of sensors, performs pre-integration processing on the IMU data to obtain the IMU prior pose, and performs optical flow tracking on the preset tracking points in the image collected by the camera to obtain the optical flow tracking result; according to the IMU prior pose and the spatial transformation relationship, the initial pose of the laser radar is obtained, and the laser radar is initialized, and the tracking points with depth information are obtained based on the optical flow tracking result and the point cloud data collected by the laser radar after initialization; the camera pose is obtained according to the tracking point with depth information, and the camera pose is used to perform loop detection and verification to obtain loop constraints; according to the loop constraints, the camera pose, the IMU prior pose and the initial pose of the laser radar, the optimal pose of each type of sensor in the multi-sensor fusion is obtained, and the multiple types of sensors are adjusted with the optimal pose.

[0072] In one embodiment, Figure 2 As shown, a multi-sensor positioning and mapping method is provided, which is applied to Figure 1 Taking the control device 104 in the example as an example, the method includes the following steps:

[0073] S100: Acquire spatial transformation relationships between different types of sensors, where the different types of sensors include lidar, IMU, and camera.

[0074] The spatial transformation relationship between different types of sensors refers to the transformation relationship between the relative positions and postures of different types of sensors in their respective coordinate systems. Simply put, the purpose of obtaining the spatial transformation relationship between different types of sensors is to determine the relative positions and postures of different types of sensors in their respective coordinate systems. Here, different types of sensors mainly include lidar, IMU and camera.

[0075] LiDAR is an advanced detection system that combines three technologies: laser, global positioning system (GPS) and inertial measurement device. IMU (Inertial Measurement Unit) is a device used to measure and report specific forces and angular velocities of an object. It is usually composed of accelerometers, gyroscopes and sometimes magnetometers. These sensors can detect the movement, rotation and direction changes of objects and are widely used in aerospace, automotive, robotics and consumer electronics. IMU plays an important role in navigation and control systems, especially in environments without GPS signals. Cameras are used to capture high-definition images. Specifically, the camera here can use a binocular fisheye camera. A binocular fisheye camera is a special camera system that combines binocular stereo vision technology and fisheye lens. A binocular fisheye camera captures two images of the same scene through two fisheye lenses placed side by side. A fisheye lens is a wide-angle lens with an extremely short focal length and a viewing angle close to or equal to 180°, which can capture a very wide picture. Binocular stereo vision technology is based on the principle of parallax. By comparing images taken at two different positions (i.e., two fisheye lenses), the depth information of the scene can be calculated.

[0076] In general, this application proposes a high-precision multi-sensor positioning and mapping solution, which analyzes and processes the data provided by a device consisting of a lidar (such as Livox mid 360) + IMU + binocular fisheye camera, and can make full use of information from multiple types of sensors to ultimately obtain high-precision sensor positioning and mapping results.

[0077] S200: Pre-integrate the IMU data to obtain the IMU prior pose, and perform optical flow tracking on preset tracking points in the image captured by the camera to obtain an optical flow tracking result.

[0078] The frequency of IMU is about 100-200hz, which is much higher than the frequency of common cameras and lidar. In a short period of time, the state of IMU is considered accurate and can be used as a priori. Here, the IMU data is pre-integrated to obtain the IMU prior pose. Specifically, the state of IMU at time i+1 is estimated as follows:

[0079]

[0080] represents the IMU state at time i+1, represents the sampling time interval of the IMU, u represents the measurement input (angular velocity and acceleration) of the IMU, and w represents Gaussian noise.

[0081]

[0082] In the above formula, x represents the state of IMU, , represents the IMU pose orientation, represents the three-dimensional position, v is the velocity, , which represents the bias of the IMU, which is composed of the acceleration bias and the gyroscope bias. The IMU measures linear acceleration, angular velocity, and magnetic field strength in real time through the internal accelerometer, gyroscope, and magnetometer. Based on these data, the orientation, position, and velocity can be obtained through mathematical calculation.

[0083] Optical flow tracking is performed on the images captured by the camera. Specifically, some tracking points can be pre-set in the captured images, and the optical flow tracking method can be used to discover the changes of the preset tracking points over time. These tracking points can be automatically acquired by performing corner point detection on the captured images. In subsequent processing, the lidar acquires three-dimensional point cloud data of the surrounding environment by emitting a laser beam and measuring the reflection time. By matching these point cloud data with the local map, depth information can be obtained to help the robot locate and understand the environment. Specifically, feature extraction can be performed on the point cloud data first to extract corner features and surface features; then the extracted corner features and surface features can be downsampled and filtered to remove noise points; finally, feature matching is performed, which is to match the features extracted from the current point cloud with the local map.

[0084] S300: According to the IMU prior posture and spatial transformation relationship, the initial posture of the laser radar is obtained, and the laser radar is initialized, and the tracking points with depth information are obtained based on the optical flow tracking results and the point cloud data collected by the laser radar after initialization.

[0085] The initial pose of the LiDAR is analyzed based on the prior pose provided by the IMU. Specifically, the initial pose of the LiDAR can be analyzed based on the spatial transformation relationship between the LiDAR and the IMU in the IMU prior pose and spatial transformation relationship. Further, the initial pose acquisition process of the LiDAR specifically includes: 1) Obtaining the IMU prior pose: The IMU provides an initial pose estimate (position and orientation) as a priori for initializing the LiDAR pose; 2) Point cloud feature extraction: Extract feature points from the LiDAR point cloud data, and infer the initial pose through point cloud registration (such as ICP). 3) Coupling of IMU and radar: Obtain the relative rotation matrix and translation vector between the two through the external parameters of the pre-calibrated IMU and radar. 4) IMU trajectory propagation and optimization: Use the IMU data to propagate the pose, and further optimize the pose of the radar through the point cloud data. 5) Graph optimization and joint optimization: Graph optimization is performed on the IMU, LiDAR and vision data to further adjust and optimize the pose of the LiDAR.

[0086] After completing the initial pose calculation of the lidar, the acquired point cloud data is transformed into the camera coordinate system through the transformation relationship (external parameters) between the lidar and the camera, and the point cloud depth information is bound to the preset tracking points on the image to obtain tracking points with depth information.

[0087] S400: Obtain camera pose according to the tracking points with depth information, perform loop detection and verification using the camera pose, and obtain loop constraints.

[0088] After obtaining the tracking points with depth information, the accurate camera pose is obtained based on the tracking points with depth information. It should be noted that the same method can be used to obtain the corresponding tracking points with depth information for each frame image in the future, and the accurate camera pose at different times can be obtained. The camera pose is then used for loop detection and verification. If the verification meets the conditions, the loop is detected and the corresponding loop constraint is recorded.

[0089] S500: According to the loop constraints, camera pose, IMU prior pose and lidar initial pose, the optimal pose of each type of sensor in multi-sensor fusion is obtained.

[0090] The loop constraints are introduced for the camera pose, IMU prior pose, and lidar initial pose to optimize and adjust the poses of these sensors, so that the entire multi-sensor positioning system can eliminate the pose offset caused by the accumulated error, and finally obtain the optimal pose of each type of sensor in the multi-sensor fusion. Specifically, in the multi-sensor graph optimization process, a new constraint, namely the loop constraint, will be added after the loop detection. Based on the loop constraint, the previous nodes of the graph optimization process are adjusted to help the entire multi-sensor behavior system eliminate the pose offset caused by the accumulated error. Furthermore, the optimization here can use nonlinear least squares optimization methods, such as Levenberg-Marquardt or Gauss-Newton algorithms. These algorithms iteratively update the states of the nodes to minimize the errors of the edges in the graph.

[0091] The above multi-sensor positioning and mapping method obtains the spatial transformation relationship between different types of sensors, performs pre-integration processing on the IMU data to obtain the IMU prior pose, performs optical flow tracking on the preset tracking points in the image collected by the camera to obtain the optical flow tracking result; according to the IMU prior pose and the spatial transformation relationship, the initial pose of the laser radar is obtained, and the laser radar is initialized, and the tracking points with depth information are obtained based on the optical flow tracking result and the point cloud data collected by the laser radar after initialization; the camera pose is obtained according to the tracking points with depth information, and the camera pose is used to perform loop detection and verification to obtain loop constraints; according to the loop constraints, camera pose, IMU prior pose and the initial pose of the laser radar, the optimal pose of each type of sensor in the multi-sensor fusion is obtained. In the whole process, the sensor data collected by multiple types of sensors are fused, and optimization is performed based on the loop constraints during the fusion process, and finally the accurate optimal pose of each type of sensor is obtained, so that accurate multi-sensor positioning and mapping can be achieved.

[0092] like Figure 3 As shown, in one embodiment, S300 includes:

[0093] S320: Obtain the initial posture of the laser radar according to the IMU prior posture and the spatial transformation relationship.

[0094] First, use the prior pose information provided by the IMU, which usually includes position and orientation (attitude). Then, combine the known spatial transformation relationships that describe the relative position and attitude between different sensors (such as IMU and LiDAR). By applying these spatial transformation relationships to the prior pose of the IMU, the initial pose of the LiDAR at the current moment can be calculated. This pose also includes the position and orientation of the LiDAR in three-dimensional space.

[0095] S340: Initialize the laser radar based on the initial position and posture of the laser radar, and obtain point cloud data collected by the laser radar after initialization.

[0096] Based on the initial position of the LiDAR obtained in the previous step, initialize the LiDAR (for example, set initial parameters, start data collection, etc.), and obtain the point cloud data collected by the initialized LiDAR. The initialization of the LiDAR can include setting parameters such as scanning frequency and resolution, and ensuring the accuracy and real-time performance of data collection.

[0097] S360: Based on the initial position and spatial transformation relationship of the laser radar, the point cloud data is transformed into the camera coordinate system through the external parameters between the laser radar and the camera to obtain the point cloud depth information. The point cloud data is transformed into the camera coordinate system through the external parameters between the laser radar and the camera to obtain the point cloud depth information.

[0098] With the initial position and posture of the LiDAR, the point cloud data can be further processed. The point cloud data was originally represented in the coordinate system of the LiDAR, but in order to facilitate subsequent processing (such as fusion with image data), it needs to be transformed into the camera coordinate system. This usually involves using the external parameters between the LiDAR and the camera (that is, the relative position and posture between them) to convert the point cloud data to the camera coordinate system through mathematical transformations (such as rotation and translation) to obtain the point cloud depth information. In simple terms, here, based on the external parameters between the LiDAR and the camera (such as the transformation matrix obtained by calibration), the point cloud data collected by the LiDAR is transformed to the camera coordinate system. In this way, each point cloud data point corresponds to a three-dimensional coordinate in the camera coordinate system, thereby obtaining the depth information of the point cloud.

[0099] S380: Binding the point cloud depth information with the tracking points determined by the optical flow tracking result to obtain the tracking points with depth information.

[0100] After the point cloud data is transformed into the camera coordinate system, the data can be further processed to extract useful information, and the depth information in the point cloud can be bound to the tracking points obtained by optical flow tracking. By binding the depth information, additional three-dimensional spatial information is provided for each tracking point, which helps to improve accuracy and robustness in subsequent tracking, positioning or map construction tasks. Specifically, after the initial pose calculation of the lidar is completed (that is, the lidar subsystem is successfully initialized), the currently acquired lidar point cloud data is transformed to the camera coordinate system through the external parameters between the lidar and the camera to obtain the point cloud depth information, and the point cloud depth information is bound to the tracking points on the image to obtain a set of tracking points with depth information. The current camera position can then be calculated based on these tracking points with depth information by minimizing the reprojection error.

[0101] In one embodiment, according to the loop constraint, the camera pose, the IMU prior pose, and the lidar initial pose, the optimal pose of each type of sensor in the multi-sensor fusion is obtained, including:

[0102] Step 1: Construct the objective function of multi-sensor fusion based on the camera pose, IMU prior pose and lidar initial pose.

[0103] The core goal of multi-sensor optimization is to optimize the state of each sensor based on the observation information and constraints. Specifically, the objective function takes into account the inconsistencies between sensor data (for example, the point cloud matching error between the camera and the lidar, or the difference between the IMU prediction and the actual observation) and tries to minimize these inconsistencies. Here we first construct the objective function. Further, the objective function is as follows:

[0104]

[0105] Where Y represents the state of the multi-sensor fusion system to be estimated.

[0106] Step 2: Iteratively optimize the objective function according to the loop constraint to obtain the optimal posture of each type of sensor in the multi-sensor fusion corresponding to the minimized objective function.

[0107] After constructing the objective function, the next step is to iteratively optimize this function using loop constraints. The iterative optimization process involves various optimization algorithms such as gradient descent, Levenberg-Marquardt algorithm, etc. Loop constraints play a key role here because they provide global consistency information and help correct local inconsistencies due to sensor noise or model errors. Through multiple iterations, the objective function is gradually minimized until a certain convergence criterion is reached. When the objective function reaches the minimum value, the corresponding sensor state parameters (i.e., camera pose, IMU pose, and lidar pose) are considered to be optimal. These optimal poses provide a more accurate basis for subsequent sensor fusion, map construction, or positioning tasks.

[0108] like Figure 4 As shown, in one embodiment, S400 includes:

[0109] S410: Obtain camera pose according to the tracking points having depth information.

[0110] The camera pose is calculated using tracking points with depth information (these points are usually obtained by transforming the lidar point cloud data into the camera coordinate system and matching them with the feature points in the image). The camera pose describes the position and orientation of the camera in three-dimensional space.

[0111] Furthermore, the camera pose can be obtained based on the tracking points with depth information and by minimizing the reprojection error. The reprojection error refers to the position difference between the projection point and the actual observed feature point when the point in the three-dimensional space is projected onto the image plane. Ideally, if the camera pose estimation is accurate, the projection point should completely coincide with the actual feature point, and the reprojection error is zero. In order to solve the camera pose, the reprojection error needs to be minimized. This is usually achieved through optimization algorithms, such as the Gauss-Newton method, the Levenberg-Marquardt algorithm, etc. These algorithms iteratively adjust the camera pose parameters (including the rotation matrix and the translation vector) until the reprojection error reaches the minimum value or meets a certain convergence condition. During the optimization process, each iteration calculates the reprojection error based on the current camera pose, and updates the pose parameters to reduce the error. When the optimization algorithm converges, the optimal camera pose estimation is obtained.

[0112] S420: Filter key frames in the image captured by the camera according to a preset threshold rule.

[0113] The preset threshold rule is a pre-set rule, which can be set based on the distance and angle deviation from the previous frame to filter out the key frames in the camera captured image. That is, only the key frames will be added to the next step of multi-sensor image optimization.

[0114] S430: When a new key frame is added, a global descriptor of the key frame is extracted using a deep learning-based method.

[0115] When a new keyframe is added, a deep learning-based method (such as NetVLAD, AlexNet, etc.) is used to extract the global descriptor of the keyframe. The global descriptor is a compact vector representation that can capture the overall characteristics of the image for subsequent image retrieval and matching.

[0116] S440: Retrieve the global descriptor and the historical key frames to obtain a rough matching result.

[0117] The global descriptor of the new keyframe is retrieved from the global descriptor of the historical keyframe to obtain a set of rough matching results. This step usually uses an efficient index structure (such as approximate nearest neighbor search) to speed up the matching process. The historical keyframe here refers to the keyframe added before the current keyframe. The rough matching here can be understood as a global matching, and the retrieval obtains a relatively rough matching result.

[0118] S450: extracting features from the rough matching results using a preset local feature extraction method, and selecting the best matching result.

[0119] After obtaining a rough matching result by global matching in S440, a local feature extraction method is used to extract local features with higher precision to select the best matching result. Specifically, for each pair of candidate matches in the rough matching result, a preset local feature extraction method (such as SIFT, SURF, ORB, etc.) is used to extract local features, and fine matching is performed, and the best matching result is selected by comparing the similarity of local features.

[0120] S460: The best matching result is verified using the camera pose. If the conditions are met, it is determined that a loop is detected and the loop constraint is recorded.

[0121] The best matching result is verified using the camera's pose information. This usually involves calculating the geometric consistency (such as reprojection error) and pose consistency (such as the similarity of the relative transformation matrix) between the matching pairs. If the verification result meets the conditions (such as the error is less than the preset threshold), it is determined that a loop is detected and the loop constraint is recorded.

[0122] In one embodiment, obtaining the spatial transformation relationship between sensors of different types includes:

[0123] The Kalibr tool is used to calibrate the relative position and attitude between the respective coordinate systems of the LiDAR, IMU, and camera. Based on the calibration results, the spatial transformation relationship between the LiDAR, IMU, and camera is constructed.

[0124] The process of acquiring the spatial transformation relationship between different types of sensors can be simply understood as the calibration process of multi-sensor parameters (extrinsic parameters). Specifically, extrinsic parameter calibration refers to establishing a spatial transformation relationship between different sensors, with the purpose of determining the relative position and posture of the lidar, IMU and camera between their respective coordinate systems. The Kalibr tool can be used for calibration here. Furthermore, during the calibration process, time synchronization is to ensure that the collected data of different sensors have consistent timestamps. Since the sampling frequencies of each sensor are different and each has an independent clock, the sensor data needs to be time-aligned. Through the hardware trigger mechanism, multiple sensors start to collect data at the same time, so that the data of different sensors can be timestamped and calibrated through the same clock source. As mentioned above, the camera can choose a binocular fisheye camera. Fisheye lenses usually have stronger distortion, especially at the edges of the image. This requires the use of a special fisheye distortion model during the calibration process, such as Equidistant or Equisolid models. The distortion model of an ordinary camera generally uses a radial or tangential distortion model. Based on the characteristics of the binocular fisheye camera, during the calibration process, the calibration plate size and point distribution suitable for the field of view of the fisheye lens are selected, and the calibration plate is photographed from different angles and positions to ensure that the entire field of view of the camera is covered and the edge area of ​​the image is included as much as possible to fully capture the distortion characteristics, thereby improving the calibration accuracy.

[0125] In one embodiment, performing optical flow tracking on preset tracking points in an image captured by a camera to obtain an optical flow tracking result includes:

[0126] Step 1: Get the image captured by the camera.

[0127] Capture continuous image frames from the camera device, which is the basic data for optical flow tracking.

[0128] Step 2: Convert the image captured by the camera into a grayscale image.

[0129] In order to reduce computational complexity and reduce noise interference, the captured color image is converted into a grayscale image. Grayscale images only contain brightness information, making subsequent feature point detection and tracking more efficient.

[0130] Step 3: Perform Shi-Tomasi corner detection on the grayscale image to obtain corner feature points.

[0131] Apply the Shi-Tomasi corner detection algorithm to the grayscale image. This algorithm is an improved version of Harris corner detection and can more accurately identify corner features in the image. These corner feature points are usually significant and stable structures in the image and are suitable for optical flow tracking.

[0132] Step 4: Use the corner feature points in the previous frame image as the initial tracking points and read the current frame image.

[0133] The corner feature points detected in the previous frame are used as initial tracking points. These points will be continuously tracked in subsequent frames to analyze their motion trajectories. During the optical flow tracking process, new image frames (i.e., the current frame) are continuously read to continuously track the initial tracking points.

[0134] Step 5: Use the Lucas-Kanade optical flow algorithm to calculate the position change of the corner feature points in the previous frame in the current frame to obtain the optical flow tracking result.

[0135] The Lucas-Kanade optical flow algorithm is used to calculate the position change of the corner feature points in the previous frame in the current frame. The Lucas-Kanade algorithm is a gradient-based iterative method that assumes that the optical flow is constant within a small window and estimates the optical flow by minimizing the brightness change within the window. This process outputs the new position of each corner feature point in the current frame. The position change information calculated by the Lucas-Kanade algorithm constitutes the optical flow tracking result. These results can be used to analyze the motion trajectory of feature points in the image, thereby achieving higher-level image processing tasks.

[0136] In one specific application example, the specific implementation process of the multi-sensor positioning and mapping method of the present application is as follows: Figure 5 As shown in the figure, the whole solution processes the data provided by the device composed of lidar + IMU + camera. The whole solution can make full use of various sensor information, connect the two subsystems VIS (visual-inertial-system) and LIS (lidar-inertial-system) through the IMU pre-integration initial value, and jointly optimize the system state in a tightly coupled manner, ultimately obtaining high-precision sensor positioning and mapping results.

[0137] It should be understood that, although the steps in the flowcharts involved in the above embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps is not strictly limited in order, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above embodiments may include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0138] Based on the same inventive concept, the embodiment of the present application also provides a multi-sensor positioning and mapping device for implementing the multi-sensor positioning and mapping method involved above. The implementation solution provided by the device to solve the problem is similar to the implementation solution recorded in the above method, so the specific limitations in one or more multi-sensor positioning and mapping device embodiments provided below can refer to the limitations of the multi-sensor positioning and mapping method above, and will not be repeated here.

[0139] In one embodiment, Figure 6 As shown, a multi-sensor positioning and mapping device is provided, comprising:

[0140] The transformation relationship acquisition module 100 is used to acquire the spatial transformation relationship between different types of sensors, including laser radar, IMU and camera;

[0141] The sensor data initial processing module 200 is used to perform pre-integration processing on the IMU data to obtain the IMU prior pose, and perform optical flow tracking on the preset tracking points in the image collected by the camera to obtain the optical flow tracking result;

[0142] The tracking module 300 is used to obtain the initial posture of the laser radar according to the IMU prior posture and the spatial transformation relationship, and initialize the laser radar, and obtain the tracking point with depth information based on the optical flow tracking result and the point cloud data collected by the laser radar after the initialization;

[0143] A loop detection module 400 is used to obtain a camera pose according to tracking points with depth information, and to perform loop detection and verification using the camera pose to obtain loop constraints;

[0144] The optimization module 500 is used to obtain the optimal pose of each type of sensor in multi-sensor fusion according to the loop constraint, the camera pose, the IMU prior pose and the lidar initial pose.

[0145] In one of the embodiments, the tracking module 300 is also used to obtain the initial pose of the laser radar based on the IMU prior pose and the spatial transformation relationship; initialize the laser radar based on the initial pose of the laser radar, and obtain the point cloud data collected by the laser radar after initialization; based on the initial pose of the laser radar and the spatial transformation relationship, transform the point cloud data to the camera coordinate system through the external parameters between the laser radar and the camera to obtain point cloud depth information; bind the point cloud depth information to the tracking points determined by the optical flow tracking results to obtain tracking points with depth information. The point cloud data is transformed to the camera coordinate system through the external parameters between the laser radar and the camera to obtain point cloud depth information.

[0146] In one embodiment, the optimization module 500 is also used to construct an objective function of multi-sensor fusion based on the camera pose, IMU prior pose and lidar initial pose; iteratively optimize the objective function according to the loop constraint to obtain the optimal pose of each type of sensor in the multi-sensor fusion corresponding to minimizing the objective function.

[0147] In one of the embodiments, the loop detection module 400 is also used to obtain the camera pose based on the tracking points with depth information; filter the key frames in the camera captured image according to the preset threshold rule; when a new key frame is added, extract the global descriptor of the key frame using a deep learning-based method; retrieve the global descriptor and the historical key frame to obtain a rough matching result; use a preset local feature extraction method to extract features in the rough matching result and filter the best matching result; verify the best matching result using the camera pose, and if the conditions are met, it is determined that a loop is detected and the loop constraint is recorded.

[0148] In one embodiment, the loop detection module 400 is further used to obtain the camera pose based on the tracking points with depth information and by minimizing the reprojection error.

[0149] In one of the embodiments, the transformation relationship acquisition module 100 is also used to calibrate the relative position and posture between the respective coordinate systems of the laser radar, IMU and camera using the Kalibr tool; based on the calibration results, the spatial transformation relationship between the laser radar, IMU and camera is constructed.

[0150] In one of the embodiments, the sensor data initial processing module 200 is also used to obtain an image captured by a camera; convert the image captured by the camera into a grayscale image; perform Shi-Tomasi corner point detection on the grayscale image to obtain corner feature points; use the corner feature points in the previous frame image as initial tracking points, and read the current frame image; use the Lucas-Kanade optical flow algorithm to calculate the position change of the corner feature points in the previous frame in the current frame to obtain the optical flow tracking result.

[0151] Each module in the above multi-sensor positioning and mapping device can be implemented in whole or in part by software, hardware, or a combination thereof. Each module can be embedded in or independent of a processor in a computer device in the form of hardware, or can be stored in a memory in a computer device in the form of software, so that the processor can call and execute operations corresponding to each module.

[0152] In one embodiment, a computer device is provided. The computer device may be a terminal, and its internal structure diagram may be as follows: Figure 7 As shown. The computer device includes a processor, a memory, a communication interface, a display screen and an input device connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface of the computer device is used to communicate with an external terminal in a wired or wireless manner, and the wireless manner can be achieved through WIFI, a mobile cellular network, NFC (near field communication) or other technologies. When the computer program is executed by the processor, a multi-sensor positioning and mapping method is implemented. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device can be a touch layer covered on the display screen, or a key, trackball or touchpad set on the computer device shell, or an external keyboard, touchpad or mouse.

[0153] Those skilled in the art will understand that Figure 7 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0154] In one embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the above-mentioned multi-sensor positioning and mapping method when executing the computer program.

[0155] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the multi-sensor positioning and mapping method described above is implemented.

[0156] In one embodiment, a computer program product is provided, including a computer program, which implements the above-mentioned multi-sensor positioning and mapping method when executed by a processor.

[0157] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited to this. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited to this.

[0158] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0159] The above-described embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

Claims

1. A multi-sensor positioning and mapping method, characterized in that: The method comprises: Acquire spatial transformation relationships between different types of sensors, wherein the different types of sensors include lidar, IMU, and camera; The IMU data is pre-integrated to obtain the IMU prior pose, and the preset tracking points in the image captured by the camera are optically tracked to obtain the optical flow tracking result; According to the IMU prior pose and the spatial transformation relationship, the initial pose of the laser radar is obtained, and the laser radar is initialized, and the tracking points with depth information are obtained based on the optical flow tracking results and the point cloud data collected by the laser radar after the initialization; Acquire a camera pose according to the tracking point with depth information, and use the camera pose to perform loop detection and verification to obtain loop constraints; According to the loop constraint, the camera pose, the IMU prior pose, and the lidar initial pose, the optimal pose of each type of sensor in multi-sensor fusion is obtained; The method of acquiring the camera pose according to the tracking points with depth information, performing loop detection and verification using the camera pose, and obtaining the loop constraint includes: acquiring the camera pose according to the tracking points with depth information; filtering the key frames in the camera captured image according to a preset threshold rule; when a new key frame is added, extracting the global descriptor of the key frame using a deep learning-based method; retrieving the global descriptor and the historical key frame to obtain a rough matching result; extracting features from the rough matching result using a preset local feature extraction method, and filtering the best matching result; verifying the best matching result using the camera pose, and if the conditions are met, determining that a loop is detected, and recording the loop constraint.

2. The method according to claim 1, characterized in that: The step of obtaining the initial posture of the laser radar according to the IMU prior posture and the spatial transformation relationship, and initializing the laser radar, and obtaining the tracking points with depth information based on the optical flow tracking result and the point cloud data collected by the laser radar after initialization includes: Obtaining an initial laser radar posture according to the IMU prior posture and the spatial transformation relationship; Initializing the laser radar based on the initial position and posture of the laser radar, and obtaining point cloud data collected by the laser radar after initialization; Based on the initial position of the laser radar and the spatial transformation relationship, the point cloud data is transformed into a camera coordinate system through external parameters between the laser radar and the camera to obtain point cloud depth information; The point cloud depth information is bound to the tracking point determined by the optical flow tracking result to obtain the tracking point with depth information.

3. The method according to claim 1, characterized in that The optimal pose of each type of sensor in multi-sensor fusion is obtained according to the loop constraint, the camera pose, the IMU prior pose and the lidar initial pose, including: Constructing an objective function of multi-sensor fusion according to the camera pose, the IMU prior pose and the lidar initial pose; The objective function is iteratively optimized according to the loop constraint to obtain the optimal posture of each type of sensor in the multi-sensor fusion corresponding to minimizing the objective function.

4. The method according to claim 1, characterized in that The acquiring the camera pose according to the tracking point having the depth information comprises: The camera position and posture are obtained according to the tracking points with depth information and by minimizing the reprojection error.

5. The method according to claim 1, characterized in that The obtaining of the spatial transformation relationship between sensors of different types includes: Use the Kalibr tool to calibrate the relative position and attitude between the laser radar, IMU and camera coordinate systems; Based on the calibration results, the spatial transformation relationship between the lidar, IMU and camera is constructed.

6. The method according to claim 1, characterized in that The optical flow tracking results obtained by performing optical flow tracking on the preset tracking points in the image captured by the camera include: Get the image captured by the camera; Convert the image captured by the camera into a grayscale image; Performing Shi-Tomasi corner detection on the grayscale image to obtain corner feature points; The corner feature points in the previous frame image are used as initial tracking points, and the current frame image is read; The Lucas-Kanade optical flow algorithm is used to calculate the position change of the corner feature points in the previous frame in the current frame to obtain the optical flow tracking result.

7. A multi-sensor positioning and mapping device, characterized in that: The device comprises: A transformation relationship acquisition module is used to acquire the spatial transformation relationship between different types of sensors, wherein the different types of sensors include laser radar, IMU and camera; The sensor data initial processing module is used to perform pre-integration processing on the IMU data to obtain the IMU prior pose, and perform optical flow tracking on the preset tracking points in the image collected by the camera to obtain the optical flow tracking result; A tracking module, used to obtain an initial posture of the laser radar according to the IMU prior posture and the spatial transformation relationship, and initialize the laser radar, and obtain tracking points with depth information based on the optical flow tracking results and the point cloud data collected by the laser radar after initialization; A loop detection module, used to obtain a camera pose according to the tracking point with depth information, and perform loop detection and verification using the camera pose to obtain loop constraints; An optimization module, used for obtaining the optimal pose of each type of sensor in multi-sensor fusion according to the loop constraint, the camera pose, the IMU prior pose and the lidar initial pose; The loop detection module is also used to obtain the camera posture according to the tracking points with depth information; filter the key frames in the camera captured image according to the preset threshold rules; when a new key frame is added, extract the global descriptor of the key frame using a deep learning-based method; retrieve the global descriptor and the historical key frame to obtain a rough matching result; use a preset local feature extraction method to extract features in the rough matching result and filter the best matching result; use the camera posture to verify the best matching result, and if it meets the conditions, it is determined that a loop is detected and the loop constraint is recorded.

8. The device according to claim 7, characterized in that The tracking module is also used to obtain the initial posture of the laser radar according to the IMU prior posture and the spatial transformation relationship; initialize the laser radar based on the initial posture of the laser radar, and obtain the point cloud data collected by the laser radar after initialization; based on the initial posture of the laser radar and the spatial transformation relationship, transform the point cloud data to the camera coordinate system through the external parameters between the laser radar and the camera to obtain the point cloud depth information; bind the point cloud depth information to the tracking point determined by the optical flow tracking result to obtain the tracking point with depth information.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images