Laser-inertia-vision tight coupling positioning method and system based on voxel map

Abstract

The invention discloses a laser-inertia-vision tight coupling positioning method and system based on a voxel map, and belongs to the technical field of unmanned aerial vehicle navigation positioning and mapping. The method comprises the following steps of: recursively predicting a pose by using IMU data, and constructing a shared state vector containing the IMU and a multi-frame camera pose in a sliding window; associating the laser point cloud with a local voxel map based on the predicted pose, constructing a laser plane residual model in a sliding window, and correcting a state vector; projecting a mature voxel plane center point in the local voxel map to the image as an initial candidate point, assisting visual feature extraction and tracking, and constructing a visual re-projection error model to update the state vector again; and registering the point cloud of the current frame into a map for updating and maintaining. According to the method, the consistency and robustness of pose estimation can be ensured, the calculation and storage overhead can be reduced, and the method is suitable for a small unmanned aerial vehicle platform with limited computing power.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle navigation, positioning and mapping (SLAM) technology, specifically relating to a laser-inertial-vision tightly coupled positioning method and system based on voxel maps. Background Technology

[0002] With the development of robotics technology, autonomous navigation and localization have become core technologies for mobile platforms such as drones. The LiDAR-Inertial-Visual Odometry (LVI-SLAM) system consists of three sensors: a LiDAR, a binocular camera, and an Inertial Measurement Unit (IMU). The IMU has a high output frequency and is used for recursive pose prediction, but its integration error accumulates over time. LiDAR provides high-precision structural information, but it is prone to degradation in scenarios such as unstructured corridors. Visual sensors offer rich texture information, but perform poorly in areas with changing lighting or no texture. Therefore, multi-sensor fusion is the mainstream approach to achieving robust localization.

[0003] In existing technologies, point cloud maps and feature maps are commonly used for multi-sensor fusion mapping. While point cloud maps can fully represent the environment, their storage and processing costs are extremely high. Feature maps, although sparse, do not accurately represent the geometric structure of the environment, and their extraction and matching computations are computationally complex, making them unsuitable for small UAV platforms with limited computing resources.

[0004] Regarding fusion localization frameworks, existing tightly coupled frameworks (such as R3LIVE and FASTLIVO2) have established photometric error or point-area error models, but they often fail to fully utilize the long-term constraint information between visual frames and laser frames within the sliding window. Furthermore, commonly used IESKF (Iterative Extended Kalman Filter) based methods typically only associate local maps observed in a single frame, making it difficult to handle joint optimization of multiple frame states. While graph optimization-based methods offer high accuracy, they require multiple linearization iterations for all keyframes, resulting in high computational and memory consumption, making them difficult to run in real-time on embedded devices. Summary of the Invention

[0005] Purpose of the invention: The purpose of this invention is to address the shortcomings of existing technologies by providing a laser-inertial-vision tightly coupled positioning method, system, and medium based on voxel maps, in order to solve the problems of poor pose estimation consistency, high resource consumption, and insufficient robustness in degraded environments in existing technologies for multi-sensor fusion.

[0006] Technical solution: The laser-inertial-vision tightly coupled positioning method based on voxel mapping described in this invention is characterized by the following steps:

[0007] Step S1: Acquire inertial measurement unit (IMU) data, binocular camera image data, and lidar point cloud data, and perform time synchronization and data preprocessing;

[0008] Step S2: Use IMU data to perform kinematic recursion to predict the carrier pose and construct a state vector, which simultaneously includes the IMU state vector and the camera state vectors at multiple historical moments within the sliding window.

[0009] Step S3: Based on the predicted carrier pose, the lidar point cloud data is associated with the local voxel map to construct a lidar plane residual model within a sliding window, and the state vector is corrected using the lidar plane residual model.

[0010] Step S4: Use the geometric structure information in the local voxel map constructed by the laser to constrain the extraction and tracking of visual features, construct a visual reprojection error model, and use the visual reprojection error model to update the state vector corrected in step S3 again, so as to realize the tight coupling estimation of the laser radar and vision under the same state vector.

[0011] Step S5: Output the updated carrier pose at the current moment, and register the current frame LiDAR point cloud to the local voxel map to update and maintain the map.

[0012] To further improve the above technical solution, the local voxel map is managed using a hierarchical spatial index structure that combines hash tables and octrees.

[0013] The present invention also provides a laser-inertial-visual positioning system based on voxel maps, including a data acquisition and preprocessing module, a state prediction and vector construction module, a lidar measurement update module, a visual measurement update module, and a map dynamic maintenance module.

[0014] Beneficial effects: Compared with the prior art, the advantages of the present invention are as follows:

[0015] This invention innovatively proposes using the projection of the plane center point of "mature voxels" in a local voxel map (laser mapping results) to assist in the extraction and tracking of visual features. This cross-modal data association method enables the system to maintain stable feature tracking even in scenarios where visual texture is missing but geometric structure is present (such as a corner of a white wall), or in scenarios where laser degradation exists but visual texture is present, effectively addressing the problem of single-sensor degradation.

[0016] This invention, based on the MSCKF framework, constructs a shared state vector incorporating IMU and multi-frame historical camera states, and establishes a laser plane residual model (cumulative averaging) and a visual reprojection error model within a sliding window. Compared to single-frame filtering, this method fully utilizes the constraint information within the time window, significantly improving the consistency and accuracy of state estimation.

[0017] A hash table combined with an octree structure is used to manage local voxel maps. A "mature voxel" mechanism and a distance-based dynamic pruning strategy are introduced to ensure the map remains sparse and contains high-quality planar features. This not only reduces memory usage but also lowers the complexity of searching for data associations, enabling it to run on embedded platforms with limited computing power, such as micro-drones.

[0018] In visual updates, features that distinguish between associated and unassociated map points are differentiated, and coordinates are calculated using map point coordinates and pure visual triangulation, respectively, maximizing information utilization and ensuring the normal operation of visual odometry even in areas where map construction is incomplete. Attached Figure Description

[0019] Figure 1 This is the overall flowchart of the laser-inertial-vision tightly coupled positioning method based on voxel maps in this invention.

[0020] Figure 2 This is a detailed flowchart of the laser-inertial-vision tightly coupled positioning method based on voxel maps in this invention.

[0021] Figure 3 This is a schematic diagram illustrating the association between local voxel map point clouds and visual image frames in this invention. Detailed Implementation

[0022] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings, but the scope of protection of the present invention is not limited to the embodiments described.

[0023] Example 1: As Figure 1 and Figure 2 As shown, this embodiment provides a tightly coupled laser-inertial-vision localization method based on voxel maps. It constructs a visual reprojection error model by associating optical flow tracking feature points with the laser point cloud under the local voxel map and performing joint triangulation. Furthermore, it establishes a laser plane residual model considering inter-frame constraints across multiple lasers. Simultaneously, the MSCKF filtering method is used to estimate the pose states of the laser and visual frames within the sliding window, obtaining a highly consistent motion trajectory. Finally, based on the calculated motion trajectory, the current frame point cloud is registered in a hash table-octree structured voxel map, constructing a high-quality local map for subsequent pose estimation. The specific process is as follows:

[0024] Step S1: Data Acquisition and Preprocessing

[0025] The system acquires acceleration and angular velocity data from the inertial measurement unit (IMU), left and right eye images from the binocular camera, and point cloud data from the lidar through sensors.

[0026] For data preprocessing:

[0027] Time synchronization: The LiDAR point cloud is backpropagated to remove distortion, and its pose is aligned with the camera image frame time according to the IMU recursion, thus completing the time synchronization of multiple sensors.

[0028] Image processing: Distortion correction and histogram equalization are performed on the binocular images to enhance image contrast;

[0029] IMU recursion: Kinematic integration is performed using high-frequency data from the IMU to obtain the predicted carrier pose (position, attitude, velocity).

[0030] Step S2: Construct a shared state vector

[0031] To optimize both IMU and camera states simultaneously within a sliding window, a shared state vector is constructed. Its mathematical expression is:

[0032] (1)

[0033] in, Indicates the first [number]th ... The position and orientation quaternions of the time-lapse camera in the world coordinate system. To adjust the sliding window size; For IMU status, the expression is: , Represents the attitude quaternion from the world frame to the IMU coordinate system; This indicates the bias of the accelerometer; This indicates the speed of the IMU within the world frame; Indicates the bias of the gyroscope; This indicates the IMU's position within the world system.

[0034] Step S3: Update the MSCKF filter of the lidar

[0035] As laser-vision frames are continuously inserted, the nearest neighbor plane of the current laser frame scan point is found in the voxel map. The coordinates of the current frame laser scan point are transformed to the world coordinate system using IMU-predicted pose, as shown in the following equation:

[0036] (2)

[0037] In the formula, This indicates the position coordinates of the lidar scanning point in the lidar coordinate system. The transfer matrix representing the laser radar reaching the IMU coordinate system is a known, fixed matrix; This represents the transition matrix from the IMU coordinate system to the camera coordinate system; This represents the transition matrix from the camera coordinate system to the world coordinate system, which is calculated using the pose updated recursively by the IMU. Once all the information is known, the position of the current laser frame scanning point in the world coordinate system can be determined. .

[0038] Retrieve via hash table If the root voxel does not contain a plane or no voxel is found, the scan point is discarded; otherwise, the search continues downwards through the octree structure, looking for the voxel block closest to the stored plane. If the retrieved leaf voxel stores planar features, the scan point is calculated. The distance to the plane is used to save the point-to-surface residual between the scan point and the plane stored in the voxel block, as shown in equation (3):

[0039] (3)

[0040] In the formula, This represents the plane thickness observed in the i-th laser frame. and These represent any point on the storage plane within the identified voxel block and its normal vector. After the sliding window is filled with observation frames, the average thickness of the planes of each voxel block in all stored frames is summed and used as the lidar measurement value. The measurement dimension depends on the number of voxel blocks stored in the laser frames within the sliding window.

[0041] To enhance robustness, the system does not rely solely on single-frame residuals; instead, it waits until the sliding window is filled with observation frames before applying all residuals within the window. The average planar residual is obtained by averaging the planar residuals of each laser frame for the same voxel block. :

[0042] (4)

[0043] In the expansion With state vector The camera pose in the image has the following mathematical relationship:

[0044] (5)

[0045] in, for Camera pose quaternion in vector The matrix form of the two can be converted to each other; correspond Camera position in vector ; Represent the zero matrix; represent the planar residuals Input into MSCKF, using With state vector The mathematical relationship between camera poses can be used to update the camera poses of multiple frames within a sliding window.

[0046] After each laser frame measurement update, the point cloud scanned in the current frame is adjusted according to the latest pose state based on equation (2). Register to the local voxel map.

[0047] The voxel map is managed using a hash table and an octree structure. The hash table manages root voxels of fixed size, and each root voxel encapsulates an octree structure. Scan points within the root voxel are checked to see if they can form a plane. If not, the octree structure is used to further divide the voxel into blocks, with the number of division levels freely set, up to a maximum of four. Otherwise, points that cannot form a plane are discarded. The octree manages leaf voxels of different sizes. Each leaf voxel represents a local plane, storing surface features, including the plane normal vector and the plane's center point. The surface features within the voxel are continuously updated based on the point cloud data newly registered to that voxel in each frame. After a certain number of updates, the plane parameters are fixed, and the voxel block is marked as a mature voxel. Simultaneously, to prevent the voxel map from continuously expanding and unnecessarily consuming memory resources, only regions no more than L in length from the current position are saved as local maps.

[0048] Step S4: Visual MSCKF filter update

[0049] This step aims to utilize the precise geometric structure (local voxel map) constructed by LiDAR to guide the extraction and association of visual features. For example... Figure 3 As shown, the process specifically includes the following sub-steps:

[0050] 1. Feature point selection and screening

[0051] Extract "mature voxels" from the local voxel map. Mature voxels are voxel blocks that have been observed multiple times and whose planar parameters are fixed.

[0052] Using the current camera pose predicted by the IMU, the center point of the plane stored in the mature voxel ( Figure 3 The solid blue dots are projected onto the pixel coordinate system of the first frame image (frame 1) within the sliding window, as shown in equation (6):

[0053] (6)

[0054] in, Represents pixel coordinates; Indicates depth value; Represents the world coordinates of the center point stored in the voxel; It is the rotation matrix from the camera coordinate system to the world coordinate system, which can be obtained from the camera's attitude quaternion. That is, the position vector of the camera in the world coordinate system; and This indicates the focal length of the camera along the x and y axes.

[0055] Assume the set of points projected onto the pixel coordinate system of this frame is To ensure the quality of visual features, they are filtered. The grayscale value of the pixel at the projection location is read. If the grayscale value is less than a preset threshold (e.g., the pixel is too dark or in a textureless area), the projection point is discarded; points with grayscale values ​​greater than the threshold are retained, resulting in an initial set of candidate feature points for subsequent optical flow tracking. .

[0056] To ensure the uniformity of feature point distribution in the image and prioritize tracking foreground objects, a gridding strategy is employed to uniformly divide the first frame of the image into pixel-size h*h blocks. Within each block, only the candidate point with the smallest projected depth value is retained as the initial candidate feature point. During subsequent tracking, each feature point stores information including its point index idx and the set of pixel coordinates from different tracking image frames. The set of coordinates of all candidate associated map points corresponding to the feature point when tracking the latest frame. ,See Figure 2 .

[0057] 2. Feature point association

[0058] The Lucas-Kanade algorithm is used to track feature points, starting with the "projection-selected feature points" in frame 1 and continuing tracking in subsequent image frames (frame 2...frame N). Successfully tracked points are marked as "tracked feature points" (…). Figure 3 The red triangle in the legend is indexed as follows: ).

[0059] During the tracking process, the current number of tracked feature points is counted in real time. For example... Figure 3 As indicated in the footnote, if the number of tracking points is less than the preset threshold, the system will perform feature point resampling in the blank pixel blocks of the image, reproject from the local map and select new feature points to supplement them in order to maintain the stability of the system.

[0060] Assume that the k-th feature point is tracked in the i-th frame, and its pixel coordinates are... The mature voxel plane points in the local map are projected onto the current pixel coordinate system using equation (6). In practical applications, the computational load can be reduced by setting the resolution of the local map during projection. For all points within a z*z region centered on the map, select the map point with the smallest projection depth value as a candidate associated map point, and save its coordinates to the candidate associated map point coordinate set. When projecting local map points into subsequent image frames, voxel plane points within a certain range around the candidate associated map point of the previous frame can be selected for projection.

[0061] During feature point tracking in subsequent frames, if the number of tracked feature points is less than the set minimum threshold, a feature point resampling operation is performed on that frame. This involves repeatedly selecting and filtering feature points within pixel regions where no feature points have been stored. New feature points are also tracked optically and associated with candidate map points in the same manner.

[0062] 3. Triangulation calculation of tracking feature points

[0063] When the cumulative number of visual frames meets the sliding window size, determine whether all feature points have been successfully associated with map points. Calculate the mean of the set of candidate associated map point coordinates for each feature point. and variance ,if Each element is less than the set threshold, and If the Euclidean distance between the initial image frame projection map points corresponding to this feature and the feature location is within a set threshold range, then... That is, the coordinates of the map points that are successfully associated with the feature point are used to assist in subsequent triangulation calculations; otherwise, the association of map points fails.

[0064] In triangulation calculations, based on the property that "the cross product of collinear vectors equals 0", the normalized pixel coordinates are... World coordinates to be determined And the known transformation from the world coordinate system to the camera coordinate system. ,have:

[0065] (7)

[0066] in, Representative vector antisymmetric matrix, Write it line by line Delete linearly correlated rows, merge the coefficient matrices observed from multiple tracking frames, and for feature points whose map point coordinates were not successfully associated:

[0067] (8)

[0068] For feature points whose map point coordinates have been successfully associated:

[0069] (9)

[0070] in, , Solving equations (8) and (9) using the least squares method yields the triangulated coordinates of the feature points. The triangulation calculation result Will be used as the coordinates of the tracking feature points The predicted value enables tight coupling between the lidar sensor and the vision sensor.

[0071] 4. Reprojection error construction and visual update

[0072] Assuming feature points The corresponding pixel coordinates are Construct its reprojection error model:

[0073] (10)

[0074] In the formula, This represents the projection function (including camera intrinsics and normalization operations); When calculating the reprojection error, in the formula... and The pose was updated using laser measurement. The obtained triangulated coordinates are used. Based on equation (10), the following is updated: and feature point coordinates This updates the camera pose. Perform MSCKF visual measurement updates and use this error measurement model to estimate the pose of multiple image frames within the sliding window.

[0075] Step S5: Map Update and Maintenance

[0076] After the measurement update is completed, the latest image frame pose in the MSCKF state vector. renew, Given and fixed values, the pose of the IMU sensor in the world frame can be obtained. , The current positioning result is encapsulated and processed before being published. Referring to equation (2), the radar scan points are positioned according to the updated pose of the latest image frame. All data is projected onto the world system and then packaged and published as the mapping result at the current moment.

[0077] This invention proposes a new data association method between laser frames and visual frames, which makes full use of the constraint information between visual frames and laser frames, and can improve the robustness of positioning results in scenarios where visual or lidar sensors degrade.

[0078] This invention constructs visual and laser residual models through sliding windows, and establishes a tightly coupled positioning method based on MSCKF for LiDAR-inertial-vision. It has low computational cost, is suitable for operation on edge computing devices with limited computing power, such as micro-small UAV-borne computer platforms, and can ensure the consistency of pose estimation.

[0079] In this invention, lidar and vision jointly maintain a low-dimensional feature voxel map. The computational cost required for map updating and maintenance is low, and it has the ability to update in real time while ensuring the integrity of spatial information.

[0080] Example 2: This example provides a voxel-based laser-inertial-vision positioning system, which runs on an onboard computer platform of a UAV (such as an NVIDIA Jetson series or STM32 embedded controller). The system includes:

[0081] Data acquisition and preprocessing module: Connects to IMU, camera and LiDAR, responsible for data reading, synchronization and distortion correction.

[0082] State prediction and vector construction module: performs IMU integration and manages the dimensions of the state vector.

[0083] LiDAR measurement update module: Executes the logic of step S3 in Example 1, and uses the laser plane residual to constrain the trajectory.

[0084] Visual measurement update module: Executes the logic of step S4 in embodiment 1, and uses map-assisted visual tracking to update the trajectory.

[0085] Map dynamic maintenance module: Executes the logic of step S5 in Example 1, and manages hash table and octree map data.

[0086] As described above, although the invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the invention itself. Various changes in form and detail may be made without departing from the spirit and scope of the invention as defined in the appended claims.

Claims

1. A laser-inertial-vision tightly coupled localization method based on voxel mapping, characterized in that, Includes the following steps: Step S1: Acquire inertial measurement unit (IMU) data, binocular camera image data, and lidar point cloud data, and perform time synchronization and data preprocessing; Step S2: Use IMU data to perform kinematic recursion to predict the carrier pose and construct a state vector, which simultaneously includes the IMU state vector and the camera state vectors at multiple historical moments within the sliding window. Step S3: Based on the predicted carrier pose, the lidar point cloud data is associated with the local voxel map to construct a lidar plane residual model within a sliding window, and the state vector is corrected using the lidar plane residual model. Step S4: Use the geometric structure information in the local voxel map constructed by the laser to constrain the extraction and tracking of visual features, construct a visual reprojection error model, and use the visual reprojection error model to update the state vector corrected in step S3 again, so as to realize the tight coupling estimation of the laser radar and vision under the same state vector. Step S5: Output the updated carrier pose at the current moment, and register the current frame LiDAR point cloud to the local voxel map to update and maintain the map.

2. The laser-inertial-vision tightly coupled positioning method based on voxel maps according to claim 1, characterized in that, The state vector in step S2 The construction is as follows: ， in, and These represent the first and second elements within the sliding window, respectively. The specific position and orientation quaternion of the time-lapse camera, The IMU state vector represents the attitude quaternion, velocity, position, accelerometer bias, and gyroscope bias in the world coordinate system.

3. The laser-inertial-vision tightly coupled positioning method based on voxel maps according to claim 1, characterized in that, The local voxel map is managed using a hierarchical spatial index structure that combines hash tables and octrees. The first-level index structure is a hash table, used to manage root voxels of a fixed size; The second-level index structure is an octree encapsulated inside each root voxel, which is used to further recursively divide the root voxel space into leaf voxels of different scales. The leaf cells are used to fit the planar features of the local environment, and the internally stored planar feature information includes the planar normal vector and the planar center point.

4. The laser-inertial-vision tightly coupled positioning method based on voxel maps according to claim 1, characterized in that, The specific steps in step S3, including constructing the laser plane residual model within the sliding window, include: The scanning points of the current laser frame are transformed to the world coordinate system through the predicted pose, and the corresponding voxel blocks are retrieved in the local voxel map; Calculate the point-to-surface distance from the scan point to the storage plane within the voxel block; After the number of observation frames fills the sliding window, all frames within the sliding window are observed. The planar residuals of the same voxel block are accumulated and averaged for each laser frame to construct the average planar residual. : ， in, For the first The point-to-surface distance corresponding to the scanning points of each laser frame is calculated; an observation equation is constructed based on the average plane residual, and its Jacobian matrix relative to the camera pose in the state vector is calculated to perform measurement updates.

5. The laser-inertial-vision tightly coupled positioning method based on voxel maps according to claim 1, characterized in that, The map update and maintenance in step S5 includes the process of generating mature voxels: Update the surface feature information within the voxel based on the point cloud data newly registered into the voxel in each frame; When the number of point cloud data updates in a certain voxel block reaches a preset counting threshold, the planar parameters in that voxel are fixed, and the voxel is marked as a mature voxel. The planar center point within the mature voxel is used for projection in step S4 to assist in visual feature extraction.

6. The laser-inertial-vision tightly coupled positioning method based on voxel maps according to claim 5, characterized in that, The extraction and tracking of visual features constrained by geometric structure information in step S4 specifically includes: The center point of the plane marked as mature voxels in the local voxel map is projected onto the pixel coordinate system of the first frame image within the sliding window through the predicted camera pose. The projection points are filtered out, and points with gray values ​​less than a preset threshold are removed. The image is then divided into grids, and only the projection point with the smallest depth value is retained in each grid as the initial candidate feature point. An optical flow algorithm is used to track the initial candidate feature points in subsequent image frames to establish cross-modal data association between two-dimensional image features and three-dimensional voxel maps.

7. The laser-inertial-vision tightly coupled positioning method based on voxel maps according to claim 6, characterized in that, Step S4 also includes a verification step for the validity of feature point association: Calculate the mean of the set of coordinates of all candidate associated map points corresponding to the tracked feature point within the sliding window. and variance ; If each element of the variance is less than a set threshold, and the Euclidean distance between the mean and the map point projected by the feature point in the initial frame is within a preset range, then the feature point is determined to be successfully associated with the map point, and the mean is used as the true value of the spatial coordinates of the feature point.

8. The laser-inertial-vision tightly coupled positioning method based on voxel maps according to claim 7, characterized in that, In step S4, the visual reprojection error model is constructed using a hybrid triangulation strategy. For feature points that have been verified as successfully associated map points, the coordinates of the center point of the associated mature voxel plane are directly used as the spatial coordinates of the feature points. For feature points that were not successfully associated with map points, a system of linear equations was constructed using multi-frame observation data within the sliding window and the collinear vector property, and the spatial coordinates of the feature points were solved by the least squares method. The shared state vector is updated by constructing a reprojection error using the determined spatial coordinates of feature points and pixel observations.

9. The laser-inertial-vision tightly coupled positioning method based on voxel maps according to claim 1, characterized in that, The map update maintenance in step S5 also includes a distance-based pruning strategy: Real-time detection of the Euclidean distance between each voxel block in the local voxel map and the current carrier position; If a voxel block is more than a preset length L away from the current carrier position, the voxel block and its internal data are deleted from the local voxel map to limit memory usage.

10. A laser-inertial-visual positioning system based on voxel mapping, characterized in that, The system includes: The data acquisition and preprocessing module is configured to acquire inertial measurement unit (IMU) data, binocular camera image data, and lidar point cloud data, and perform time synchronization and data distortion correction processing. The state prediction and vector construction module is configured to use the IMU data to perform kinematic recursion to predict the current carrier pose and construct a shared state vector containing the IMU state and the camera state at N historical moments within a sliding window. The lidar measurement update module is configured to convert the lidar point cloud of the current frame to the world coordinate system, search for the nearest neighbor plane in the local voxel map, construct a lidar plane residual model within a sliding window, and perform an initial correction based on MSCKF on the shared state vector using the lidar plane residual model. The visual measurement update module is configured to use a local voxel map to assist in visual feature association, project the mature voxel plane center points stored in the map onto the image coordinate system within the sliding window as initial candidate feature points, perform optical flow tracking and construct a visual reprojection error model, and use the visual reprojection error model to update the shared state vector after the initial correction. The map dynamic maintenance module is configured to register the point cloud data of the current frame to the local voxel map based on the updated pose state, and to update and prune the local voxel map according to the number of times the data in the voxel is updated and its distance from the carrier.

Images