Method and device for processing three-dimensional point cloud data
By using pre-integrated pose and dedistortion processing technology on laser SLAM devices, combined with point cloud matching and pose optimization, the problem of insufficient computing capabilities of laser SLAM devices is solved, and the efficiency and compactness of three-dimensional point cloud data processing is improved.
Patent Information
- Application Number
- CN202411252674.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-09
- Publication Date
- 2024-11-22
- Estimated Expiration
- 2044-09-09
AI Technical Summary
The computing power of laser SLAM devices is limited, which results in large CPU consumption of SLAM algorithms and it is difficult to operate effectively on the device, especially when processing three-dimensional point cloud data.
By performing pre-integrated pose processing based on the optimized pose and inertial measurement data of the previous frame, combined with dedistortion, point cloud matching and pose optimization, the computing power consumption of point cloud data processing is reduced, and point cloud features are not extracted. The processing is carried out in the case of the process, which improves the processing efficiency.
The CPU consumption of SLAM algorithm is reduced, the compactness and efficiency of point cloud processing are improved, and the computing resource requirements for laser SLAM devices are reduced.
Smart Images

Figure CN118776550B_ABST
Abstract
Description
Technical Field
[0001] The present disclosure relates to the technical field of three-dimensional point cloud data processing, and in particular to a method and device for processing three-dimensional point cloud data. Background Art
[0002] With the development of laser radar and simultaneous localization and mapping (SLAM) technology, laser SLAM technology has become a new scanning and reconstruction solution. By carrying laser SLAM equipment for mobile scanning, it can achieve the technical effect of long scanning distance and high precision, support fast scanning of ultra-large scenes, and can well solve the problem of visual occlusion and adapt to many scenes. However, due to the weight, power supply and power consumption of laser SLAM equipment, the computing power of SLAM equipment is usually not too strong. In order to run sensor drivers, data acquisition and storage programs and SLAM algorithms on SLAM devices, it is necessary to reduce the consumption of the central processing unit (CPU) of the SLAM algorithm as much as possible.
[0003] Therefore, how to reduce the CPU consumption of SLAM algorithm has become a technical problem that needs to be solved urgently. Summary of the invention
[0004] In order to solve the above technical problems, an embodiment of the present disclosure provides a method and device for processing three-dimensional point cloud data.
[0005] According to a first aspect of an embodiment of the present disclosure, a method for processing three-dimensional point cloud data is provided, comprising:
[0006] Based on the optimized pose corresponding to the first point cloud frame and the pre-integrated pose of the inertial measurement data collected in the time period corresponding to the second point cloud frame, the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame is processed to obtain the point cloud data to be processed, wherein the first point cloud frame is an adjacent frame located before the second point cloud frame in time sequence;
[0007] Matching the point cloud data to be processed with an original point cloud map to obtain a matching point cloud set and a point cloud residual, wherein the original point cloud map is a point cloud map generated based on the point cloud data including the first point cloud frame and previous point cloud frames;
[0008] Iteratively determine a pose optimization matrix based on the matching point cloud set and the point cloud residual until an iteration stop condition is met;
[0009] Based on the pose optimization matrix, the pose of the point cloud data to be processed is optimized to obtain optimized point cloud data;
[0010] Based on the optimized point cloud data, the original point cloud map is updated.
[0011] In some embodiments of the present disclosure, after the iterative determination of the pose optimization matrix, the method further includes:
[0012] Based on the pose optimization matrix, factor graph optimization is performed to obtain an optimized pose of the second point cloud frame;
[0013] The optimized pose of the second point cloud frame is stored in a cache, and the optimized pose of the second point cloud frame is used to process point cloud data of a third point cloud frame, and the third point cloud frame is an adjacent frame located after the second point cloud frame in time sequence.
[0014] In some further embodiments of the present disclosure, the pre-integrated pose of the inertial measurement data collected in the time period corresponding to the second point cloud frame based on the optimized pose corresponding to the first point cloud frame, before processing the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame, further includes:
[0015] In response to acquiring the point cloud data of the second point cloud frame, downsampling the point cloud data of the second point cloud frame by setting a downsampling method, wherein the set downsampling method includes: at least one of a random downsampling method, a spatial uniform sampling method, and a voxel grid sampling method;
[0016] The downsampled point cloud data is subjected to a coordinate system transformation to obtain point cloud data of the second point cloud frame in an inertial measurement unit coordinate system.
[0017] In some further embodiments of the present disclosure, the step of performing coordinate system conversion on the downsampled point cloud data to obtain point cloud data in an inertial measurement unit coordinate system of the second point cloud frame includes:
[0018] Based on the motor coordinate system of the motor where the radar is located, the downsampled point cloud data is rotated;
[0019] The rotated point cloud data is converted to an inertial measurement unit coordinate system according to an external parameter conversion matrix to obtain point cloud data in the inertial measurement unit coordinate system of the second point cloud frame.
[0020] In some further embodiments of the present disclosure, before processing the point cloud data of the second point cloud frame in the inertial measurement unit coordinate system, the method further includes:
[0021] The inertial measurement unit and at least one laser radar for collecting point cloud data are clock synchronized to obtain clock synchronized point cloud data and inertial measurement data with timestamps.
[0022] In some further embodiments of the present disclosure, the step of processing the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame to obtain the point cloud data to be processed includes:
[0023] Determine the distortion amount of each point in the second point cloud frame based on the timestamp of each point in the second point cloud frame and the timestamp of each inertial measurement data collected in a time period corresponding to the second point cloud frame;
[0024] Based on the optimized posture corresponding to the first point cloud frame and the distortion amount of each point in the second point cloud frame, the point cloud data of the second point cloud frame is dedistorted to obtain the point cloud data to be processed.
[0025] In some further embodiments of the present disclosure, the process of matching the to-be-processed point cloud data with the original point cloud map further includes:
[0026] Determine at least one to-be-added point in the to-be-processed point cloud data to be added to the original point cloud map based on a matching relationship between each point in the to-be-processed point cloud data and each point in the original point cloud map;
[0027] At least one of the to-be-added points and corresponding search information are stored in the to-be-added point list.
[0028] In some further embodiments of the present disclosure, updating the original point cloud map based on the optimized point cloud data includes:
[0029] Based on at least one of the to-be-added points and the corresponding search information, obtaining a target to-be-added point from the optimized point cloud data;
[0030] Based on the target point to be added, the origin cloud map is updated.
[0031] In some further embodiments of the present disclosure, the matching point cloud set includes a matching relationship between the point cloud data to be processed and a point in the original point cloud map;
[0032] The iterative determination of the pose optimization matrix based on the matching point cloud set and the point cloud residual until an iteration stop condition is satisfied includes:
[0033] Using a least squares optimization algorithm, based on the matching point cloud set, iteratively optimize the relative position of the to-be-processed point cloud data to the original point cloud map;
[0034] In response to satisfying the first iteration stop condition, obtaining the posture optimization matrix;
[0035] In response to satisfying the second iteration stop condition, based on the relative posture of the previous round of iterative optimization, the operation of matching the point cloud data to be processed with the original point cloud map is performed, and the next round of iterative optimization is started until the first iteration stop condition is satisfied to obtain the posture optimization matrix.
[0036] According to a second aspect of an embodiment of the present disclosure, a three-dimensional point cloud data processing device includes:
[0037] a point cloud processing module, configured to process the point cloud data of the second point cloud frame in the inertial measurement unit coordinate system based on the optimized pose corresponding to the first point cloud frame and the pre-integrated pose of the inertial measurement data collected in the time period corresponding to the second point cloud frame, so as to obtain the point cloud data to be processed, wherein the first point cloud frame is an adjacent frame located before the second point cloud frame in time sequence;
[0038] A point cloud matching module, used for matching the point cloud data to be processed with an original point cloud map to obtain a matching point cloud set and a point cloud residual, wherein the original point cloud map is a point cloud map generated based on the point cloud data including the first point cloud frame and previous point cloud frames;
[0039] A posture determination module, used for iteratively determining a posture optimization matrix based on the matching point cloud set and the point cloud residual until an iteration stop condition is met;
[0040] A posture optimization module, used for performing posture optimization on the point cloud data to be processed based on the posture optimization matrix to obtain optimized point cloud data;
[0041] A point cloud update module is used to update the original point cloud map based on the optimized point cloud data.
[0042] In some embodiments of the present disclosure, the device further comprises:
[0043] An information determination module, configured to perform factor graph optimization based on the pose optimization matrix to obtain an optimized pose of the second point cloud frame;
[0044] The first storage module is used to store the optimized posture of the second point cloud frame in a cache, and the optimized posture of the second point cloud frame is used to process the point cloud data of the third point cloud frame, and the third point cloud frame is an adjacent frame located after the second point cloud frame in time sequence.
[0045] In some further embodiments of the present disclosure, the device further comprises:
[0046] a downsampling module, configured to, in response to acquiring the point cloud data of the second point cloud frame, perform downsampling processing on the point cloud data of the second point cloud frame by setting a downsampling method, wherein the set downsampling method includes: at least one of a random downsampling method, a spatial uniform sampling method, and a voxel grid sampling method;
[0047] The coordinate conversion module is used to perform coordinate system conversion on the downsampled point cloud data to obtain the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame.
[0048] In some further embodiments of the present disclosure, the coordinate conversion module includes:
[0049] A first conversion submodule is used to rotate the downsampled point cloud data based on the motor coordinate system of the motor where the radar is located;
[0050] The second conversion submodule is used to convert the rotated point cloud data to the inertial measurement unit coordinate system according to the external parameter conversion matrix to obtain the point cloud data of the second point cloud frame in the inertial measurement unit coordinate system.
[0051] In some further embodiments of the present disclosure, the device further comprises:
[0052] The clock synchronization module is used to perform clock synchronization on the inertial measurement unit and at least one laser radar for collecting point cloud data, so as to obtain clock-synchronized point cloud data and inertial measurement data with timestamps.
[0053] In some further embodiments of the present disclosure, the point cloud processing module includes:
[0054] A determination submodule, configured to determine the distortion amount of each point in the second point cloud frame based on the timestamp of each point in the second point cloud frame and the timestamp of each inertial measurement data collected within a time period corresponding to the second point cloud frame;
[0055] A processing submodule is used to dedistort the point cloud data of the second point cloud frame based on the optimized posture corresponding to the first point cloud frame and the distortion amount of each point in the second point cloud frame to obtain the point cloud data to be processed.
[0056] In some further embodiments of the present disclosure, the device further comprises:
[0057] A point determination module, used for determining at least one to-be-added point in the to-be-processed point cloud data to be added to the original point cloud map based on the matching relationship between each point in the to-be-processed point cloud data and each point in the original point cloud map during the matching operation between the to-be-processed point cloud data and the original point cloud map;
[0058] The second storage module is used to store at least one point to be added and corresponding search information in the list of points to be added.
[0059] In some further embodiments of the present disclosure, the point cloud updating module includes:
[0060] An acquisition submodule, configured to acquire a target point to be added from the optimized point cloud data based on at least one point to be added and corresponding search information;
[0061] The updating submodule is used to update the origin cloud map based on the target point to be added.
[0062] In some further embodiments of the present disclosure, the matching point cloud set includes a matching relationship between the point cloud data to be processed and a point in the original point cloud map;
[0063] The posture determination module comprises:
[0064] A first iterative submodule is used to iteratively optimize the relative position of the to-be-processed point cloud data to the original point cloud map based on the matching point cloud set by using a least squares optimization algorithm;
[0065] A second iteration submodule, configured to obtain the posture optimization matrix in response to satisfying the first iteration stop condition;
[0066] The third iterative submodule is used to respond to the satisfaction of the second iteration stop condition, based on the relative posture of the previous round of iterative optimization, perform the operation of matching the point cloud data to be processed with the original point cloud map, start the next round of iterative optimization, until the first iteration stop condition is met, and obtain the posture optimization matrix.
[0067] According to a third aspect of an embodiment of the present disclosure, a computer-readable storage medium is provided, wherein the storage medium stores computer program instructions, and when the computer program instructions are executed, the above-mentioned three-dimensional point cloud data processing method is implemented.
[0068] According to a fourth aspect of an embodiment of the present disclosure, an electronic device is provided, the electronic device comprising:
[0069] A memory for storing a computer program product;
[0070] The processor is used to execute the computer program product stored in the memory, and when the computer program product is executed, the above-mentioned three-dimensional point cloud data processing method is implemented.
[0071] According to a fifth aspect of an embodiment of the present disclosure, a computer program product is provided, comprising computer program instructions, which implement the above-mentioned three-dimensional point cloud data processing method when executed by a processor.
[0072] Based on the above-mentioned embodiments of the present disclosure, when a new laser point cloud frame (second point cloud frame) is collected, the point cloud data of the second point cloud frame can be dedistorted, point cloud matched, optimized in position and point cloud map updated based on the position and posture corresponding to the previous point cloud frame (first point cloud frame) and the inertial measurement data collected in the corresponding time period of the second point cloud frame, so as to realize the processing of point cloud data without extracting point cloud features, thereby reducing the computing power consumption. Moreover, after pre-integrating the IMU data, the point cloud is dedistorted in combination with the pre-integrated posture, thereby improving the compactness of point cloud processing, reducing the CPU consumption of the SLAM algorithm, and improving the efficiency of point cloud processing.
[0073] The technical solution of the present disclosure is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS
[0074] By describing the embodiments of the present disclosure in more detail in conjunction with the accompanying drawings, the above and other purposes, features and advantages of the present disclosure will become more apparent. The accompanying drawings are used to provide a further understanding of the embodiments of the present disclosure and constitute a part of the specification. Together with the embodiments of the present disclosure, they are used to explain the present disclosure and do not constitute a limitation of the present disclosure. In the accompanying drawings, the same reference numerals generally represent the same components or steps. The present disclosure can be more clearly understood based on the following detailed description, in which:
[0075] Figure 1 A schematic diagram of the structure of a device for implementing a method for processing three-dimensional point cloud data disclosed herein;
[0076] Figure 2 A flowchart of an embodiment of a method for processing three-dimensional point cloud data disclosed herein;
[0077] Figure 3 A flowchart of another embodiment of the method for processing three-dimensional point cloud data disclosed in the present invention;
[0078] Figure 4 A flowchart of another embodiment of the method for processing three-dimensional point cloud data disclosed herein;
[0079] Figure 5 A flowchart of an embodiment of the de-distortion processing of three-dimensional point cloud data disclosed in the present invention;
[0080] Figure 6 A flowchart of another embodiment of the method for processing three-dimensional point cloud data disclosed herein;
[0081] Figure 7 A flowchart of another embodiment of the method for processing three-dimensional point cloud data disclosed herein;
[0082] Figure 8A schematic diagram of the structure of an embodiment of a three-dimensional point cloud data processing device disclosed in the present invention;
[0083] Fig. 9 A schematic structural diagram of another embodiment of a three-dimensional point cloud data processing device disclosed in the present invention;
[0084] Fig.10 It is a structural diagram of an electronic device provided by an exemplary embodiment of the present application. DETAILED DESCRIPTION
[0085] The exemplary embodiments according to the present disclosure will be described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present disclosure, rather than all the embodiments of the present disclosure, and it should be understood that the present disclosure is not limited to the exemplary embodiments described here.
[0086] It should be noted that the relative arrangement of components and steps, the numerical expressions and numerical values set forth in these embodiments do not limit the scope of the present disclosure unless specifically stated otherwise.
[0087] Those skilled in the art can understand that the terms "first" and "second" in the embodiments of the present disclosure are only used to distinguish different steps, devices or modules, etc., and neither represent any specific technical meaning nor indicate the necessary logical order between them.
[0088] It should also be understood that in the embodiments of the present disclosure, “plurality” may refer to two or more than two, and “at least one” may refer to one, two, or more than two.
[0089] It should also be understood that any component, data or structure mentioned in the embodiments of the present disclosure can generally be understood as one or more, unless explicitly limited or otherwise indicated in the context.
[0090] In addition, the term "and / or" in this disclosure is only a description of the association relationship of associated objects, indicating that there may be three relationships, such as A and / or B, which can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this disclosure generally indicates that the associated objects before and after are in an "or" relationship.
[0091] It should also be understood that the description of the various embodiments in the present disclosure focuses on the differences between the various embodiments, and the same or similar aspects thereof can be referenced to each other, and for the sake of brevity, they will not be described one by one.
[0092] At the same time, it should be understood that for the convenience of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship.
[0093] The following description of at least one exemplary embodiment is merely illustrative in nature and is in no way intended to limit the present disclosure, its application, or uses.
[0094] Technologies, methods, and equipment known to ordinary technicians in the relevant art may not be discussed in detail, but where appropriate, the technologies, methods, and equipment should be considered as part of the specification.
[0095] It should be noted that like reference numerals and letters refer to similar items in the following figures, and therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.
[0096] The embodiments of the present disclosure can be applied to electronic devices such as terminal devices, computer systems, servers, etc., which can operate with many other general or special computing system environments or configurations. Examples of well-known terminal devices, computing systems, environments and / or configurations suitable for use with electronic devices such as terminal devices, computer systems or servers include, but are not limited to: personal computer systems, server computer systems, thin clients, thick clients, handheld or laptop devices, microprocessor-based systems, set-top boxes, programmable consumer electronics, network personal computers, small computer systems, large computer systems, and distributed cloud computing technology environments including any of the above systems, etc.
[0097] Electronic devices such as terminal devices, computer systems, servers, etc. can be described in the general context of computer system executable instructions (such as program modules) executed by computer systems. Generally, program modules can include routines, programs, target programs, components, logic, data structures, etc., which perform specific tasks or implement specific abstract data types. Computer systems / servers can be implemented in a distributed cloud computing environment. In a distributed cloud computing environment, tasks can be performed by remote processing devices linked through a communication network. In a distributed cloud computing environment, program modules can be located on local or remote computing system storage media including storage devices.
[0098] SUMMARY OF THE DISCLOSURE
[0099] In the process of implementing the present disclosure, the inventors found that when using a laser SLAM device to reconstruct a three-dimensional spatial scene, the laser SLAM device can quickly scan spatial scenes of different sizes for reconstruction. Its low CPU computing power and high robustness requirements, as well as scene adaptability requirements, pose challenges to the processing efficiency and CPU power consumption of the SLAM algorithm.
[0100] In the processing method of three-dimensional point cloud data provided by the present disclosure, based on the optimized posture corresponding to the previous point cloud frame (first point cloud frame) and the inertial measurement data collected in the corresponding time period of the second point cloud frame, the point cloud data of the second point cloud frame can be subjected to dedistortion processing, point cloud matching, posture optimization, and point cloud map update operations, thereby realizing the processing of point cloud data, improving the compactness of point cloud processing, and eliminating the need to extract point cloud features, thereby reducing computing power consumption. In addition, after pre-integrating the inertial measurement unit (IMU) data, the point cloud is dedistorted in combination with the pre-integrated posture, thereby reducing the CPU consumption of the SLAM algorithm.
[0101] See also Figure 1 , which illustrates a schematic diagram of the structure of a laser SLAM device, wherein the laser radar is used to collect point cloud data, and the built-in IMU is used to provide positioning function. The built-in IMU can locate the laser SLAM device in real time by collecting inertial measurement data such as three-axis acceleration, three-axis angular velocity, and three-axis attitude angle of the laser SLAM device at a high frequency. The motor is used to drive the laser radar to rotate and swing, so as to achieve large-scale scanning in the horizontal and vertical directions and obtain three-dimensional point cloud data of the surrounding environment. The internal computing components may include a motherboard, a CPU, a memory, a storage device, etc., which are used to process the point cloud data collected by the laser radar and the inertial measurement data collected by the IMU through the technical solution disclosed in this disclosure, so as to realize the processing of point cloud data and reconstruction of three-dimensional space.
[0102] In addition, the laser SLAM device can be provided with multiple interfaces for connecting the mounting handle or bracket, so that the laser SLAM device can be installed on carriers such as drones and cars to obtain point cloud data of three-dimensional space and reconstruct three-dimensional space.
[0103] Exemplary Methods
[0104] Figure 2 FIG. 1 is a flow chart of an embodiment of a method for processing three-dimensional point cloud data disclosed in the present invention. Figure 2 The method shown includes steps 201 to 205. Each step is described below.
[0105] In step 201, based on the optimized pose corresponding to the first point cloud frame and the pre-integrated pose of the inertial measurement data collected in the time period corresponding to the second point cloud frame, the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame is processed to obtain the point cloud data to be processed.
[0106] Among them, the first point cloud frame is an adjacent frame that is located before the second point cloud frame in time sequence. The second point cloud frame can be understood as the newly collected current point cloud frame, that is, the point cloud frame that is currently to be added to the point cloud map. The optimized pose of the first point cloud frame is used to characterize the optimized pose obtained after the pose optimization of the point cloud data of the first point cloud frame when the point cloud data of the first point cloud frame is added to the point cloud map. The optimized pose corresponding to the first point cloud frame can be used as the historical optimized pose of the second point cloud frame to determine the initial pose of the second point cloud frame. The time period corresponding to the second point cloud frame is used to indicate the time period for collecting the second point cloud frame. For example, if the acquisition frequency of the laser radar is 10Hz, the time period for the laser radar to collect a frame of point cloud data is 0.1 seconds. The inertial measurement data is the data collected by the IMU, which can include inertial measurement data such as three-axis acceleration, three-axis angular velocity, and three-axis attitude angle. The pre-integrated pose of the inertial measurement data is used to indicate the relative pose change between the first point cloud frame and the second point cloud frame obtained by integrating the IMU data, including changes in position, velocity, and direction. In this implementation, by using IMU pre-integration to dedistort point cloud data, repeated integration calculations can be reduced, computational efficiency can be improved, and IMU measurement noise and deviation can be processed more accurately.
[0107] In some embodiments, the acquisition frequency of the lidar is lower than the acquisition frequency of the IMU. For example, the acquisition frequency of the IMU is 200 Hz and the acquisition frequency of the lidar is 10 Hz. Then, 20 sets of inertial measurement data can be collected in the time period of collecting one point cloud frame.
[0108] The point cloud data in the inertial measurement unit coordinate system of the second point cloud frame is used to indicate the point cloud data of the second point cloud frame collected by the laser radar is converted to data in the IMU coordinate system. The point cloud data to be processed is the point cloud data after de-distortion processing according to the IMU pre-integrated posture.
[0109] In step 202, the point cloud data to be processed is matched with the original point cloud map to obtain a matching point cloud set and a point cloud residual.
[0110] Among them, the original point cloud map is a point cloud map generated based on the point cloud data including the first point cloud frame and the previous point cloud frames. The original point cloud map is composed of countless historical point cloud frames collected by the laser radar, which is used to retain the original environmental measurement information as much as possible and better describe the environment. After each new laser point cloud frame is collected, the new laser point cloud frame can be processed and inserted into the original point cloud map to update the original point cloud map. Among them, the data structure of the point cloud map can be a tree structure, and each node in the tree structure can store the three-dimensional coordinate information of each point (for example, the three-dimensional coordinates of point P). The present disclosure does not limit the data structure of the point cloud map, which can be an incremental K-dimensional tree (k-dtree), or an incremental hashmap (hash-based map), voxel map (voxel map), etc. can also be used.
[0111] In some embodiments, a K-Nearest Neighbor (KNN) classification algorithm may be used to query the K nearest points in the original point cloud map for each point in the current point cloud frame (the second point cloud frame mentioned above) to obtain a matching point cloud set between the point cloud data to be processed and the original point cloud map. The matching point cloud set records the matching relationship between each point in the point cloud data to be processed and each point in the original point cloud map. For example, point A in the point cloud data to be processed and five points X, P, M, O, and E in the original point cloud map are matching points.
[0112] Among them, the point cloud residual is used to characterize the distance between each point in the point cloud data to be processed and the plane constructed by the corresponding K nearest points retrieved, and is used to evaluate the precision and accuracy of each point in the point cloud data to be processed.
[0113] In step 203, based on the matching point cloud set and the point cloud residual, the pose optimization matrix is iteratively determined until the iteration stop condition is met.
[0114] Among them, the pose optimization matrix can be understood as a matrix for translating and rotating the point cloud data to be processed. The iteration stop condition can be that the number of iterations reaches the maximum number of iterations, or the value of the point cloud residual is less than the target residual threshold, or the rate of change of the iteration error between two adjacent iterations is less than the set threshold, etc. Based on the matching point cloud set determined in step 202, iterative optimization can be performed no more than the maximum number of times until the iteration stop condition is met. This implementation method can use as few matching times and optimization times as possible to reach convergence as quickly as possible, and the effect on the accuracy and stability of the point cloud is also very small.
[0115] In some embodiments, if the pose optimization matrix cannot be obtained based on the matching point cloud set determined in step 202, for example, the value of the point cloud residual is greater than the target residual threshold, then the point cloud data to be processed can be further matched with the original point cloud map based on the relative pose after iterative optimization to obtain a new matching point cloud set and relative residual, and iterative optimization can be continued based on the new matching point cloud set and relative residual until the above-mentioned pose optimization matrix is determined.
[0116] In step 204, the pose of the point cloud data to be processed is optimized based on the pose optimization matrix to obtain optimized point cloud data.
[0117] The optimized point cloud data is used to indicate the point cloud data obtained after performing posture optimization (translation, rotation) on the point cloud data to be processed.
[0118] In step 205, the original point cloud map is updated based on the optimized point cloud data.
[0119] In some embodiments, whether to add the corresponding point to the original point cloud map can be determined based on the distance between each point in the optimized point cloud data and the nearest K points. For example, if the maximum distance to the nearest K points is less than a certain threshold, it means that there are already many points near the point cloud in the original point cloud map, so there is no need to insert a new point, then this point will not be inserted into the original point cloud map. If the maximum distance to the nearest K points is not less than a certain threshold, it means that this point needs to be inserted into the original point cloud map.
[0120] Through the above steps 201-205, when a new laser point cloud frame (second point cloud frame) is collected, the point cloud data of the second point cloud frame can be dedistorted, point cloud matched, posture optimized, and point cloud map updated based on the posture corresponding to the previous point cloud frame (first point cloud frame) and the inertial measurement data collected in the corresponding time period of the second point cloud frame, so as to realize the processing of point cloud data without extracting point cloud features, thereby reducing computing power consumption, and after pre-integrating the IMU data, the point cloud is dedistorted in combination with the pre-integrated posture, thereby improving the compactness of point cloud processing, reducing the CPU consumption of the SLAM algorithm, and improving the efficiency of point cloud processing.
[0121] Figure 3 A flowchart of another embodiment of the method for processing three-dimensional point cloud data disclosed in the present invention is shown in FIG. Figure 2 Based on the embodiment shown, before step 201, it is necessary to perform downsampling and coordinate conversion processing on the point cloud data, and then perform dedistortion processing in step 201. Figure 3 As shown, the following steps are included:
[0122] In step 301, in response to acquiring the point cloud data of the second point cloud frame, downsampling processing is performed on the point cloud data of the second point cloud frame by setting a downsampling method.
[0123] The downsampling method is set to include at least one of the following: a random downsampling method, a spatial uniform sampling method, and a voxel grid sampling method.
[0124] Among them, the random downsampling method refers to randomly selecting a certain number of points from the point cloud data of the acquired second point cloud frame to generate low-resolution point cloud data, and this method is simple to implement. The spatial uniform sampling method is to divide the point cloud into grids, and then randomly select a point from each grid as a sampling point to generate low-resolution point cloud data. The voxel grid sampling method refers to dividing the point cloud data of the second point cloud frame into three-dimensional grids, and only selecting one point in each grid to generate low-resolution point cloud data. The ideas of voxel grid downsampling and spatial uniform downsampling are similar, but voxel grid sampling will use a grid that adapts to the needs to divide the space, and then in each grid, a point is calculated and retained according to a certain method. The random downsampling method can also adopt an adaptive method with higher sampling granularity near and lower sampling granularity far away, so as to retain as many distant points as possible and enhance the stability of the algorithm.
[0125] In this implementation, after the laser radar collects the point cloud data of the second point cloud frame, a downsampling operation can be performed first, and then subsequent coordinate transformation and dedistortion processing operations can be performed, which can achieve the effect of saving the computing resources of the SLAM algorithm when performing subsequent processing based on the downsampled point cloud data.
[0126] In step 302, coordinate system conversion is performed on the downsampled point cloud data to obtain point cloud data in the inertial measurement unit coordinate system of the second point cloud frame.
[0127] In some embodiments, the downsampled point cloud data can be rotated based on the motor coordinate system of the motor where the radar is located; then the rotated point cloud data is converted to the inertial measurement unit coordinate system according to the external parameter conversion matrix to obtain the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame. In specific implementation, the point cloud data can be rotated according to the fixed coordinate system on the motor installed with the laser radar, where the angle of rotation of the point cloud data can correspond to the value output by the encoder of the motor. Then, the rotated point cloud data can be converted to the IMU coordinate system according to the calibrated external parameter conversion matrix between the IMU and the laser radar.
[0128] In other embodiments, in order to facilitate the registration calculation, different acquisition systems may have motors or not have motors according to the hardware design parameters. Different hardware design parameters may also result in slightly different ways of performing coordinate system conversion. The technical solution disclosed in the present invention is only illustrative and not limiting.
[0129] Through the above steps 301-302, the technical solution discloses how to downsample and convert external parameters of point cloud data. By performing downsampling when acquiring point cloud data, it helps to save computing resources of the entire SLAM algorithm. By converting the point cloud data to the IMU coordinate system, de-distortion processing can be performed in combination with IMU pre-integration to improve the accuracy of three-dimensional reconstruction.
[0130] Figure 4 A flowchart of another embodiment of the method for processing three-dimensional point cloud data disclosed in the present invention is shown in FIG. Figure 2 Based on the embodiment shown, after determining the pose optimization matrix in step 203, the optimized pose of the second point cloud frame can also be determined. Figure 4 As shown, the following steps are included:
[0131] In step 401, factor graph optimization is performed based on the pose optimization matrix to obtain an optimized pose of the second point cloud frame.
[0132] Among them, in SLAM, the factor graph is used to describe the position and map information of the robot in an unknown environment, and is used to predict the optimized posture corresponding to the second point cloud frame through the state quantity of the robot in the previous point cloud frame (first point cloud frame) and the IMU observation quantity between the previous point cloud frame (first point cloud frame) and the end time of the current point cloud frame (second point cloud frame).
[0133] In specific implementation, the IMU information collected by the IMU in the time period corresponding to the second point cloud frame and the above-mentioned posture optimization matrix can be used to fuse a more uniform and consistent optimized posture. In this implementation, the optimized posture after factor graph optimization is used to perform IMU pre-integration in the time period corresponding to the next point cloud frame, and the IMU state (including position, angle attitude, IMU bias deviation, speed, covariance matrix, etc.) is reversely corrected. The optimized IMU state is used as the initial state of the IMU pre-integration of the next point cloud frame, and a more stable and consistent IMU pre-integration state can be obtained.
[0134] In step 402, the optimized pose of the second point cloud frame is stored in a cache, and the optimized pose of the second point cloud frame is used to process point cloud data of a third point cloud frame, which is an adjacent frame located after the second point cloud frame in time sequence.
[0135] The optimized pose of the second point cloud frame is stored in the cache, and in the process of acquiring a new third point cloud frame, the point cloud data of the newly acquired third point cloud frame can be processed according to the optimized pose of the second point cloud frame. When it is necessary to pre-integrate the IMU data collected in the time period corresponding to the third point cloud frame, the optimized pose of the second point cloud frame can be taken out from the cache, thereby achieving the periodic form of caching only one point cloud frame in the system for use.
[0136] The method for processing three-dimensional point cloud data provided in this embodiment can make the processing of the second point cloud frame more stable and accurate by using the optimized posture corresponding to the historical point cloud frame (first point cloud frame) to process the point cloud data of the current point cloud frame (second point cloud frame).
[0137] In some embodiments, in order to use IMU pre-integration to process each point cloud frame, it is necessary to synchronize the clocks of the inertial measurement unit and at least one laser radar that collects point cloud data to obtain clock-synchronized point cloud data and inertial measurement data with timestamps. In the disclosed method, an independent clock (single-chip microcomputer) can be used to periodically send a pulse per second (PPS) signal to each sensor (lidar, IMU, motor). After receiving the PPS signal, the internal clock of the sensor will return to zero, thereby achieving clock synchronization of each sensor. When processing point cloud data using IMU data, it can be processed according to the timestamp of the data collected by each sensor, as shown in the following example. Figure 5 The embodiment shown.
[0138] Figure 5 The flowchart of the embodiment of the de-distortion processing of the three-dimensional point cloud data disclosed in the present invention is as follows: Figure 2 Based on the illustrated embodiment, step 201 may include the following steps 211 - 212 .
[0139] In step 211, based on the timestamp of each point in the second point cloud frame and the timestamp of each inertial measurement data collected in a time period corresponding to the second point cloud frame, the distortion amount of each point in the second point cloud frame is determined.
[0140] In some embodiments, the initial timestamp of the second point cloud frame (the timestamp of the first point collected in the time period corresponding to the second point cloud frame) can be used to interpolate the IMU pose P0 corresponding to the initial timestamp in the IMU pre-integration sequence as the initial pose of the second point cloud frame. Then, the timestamps of the remaining points in the second point cloud frame, such as point O, can be interpolated in the IMU pre-integration sequence in the same manner to obtain the IMU pose Pn, where Pn-P0 is the distortion amount of point O.
[0141] In step 212, based on the optimized posture corresponding to the first point cloud frame and the distortion amount of each point in the second point cloud frame, the point cloud data of the second point cloud frame is dedistorted to obtain the point cloud data to be processed.
[0142] In some implementations, the dedistorted point can be obtained by multiplying each point by the distortion amount.
[0143] The method for processing three-dimensional point cloud data provided in this embodiment can obtain the pose of each point in the second point cloud frame based on the timestamp of each point in the second point cloud frame and the timestamp and pose of each point in the IMU pre-integration sequence, thereby realizing the acquisition of the pose of the second point cloud frame (current point cloud frame) based on the IMU pre-integration sequence, thereby improving the accuracy and stability of the pose of the determined point cloud data.
[0144] In order to reduce the computing power consumption of the SLAM algorithm, the retrieval information of the points to be added to the original point cloud map can be recorded when executing point cloud matching. After the pose optimization of the point cloud data to be processed is performed to obtain the optimized point cloud data, the points can be added directly based on the retrieval information. Figure 6 A flowchart of another embodiment of the method for processing three-dimensional point cloud data disclosed in the present invention is shown in FIG. Figure 6 As shown, the following steps are included:
[0145] Step 601, based on the optimized pose corresponding to the first point cloud frame and the pre-integrated pose of the inertial measurement data collected in the time period corresponding to the second point cloud frame, the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame is processed to obtain the point cloud data to be processed.
[0146] The first point cloud frame is an adjacent frame that is located before the second point cloud frame in time sequence.
[0147] Step 602, matching the point cloud data to be processed with the original point cloud map to obtain a matching point cloud set and a point cloud residual, wherein the original point cloud map is a point cloud map generated based on the point cloud data including the first point cloud frame and the previous point cloud frames.
[0148] The implementation of step 601-step 602 can be found in Figure 2 The description of steps 201-202 in the illustrated embodiment will not be described in detail here.
[0149] In some embodiments, when querying points in the original point cloud map to obtain points that match points in the point cloud data to be processed, step 603 is executed simultaneously to determine at least one point to be added to the original point cloud map in the point cloud data to be processed.
[0150] Step 603: Based on the matching relationship between each point in the point cloud data to be processed and each point in the original point cloud map, determine at least one point to be added in the point cloud data to be processed to be added to the original point cloud map.
[0151] In some embodiments, based on some set conditions, it is determined whether each point in the point cloud data to be processed needs to be inserted into the original point cloud map. Exemplarily, whether to add the corresponding point to the original point cloud map can be determined based on the distance between each point in the optimized point cloud data and the nearest K points. For example, if the maximum distance to the nearest K points is less than a certain threshold, it means that there are already many points near the point cloud in the original point cloud map, so there is no need to insert a new point, then this point will not be inserted into the original point cloud map. If the maximum distance to the nearest K points is not less than a certain threshold, it means that this point needs to be inserted into the original point cloud map. In this implementation, by obtaining the points to be added in the process of querying the matching points in the original point cloud map, it is possible to avoid querying the point cloud map again when the point cloud map is subsequently updated, thereby saving computing power and improving SLAM efficiency.
[0152] Step 604: store at least one point to be added and corresponding search information in the list of points to be added.
[0153] The list of points to be added is a list for storing the points to be added, and the list of points to be added can also store the points to be added and the corresponding search information in the form of an array or a data table. The search information can be the identification information and storage location of the points to be added.
[0154] Step 605, iteratively determine the pose optimization matrix based on the matching point cloud set and the point cloud residual until the iteration stop condition is met.
[0155] Among them, the matching point cloud concentrates the matching relationship between the point cloud data to be processed and the points in the original point cloud map.
[0156] In some embodiments, a least squares optimization algorithm can be used to iteratively optimize the relative pose of the point cloud data to be processed to the original point cloud map based on the matching point cloud set; in response to satisfying the first iteration stop condition, a pose optimization matrix is obtained; in response to satisfying the second iteration stop condition, based on the relative pose of the previous round of iterative optimization, an operation of matching the point cloud data to be processed with the original point cloud map is performed, and the next round of iterative optimization is started until the first iteration stop condition is met and the pose optimization matrix is obtained.
[0157] In specific implementation, each point in the second point cloud frame and the corresponding K points in the retrieved original point cloud map can be used to construct the least squares optimization of the point to plane, and the optimization value is the pose from the second point cloud frame to the original point cloud map. In the implementation method disclosed in the present invention, the pose can be iteratively optimized several times based on the matching point cloud set obtained by matching once, until the first iteration stop condition is met, such as the point cloud residual is less than the set threshold, then the pose optimization matrix can be obtained; if the iteration meets the second iteration stop condition, such as the iteration reaches the maximum number of iterations or the iterative error change rate between two adjacent iterations is less than the set threshold, then the matching process can be performed again based on the relative pose after iterative optimization and the next round of iterative optimization can be started until the first iteration stop condition is met to obtain the pose optimization matrix.
[0158] In this implementation, the number of optimization iterations for each match can be limited, such as 2 times, so that the convergence can be achieved as quickly as possible with as few matches and optimizations as possible, and the effect on the accuracy and stability of the point cloud is also minimal.
[0159] Step 606: Based on the pose optimization matrix, the pose of the point cloud data to be processed is optimized to obtain optimized point cloud data.
[0160] Step 607: based on at least one point to be added and corresponding search information, obtain a target point to be added from the optimized point cloud data.
[0161] In some implementations, after the pose of the point cloud data to be processed is optimized, the search information corresponding to each point to be added may be obtained from the list of points to be added, and then the target point to be added may be obtained from the optimized point cloud data based on the search information.
[0162] Step 608: Update the origin point cloud map based on the target point to be added.
[0163] In the disclosed embodiment, in the process of executing point cloud matching, the points to be added to the point cloud map are synchronously acquired, and the retrieval information of the points to be added is recorded in the list of points to be added. Therefore, when updating the original point cloud map, the target points to be added can be directly acquired according to the retrieval information and the original point cloud map can be updated without performing the query operation in the original point cloud map again, thereby reducing the computational complexity of the SLAM algorithm and improving the efficiency of the SLAM algorithm.
[0164] Figure 7 FIG. 1 is a flowchart of another embodiment of the method for processing three-dimensional point cloud data disclosed in the present invention, which illustrates the process of three-dimensional point cloud processing. Figure 7As shown, after collecting point cloud data and IMU data, IMU pre-integration can be performed on the IMU data to obtain pre-integrated pose, and downsampling, external parameter conversion encoder correspondence and motor encoder correspondence and de-distortion processing can be performed on the point cloud data in sequence to obtain the initial pose estimation of the point cloud data, and then the point cloud data can be matched with the point cloud in the original point cloud map, such as using K-nearest neighbor (KNN) matching query, and obtaining the retrieval information of each point to be added during the matching process, and then performing pose optimization, and updating the original point cloud map with the points to be added in the optimized point cloud data after pose optimization according to the retrieval information, and realizing factor graph optimization according to pose optimization to obtain the optimized IMU state, and according to the optimized IMU state, continue to perform IMU pre-integration based on the IMU data collected in the next point cloud frame to achieve a more stable IMU pre-integration state.
[0165] In this implementation, each step in the SLAM algorithm is more compact, which improves the efficiency of the SLAM algorithm, and improves the stability and accuracy of the IMU pre-integration by reversely correcting the IMU state of the point cloud.
[0166] Exemplary Devices
[0167] Figure 8 FIG. 1 is a schematic diagram of a structure of an embodiment of a three-dimensional point cloud data processing device disclosed in the present invention. Figure 8 As shown, the device comprises:
[0168] The point cloud processing module 81 is used to process the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame based on the optimized posture corresponding to the first point cloud frame and the pre-integrated posture of the inertial measurement data collected in the time period corresponding to the second point cloud frame, so as to obtain the point cloud data to be processed, wherein the first point cloud frame is an adjacent frame located before the second point cloud frame in time sequence;
[0169] A point cloud matching module 82 is used to match the point cloud data to be processed with an original point cloud map to obtain a matching point cloud set and a point cloud residual, wherein the original point cloud map is a point cloud map generated based on the point cloud data including the first point cloud frame and the previous point cloud frames;
[0170] A posture determination module 83, used for iteratively determining a posture optimization matrix based on a matching point cloud set and a point cloud residual until an iteration stop condition is met;
[0171] A posture optimization module 84 is used to optimize the posture of the point cloud data to be processed based on the posture optimization matrix to obtain optimized point cloud data;
[0172] The point cloud updating module 85 is used to update the original point cloud map based on the optimized point cloud data.
[0173] Fig. 9 A schematic diagram of the structure of another embodiment of the three-dimensional point cloud data processing device disclosed in the present invention. Fig. 9 As shown, in Figure 8 Based on the illustrated embodiment, in some embodiments of the present disclosure, the device further includes:
[0174] An information determination module 86 is used to perform factor graph optimization based on the pose optimization matrix to obtain an optimized pose of the second point cloud frame;
[0175] The first storage module 87 is used to store the optimized posture of the second point cloud frame in the cache, and the optimized posture of the second point cloud frame is used to process the point cloud data of the third point cloud frame, and the third point cloud frame is an adjacent frame located after the second point cloud frame in time sequence.
[0176] In some further embodiments of the present disclosure, the device further comprises:
[0177] A downsampling module 88 is used for downsampling the point cloud data of the second point cloud frame in response to acquiring the point cloud data of the second point cloud frame by setting a downsampling method, wherein the set downsampling method includes: at least one of a random downsampling method, a spatial uniform sampling method, and a voxel grid sampling method;
[0178] The coordinate conversion module 89 is used to perform coordinate conversion on the downsampled point cloud data to obtain point cloud data in the inertial measurement unit coordinate system of the second point cloud frame.
[0179] In some further embodiments of the present disclosure, the coordinate conversion module 89 includes:
[0180] The first conversion submodule 891 is used to rotate the downsampled point cloud data based on the motor coordinate system of the motor where the radar is located;
[0181] The second conversion submodule 892 is used to convert the rotated point cloud data into the inertial measurement unit coordinate system according to the external parameter conversion matrix to obtain the point cloud data of the second point cloud frame in the inertial measurement unit coordinate system.
[0182] In some further embodiments of the present disclosure, the device further comprises:
[0183] The clock synchronization module 90 is used to perform clock synchronization on the inertial measurement unit and at least one laser radar for collecting point cloud data, so as to obtain clock-synchronized point cloud data and inertial measurement data with timestamps.
[0184] In some further embodiments of the present disclosure, the point cloud processing module 81 includes:
[0185] A determination submodule 811, configured to determine the distortion amount of each point in the second point cloud frame based on the timestamp of each point in the second point cloud frame and the timestamp of each inertial measurement data collected in a time period corresponding to the second point cloud frame;
[0186] The processing submodule 812 is used to perform dedistortion processing on the point cloud data of the second point cloud frame based on the optimized posture corresponding to the first point cloud frame and the distortion amount of each point in the second point cloud frame to obtain the point cloud data to be processed.
[0187] In some further embodiments of the present disclosure, the device further comprises:
[0188] A point determination module 91 is used to determine at least one to-be-added point in the to-be-processed point cloud data to be added to the original point cloud map based on the matching relationship between each point in the to-be-processed point cloud data and each point in the original point cloud map during the matching operation between the to-be-processed point cloud data and the original point cloud map;
[0189] The second storage module 92 is used to store at least one to-be-added point and corresponding search information in the to-be-added point list.
[0190] In some further embodiments of the present disclosure, the point cloud updating module 85 includes:
[0191] An acquisition submodule 851 is used to acquire a target point to be added from the optimized point cloud data based on at least one point to be added and corresponding search information;
[0192] The updating submodule 852 is used to update the origin point cloud map based on the target point to be added.
[0193] In some further embodiments of the present disclosure, the matching relationship between the point cloud data to be processed in the matching point cloud set and the points in the original point cloud map;
[0194] The posture determination module 83 includes:
[0195] The first iterative submodule 831 is used to iteratively optimize the relative position of the point cloud data to be processed to the original point cloud map based on the matching point cloud set by using the least squares optimization algorithm;
[0196] The second iteration submodule 832 is used to obtain a posture optimization matrix in response to satisfying the first iteration stop condition;
[0197] The third iterative submodule 833 is used to respond to the satisfaction of the second iteration stop condition, and based on the relative posture of the previous round of iterative optimization, perform the operation of matching the processed point cloud data with the original point cloud map, and start the next round of iterative optimization until the first iteration stop condition is met to obtain the posture optimization matrix.
[0198] It should be pointed out that the modules in the present device can be decomposed and / or reassembled, and such decomposition and / or reassembly should be regarded as equivalent solutions of the present device.
[0199] The exemplary embodiment of the device corresponds to the exemplary method described above, and the relevant contents can be referenced and quoted to each other. The beneficial technical effects corresponding to the exemplary embodiment of the device can refer to the corresponding beneficial technical effects of the exemplary method described above, and will not be repeated here.
[0200] Exemplary electronic devices, computer program products, and computer-readable storage media
[0201] An embodiment of the present disclosure further provides an electronic device, comprising: a memory for storing a computer program; a processor for executing the computer program stored in the memory, and when the computer program is executed, a method for processing three-dimensional point cloud data of any of the above embodiments of the present disclosure is implemented.
[0202] Below, reference Fig.10 An electronic device according to an embodiment of the present disclosure is described, in which a device for implementing the method according to an embodiment of the present disclosure may be integrated. Fig.10 A structural diagram of an electronic device provided in an exemplary embodiment of the present disclosure, such as Fig.10 As shown, the electronic device includes one or more processors 101, one or more computer-readable storage media memory 102, and a computer program stored in the memory and executable on the processor. When executing the program in the memory 102, the above-mentioned method for processing three-dimensional point cloud data can be implemented.
[0203] Specifically, in practical applications, the electronic device may further include components such as an input device 103 and an output device 104, and these components are interconnected via a bus system and / or other forms of connection mechanisms (not shown). Those skilled in the art will appreciate that Fig.10 The structure of the electronic device shown in the figure does not constitute a limitation on the electronic device, and may include more or fewer components than shown in the figure, or certain components, or different component arrangements.
[0204] The processor 101 may be a central processing unit (CPU) or other forms of processing units having the capability of processing three-dimensional point cloud data and / or the capability of executing instructions. The processor 101 may perform various functions and process data by running or executing software programs and / or modules stored in the memory 102 and calling data stored in the memory 102, thereby monitoring the electronic device as a whole.
[0205] The memory 102 may store one or more computer program products, and the memory may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The above-mentioned volatile memory may include, for example, random access memory (RAM) and / or cache memory (cache), etc. The above-mentioned non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program products may be stored on the computer-readable storage medium, and the processor 101 may run the computer program product to implement the above-mentioned three-dimensional point cloud data processing method of each embodiment of the present disclosure and / or other desired functions.
[0206] The input device 103 may be used to receive input digital or character information and may include a keyboard, a mouse, a joystick, etc., which are related to user settings and function control.
[0207] The output device 104 can output various information to the outside, including the determined distance information, direction information, etc. The output device 104 can include, for example, a display, a speaker, a printer, a communication network and a remote output device connected thereto, and the like.
[0208] The electronic device may also include a power supply for supplying power to various components, which may be logically connected to the processor 101 through a power management system, so as to manage charging, discharging, power consumption and other functions through the power management system. The power supply may also include any components such as one or more DC or AC power supplies, recharging systems, power failure detection circuits, power converters or inverters, and power status indicators.
[0209] Of course, to simplify, Fig.10 Only some of the components related to the present disclosure in the electronic device are shown, and components such as a bus, an input / output interface, etc. are omitted. In addition, according to specific application situations, the electronic device may further include any other appropriate components.
[0210] In addition to the above-mentioned methods and devices, an embodiment of the present disclosure may also be a computer program product, which includes computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the method for processing three-dimensional point cloud data according to various embodiments of the present disclosure described in the above-mentioned "Exemplary Method" section of this specification.
[0211] The computer program product may be written in any combination of one or more programming languages to write program code for performing the operations of the disclosed embodiments, including object-oriented programming languages such as Java, C++, etc., and conventional procedural programming languages such as "C" or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as a separate software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server.
[0212] In addition, an embodiment of the present disclosure may also be a computer-readable storage medium on which computer program instructions are stored. When the computer program instructions are executed by a processor, the processor executes the steps of the method for processing three-dimensional point cloud data according to various embodiments of the present disclosure described in the above "Exemplary Method" section of this specification.
[0213] Computer readable storage media can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can include, for example, but is not limited to, a system, device or device of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.
[0214] The basic principles of the present disclosure are described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, effects, etc. mentioned in the present disclosure are only examples and not limitations, and it cannot be considered that these advantages, strengths, effects, etc. are required by each embodiment of the present disclosure. In addition, the specific details disclosed above are only for the purpose of illustration and ease of understanding, and are not limitations. The above details do not limit the present disclosure to the necessity of adopting the above specific details to be implemented.
[0215] Each embodiment in this specification is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system embodiment, since it basically corresponds to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.
[0216] A person of ordinary skill in the art can understand that all or part of the steps of implementing the above method embodiment can be completed by hardware related to program instructions, and the aforementioned program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps of the above method embodiment; and the aforementioned storage medium includes: ROM, RAM, magnetic disk or optical disk, etc., various media that can store program codes.
[0217] The method and apparatus of the present disclosure may be implemented in many ways. For example, the method and apparatus of the present disclosure may be implemented by software, hardware, firmware, or any combination of software, hardware, and firmware. The above order of steps for the method is for illustration only, and the steps of the method of the present disclosure are not limited to the order specifically described above, unless otherwise specifically stated. In addition, in some embodiments, the present disclosure may also be implemented as a program recorded in a recording medium, which includes machine-readable instructions for implementing the method according to the present disclosure. Therefore, the present disclosure also covers a recording medium storing a program for executing the method according to the present disclosure.
[0218] The description of the present disclosure is given for the purpose of illustration and description, and is not intended to be exhaustive or to limit the present disclosure to the disclosed form. Many modifications and variations will be apparent to those of ordinary skill in the art. The embodiments are selected and described in order to better illustrate the principles and practical applications of the present disclosure, and to enable those of ordinary skill in the art to understand the present disclosure and thereby design various embodiments with various modifications suitable for specific uses.
Claims
1. A method for processing three-dimensional point cloud data, characterized in that: include: Based on the optimized pose corresponding to the first point cloud frame and the pre-integrated pose of the inertial measurement data collected in the time period corresponding to the second point cloud frame, the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame is processed to obtain the point cloud data to be processed, wherein the first point cloud frame is an adjacent frame located before the second point cloud frame in time sequence; Performing matching processing on the point cloud data to be processed and the original point cloud map to obtain a matching point cloud set and a point cloud residual, wherein the original point cloud map is a point cloud map generated based on the point cloud data including the first point cloud frame and the previous point cloud frames, wherein the matching point cloud set contains the matching relationship between the points in the point cloud data to be processed and the original point cloud map; Based on the matching point cloud set and the point cloud residual, iteratively determine the pose optimization matrix until the iterative stop condition is met, including: using a least squares optimization algorithm, based on the matching point cloud set, iteratively optimize the relative pose of the point cloud data to be processed to the original point cloud map; in response to satisfying the first iterative stop condition, obtain the pose optimization matrix; in response to satisfying the second iterative stop condition, based on the relative pose of the previous round of iterative optimization, perform the operation of matching the point cloud data to be processed with the original point cloud map, start the next round of iterative optimization, until the first iterative stop condition is met, and obtain the pose optimization matrix; Based on the pose optimization matrix, the pose of the point cloud data to be processed is optimized to obtain optimized point cloud data; Based on the optimized point cloud data, updating the original point cloud map; After the iterative determination of the pose optimization matrix, the method further includes: Based on the pose optimization matrix, factor graph optimization is performed to obtain an optimized pose of the second point cloud frame; The optimized pose of the second point cloud frame is stored in a cache, wherein the optimized pose of the second point cloud frame is used to process point cloud data of a third point cloud frame, and the third point cloud frame is an adjacent frame located after the second point cloud frame in time sequence; The process of matching the to-be-processed point cloud data with the original point cloud map also includes: Determine at least one to-be-added point in the to-be-processed point cloud data to be added to the original point cloud map based on a matching relationship between each point in the to-be-processed point cloud data and each point in the original point cloud map; At least one of the to-be-added points and corresponding search information are stored in the to-be-added point list.
2. The method according to claim 1, characterized in that Before processing the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame based on the optimized pose corresponding to the first point cloud frame and the pre-integrated pose of the inertial measurement data collected in the time period corresponding to the second point cloud frame, the method further includes: In response to acquiring the point cloud data of the second point cloud frame, downsampling the point cloud data of the second point cloud frame by setting a downsampling method, wherein the set downsampling method includes: at least one of a random downsampling method, a spatial uniform sampling method, and a voxel grid sampling method; The downsampled point cloud data is subjected to a coordinate system transformation to obtain point cloud data of the second point cloud frame in an inertial measurement unit coordinate system.
3. The method according to claim 2, characterized in that The step of performing coordinate system conversion on the downsampled point cloud data to obtain point cloud data in the inertial measurement unit coordinate system of the second point cloud frame includes: Based on the motor coordinate system of the motor where the radar is located, the downsampled point cloud data is rotated; The rotated point cloud data is converted to an inertial measurement unit coordinate system according to an external parameter conversion matrix to obtain point cloud data in the inertial measurement unit coordinate system of the second point cloud frame.
4. The method according to claim 1, characterized in that Before processing the point cloud data of the second point cloud frame in the inertial measurement unit coordinate system, the method further includes: The inertial measurement unit and at least one laser radar for collecting point cloud data are clock synchronized to obtain clock synchronized point cloud data and inertial measurement data with timestamps.
5. The method according to claim 4, characterized in that The step of processing the point cloud data in the inertial measurement unit coordinate system of the second point cloud frame to obtain the point cloud data to be processed includes: Determine the distortion amount of each point in the second point cloud frame based on the timestamp of each point in the second point cloud frame and the timestamp of each inertial measurement data collected in a time period corresponding to the second point cloud frame; Based on the optimized posture corresponding to the first point cloud frame and the distortion amount of each point in the second point cloud frame, the point cloud data of the second point cloud frame is dedistorted to obtain the point cloud data to be processed.
6. The method according to claim 1, characterized in that The updating of the original point cloud map based on the optimized point cloud data includes: Based on at least one of the to-be-added points and the corresponding search information, obtaining a target to-be-added point from the optimized point cloud data; Based on the target point to be added, the origin cloud map is updated.
7. A three-dimensional point cloud data processing device, characterized in that: include: a point cloud processing module, configured to process the point cloud data of the second point cloud frame in the inertial measurement unit coordinate system based on the optimized pose corresponding to the first point cloud frame and the pre-integrated pose of the inertial measurement data collected in the time period corresponding to the second point cloud frame, so as to obtain the point cloud data to be processed, wherein the first point cloud frame is an adjacent frame located before the second point cloud frame in time sequence; a point cloud matching module, configured to match the point cloud data to be processed with an original point cloud map to obtain a matching point cloud set and a point cloud residual, wherein the original point cloud map is a point cloud map generated based on the point cloud data including the first point cloud frame and previous point cloud frames, wherein the matching point cloud set contains matching relationships between the point cloud data to be processed and the points in the original point cloud map; A posture determination module, used for iteratively determining a posture optimization matrix based on the matching point cloud set and the point cloud residual until an iteration stop condition is met; A posture optimization module, used for performing posture optimization on the point cloud data to be processed based on the posture optimization matrix to obtain optimized point cloud data; A point cloud update module, used for updating the original point cloud map based on the optimized point cloud data; It also includes: an information determination module, used to perform factor graph optimization based on the pose optimization matrix to obtain the optimized pose of the second point cloud frame; A first storage module is used to store the optimized pose of the second point cloud frame in a cache, wherein the optimized pose of the second point cloud frame is used to process point cloud data of a third point cloud frame, and the third point cloud frame is an adjacent frame located after the second point cloud frame in time sequence; A point determination module, used for determining at least one to-be-added point in the to-be-processed point cloud data to be added to the original point cloud map based on a matching relationship between each point in the to-be-processed point cloud data and each point in the original point cloud map; A second storage module, used for storing at least one point to be added and corresponding search information in the list of points to be added; The posture determination module includes: a first iterative submodule, used to iteratively optimize the relative posture of the point cloud data to be processed to the original point cloud map based on the matching point cloud set using a least squares optimization algorithm; a second iterative submodule, used to obtain the posture optimization matrix in response to satisfying the first iterative stop condition; a third iterative submodule, used to perform the operation of matching the point cloud data to be processed with the original point cloud map based on the relative posture of the previous round of iterative optimization in response to satisfying the second iterative stop condition, and start the next round of iterative optimization until the first iterative stop condition is satisfied to obtain the posture optimization matrix.
