A scene point cloud data acquisition method based on a handheld scene scanning system

Abstract

The present invention provides a scene point cloud data acquisition method based on a handheld scene scanning system, which belongs to the field of building data processing technology, including: collecting laser radar point cloud frames, inertial measurement unit data and mobile phone image data at different times in the scene to be scanned; using a fixed delay smoothing laser radar inertial odometer based on adaptive feature matching, the point cloud frames and inertial measurement unit data of the laser radar at different times are processed in real time to obtain the device motion state and the scene sparse point cloud; after completing the panoramic scan of the scene to be scanned, the full scene color point cloud of the scene to be scanned is constructed by the motion trajectory of the device, the saved laser radar original point cloud and the mobile phone image data. The present invention uses a consumer-grade low-cost scene three-dimensional scanning system to dynamically scan the building scene, can estimate the position and posture of the device in real time, and obtain a sparse three-dimensional point cloud map of the scene in real time, which can be widely used in the fields of building structure inspection, underground mineral inventory, etc., and has the characteristics of simple implementation and high computational efficiency.

Description

Technical Field

[0001] The present invention relates to the technical field of building data processing, and in particular to a scene point cloud data acquisition method based on a handheld scene scanning system. Background Art

[0002] In the construction field, it is usually necessary to conduct a structural inspection of a house to avoid housing safety accidents. Generally, professional equipment is used to measure, obtain and calculate the internal data of the house.

[0003] At present, in the field of mine mapping and building 3D reconstruction, there are already some handheld scene information acquisition systems based on LiDAR, such as handheld real-scene scanners. These devices can obtain 3D scene information with an accuracy better than 3 cm. However, these devices are bulky and expensive, making them difficult to apply on a large scale in actual scenarios.

[0004] In the process of using a scene scanning device, it is necessary to estimate the position and posture (pose) of the device and generate a point cloud map of the scene to provide feedback to the user on the progress of the scene scanning and guide the next scan. Although there are a variety of real-time pose estimation algorithms based on laser radar (lidar) point clouds and inertial measurement units (IMUs), the computational consumption of the related algorithms is still large and the running efficiency is not high on mid-range mobile phones. For example, the current leading pose estimation algorithm Fast-LIO2 processes point clouds at a rate of about 5Hz on a certain mobile terminal, which is significantly lower than the acquisition frame rate of lidar point clouds of 10Hz, making it difficult to meet the requirements of real-time processing. Summary of the invention

[0005] The present invention provides a scene point cloud data acquisition method based on a handheld scene scanning system, which is used to solve the defects of the solution adopted in the prior art for reconstructing the cloud data of the scenic spot at the scene, such as high cost and great implementation difficulty.

[0006] In a first aspect, the present invention provides a scene point cloud data acquisition method based on a handheld scene scanning system, comprising:

[0007] Collect the point cloud frames, inertial measurement unit data and mobile phone image data of the lidar at different times in the scene to be scanned;

[0008] Using a fixed-delay smoothed lidar inertial odometer based on adaptive feature matching, the point cloud frames of the lidar at different times and the inertial measurement unit data are processed in real time to obtain a device motion trajectory and a sparse point cloud of the scene;

[0009] After completing the panoramic scan of the scene to be scanned, a three-dimensional scene color point cloud of the scene to be scanned is established based on the device motion trajectory, the scene sparse point cloud and the mobile phone image data.

[0010] According to a scene point cloud data acquisition method based on a handheld scene scanning system provided by the present invention, before acquiring point cloud frames of a laser radar at different times in a scene to be scanned, inertial measurement unit data, and mobile phone image data, the method further includes:

[0011] The mobile phone, laser radar and power supply are fixedly installed on the three-axis bracket;

[0012] Configure the IP addresses of the mobile phone and the laser radar;

[0013] The camera of the mobile phone and the laser radar are calibrated to complete the external parameter calibration of the camera and the laser radar.

[0014] According to a scene point cloud data acquisition method based on a handheld scene scanning system provided by the present invention, point cloud frames of a laser radar at different times in a scene to be scanned, inertial measurement unit data, and mobile phone image data are collected, including:

[0015] Assigning a timestamp to the laser radar through the mobile phone, and collecting point cloud frames and inertial measurement unit data with the timestamp;

[0016] The mobile phone image data with the timestamp is collected synchronously.

[0017] According to a scene point cloud data acquisition method based on a handheld scene scanning system provided by the present invention, a fixed-delay smoothed laser radar inertial odometer based on adaptive feature matching is used to process the point cloud frames and inertial measurement unit data of the laser radar at different times, and the motion posture is estimated in real time to obtain a scene sparse point cloud, including:

[0018] Determining a point cloud frame and inertial measurement unit data of the laser radar;

[0019] Based on the inertial measurement unit data, an inertial measurement unit state is recursively deduced to obtain a system prediction state, and the point cloud frame is dedistorted and downsampled to obtain a processed point cloud frame;

[0020] Obtaining the estimated point cloud submap at the last moment, initializing the point cloud submap, and obtaining an initialized point cloud submap;

[0021] Acquire a point-to-surface matching set of the processed point cloud frame and the surface elements in the initialized point cloud submap by adaptive feature matching;

[0022] Based on a fixed delay smoother, a least squares problem is constructed for the point-to-surface matching set, the system state at the previous moment and the system state at the current moment are optimized, and an optimized posture and point cloud frame are obtained;

[0023] The point cloud submap is updated using the optimized pose and point cloud frame, and the sparse point cloud of the scene is output.

[0024] According to a scene point cloud data acquisition method based on a handheld scene scanning system provided by the present invention, an inertial measurement unit state recursion is performed based on the inertial measurement unit data to obtain a system prediction state, and the point cloud frame is subjected to point cloud dedistortion and downsampling to obtain a downsampled point cloud frame, including:

[0025] Determine a point cloud frame at any time, and determine the inertial measurement unit data from the last point cloud frame time to the current point cloud frame time in the inertial measurement unit data sequence;

[0026] Using the inertial measurement unit data between two moments, the system state at the previous moment is predicted to obtain the system predicted state at the current moment, as well as the observed state of multiple inertial measurement units between the previous moment and the current moment;

[0027] Based on the system prediction state and the inertial measurement unit observation state, removing the motion distortion of the points in the point cloud frame sequence to obtain a dedistorted point cloud frame;

[0028] The dedistorted point cloud frame is downsampled using a voxel grid filter to obtain the downsampled point cloud frame.

[0029] According to a scene point cloud data acquisition method based on a handheld scene scanning system provided by the present invention, a point cloud submap estimated at the last moment is obtained, and the point cloud submap is initialized at an initial moment, including:

[0030] A voxel grid based on hash retrieval is used to store the point cloud submap estimated at the last moment; wherein the voxel grid uses a fixed grid size, and the storage information of each voxel includes the coordinates of a point in the world coordinate system and the normal vector of the plane on which it is located;

[0031] Initialize the normal vector of the plane to the zero vector and the submap to an empty map;

[0032] Start receiving the first frame point cloud, add each point of the first frame point cloud to the empty map according to the grid coordinates, for a point P, determine m points near the dedistorted first frame point cloud, calculate the covariance matrix of the m+1 points including point P, initialize the normal vector of each point using the eigenvector corresponding to the minimum eigenvalue, process subsequent frame point clouds in sequence, and obtain the initialized point cloud submap.

[0033] According to a scene point cloud data acquisition method based on a handheld scene scanning system provided by the present invention, a point-to-surface matching set of the processed point cloud frame and the surface elements in the initialization point cloud submap is obtained by adaptive feature matching, including:

[0034] Determine the number of target matches, relaxation coefficient, matching quality evaluation function and upper limit of operation time, and input the downsampled point cloud frame, the point cloud submap, the system state at the last moment and the predicted system state at the current moment;

[0035] Initialize the point-to-surface matching set to an empty set;

[0036] An arbitrary subset of the point cloud frame after downsampling is selected according to the target matching number and the relaxation coefficient; a lazy greedy algorithm is used to search the arbitrary subset, a matching face is searched in the submap for each point of the arbitrary subset, and it is determined whether the point-to-face matching satisfies a preset face normal vector direction determination condition, so as to obtain an initial matching set of all points to faces formed by the arbitrary subset; a matching with the largest information gain in the point-to-face matching of the arbitrary subset is determined according to the matching quality evaluation function, and the matching is added to the initial matching set, and the operation is repeated multiple times until the operation time limit or the target matching number is reached, thereby obtaining the point-to-face matching set.

[0037] According to a scene point cloud data acquisition method based on a handheld scene scanning system provided by the present invention, based on a fixed delay smoother, a least squares problem is constructed for the point-to-surface matching set, the system state at the previous moment and the system state at the current moment are optimized, and an optimized posture and point cloud frame are obtained, including:

[0038] The prior constraint of the system state at the previous moment in the least squares problem is determined by the initial posture including uncertainty, the pre-integration constraint of the inertial measurement unit data in the least squares problem is determined by the pre-integration of the inertial measurement unit data between the system state at the previous moment and the system state at the current moment, and the matching constraint from any point in the point cloud frame at the current moment to the face element in the least squares problem is determined by the posture of the inertial measurement unit relative to the world coordinate system at the current moment, the external parameters of the laser radar relative to the inertial measurement unit, the coordinates of the point in the laser radar coordinate system, any point in the face element in the point cloud submap, and the face element normal vector;

[0039] Based on the prior constraints of the system state at the previous moment, the pre-integration constraints of the inertial measurement unit data, and the constraints of matching any point to a surface element in the point cloud frame at the current moment, a least squares problem objective function is obtained;

[0040] The least squares problem objective function is optimized and solved to obtain an optimized posture and a point cloud frame in a world coordinate system.

[0041] According to a scene point cloud data acquisition method based on a handheld scene scanning system provided by the present invention, the point cloud submap is updated using the optimized posture and point cloud frame to output the scene sparse point cloud, including:

[0042] Using the optimized posture, transforming the midpoint of the downsampled point cloud frame to a world coordinate system, and obtaining the voxel corresponding to the transformed point according to the hash index;

[0043] Screening points whose distance from any point in the point cloud frame after the downsampling process to the voxel center is less than or equal to the distance from the midpoint of the current voxel to the voxel center, and adding the screened points to the current voxel;

[0044] The normal vector of the midpoint of the matching facet in the point cloud submap is set as the normal vector of the matching facet, a number of neighboring points are searched for points of the unmatched facet in the point cloud submap, the covariance matrix of the number of neighboring points is calculated, and the normal vectors of the number of neighboring points are initialized using the eigenvector corresponding to the minimum eigenvalue, and after all points are updated, an updated point cloud submap is obtained;

[0045] The updated point cloud submap is removed from obsolete grids that are not in the area of ​​interest, and the optimized pose and corresponding point cloud frame are output.

[0046] In a second aspect, the present invention further provides a scene point cloud data acquisition system based on a handheld scene scanning system, comprising:

[0047] The acquisition module is used to collect the point cloud frames of the lidar, the inertial measurement unit data and the mobile phone image data at different times in the scene to be scanned;

[0048] A real-time device motion tracking module, which is used to process the point cloud frames and inertial measurement unit data of the lidar at different times in real time by using a fixed-delay smoothed lidar inertial odometer based on adaptive feature matching to obtain a device motion trajectory and a scene sparse point cloud;

[0049] The map optimization module is used to establish a three-dimensional scene color point cloud of the scene to be scanned based on the device motion trajectory, the scene sparse point cloud and the mobile phone image data after completing the panoramic scan of the scene to be scanned.

[0050] In a third aspect, the present invention further provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the method for collecting scene point cloud data based on a handheld scene scanning system as described above is implemented.

[0051] In a fourth aspect, the present invention further provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements a scene point cloud data acquisition method based on a handheld scene scanning system as described in any one of the above.

[0052] The scene point cloud data acquisition method based on the handheld scene scanning system provided by the present invention can generate a three-dimensional point cloud map of the scene in real time by dynamically scanning the building scene with a consumer-grade low-cost scene three-dimensional scanning system. It adopts a fixed-delay smoothed lidar inertial odometer based on adaptive feature matching and a single-step fixed-delay smoother, which can not only provide an information matrix but also flexibly select a robust loss function. It uses an adaptive feature matching strategy and selects observations that contribute greatly to state estimation for the optimization process, thereby controlling the computational consumption of state estimation and ensuring the accuracy of pose estimation. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0054] Figure 1 It is a flow chart of a scene point cloud data acquisition method based on a handheld scene scanning system provided by the present invention;

[0055] Figure 2 This is a schematic diagram of the reference hardware composition of the handheld laser radar camera combined scanning system provided by the present invention;

[0056] Figure 3 It is a schematic diagram of the software module composition of the handheld scanning system provided by the present invention;

[0057] Figure 4 It is a flow chart of the laser radar inertial odometer (FLS-LIO-GF) algorithm provided by the present invention;

[0058] Figure 5 It is a three-dimensional point cloud image of an office scene generated in real time by the reference configuration provided by the present invention;

[0059] Figure 6 is a two-dimensional occupancy grid map corresponding to an office scene generated in real time by the reference configuration provided by the present invention;

[0060] Figure 7 It is a structural schematic diagram of a scene point cloud data acquisition system based on a handheld scene scanning system provided by the present invention;

[0061] Figure 8 It is a structural schematic diagram of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0062] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0063] Figure 1 is a flow chart of a scene point cloud data acquisition method based on a handheld scene scanning system provided by an embodiment of the present invention, such as Figure 1 As shown, including:

[0064] Step 100: Collecting point cloud frames of the laser radar, inertial measurement unit data, and mobile phone image data at different times in the scene to be scanned;

[0065] Step 200: using a fixed-delay smoothed lidar inertial odometer based on adaptive feature matching, processing the point cloud frames of the lidar at different times and the inertial measurement unit data in real time to obtain a motion trajectory of the device and a sparse point cloud of the scene;

[0066] Step 300: After completing the panoramic scan of the scene to be scanned, a three-dimensional scene color point cloud of the scene to be scanned is established based on the device motion trajectory, the scene sparse point cloud and the mobile phone image data.

[0067] In order to achieve low-cost handheld scene three-dimensional scanning, an embodiment of the present invention proposes a scene point cloud acquisition system based on a combination of a laser radar and a camera based on a smartphone. The embodiment of the present invention uses consumer-grade components, such as a smartphone, a solid-state laser radar, etc., to achieve a cheap device for accurately and quickly acquiring three-dimensional scene information. The location accuracy of the scene point cloud collected by the device is better than 5 cm within a range of 40 m, which can support rapid modeling and investigation of building structures, as well as preliminary modeling of scenes such as mining areas and tunnels. The hardware of the system consists of four parts: a consumer-grade solid-state laser radar, a consumer-grade smartphone (including a camera), a lithium battery, and a three-axis bracket. The three-axis bracket is used to fix the lithium battery, the smartphone, and the solid-state laser radar for easy handholding. The solid-state laser radar is connected to the smartphone via a USB type C interface, and the smartphone can obtain the point cloud data and inertial measurement unit (IMU) data of the laser radar through Ethernet network sharing. The lithium battery powers the solid-state laser radar. The smartphone contains a camera, and a video stream can be obtained through a self-developed application. Most smartphones also contain an IMU, and the mobile phone IMU data stream can also be obtained through a self-developed application.

[0068] The example hardware device shown in the present invention includes a smart Android phone Redmi K40 Pro, a lithium battery, a three-axis bracket, and a Livox Mid360 solid-state lidar. The hardware cost of this set of equipment is less than RMB 10,000. Since the smartphone software system has good scalability, the solid-state lidar can be replaced with other models of lidar, such as Yushu L1 lidar; the mobile phone can be replaced with other Android phones (with Android version not less than 11).

[0069] The software solution of the present invention can be implemented by computer software technology. The main steps of the software implementation are as follows: the laser radar communicates with the mobile phone to receive the laser radar message; the mobile phone starts the camera; the mobile phone monitors the frames of the laser radar and the camera; after receiving the corresponding laser point cloud frame, the laser radar odometer real-time pose estimation is performed; after receiving the laser point cloud and camera image frame, the laser point cloud and the camera image frame are stored, such as Figure 2 The software is partly developed based on mobile phone operating systems, such as Android. The software system includes a laser radar and IMU data acquisition and storage module, a camera data acquisition and storage module, a camera and laser radar calibration module, a laser radar inertial odometer (described later) and a user interaction interface, as shown in the figure. Figure 3 shown.

[0070] Specifically, the process of the present invention is specifically described by taking Livox Mid360 radar and Xiaomi Redmi K40 Pro as examples.

[0071] First, share the Ethernet of the mobile phone through the mobile phone settings, so that the laser radar and the mobile phone are on the same Ethernet subnet, such as 192.168.135.xx, so that the developed mobile app can receive the data released by the laser radar;

[0072] By calling the mobile phone development API, open the rear-view camera of the mobile phone;

[0073] The mobile app can receive the LiDAR point cloud frame and the LiDAR inertial measurement unit information by monitoring the LiDAR IP address. It can also receive image frames by monitoring the video frame through the mobile OS API callback. In addition, it can monitor the mobile phone's inertial measurement unit information through the mobile OS API callback.

[0074] The proposed LiDAR inertial odometry (FixedLag Smoother-LiDAR InertialOdometry -Good Feature matching, FLS-LIO-GF) is used to estimate the position and attitude of the device relative to the initial moment. After receiving the IMU information provided by the new LiDAR, FLS-LIO-GF can infer the movement of the device using the mechanical arrangement algorithm. After receiving the new LiDAR point cloud, the point-to-plane adaptive feature matching method is used to associate the current point cloud frame with the sub-map formed by multiple historical point cloud frames. These point-to-plane associations are used as observations in the fixed delay smoother to optimize the system state corresponding to the current point cloud frame. The optimized device posture and point cloud map can be displayed on the mobile phone screen;

[0075] The lidar point cloud frames, IMU data and image frames received by the mobile app can be saved in real time for subsequent offline mapping and analysis.

[0076] The corresponding software implementation method can be to use C++ to implement the lidar driver, lidar and camera calibration program, and lidar inertial odometer, and then use Android NDK to compile and package these modules to obtain the corresponding library files, and finally use jni in the Android application to call the functions in these library files, thereby realizing the collection, processing and display of lidar, IMU and camera data.

[0077] The present invention uses a consumer-grade low-cost scene 3D scanning system to dynamically scan the building scene, and can generate a 3D point cloud map of the scene in real time. It adopts a fixed-delay smoothed lidar inertial odometer based on adaptive feature matching and uses a single-step fixed-delay smoother, which can not only provide an information matrix, but also flexibly select a robust loss function. It uses an adaptive feature matching strategy and selects observations that contribute greatly to state estimation for the optimization process, thereby controlling the amount of calculation and ensuring the accuracy of pose estimation.

[0078] Based on the above embodiment, before collecting the point cloud frames of the laser radar at different times in the scene to be scanned, the inertial measurement unit data and the mobile phone image data, it also includes:

[0079] The mobile phone, laser radar and power supply are fixedly installed on the three-axis bracket;

[0080] Configure the IP addresses of the mobile phone and the laser radar;

[0081] The camera of the mobile phone and the laser radar are calibrated to complete the external parameter calibration of the camera and the laser radar.

[0082] It is understandable that before implementing scene scanning in the embodiment of the present invention, it is necessary to set and calibrate the relevant hardware.

[0083] First, fix the mobile phone, lidar and power supply through a three-axis bracket or a 3D printed bracket. Usually, the lidar will turn on when it is powered on.

[0084] Then configure the IP address of the LiDAR. If the phone is turned on for the first time, you need to use a laptop to configure the IP address of the LiDAR based on the IP address of the Ethernet shared by the phone, so that the phone can receive the UDP message of the LiDAR. For the configuration steps and basic information, refer to the user manual of the LiDAR. In subsequent use, if the phone is always in standby mode, because the Ethernet IP address of the phone remains unchanged, there is no need to reconfigure the IP address for the LiDAR.

[0085] Then perform relative calibration of the camera and lidar. If the device is assembled for the first time, the calibration procedures of the camera and lidar need to be executed to accurately determine the external parameters of the two sensors for subsequent point cloud rendering. The calibration procedure can be developed into an application app based on the mobile phone API. The app can control the camera to focus to infinity. The camera needs to keep the focus at infinity during calibration and subsequent scene scanning. After opening the app, place the device still in three different positions to complete the collection of calibration data. Then, the calibration app can process this data to estimate the relative external parameters between the camera and lidar. The calibration app calls the camera and lidar calibration algorithm developed based on C++ at the bottom layer. The calibration algorithm can adopt an open source method, such as. If the hardware structure of the device has never changed, there is no need to repeat the calibration.

[0086] Based on the above embodiment, the point cloud frames of the laser radar at different times in the scene to be scanned, the inertial measurement unit data and the mobile phone image data are collected, including:

[0087] Assigning a timestamp to the laser radar through the mobile phone, and collecting point cloud frames and inertial measurement unit data with the timestamp;

[0088] The mobile phone image data with the timestamp is collected synchronously.

[0089] Specifically, in an embodiment of the present invention, data collection and real-time processing can be achieved by developing an app based on the mobile phone API, which can receive the point cloud and IMU messages of the lidar and start the rear-view camera of the mobile phone; collect and save the point cloud and IMU data with the timestamps assigned by the mobile phone and lidar, and the image data with the timestamps assigned by the mobile phone. Because these data have the timestamps given by the mobile phone clock, accurate time synchronization can be achieved. These lidar data and image data can be stored in real time in the mobile phone in the format of rosbag and mp4 respectively, for subsequent offline data analysis.

[0090] While recording data, the real-time LiDAR inertial odometer can estimate the device's position and attitude relative to the initial moment and generate a sparse point cloud of the scene. These positions and sparse point clouds can be displayed on the phone screen, making it easier for users to view and plan scene scanning routes.

[0091] Based on the above embodiment, the fixed-delay smoothed lidar inertial odometer based on adaptive feature matching is used to process the point cloud frames and inertial measurement unit data of the lidar at different times, estimate the motion posture in real time, and obtain a sparse point cloud of the scene, including:

[0092] Determining a point cloud frame and inertial measurement unit data of the laser radar;

[0093] Performing inertial measurement unit state recursion based on the inertial measurement unit data to obtain a system prediction state, and performing point cloud dedistortion and downsampling based on the point cloud frame to obtain a processed point cloud frame;

[0094] Obtaining the estimated point cloud submap at the last moment, initializing the point cloud submap, and obtaining an initialized point cloud submap;

[0095] Acquire a point-to-surface matching set of the processed point cloud frame and the surface elements in the initialized point cloud submap by adaptive feature matching;

[0096] Based on a fixed delay smoother, a least squares problem is constructed for the point-to-surface matching set, the system state at the previous moment and the system state at the current moment are optimized, and an optimized posture and point cloud frame are obtained;

[0097] The point cloud submap is updated using the optimized pose and point cloud frame, and the sparse point cloud of the scene is output.

[0098] Specifically, Figure 4As shown, the embodiment of the present invention adopts FLS-LIO-GF, which uses adaptive feature matching to select matches that have key contributions to pose estimation. Most existing LIO methods match all points in a point cloud frame and use all matches for state estimation. The present invention only uses the key matches selected in the frame for state estimation, which can significantly reduce the computational consumption of state estimation.

[0099] Since adaptive feature matching requires the information matrix of the system state (the inverse of the covariance matrix), the present invention uses a fixed lag smoother (FLS) to optimize the state quantity. Currently, most LIO methods use iterative extended Kalman filtering (IEKF) or sliding window smoother (SlidingWindow Smoother). However, the sliding window smoother has low computational efficiency, and the IEKF can neither provide an information matrix nor be compatible with most robust loss functions, such as Huber loss. Since the odometer (LIO) method of the present invention is unique compared to the existing LIO method in terms of fixed lag smoother and good feature matching, the odometer method is hereinafter referred to as FLS-LIO-GF.

[0100] LIO needs to build a local point cloud map (submap) and track the pose of the current frame based on the submap. In order to estimate the pose of the current frame, it is necessary to find the match between the points in the current frame and the submap. In order to accelerate map construction and matching, the current methods use a variety of data structures, which can be roughly divided into: ikd-tree, iVox and voxelgrid (LIO-EKF) for storing points; Voxelmap and Voxelmap++ for storing surface element (i.e., a small plane formed by several neighboring points); voxel grid for storing Gaussian distribution (mean and variance), etc. The above studies basically show that voxel grid is more efficient than ikd-tree matching and composition. Therefore, the LIO proposed in this invention also searches for point-to-surface element matching and constructs point cloud submaps based on voxel grid. Unlike the above methods, the voxel grid of this LIO method stores the position and normal vector of the point. The normal vector of the point is used to eliminate unreasonable point-to-surface element matching.

[0101] The input of FLS-LIO-GF is a time-synchronized LiDAR point cloud frame sequence and an inertial measurement unit data sequence. After being processed by the LIO algorithm, it outputs high-frequency (>100Hz) instant pose information at each IMU data moment and low-frequency (10Hz) optimized pose information at each point cloud frame moment. The system state estimated by FLS-LIO-GF is

[0102]

[0103] Among them, it includes the position of IMU in the world coordinate system W , Posture ,speed , and the IMU's gyro bias and accelerometer bias For the convenience of expression, Indicated in The system status at a moment.

[0104] like Figure 4 The FLS-LIO-GF algorithm flow shown in the figure first assumes that the initial state is still and uses the initial received IMU data to initialize the FLS estimator. That is, the IMU data averaging method is used to initialize ,initialization is a zero vector and initialized Make the z-axis of the W coordinate system opposite to gravity.

[0105] In the subsequent processing, the IMU and lidar data received by LIO will be added to their respective data queues, and the subsequent steps will be continuously cycled.

[0106] Based on the above embodiment, the inertial measurement unit state is recursively deduced based on the inertial measurement unit data sequence to obtain a system prediction state, and the point cloud frame is dedistorted and downsampled to obtain a downsampled point cloud frame, including:

[0107] Determine a point cloud frame at any time, and determine the inertial measurement unit data from the last point cloud frame time to the current point cloud frame time in the inertial measurement unit data sequence;

[0108] Using the inertial measurement unit data between two moments, the system state at the previous moment is predicted to obtain the system prediction state at the current point cloud frame moment, as well as the observation state of multiple inertial measurement units between the previous moment and the current moment;

[0109] Based on the system prediction state and the inertial measurement unit observation state, removing the motion distortion of the points in the point cloud frame to obtain a dedistorted point cloud frame;

[0110] The dedistorted point cloud frame is downsampled using a voxel grid filter to obtain the downsampled point cloud frame.

[0111] Specifically, in the embodiment of the present invention, for point cloud dedistortion and downsampling processing, Receive a lidar frame at any time After that, find the last moment from the IMU data queue and the current moment Since the IMU data may arrive late, this can be done by locking the lidar processing thread (such as condition_variable and mutex) to wait for the IMU data. Using the IMU data, you can System status Predict System status at the moment ,as well as and The system states at these IMU observation times are calculated. Using the system states predicted by these IMUs, the motion distortion of the points in the lidar frame can be removed to obtain In order to reduce time consumption, a voxel grid filter (for example, a grid size of 0.3 m) is used to downsample the point cloud frame to obtain .

[0112] Based on the above embodiment, a point cloud submap after the previous point cloud frame time estimation is completed is obtained, and the point cloud submap is initialized at the initial time, including:

[0113] A voxel grid based on hash retrieval is used to store the point cloud submap estimated at the last moment; wherein the voxel grid uses a fixed grid size, and the storage information of each voxel includes the coordinates of the point in the world coordinate system and the normal vector of the plane on which it is located;

[0114] Initialize the normal vector of the plane to the zero vector and the submap to an empty map;

[0115] Start receiving the first frame point cloud, add each point of the first frame point cloud to the empty map according to the grid coordinates, for a point P, determine m points near the dedistorted first frame point cloud, calculate the covariance matrix of the m+1 points including point P, initialize the normal vector of each point using the eigenvector corresponding to the minimum eigenvalue, process subsequent frame point clouds in sequence, and obtain the initialized point cloud submap.

[0116] Specifically, the embodiment of the present invention is directed to the initialization of the point cloud submap. Represents the point cloud submap after the last estimation. The point cloud map is stored using a voxel grid based on hash indexing. The side length of the grid (voxel) can be adjusted adaptively according to the scene, but the present invention uses a fixed grid size, such as 0.5m. The information of the point stored in each voxel includes the coordinates of the point in the world coordinate system W and the normal vector of the plane. The plane normal vector is initialized to a zero vector. The submap M is initially empty, and the first frame of the lidar point cloud After that, directly add each point in it to M according to the grid coordinates. At each point in Find the nearest m (such as 5) points, calculate the covariance matrix of these m+1 (including itself) points, and initialize the normal vector of the point with the eigenvector corresponding to the minimum eigenvalue.

[0117] Based on the above embodiment, obtaining a point-to-surface matching set of the processed point cloud frame and the surface elements in the initialized point cloud submap through adaptive feature matching includes:

[0118] Determine the number of target matches, relaxation coefficient, matching quality evaluation function and upper limit of operation time, and input the downsampled point cloud frame, the point cloud submap, the system state at the last moment and the predicted system state at the current moment;

[0119] Initialize the point-to-surface matching set to an empty set;

[0120] Selecting any subset of the processed point cloud frame according to the target matching number and the relaxation coefficient;

[0121] A lazy greedy algorithm is used to search the arbitrary subset, and a matching face is searched in the submap for each point in the arbitrary subset, and it is determined whether the point-to-face match satisfies the preset face normal vector direction determination condition, so as to obtain an initial matching set of all points to faces formed by the arbitrary subset, and the match with the largest information gain in the point-to-face match of the arbitrary subset is determined according to the matching quality evaluation function, and the match is added to the initial matching set, and the operation is repeated multiple times until the operation time limit or the target number of matches is reached, thereby obtaining the point-to-face matching set. Specifically, in the embodiment of the present invention, an adaptive feature matching method is used to find The point in The uniqueness of the present invention is that the quality evaluation function of the matching takes into account the prior information and IMU observations, while the related algorithms only consider the matching of points to surfaces. Adaptive feature matching draws on the idea of ​​lazier greedy selection to adaptively find k The point in The specific implementation steps are as follows:

[0122] Input: number of target matches k, relaxation coefficient , matching quality evaluation function , operation time limit , point cloud frame , submap , the system status optimized at the last moment And the system state predicted by the IMU at the current moment , .

[0123] Output: A set S of point-to-surface matching.

[0124] initialization .

[0125] while and , do

[0126] R from Randomly select Points form a subset

[0127] For every point in R

[0128] exist Search for the nearest neighbor points (such as 5),

[0129] Check whether the directions of the normal vectors of these points are close enough (for example, the dot products of these normal vectors are greater than 0.8). If so, calculate the Jacobian matrix of the loss function of the point and information increment ,

[0130] Otherwise, from R and Remove this point from

[0131] j*

[0132]

[0133]

[0134] Based on the above embodiment, based on a fixed delay smoother, a least squares problem is constructed for the point-to-surface matching set, the system state at the previous moment and the system state at the current moment are optimized, and the optimized posture and point cloud frame are obtained, including:

[0135] The prior constraint of the system state at the previous moment in the least squares problem is determined by the initial posture including uncertainty, the pre-integration constraint of the inertial measurement unit data in the least squares problem is determined by the pre-integration of the inertial measurement unit data between the system state at the previous moment and the system state at the current moment, and the matching constraint from any point in the point cloud frame at the current moment to the surface element in the least squares problem is determined by the posture of the inertial measurement unit relative to the world coordinate system at the current moment, the external parameters of the laser radar relative to the inertial measurement unit, the coordinates of the point in the laser radar coordinate system, any point in the surface element in the point cloud submap, and the surface element normal vector;

[0136] Based on the prior constraints of the system state at the previous moment, the pre-integration constraints of the inertial measurement unit data, and the constraints of matching any point to a surface element in the point cloud frame at the current moment, a least squares problem objective function is obtained;

[0137] The least squares problem objective function is optimized and solved to obtain an optimized posture and a point cloud frame in a world coordinate system.

[0138] Specifically, the embodiment of the present invention uses the matching set S of points to facets found by adaptive matching to construct a least squares problem and optimize and Before constructing the problem, it is necessary to refit the planes for these matches to eliminate inappropriate planes. Plane fitting can be achieved by using SVD to decompose the covariance matrix. The constructed least squares problem contains The prior constraints , and IMU pre-integration constraints between ,and time midpoint Matching constraints to panels , and its corresponding weight matrix is , and . Prior constraints From the initial pose with uncertainty or the marginalization step below, only works on , whose weight can be determined by the uncertainty of the initial pose or the marginalization process. IMU constraint From arrive Pre-integration of IMU data at each moment, with weights determined based on the pre-integration process.

[0139] To one Lidar points in (L represents the lidar coordinate system) and the corresponding surface element ,in is the normal vector of the surface element, for A point in the region that forms the surface element, constraining Expressed as

[0140]

[0141] in is the external parameter of lidar relative to IMU, for The position of the IMU relative to the world system at this moment. Weight Can be set to The noise covariance of .

[0142] In summary, the objective function of the least squares problem is for:

[0143]

[0144] In order to reduce the impact of external matching, The loss is calculated through the robust loss function The flexible use of robust loss functions such as Huberloss is an important advantage of the FLS-LIO-GF of the present invention compared to other filtering-based LIO methods.

[0145] Iteratively solving the nonlinear least squares problem, for example using the Levenberg-Marquardt solver, can yield the optimized and In order to maintain a low computational load, the Schul complement strategy is used to marginalize , thus obtaining The prior constraints and information matrix ( ).

[0146] This information will be used in the next step ( arrive )Used to calculate the matching quality evaluation function in adaptive feature selection .exist arrive In the step, the quality evaluation function Considering the prior information ( Information Matrix ), IMU information and lidar frame Midpoint to polygon matching set .

[0147]

[0148] The size is The prior information matrix for

[0149]

[0150] IMU Information Matrix for

[0151]

[0152]

[0153] point To Bin The matching information matrix is

[0154]

[0155]

[0156] Note that the derivatives here are all derivatives of the minimum spatial perturbation of the state quantity. For example Minimum spatial perturbation It can be defined as

[0157]

[0158] in represents the antisymmetric matrix of a vector, Represents an estimate of a quantity.

[0159] The perturbation of the state quantity in Euclidean space is defined by ordinary addition, such as .

[0160] Based on the above embodiment, the point cloud submap is updated using the optimized pose and the downsampled point cloud frame to output the scene sparse point cloud, including:

[0161] Using the optimized posture, transforming the midpoint of the downsampled point cloud frame to a world coordinate system, and obtaining the voxel corresponding to the transformed point according to the hash index;

[0162] Screening out points whose distance from any point in the point cloud frame after the downsampling process to the voxel center is less than the distance from the midpoint of the current voxel to the voxel center, and adding the screened points to the current voxel;

[0163] The normal vector of the midpoint of the matching facet in the point cloud submap is set as the normal vector of the matching facet, a number of neighboring points are searched for points of the unmatched facet in the point cloud submap, the covariance matrix of the number of neighboring points is calculated, and the normal vectors of the number of neighboring points are initialized using the eigenvector corresponding to the minimum eigenvalue, and after all points are updated, an updated point cloud submap is obtained;

[0164] The updated point cloud submap is removed from obsolete grids that are not in the area of ​​interest, and the optimized pose and corresponding point cloud frame are output.

[0165] Specifically, the embodiment of the present invention uses the obtained optimized posture and Update point cloud submap This step first uses the optimized pose to The points in are transformed into the world coordinate system, and then the corresponding voxel is found according to the hash index. ,if If the distance to the center of the voxel is greater than the distance from the point in the current voxel to the center of the voxel, it will be discarded. point, otherwise Add this voxel. About new additions The normal vector of the point In the adaptive matching step, the point matching the surface element has been found, and the normal vector of the point is set to the normal vector of the matching surface element. For the point where the matching surface element has not been found, Search for the m nearest points in the equation, calculate the covariance matrix of these m+1 (including itself) points, and use the eigenvector corresponding to the minimum eigenvalue to initialize the normal vector of the point. After finding all the points, get the updated map .

[0166] To avoid submaps If the size of the grid is too large, the LRU cache strategy of Faster-LIO can be used to remove redundant grids. That is, the grids that have been recently matched are recorded in a queue. If the queue exceeds a certain length L, the old grids are deleted from the submap. Alternatively, the method of limiting the spatial range can be used to remove redundant grids. That is, after each pose estimation is completed, each grid is checked against the current device position. If the distance is greater than D, the grid is deleted.

[0167] In the actual processing process, a separate thread can be used to process the IMU data to obtain the state estimation of the IMU frequency. Whenever an IMU data comes, the thread calculates the state from the current state and outputs it; whenever After the FLS optimization is completed, the thread updates the saved The system status at the moment is propagated to the latest IMU data moment.

[0168] After the entire scene is scanned, the real-time LiDAR inertial odometer can obtain an instant full-scene point cloud and the corresponding device trajectory. The device motion trajectory, the original point cloud and image data saved on the mobile phone can be exported to the computer for subsequent analysis and detailed mapping, such as Figure 5 The 3D point cloud of the office scene shown, and Figure 6 The corresponding two-dimensional occupancy grid diagram is shown.

[0169] The scene point cloud data acquisition system based on the handheld scene scanning system provided by the present invention is described below. The scene point cloud data acquisition system based on the handheld scene scanning system described below and the scene point cloud data acquisition method based on the handheld scene scanning system described above can be referenced to each other.

[0170] Figure 7: is a structural schematic diagram of a scene point cloud data acquisition system based on a handheld scene scanning system provided by an embodiment of the present invention, comprising: an acquisition module 71, a real-time device motion tracking module 72 and a map optimization module 73, wherein:

[0171] The acquisition module 71 is used to collect the point cloud frames, inertial measurement unit data and mobile phone image data of the lidar at different times in the scene to be scanned; the real-time device motion tracking module 72 is used to use the fixed-delay smoothed lidar inertial odometer based on adaptive feature matching to perform real-time processing on the point cloud frames and inertial measurement unit data of the lidar at different times to obtain the device motion trajectory and the scene sparse point cloud; the map optimization module 73 is used to complete the panoramic scan of the scene to be scanned, and then establish the three-dimensional scene color point cloud of the scene to be scanned from the device motion trajectory, the scene sparse point cloud and the mobile phone image data.

[0172] Figure 8 An example of a physical structure diagram of an electronic device is shown in FIG. Figure 8 As shown, the electronic device may include: a processor 810, a communication interface 820, a memory 830 and a communication bus 840, wherein the processor 810, the communication interface 820 and the memory 830 communicate with each other through the communication bus 840. The processor 810 may call the logic instructions in the memory 830 to execute a scene point cloud data acquisition method based on a handheld scene scanning system, the method comprising: collecting the point cloud frames of the laser radar at different times in the scene to be scanned, the inertial measurement unit data and the mobile phone image data; using a fixed delay smoothing laser radar inertial odometer based on adaptive feature matching, the point cloud frames of the laser radar at different times and the inertial measurement unit data are processed in real time to obtain the device motion trajectory and the scene sparse point cloud; after completing the panoramic scan of the scene to be scanned, the three-dimensional scene color point cloud of the scene to be scanned is established by the device motion trajectory, the scene sparse point cloud and the mobile phone image data.

[0173] In addition, the logic instructions in the above-mentioned memory 830 can be implemented in the form of software functional units and can be stored in a computer-readable storage medium when sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions for a computer device (which can be a mobile handheld device, a personal computer, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc. Various media that can store program codes.

[0174] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, it is implemented to execute the scene point cloud data acquisition method based on a handheld scene scanning system provided by the above-mentioned methods, the method comprising: collecting point cloud frames, inertial measurement unit data and mobile phone image data of the laser radar at different times in the scene to be scanned; using a fixed-delay smoothing laser radar inertial odometer based on adaptive feature matching to perform real-time processing on the point cloud frames and inertial measurement unit data of the laser radar at different times to obtain the device motion state and the scene sparse point cloud; after completing a panoramic scan of the scene to be scanned, establishing a three-dimensional scene color point cloud of the scene to be scanned from the device motion trajectory, the scene sparse point cloud and the mobile phone image data.

[0175] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0176] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, by hardware. Based on this understanding, the above technical solution can essentially or in other words be embodied in the form of a software product that contributes to the prior art. The computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions for a computer device (which can be a mobile handheld device, a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0177] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A scene point cloud data acquisition method based on a handheld scene scanning system, characterized in that: include: Collect the point cloud frames, inertial measurement unit data and mobile phone image data of the lidar at different times in the scene to be scanned; Using a fixed-delay smoothed lidar inertial odometer based on adaptive feature matching, the point cloud frames of the lidar at different times and the inertial measurement unit data are processed in real time to obtain the device motion state and the scene sparse point cloud; After completing the panoramic scan of the scene to be scanned, a three-dimensional scene color point cloud of the scene to be scanned is established based on the device motion trajectory, the scene sparse point cloud and the mobile phone image data; Collect the laser radar point cloud frames, inertial measurement unit data and mobile phone image data at different times in the scene to be scanned, including: Assigning a timestamp to the laser radar through the mobile phone, and collecting point cloud frames and inertial measurement unit data with the timestamp; Synchronously collecting mobile phone image data with the timestamp; The fixed-delay smoothed lidar inertial odometer based on adaptive feature matching is used to process the lidar point cloud frames and inertial measurement unit data at different times, estimate the motion posture in real time, and obtain a sparse point cloud of the scene, including: Determining a point cloud frame and inertial measurement unit data of the laser radar; Based on the inertial measurement unit data, an inertial measurement unit state is recursively deduced to obtain a system prediction state, and the point cloud frame is dedistorted and downsampled to obtain a processed point cloud frame; Obtaining the estimated point cloud submap at the last moment, initializing the point cloud submap, and obtaining an initialized point cloud submap; Acquire a point-to-surface matching set of the processed point cloud frame and the surface elements in the initialized point cloud submap by adaptive feature matching; Based on a fixed delay smoother, a least squares problem is constructed for the point-to-surface matching set, the system state at the previous moment and the system state at the current moment are optimized, and an optimized posture and point cloud frame are obtained; The point cloud submap is updated using the optimized pose and point cloud frame, and the sparse point cloud of the scene is output.

2. The scene point cloud data acquisition method based on a handheld scene scanning system according to claim 1, characterized in that: Before collecting the point cloud frames of the lidar, the inertial measurement unit data, and the mobile phone image data at different times in the scene to be scanned, it also includes: The mobile phone, laser radar and power supply are fixedly installed on the three-axis bracket; Configure the IP addresses of the mobile phone and the laser radar; The camera of the mobile phone and the laser radar are calibrated to complete the external parameter calibration of the camera and the laser radar.

3. The scene point cloud data acquisition method based on a handheld scene scanning system according to claim 1, characterized in that: Based on the inertial measurement unit data, the inertial measurement unit state is recursively deduced to obtain a system prediction state, and the point cloud frame is dedistorted and downsampled to obtain a downsampled point cloud frame, including: Determine a point cloud frame at any time, and determine the inertial measurement unit data from the last point cloud frame time to the current point cloud frame time in the inertial measurement unit data sequence; Using the inertial measurement unit data between two moments, the system state at the previous moment is predicted to obtain the system predicted state at the current moment, as well as the observed state of multiple inertial measurement units between the previous moment and the current moment; Based on the system prediction state and the inertial measurement unit observation state, removing the motion distortion of the points in the point cloud frame sequence to obtain a dedistorted point cloud frame; The dedistorted point cloud frame is downsampled using a voxel grid filter to obtain the downsampled point cloud frame.

4. The scene point cloud data acquisition method based on a handheld scene scanning system according to claim 1, characterized in that: Obtain the estimated point cloud submap completed at the previous moment, and initialize the point cloud submap at the start moment, including: A voxel grid based on hash retrieval is used to store the point cloud submap estimated at the last moment; wherein the voxel grid uses a fixed grid size, and the storage information of each voxel includes the coordinates of a point in the world coordinate system and the normal vector of the plane on which it is located; Initialize the normal vector of the plane to the zero vector and the submap to an empty map; Start receiving the first frame point cloud, add each point of the first frame point cloud to the empty map according to the grid coordinates, for a point P, determine m points near the dedistorted first frame point cloud, calculate the covariance matrix of the m+1 points including point P, initialize the normal vector of each point using the eigenvector corresponding to the minimum eigenvalue, process subsequent frame point clouds in sequence, and obtain the initialized point cloud submap.

5. The scene point cloud data acquisition method based on a handheld scene scanning system according to claim 1, characterized in that: Acquiring a point-to-surface matching set of the processed point cloud frame and the surface elements in the initialization point cloud submap by adaptive feature matching, including: Determine the number of target matches, relaxation coefficient, matching quality evaluation function and upper limit of operation time, and input the downsampled point cloud frame, the point cloud submap, the system state at the last moment and the predicted system state at the current moment; Initialize the point-to-surface matching set to an empty set; Selecting any subset of the downsampled point cloud frame according to the target matching number and the relaxation coefficient; A lazy greedy algorithm is used to search the arbitrary subset, and a matching face is searched in the submap for each point in the arbitrary subset, and it is determined whether the point-to-face match satisfies a preset face normal vector direction determination condition, so as to obtain an initial matching set of all points to faces formed by the arbitrary subset, and a match with the largest information gain in the point-to-face match of the arbitrary subset is determined according to the matching quality evaluation function, and the match is added to the initial matching set, and the operation is repeated multiple times until the operation time limit or the target number of matches is reached, thereby obtaining the point-to-face matching set.

6. The scene point cloud data acquisition method based on a handheld scene scanning system according to claim 1, characterized in that: Based on a fixed delay smoother, a least squares problem is constructed for the point-to-surface matching set, the system state at the previous moment and the system state at the current moment are optimized, and the optimized posture and point cloud frame are obtained, including: The prior constraint of the system state at the previous moment in the least squares problem is determined by the initial posture including uncertainty, the pre-integration constraint of the inertial measurement unit data in the least squares problem is determined by the pre-integration of the inertial measurement unit data between the system state at the previous moment and the system state at the current moment, and the matching constraint from any point in the point cloud frame at the current moment to the face element in the least squares problem is determined by the posture of the inertial measurement unit relative to the world coordinate system at the current moment, the external parameters of the laser radar relative to the inertial measurement unit, the coordinates of the point in the laser radar coordinate system, any point in the face element in the point cloud submap and its face element normal vector; Based on the prior constraints of the system state at the previous moment, the pre-integration constraints of the inertial measurement unit data, and the constraints of matching any point to a surface element in the point cloud frame at the current moment, a least squares problem objective function is obtained; The least squares problem objective function is optimized and solved to obtain an optimized posture and a point cloud frame in a world coordinate system.

7. The scene point cloud data acquisition method based on a handheld scene scanning system according to claim 1, characterized in that: Updating the point cloud submap using the optimized pose and point cloud frame to output the scene sparse point cloud includes: Using the optimized posture, transforming the midpoint of the downsampled point cloud frame to a world coordinate system, and obtaining the voxel corresponding to the transformed point according to the hash index; Screening out points whose distance from any point in the point cloud frame after the downsampling process to the voxel center is less than the distance from the midpoint of the current voxel to the voxel center, and adding the screened points to the current voxel; The normal vector of the midpoint of the matched facet in the point cloud submap is set as the normal vector of the matched facet, a number of neighboring points are searched for the points of the unmatched facet in the point cloud submap, the covariance matrix of the number of neighboring points is calculated, and the normal vectors of the number of neighboring points are initialized using the eigenvector corresponding to the minimum eigenvalue, and after completing the update matching of all points, an updated point cloud submap is obtained; The updated point cloud submap is removed from obsolete grids that are not in the area of ​​interest, and the optimized pose and corresponding point cloud frame are output.

8. A scene point cloud data acquisition system based on a handheld scene scanning system, characterized in that: include: The acquisition module is used to collect the point cloud frames of the lidar, the inertial measurement unit data and the mobile phone image data at different times in the scene to be scanned; A real-time motion tracking module, which is used to process the point cloud frames and inertial measurement unit data of the lidar at different times in real time by using a fixed-delay smoothed lidar inertial odometer based on adaptive feature matching to obtain a device motion trajectory and a scene sparse point cloud; A map optimization module, which is used to establish a three-dimensional scene color point cloud of the scene to be scanned based on the device motion trajectory, the scene sparse point cloud and the mobile phone image data after completing a panoramic scan of the scene to be scanned; The acquisition module is specifically used for: Assigning a timestamp to the laser radar through the mobile phone, and collecting point cloud frames and inertial measurement unit data with the timestamp; Synchronously collecting mobile phone image data with the timestamp; The real-time motion tracking module is specifically used for: Determining a point cloud frame and inertial measurement unit data of the laser radar; Based on the inertial measurement unit data, an inertial measurement unit state is recursively deduced to obtain a system prediction state, and the point cloud frame is dedistorted and downsampled to obtain a processed point cloud frame; Obtaining the estimated point cloud submap at the last moment, initializing the point cloud submap, and obtaining an initialized point cloud submap; Acquire a point-to-surface matching set of the processed point cloud frame and the surface elements in the initialized point cloud submap by adaptive feature matching; Based on a fixed delay smoother, a least squares problem is constructed for the point-to-surface matching set, the system state at the previous moment and the system state at the current moment are optimized, and an optimized posture and point cloud frame are obtained; The point cloud submap is updated using the optimized pose and point cloud frame, and the sparse point cloud of the scene is output.

Images