CN113424232B

CN113424232B - Three-dimensional point cloud map construction method, system and equipment

Info

Publication number: CN113424232B
Application number: CN201980091861.6A
Authority: CN
Inventors: 钟阳; 周游; 朱振宇
Original assignee: SZ DJI Technology Co Ltd
Current assignee: SZ DJI Technology Co Ltd
Priority date: 2019-12-27
Filing date: 2019-12-27
Publication date: 2024-03-15
Anticipated expiration: 2039-12-27
Also published as: WO2021128297A1; CN113424232A

Abstract

A three-dimensional point cloud map construction method, system and device, the method includes: acquiring multi-frame initial point cloud data acquired by a laser radar system carried on a movable platform, and acquiring pose data (301) when the laser radar system acquires the multi-frame initial point cloud data; determining at least one loop constraint, the loop constraint being determined based on the pose data having a difference between two frames of spatial poses within a first preset range and being time-discontinuous (302); processing the pose data according to the at least one loop constraint condition to obtain optimized pose data (303) of the initial point cloud data of each frame; and constructing a three-dimensional point cloud map (304) based on a plurality of frames of initial point cloud data according to the optimized pose data.

Description

Three-dimensional point cloud map construction method, system and equipment

Technical Field

The disclosure relates to the technical field of high-precision maps, in particular to a three-dimensional point cloud map construction method, system and equipment.

Background

The high-precision electronic Map is also called a high-resolution Map (HD Map, high Definition Map), and is a Map specially used for unmanned service. Unlike conventional navigation maps, high-precision maps can provide navigation information at the Lane (Lane) level in addition to Road (Road) level navigation information. Both in terms of the richness of the information and the accuracy of the information, are far higher than conventional navigational maps. As a preceding step of the high-precision map module, the quality of the map construction directly affects the precision of the downstream high-precision map labeling and high-precision map positioning, however, the conventional map construction method has lower precision. Therefore, it is necessary to optimize the map construction process.

Disclosure of Invention

In view of this, the embodiments of the present disclosure provide a three-dimensional point cloud map construction method, system and device, so as to solve the technical problem of lower accuracy in the related art.

According to a first aspect of an embodiment of the present disclosure, a three-dimensional point cloud map construction method is provided, the method including: acquiring multi-frame initial point cloud data acquired by a laser radar system carried on a movable platform, and acquiring pose data when the laser radar system acquires the multi-frame initial point cloud data; determining at least one loop constraint condition, wherein the loop constraint condition is determined based on the pose data of which the difference between two frames of space poses is within a first preset range and is discontinuous in time sequence; processing the pose data according to the at least one loop constraint condition to obtain optimized pose data of the initial point cloud data of each frame; and constructing a three-dimensional point cloud map based on the initial point cloud data of multiple frames according to the optimized pose data.

According to a second aspect of the embodiments of the present disclosure, a three-dimensional point cloud map construction system is provided, including: the processing device and the laser radar system are mounted on the movable platform; the laser radar system is used for collecting multi-frame initial point cloud data; the processor is configured to determine at least one loop constraint; processing pose data when the laser radar system acquires multiple frames of initial point cloud data according to the at least one loop constraint condition to obtain optimized pose data of each frame of initial point cloud data; according to the optimized pose data, constructing a three-dimensional point cloud map based on a plurality of frames of initial point cloud data; the loop constraint condition is determined based on the pose data in which a difference between two frames of spatial poses is within a first preset range and is not continuous in time sequence.

According to a third aspect of the embodiments of the present disclosure, a three-dimensional point cloud map construction apparatus is provided, the noise filtering apparatus includes a memory, a processor, and a computer program stored on the memory and executable on the processor, the processor implementing the method of any of the embodiments of the present disclosure when executing the program.

According to a fourth aspect of the disclosed embodiments, a computer readable storage medium is provided, having stored thereon a number of computer instructions which, when executed, implement the steps of the method of any of the embodiments.

By applying the scheme of the embodiment of the specification, multi-frame initial point cloud data and pose data when multi-frame initial point cloud data are acquired, loop constraint conditions are established to optimize the pose data, optimized pose data of each frame of initial point cloud data are obtained, then according to the optimized pose data, a three-dimensional point cloud map is constructed based on the multi-frame initial point cloud data, three-dimensional reconstruction of a scene in centimeter level can be performed, an accurate three-dimensional point cloud map is acquired, and the accuracy of map construction is improved.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, the drawings required for the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present disclosure, and other drawings may be obtained according to these drawings without inventive effort to a person of ordinary skill in the art.

Fig. 1A is a schematic diagram of a conventional map of some embodiments.

FIG. 1B is a schematic diagram of a high-precision map of some embodiments.

Fig. 2A is a schematic diagram of a navigation line of a conventional map of some embodiments.

Fig. 2B is a schematic diagram of a navigation line of a high-precision map of some embodiments.

Fig. 3 is a flow chart of a three-dimensional point cloud map construction method according to the present disclosure.

Fig. 4 is a schematic diagram of adjustment logic for loop-back constraints, shown in accordance with an embodiment of the present disclosure.

Fig. 5 is a schematic structural view of a three-dimensional point cloud map building system shown according to another embodiment of the present disclosure.

Fig. 6 is a block diagram of a three-dimensional point cloud mapping apparatus shown according to an embodiment of the present disclosure.

Detailed Description

Reference will now be made in detail to exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, the same numbers in different drawings refer to the same or similar elements, unless otherwise indicated. The implementations described in the following exemplary examples do not represent all implementations consistent with the present specification. Rather, they are merely examples of apparatus and methods consistent with some aspects of the present description as detailed in the accompanying claims.

The terminology used in the description presented herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the description. As used in this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It should also be understood that the term "and/or" as used herein refers to and encompasses any or all possible combinations of one or more of the associated listed items.

It should be understood that although the terms first, second, third, etc. may be used in this specification to describe various information, these information should not be limited to these terms. These terms are only used to distinguish one type of information from another. For example, the first information may also be referred to as second information, and similarly, the second information may also be referred to as first information, without departing from the scope of the present description. The word "if" as used herein may be interpreted as "at … …" or "at … …" or "responsive to a determination", depending on the context.

Fig. 1A and 1B are schematic diagrams of a conventional map and a high-precision map, respectively, of some embodiments. The high-precision electronic Map is also called a high-resolution Map (HD Map, high Definition Map), which is a Map well suited for serving unmanned vehicles. Unlike conventional navigation maps, high-precision maps can provide navigation information at the Lane (Lane) level in addition to Road (Road) level navigation information. Fig. 2A and 2B are schematic diagrams of navigation lines of a conventional map and a high-precision map, respectively, of some embodiments. As shown in fig. 2A, only the information of the navigation roads (shown as R1 to R4 in the drawing) and the information of the road nodes (shown as N1 and N2 in the drawing) are among the navigation information of the conventional map. As shown in fig. 2B, the navigation information of the high-precision map includes not only the information of the navigation road but also the Lane information (Lane 1 to Lane10 in the drawing) on each road. It can be seen that the high-precision map is far higher than the conventional navigation map, both in terms of the richness of information and the precision of information. The high-precision map can provide accurate data which is not provided by the traditional map, so that more accurate navigation guidance information is provided for the unmanned vehicle.

The map construction is used as a preceding step of a high-precision map module, and the construction quality directly influences the precision of downstream high-precision map labeling and high-precision map positioning. A common map construction method may be: the vehicle is provided with a laser radar, a worker drives the vehicle to travel in an area where a high-precision map needs to be constructed, and the laser radar on the vehicle scans the surrounding environment during the period; and collecting point cloud data of the scanning plane of the laser radar after the driving is completed, and processing the point cloud data to obtain a constructed map. In the method, when a high-precision map is constructed, point cloud data acquired by a laser radar are required to be fused and overlapped in the later stage to form the high-precision map with enough point cloud density, so that the processing can be performed only by relying on pose data provided by a navigation system on a vehicle, but the precision and the stability of the navigation system depend on positioning signals, and the positioning accuracy is lower in some complex scenes, such as urban environments of a high-rise forest or tunnels in which signals are blocked, so that the pose data provided by the navigation system can not be directly used for constructing the high-precision map at times.

Meanwhile, as many as tens of thousands of three-dimensional points are in each frame of map data, the map data obtained by scanning every second is tens of frames, the data volume is huge, and if the direct processing takes a long time, only one section of road section with a few kilometers, the construction of a high-precision map takes about 1 week.

In summary, the existing method for constructing the high-precision map has the problems of low map construction precision and low map construction efficiency. Based on the above, the embodiment of the disclosure provides a three-dimensional point cloud map construction method, system and device. In the disclosed embodiments. The mobile platform may be a vehicle, an unmanned aerial vehicle, a mobile robot, or the like having mobile capabilities. The target area is the area needing to construct the three-dimensional point cloud map, the area can be a road, a cell or an indoor space, and the initial point cloud data can be acquired in the target area through a laser radar system carried on the movable platform. The loop closure constraint condition refers to a pose constraint condition when the laser radar system collects point cloud data at the same place at different moments. The following description will take, as an example, a case where the movable platform is a vehicle.

As shown in fig. 3, the three-dimensional point cloud map construction method according to the embodiment of the present disclosure may include:

Step 301: acquiring multi-frame initial point cloud data acquired by a laser radar system carried on a movable platform, and acquiring pose data when the laser radar system acquires the multi-frame initial point cloud data;

step 302: determining at least one loop constraint condition, wherein the loop constraint condition is determined based on the pose data of which the difference between two frames of space poses is within a first preset range and is discontinuous in time sequence;

step 303: processing the multi-frame pose data according to the at least one loop constraint condition to obtain optimized pose data of the initial point cloud data of each frame;

step 304: and constructing a three-dimensional point cloud map based on the initial point cloud data of multiple frames according to the optimized pose data.

In step 301, first, multi-frame initial point cloud data of a target area needs to be acquired. The vehicle can be driven to the target area, and a laser radar system carried on the vehicle collects multi-frame initial point cloud data in the target area. For the same target area, the operation of acquiring the initial point cloud data may be performed twice or more, that is, the vehicle is repeatedly driven under the target area a plurality of times, and the initial point cloud data is acquired once by the lidar system mounted on the vehicle every time the vehicle is driven.

Further, initial point cloud data may be collected at different time periods each time. For example, initial point cloud data is acquired under target area A at 8:00-9:00 for the first acquisition, under target area A at 13:00-14:00 for the second acquisition, under target area A at 17:00-18:00 for the third acquisition, and so on. Since there may be moving objects (e.g., vehicles or pedestrians) in the target area, multiple acquisitions are required to ensure more comprehensive coverage of the target area; in addition, some target areas have the lowest speed limit, vehicles cannot run too slowly, the quantity of point cloud data acquired in one acquisition process may not be enough to construct a three-dimensional point cloud map, details are required to be ensured through multiple acquisitions, and the number of points in a unit area is ensured to be enough. And initial point cloud data are acquired at different time periods, so that the point cloud density can be increased, and interference of moving objects, interference of different light rays, shielding interference and the like can be reduced.

The pose data of the laser radar system when acquiring a plurality of frames of initial point cloud data can be determined by the pose data of a vehicle carrying the laser radar system. In some embodiments, a navigation system is further mounted on the vehicle, and the pose data is acquired based on navigation data of the navigation system. The navigation system may be a GNSS (Global Navigation Satellite System, global satellite navigation system), inertial navigation system (INS, inertial Navigation System), visual inertial navigation system (VINS, visual-Inertial Navigation System), real-Time Kinematic (RTK) carrier phase differential positioning system (RTK system), etc., wherein the GNSS may include a GPS (Global Positioning System ) navigation system, galileo satellite positioning system, beidou satellite positioning system (BeiDou Navigation Satellite System, BDS), or GLONASS, etc. The pose data may include position data, that is, a three-dimensional coordinate position (longitude and latitude coordinates may be employed) of the vehicle, and pose data, that includes a roll angle, a heading angle, and a pitch angle of the vehicle.

In some embodiments, the navigation system comprises a first navigation system and a second navigation system; the pose data is acquired based on first navigation data output by the first navigation system and second navigation data output by the second navigation system. The pose data are acquired through the combined navigation system comprising the first navigation system and the second navigation system, so that the positioning accuracy can be improved, and the accuracy of the constructed three-dimensional point cloud map is higher. The first navigation system and the second navigation system can be any type of navigation system, and the types of the first navigation system and the second navigation system can be the same or different. For example, the first navigation system may be an RTK system and the second navigation system may be an inertial navigation system, and the pose data is generated based on an RTK system data and an inertial navigation system data fusion.

In some embodiments, different weights may be set for the first navigation system and the second navigation system, respectively, and the pose data may be derived from a weighted average of the first navigation data and the second navigation data. The weight can be dynamically set according to the signal strength of the positioning signals of the first navigation system and the second navigation system. When the difference of the signal intensity of the positioning signals of the two navigation systems is large, a large weight is set for the navigation system with strong signal intensity, and a small weight is set for the navigation system with weak signal intensity; when the signal strengths of the positioning signals of the two navigation systems are close, the same or similar weights are set for the two navigation systems.

Because the initial point cloud data and the pose data are data acquired by two sets of different systems, after the initial point cloud data and the pose data are acquired, the pose data and the initial point cloud data are required to be aligned, so that one-to-one correspondence and matching of the initial point cloud data of each frame and the pose data when the initial point cloud data of the frame are acquired are ensured. Specifically, the pose data and the initial point cloud data may be aligned based on a global timestamp of the navigation system and a global timestamp of the lidar system, so that each frame of initial point cloud data corresponds to one frame of pose data. In some embodiments of the invention, the frequency of the lidar system may be in the tens of hertz, e.g., 5-20 Hz, i.e., 5 to 20 frames of initial point cloud data are acquired per second; the frequency of the navigation system may be higher than the frequency of the lidar system, for example, hundreds of hertz when using an inertial navigation system and/or an RTK system. Thus, one frame of pose data may be obtained for each frame of initial point cloud data aligned in time based on the global timestamp. In other embodiments of the present invention, the frequency of the laser radar system may be higher than the frequency of the navigation system, and at this time, since the pose data is less than the initial point cloud data, the initial point cloud data of a specific frame may be selectively removed, and the frame number of the pose data may be increased by adopting an interpolation method to achieve time alignment of the initial point cloud data and the pose data.

Since the initial point cloud data collected by the laser radar system may include moving objects such as vehicles and pedestrians, and the constructed three-dimensional point cloud map generally does not retain the moving objects, the point cloud data corresponding to the moving objects are filtered from the initial point cloud data, and after the point cloud data corresponding to the moving objects are removed, the rest point cloud data are used for constructing the three-dimensional point cloud map. For example, the initial point cloud data collected on one road may include initial point cloud data corresponding to moving objects such as pedestrians and vehicles, and may include initial point cloud data corresponding to buildings on both sides of the road, traffic signs around the road, and road guardrails, and the like, and then the initial point cloud data corresponding to moving objects such as pedestrians and vehicles need to be filtered, and only the initial point cloud data corresponding to the buildings, the traffic signs, the road guardrails, and the like are reserved.

In step 302, the plurality of frames of initial point cloud data may include initial point cloud data collected by the lidar system when the movable platform passes through the same location at least twice. Two frames of initial point cloud data acquired when the movable platform passes through the same place twice can be used for establishing a loop constraint condition; when the vehicle passes through the same location three times or more, then multiple loop constraints may be established. Specifically, when the vehicle-mounted laser radar system performs map data acquisition, the vehicle-mounted laser radar system can pass through the same place twice or multiple times to improve the density of point clouds of a high-precision map finally constructed at the place, and can also provide more loop constraint conditions.

Besides establishing loop constraint conditions by adopting two frames of initial point cloud data acquired when the movable platform passes through the same place twice, the loop constraint conditions can be determined based on the pose data with the difference of two frames of spatial poses in a second preset range and opposite directions. The pose data with the difference between the spatial poses within the second preset range and opposite to each other generally refers to pose data of a vehicle on a forward and reverse bidirectional road corresponding to the same place, for example, pose data when the vehicle runs from north to south on the bidirectional road 1 and passes a certain position, and pose data when the vehicle runs from north to south on the bidirectional road 1 and passes the position again. In some scenes such as expressways, objects such as green belts are arranged in the middle of the roads, and whether the two-way roads are needed cannot be directly determined, so that whether the two-way roads are the forward and the backward of the current point for collecting the initial point cloud data and the point for collecting the initial point cloud data before can be judged according to the position and the orientation of the initial point cloud data. The pose data which are in a second preset range and are opposite in orientation can be determined by target objects which can be detected by two-way lanes such as guideboards, peripheral buildings and tunnel openings.

The loop constraint condition is established, the accumulated error in the pose calculation process can be reduced, and the optimization accuracy of pose data is improved, so that the accuracy of the three-dimensional point cloud map is improved.

In some embodiments, the number of loop-back constraints may be dynamically adjusted. For example, the number of loop constraints may be adjusted according to the confidence level of the pose data; for another example, the number of loop constraints may be adjusted according to three-dimensional size information of at least one target object in the three-dimensional point cloud map.

In an embodiment in which the number of loop constraints is adjusted according to the confidence level of the pose data, the confidence level of the pose data is used to characterize the confidence level of the acquired pose data. The confidence of the pose data is related to the signal strength of a positioning signal used for positioning the vehicle and the accuracy of the three-dimensional point cloud map. Accordingly, the confidence level of the pose data may be determined according to any one of the intensity of a positioning signal for positioning the vehicle and the accuracy of the three-dimensional point cloud map.

On the one hand, if the signal strength of the positioning signal is strong, the confidence of the pose data can be considered to be high; conversely, if the signal strength of the positioning signal is weak, the confidence of the pose data may be considered to be low. For example, in a relatively open area, the signal strength of the GPS signal is often relatively high, the positioning accuracy is relatively high, and the reliability of the acquired pose data is relatively high. In the cities of high-rise forestation or in areas such as viaducts and tunnels, GPS signals are shielded by buildings, and the signal strength is often weak, so that the positioning error is large, and the reliability of the acquired pose data is low.

On the other hand, if the accuracy of the obtained pose data is high, the accuracy of the three-dimensional point cloud map generally established according to the pose data is also high, so that the confidence of the pose data can be determined according to the accuracy of the three-dimensional point cloud map. The accuracy of the three-dimensional point cloud map can be determined by the deviation between the three-dimensional coordinates of a point in the three-dimensional point cloud map and the actual coordinates of the point. The smaller the deviation, the higher the accuracy; conversely, the greater the deviation, the lower the accuracy. If the accuracy of the three-dimensional point cloud map is higher, the confidence of the pose data can be considered to be higher; conversely, if the accuracy of the three-dimensional point cloud map is low, the confidence of the pose data can be considered to be low.

Wherein the accuracy of the three-dimensional point cloud map may be determined according to the following manner: sampling a three-dimensional point cloud map in a certain range on the moving track of the movable platform at intervals; calculating the height difference of each ground point in the sampled three-dimensional point cloud map; and determining the accuracy of the three-dimensional point cloud map according to the height difference. Further, if the height difference is greater than a preset height threshold, determining that the accuracy of the three-dimensional point cloud map is lower than a preset accuracy threshold. And if the height difference is smaller than or equal to the height threshold, judging that the accuracy of the three-dimensional point cloud map is not lower than a preset accuracy threshold.

In an actual application scene, according to a moving track of a vehicle, points in a square range of 1 meter x 1 meter in a three-dimensional point cloud map can be sampled every 200 meters, the height difference d of each ground point cloud in the square range is calculated, the square range can be considered to be on a plane, each ground point in the square range should be on the same horizontal plane theoretically, the height difference is 0, but if ground points with the height difference d being greater than a certain threshold value exist, the accuracy of the three-dimensional point cloud map constructed in the square range is considered to be lower, and the number of loop matching times (namely, the number of loop constraint conditions) is required to be increased for pose data corresponding to the three-dimensional point cloud data in the square range.

After determining the confidence level of the pose data, the number of loop constraints may be adjusted according to the confidence level. If the confidence level is less than a preset confidence level threshold, the number of loop-back constraints may be increased. For example, the number of loop constraints is increased from 3 to 5. If the confidence level is greater than or equal to a preset confidence level threshold, the number of loop-back constraints may be reduced. For example, at the time of data acquisition, five times of initial point cloud data may be repeatedly acquired in the same target area, but only three of the three times of initial point cloud data are employed to establish the loop constraint condition.

In an embodiment in which the number of loop constraints is adjusted according to the three-dimensional size information of at least one target object in the three-dimensional point cloud map, the target object may be a landmark object in the target area, such as a traffic sign, a traffic light, a lamp post, etc., and the reference three-dimensional size information (length, width, height) of these target objects may be known according to national standards. A difference between the three-dimensional size information and reference three-dimensional size information of the target object may be calculated; and adjusting the number of the loop constraint conditions according to the difference value. If the difference value between the three-dimensional size information of the target object in the constructed three-dimensional point cloud map and the reference three-dimensional size information is larger than a preset size threshold value, the constructed three-dimensional point cloud map is considered to be lower in accuracy, and the number of loop constraint conditions needs to be increased. For example, one standard size for triangular traffic signs is a side length of 70 cm, according to national standard requirements. If the size of a certain triangular traffic sign plate in the three-dimensional point cloud map is greatly different from the standard size, the number of loop constraint conditions needs to be increased.

Fig. 4 is a schematic diagram of adjustment logic for loop-back constraints, shown in accordance with an embodiment of the present disclosure. In summary, the number adjustment strategy of the loop constraint condition in the embodiment of the disclosure is to spend more calculation resources and more optimization times in the area with poor accuracy, and simultaneously perform bidirectional road detection, so as to reduce the accumulated error.

In step 303, the pose data may be processed according to the at least one loop constraint condition, to obtain optimized pose data of the initial point cloud data of each frame.

In some embodiments, the optimized pose data may be solved in a Graph-optimized (Graph-based optimization) manner. The Graph is composed of nodes and edges, and in Graph-optimized SLAM (Graph-based Simultaneous Localization And Mapping, synchronous positioning and Graph construction), the pose is a node or vertex, and the relationship between the poses forms an edge. Specifically, for example, the geometric relationship between the time t+1 and the time t constitutes an edge, or the pose conversion matrix calculated from the vision may also constitute an edge. Once the graph construction is complete, the pose is adjusted to meet the constraints imposed by these edges as much as possible.

Therefore, the processing the pose data according to the at least one loop constraint condition to obtain optimized pose data of the initial point cloud data of each frame includes: and solving an optimization objective function (such as minimum sum of squares of residual errors) of the graph by taking the pose data as a graph vertex and the loop constraint condition as a graph edge so as to acquire the optimized pose data. Further, the graph optimization can be solved by taking the relative pose relation between the loop constraint condition and the pose data of the adjacent frames as the edges of the graph.

The specific steps of the graph optimization are as follows:

first, the position (rough pose data) acquired by the navigation system is set as the vertex of the map.

Then, traversing all three-dimensional point cloud data according to the time sequence, carrying out time sequence inter-frame registration, wherein the inter-frame registration can use NSS (normal space sampling ), then carrying out ICP processing, and taking the relative pose relation of the front frame pose data and the rear frame pose data, namely inter-frame constraint as the edge of the graph.

Assume that the previous frame initial point cloud data p= { P is acquired in the first pose ₁ ,p ₂ ,…,p _n The initial point cloud data q= { Q of the next frame acquired in the second pose ₁ ,q ₂ ,…,q _n The coordinates of P and Q correspond to the coordinate system in the first pose and the coordinate system in the second pose, respectively. Assuming that the ith point in P and the ith point in Q correspond to the same point in three-dimensional space, e.g., P ₉₉ And q ₉₉ Corresponding to the same point in three-dimensional space. In the absence of errors, the formula for converting from the P coordinate system to the Q coordinate system is:

q _i ＝Rp _i +t (1)；

wherein R and t are rotation matrix and translation matrix of the first pose to the second pose respectively. Due to noise and mismatch (p _i And q _i In fact does not correspond to the same point in space, but the feature matching algorithm erroneously considers both to be the same point), equation (1) is not always true, and therefore the objective function that needs to be minimized is:

Where n is the number of point cloud data per frame. Parameters R and t used for representing the relative pose relation of the pose data of the front frame and the rear frame can be obtained through the formula (1) and the formula (2).

Traversing the initial point cloud data again, and taking multiple frames of the initial point cloud data with adjacent spatial positions but discontinuous time sequence (namely the same position of the second time or the third time of opening) to estimate the relative pose relation between pose data of the frames by using ICP, and adding graph optimization as loop constraint (also as the edge of the graph).

And finally, solving an optimization objective function of the graph according to the graph vertex and the graph edge established in the step, and obtaining the optimization pose data.

If it is determined in step 302 that the number of loop constraints needs to be increased according to any condition, in step 303, a new loop constraint is added when the pose data is optimized for solving, and step 303 is executed again according to the added loop constraint, so as to solve the pose data for optimizing, and obtain the pose data for optimizing with higher precision. Repeating the steps for a plurality of times until the accuracy of the optimized pose data reaches the standard.

In general, better performance can be obtained by loop-back detection. However, in areas with fewer map features, such as a highway section, the error of loop detection may be larger, so that the error of the constructed three-dimensional point cloud map is larger. To solve this problem, continuous multi-frame initial point cloud data may be processed as one frame of initial point cloud data, that is, the initial point cloud data includes registered continuous multi-frame point cloud data. Since the pose of each frame of initial point cloud data may be different, the multiple frames of initial point cloud data need to be registered first. A registration process may be performed according to the optimized pose data.

Initial point cloud data in a certain distance range (such as 100 m) can be accumulated according to the optimized pose data, and then loop registration of the point set is performed, so that accuracy of loop registration is improved. Specifically, all three-dimensional point cloud data can be subjected to time sequence accumulation by using the optimized pose data according to a certain distance, the accumulated three-dimensional point cloud data is used as input of registration, and graph optimization is performed according to the mode, so that the pose data accuracy is further improved.

The optimized pose data can be obtained by searching from pose data in a certain range; the searching range of the optimized pose data is set according to the signal intensity of a positioning signal for positioning the movable platform and/or the confidence of the optimized pose data. In the process of solving the map optimization, the search range of the optimized pose data is dynamically adjusted, for example, when the signal strength of the positioning signals is poor, the search range of the optimized pose data is enlarged, and otherwise, the search range of the optimized pose data can be reduced. When the confidence is low, the search range of the optimized pose data is enlarged, otherwise, the search range of the optimized pose data can be reduced. In actual operation, pose data before optimization can be used as a center, and the optimized pose data can be searched in the neighborhood of the pose data. Expanding the neighborhood radius when the signal strength of the positioning signal is poor; when the signal strength of the positioning signal is strong, the neighborhood radius is reduced.

In step 304, after the three-dimensional point cloud map is constructed, the optimized pose data may be verified, and based on the initial point cloud data of multiple frames, the three-dimensional point cloud map may be constructed according to the optimized pose data that is verified successfully. The trajectory of the vehicle acquisition initial point cloud data should have smoothness, and thus, the optimized pose data may be verified according to the smoothness of the optimized pose data.

Specifically, the optimized pose data and the optimized pose data of the previous frame or the previous frames of the optimized pose data and the average value of the optimized pose data of the next frame or the next frames of the optimized pose data can be calculated, if the difference between the optimized pose data and the average value is greater than a preset value, the optimized pose data is illustrated to have a larger change (i.e. not smooth) than the adjacent optimized pose data, and therefore the failure of verification of the optimized pose data is determined. For the k-th frame of optimized pose data, an average of the k-th frame of optimized pose data and the k-1 st frame of optimized data, the k+1 st frame of optimized pose data may be taken, and the k-th frame of optimized pose data may be compared with the average. And if the difference value between the k-th frame of optimized pose data and the average value is larger than a preset value, judging that the verification of the optimized pose data fails.

The above embodiment may employ the course angle in the optimized pose data to verify the optimized pose data. For example, for the optimized pose data of the kth frame, calculating the average value of the course angle of the optimized pose data of the kth frame and the course angle of the optimized pose data of the previous frame (kth-1 frame) and the course angle of the optimized pose data of the next frame (kth+1 frame), and if the difference between the course angle of the optimized pose data of the kth frame and the average value of the course angle is greater than a certain threshold (such as 1 °), considering that the verification of the optimized pose data fails. In this case, the number of loop constraints can be increased.

Generally, after the optimized pose data optimized for the first time is obtained, verification of the optimized pose data is performed, if the confidence coefficient of the optimized pose data with verification failure is low, the searching range of the optimized pose data needs to be enlarged, and/or the number of loop constraint conditions needs to be increased.

The method of the embodiment of the disclosure can perform centimeter-level three-dimensional reconstruction on the target area, and obtain accurate map information. In the area (such as under an overhead bridge in a tunnel) with a limited traditional navigation positioning mode, the map can be successfully constructed by adjusting the number of loop constraint conditions. And furthermore, the GPU can be used for carrying out parallel processing on multi-frame initial point cloud data, so that the parallel processing method has good parallelism and the time required for constructing the map is less than that of the traditional method.

In some embodiments, as shown in fig. 5, embodiments of the present disclosure further provide a three-dimensional point cloud mapping system, the three-dimensional point cloud mapping system including:

a processing device 501 and a lidar system 502 mounted on a movable platform;

the laser radar system 502 is configured to collect multiple frames of initial point cloud data;

the processor 501 is configured to determine at least one loop constraint; processing the multi-frame pose data according to the at least one loop constraint condition to obtain optimized pose data of the initial point cloud data of each frame, and obtaining optimized pose data of the initial point cloud data of each frame; according to the optimized pose data, constructing a three-dimensional point cloud map based on a plurality of frames of initial point cloud data; the loop constraint condition is determined based on the pose data in which a difference between two frames of spatial poses is within a first preset range and is not continuous in time sequence.

In some embodiments, the three-dimensional point cloud mapping system further comprises: a navigation system 503 mounted on the movable platform; the navigation system 503 is configured to obtain pose data when the lidar system collects multiple frames of the initial point cloud data.

In some embodiments, the navigation system 503 includes: a first navigation system and a second navigation system; the first navigation system is used for acquiring first pose data when the laser radar system acquires multiple frames of initial point cloud data; the second navigation system is used for acquiring second pose data when the laser radar system acquires multiple frames of initial point cloud data; the processing device is also used for: and acquiring the pose data based on the first navigation data and the second navigation data.

In some embodiments, the processing device 501 is further configured to: and aligning the pose data with the initial point cloud data.

In some embodiments, the processing device 501 aligns the pose data with the initial point cloud data, comprising: and aligning the pose data with the initial point cloud data based on the global time stamp of the navigation system and the global time stamp of the laser radar system.

In some embodiments, the plurality of frames of initial point cloud data includes initial point cloud data collected by the lidar system at least twice through the same location of the movable platform.

In some embodiments, the processing device 501 is further configured to: and adjusting the number of the loop constraint conditions.

In some embodiments, the processing device 501 adjusts the number of loop-back constraints, including: adjusting the number of loop constraint conditions according to the confidence level of the pose data; and/or adjusting the number of the loop constraint conditions according to the three-dimensional size information of at least one target object in the three-dimensional point cloud map.

In some embodiments, the confidence level of the pose data is determined according to the intensity of a positioning signal positioning the movable platform and/or the accuracy of the three-dimensional point cloud map.

In some embodiments, the processing device 501 is further configured to: sampling a three-dimensional point cloud map in a certain range on the moving track of the movable platform at intervals; calculating the height difference of each ground point in the sampled three-dimensional point cloud map; and determining the accuracy of the three-dimensional point cloud map according to the height difference.

In some embodiments, the processing device 501 determines an accuracy of the three-dimensional point cloud map from the altitude differences, comprising: if the height difference is larger than a preset height threshold, judging that the accuracy of the three-dimensional point cloud map is lower than a preset accuracy threshold; and/or if the height difference is smaller than or equal to the height threshold, judging that the accuracy of the three-dimensional point cloud map is not lower than a preset accuracy threshold.

In some embodiments, the processing device 501 adjusts the number of loop constraints according to the confidence of the pose data, including: if the confidence coefficient is smaller than a preset confidence coefficient threshold value, increasing the number of loop constraint conditions; and/or if the confidence coefficient is greater than or equal to a preset confidence coefficient threshold value, reducing the number of loop-back constraint conditions.

In some embodiments, the processing device 501 adjusts the number of loop constraints according to three-dimensional size information of at least one target object in the three-dimensional point cloud map, including: calculating a difference between the three-dimensional size information and reference three-dimensional size information of the target object; and adjusting the number of the loop constraint conditions according to the difference value.

In some embodiments, the loop constraint is also determined based on the pose data having a difference between two frames of spatial poses within a second preset range and facing opposite.

In some embodiments, the initial point cloud data comprises registered continuous multi-frame point cloud data.

In some embodiments, the processing device 501 is further configured to: and filtering point cloud data corresponding to the moving object from the initial point cloud data.

In some embodiments, the processing device 501 processes the pose data according to the at least one loop constraint condition to obtain optimized pose data of the initial point cloud data of each frame, including: and solving an optimization objective function of the graph by taking the pose data as a graph vertex and the loop constraint condition as a graph edge so as to acquire the optimized pose data.

In some embodiments, the processing device 501 uses the pose data as a vertex of the graph and uses the loop constraint condition as an edge of the graph, and solves an optimization objective function of the graph to obtain the optimized pose data, including: and solving an optimization objective function of the graph by taking the pose data as a graph vertex, the loop constraint condition and the relative pose relation between the pose data of adjacent frames as the edges of the graph, so as to acquire the optimized pose data.

In some embodiments, the optimized pose data is found from a range of pose data; the searching range of the optimized pose data is set according to the strength of a positioning signal for positioning the movable platform and/or the confidence of the optimized pose data.

In some embodiments, the processing device 501 constructs a three-dimensional point cloud map based on a plurality of frames of the initial point cloud data according to the optimized pose data, including: verifying the optimized pose data; and constructing a three-dimensional point cloud map based on the initial point cloud data of multiple frames according to the optimized pose data successfully checked.

In some embodiments, the processing device 501 performs verification on the optimized pose data, including: calculating the average value of the optimized pose data and the optimized pose data of a plurality of adjacent frames; calculating a difference between the optimized pose data and the average value; and verifying the optimized pose data according to the difference value.

In some embodiments, the processing device 501 verifies the optimized pose data according to the difference value, including: and if the difference value is larger than a preset value, judging that the verification of the optimized pose data is successful, and/or if the difference value is smaller than or equal to the preset value, judging that the verification of the optimized pose data is failed.

In some embodiments, the processing device 501 constructs a three-dimensional point cloud map based on a plurality of frames of the initial point cloud data according to the optimized pose data, including: registering a plurality of frames of initial point cloud data according to the optimized pose data; and constructing the three-dimensional point cloud map according to the registered multi-frame initial point cloud data.

The specific embodiments of the method executed by the processing device 501 in the three-dimensional point cloud map building system are detailed in the embodiments of the three-dimensional point cloud map building method of the present disclosure, and are not described herein again.

In some embodiments, the present disclosure also provides a three-dimensional point cloud mapping apparatus comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, which when executed implements the steps of the method of any of the embodiments.

Fig. 6 shows a schematic hardware structure of a more specific three-dimensional point cloud mapping apparatus according to an embodiment of the disclosure, where the apparatus may include: a processor 601, a memory 602, an input/output interface 603, a communication interface 604, and a bus 605. Wherein the processor 601, the memory 602, the input/output interface 603 and the communication interface 604 are communicatively coupled to each other within the device via a bus 605.

The processor 601 may be implemented by a general-purpose CPU (Central Processing Unit ), a microprocessor, an application specific integrated circuit (Application Specific Integrated Circuit, ASIC), or one or more integrated circuits, etc. for executing relevant programs to implement the technical solutions provided in the embodiments of the present disclosure.

The Memory 602 may be implemented in the form of ROM (Read Only Memory), RAM (Random Access Memory ), static storage device, dynamic storage device, or the like. The memory 602 may store an operating system and other application programs, and when the technical solutions provided in the embodiments of the present specification are implemented in software or firmware, relevant program codes are stored in the memory 602 and executed by the processor 602.

The input/output interface 603 is used for connecting with an input/output module to realize information input and output. The input/output module may be configured as a component in a device (not shown) or may be external to the device to provide corresponding functionality. Wherein the input devices may include a keyboard, mouse, touch screen, microphone, various types of sensors, etc., and the output devices may include a display, speaker, vibrator, indicator lights, etc.

The communication interface 604 is used to connect a communication module (not shown in the figure) to enable the present device to interact with other devices for communication. The communication module may implement communication through a wired manner (such as USB, network cable, etc.), or may implement communication through a wireless manner (such as mobile network, WIFI, bluetooth, etc.).

The bus 605 includes a path to transfer information between the various components of the device, such as the processor 601, memory 602, input/output interfaces 603, and communication interfaces 604.

It should be noted that although the above device only shows the processor 601, the memory 602, the input/output interface 603, the communication interface 604, and the bus 605, in the implementation, the device may further include other components necessary for realizing normal operation. Furthermore, it will be understood by those skilled in the art that the above-described apparatus may include only the components necessary to implement the embodiments of the present description, and not all the components shown in the drawings.

In some embodiments, the present disclosure also provides a computer readable storage medium having stored thereon computer instructions which, when executed, implement the steps of the method of any of the embodiments.

Computer readable media, including both non-transitory and non-transitory, removable and non-removable media, may implement information storage by any method or technology. The information may be computer readable instructions, data structures, modules of a program, or other data. Examples of storage media for a computer include, but are not limited to, phase change memory (PRAM), static Random Access Memory (SRAM), dynamic Random Access Memory (DRAM), other types of Random Access Memory (RAM), read Only Memory (ROM), electrically Erasable Programmable Read Only Memory (EEPROM), flash memory or other memory technology, compact disc read only memory (CD-ROM), digital Versatile Discs (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices, or any other non-transmission medium, which can be used to store information that can be accessed by a computing device. Computer-readable media, as defined herein, does not include transitory computer-readable media (transmission media), such as modulated data signals and carrier waves.

From the foregoing description of embodiments, it will be apparent to those skilled in the art that the present embodiments may be implemented in software plus a necessary general purpose hardware platform. Based on such understanding, the technical solutions of the embodiments of the present specification may be embodied in essence or what contributes to the prior art in the form of a software product, which may be stored in a storage medium, such as a ROM/RAM, a magnetic disk, an optical disk, etc., including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method described in the embodiments or some parts of the embodiments of the present specification.

In this specification, each embodiment is described in a progressive manner, and identical and similar parts of each embodiment are all referred to each other, and each embodiment mainly describes differences from other embodiments. In particular, for the device embodiments, since they are substantially similar to the method embodiments, the description is relatively simple, and reference is made to the description of the method embodiments for relevant points. The apparatus embodiments described above are merely illustrative, in which the modules illustrated as separate components may or may not be physically separate, and the functions of the modules may be implemented in the same piece or pieces of software and/or hardware when implementing the embodiments of the present disclosure. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art will understand and implement the present invention without undue burden.

The various technical features in the above embodiments may be arbitrarily combined as long as there is no conflict or contradiction between the combinations of the features, but are not described in detail, so that the arbitrary combination of the various technical features in the above embodiments also falls within the scope of the disclosure of the present specification.

Other embodiments of the disclosure will be apparent to those skilled in the art from consideration of the specification and practice of the disclosure herein. This disclosure is intended to cover any adaptations, uses, or adaptations of the disclosure following the general principles of the disclosure and including such departures from the present disclosure as come within known or customary practice within the art to which the disclosure pertains. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the disclosure being indicated by the following claims.

It is to be understood that the present disclosure is not limited to the precise arrangements and instrumentalities shown in the drawings, and that various modifications and changes may be effected without departing from the scope thereof. The scope of the present disclosure is limited only by the appended claims.

The foregoing description of the preferred embodiments of the present disclosure is not intended to limit the disclosure, but rather to cover all modifications, equivalents, improvements and alternatives falling within the spirit and principles of the present disclosure.

Claims

1. A method for constructing a three-dimensional point cloud map, the method comprising:

acquiring multi-frame initial point cloud data acquired by a laser radar system carried on a movable platform, and acquiring pose data when the laser radar system acquires the multi-frame initial point cloud data;

determining at least one loop constraint condition, wherein the loop constraint condition is determined based on the pose data of which the difference between two frames of space poses is within a first preset range and is discontinuous in time sequence;

processing the multi-frame pose data according to the at least one loop constraint condition to obtain optimized pose data of the initial point cloud data of each frame;

according to the optimized pose data, constructing a three-dimensional point cloud map based on a plurality of frames of initial point cloud data;

the method further comprises the steps of:

adjusting the number of the loop constraint conditions; wherein,

if the accuracy of the three-dimensional point cloud map is higher than a preset accuracy threshold, reducing the number of loop constraint conditions, otherwise, increasing the number of loop constraint conditions; and/or

If the intensity of the positioning signal is larger than the preset intensity, reducing the number of the loop constraint conditions, otherwise, increasing the number of the loop constraint conditions; and/or

If the difference value between the three-dimensional size information of the target object in the three-dimensional point cloud map and the reference three-dimensional size information is smaller than or equal to a preset size threshold value, the number of loop constraint conditions is reduced, otherwise, the number of loop constraint conditions is increased.

2. The method of claim 1, wherein a navigation system is further onboard the mobile platform, the pose data being acquired based on navigation data of the navigation system.

3. The method of claim 2, wherein the navigation system comprises a first navigation system and a second navigation system;

the obtaining pose data when the laser radar system collects multi-frame initial point cloud data comprises the following steps:

acquiring first navigation data output by the first navigation system and second navigation data output by the second navigation system;

and acquiring the pose data based on the first navigation data and the second navigation data.

4. The method according to claim 2, wherein the method further comprises:

and aligning the pose data with the initial point cloud data.

5. The method of claim 4, wherein said aligning said pose data with said initial point cloud data comprises:

And aligning the pose data with the initial point cloud data based on the global time stamp of the navigation system and the global time stamp of the laser radar system.

6. The method of claim 1, wherein the plurality of frames of initial point cloud data comprise initial point cloud data collected by the lidar system at least two passes of the movable platform through the same site.

7. The method according to claim 1, wherein the method further comprises:

sampling a three-dimensional point cloud map in a certain range on the moving track of the movable platform at intervals;

calculating the height difference of each ground point in the sampled three-dimensional point cloud map;

and determining the accuracy of the three-dimensional point cloud map according to the height difference.

8. The method of claim 7, wherein said determining the accuracy of the three-dimensional point cloud map from the height difference comprises:

if the height difference is larger than a preset height threshold, judging that the accuracy of the three-dimensional point cloud map is lower than a preset accuracy threshold; and/or

And if the height difference is smaller than or equal to the height threshold, judging that the accuracy of the three-dimensional point cloud map is not lower than a preset accuracy threshold.

9. The method of claim 1, wherein the loop constraint is further determined based on the pose data having a difference between two frames of spatial poses within a second preset range and facing opposite.

10. The method of claim 1, wherein the initial point cloud data comprises registered continuous multi-frame point cloud data.

11. The method according to claim 1, wherein the method further comprises:

and filtering point cloud data corresponding to the moving object from the initial point cloud data.

12. The method according to claim 1, wherein the processing the pose data according to the at least one loop constraint condition to obtain optimized pose data of the initial point cloud data for each frame includes:

and solving an optimization objective function of the graph by taking the pose data as a graph vertex and the loop constraint condition as a graph edge so as to acquire the optimized pose data.

13. The method of claim 12, wherein solving the optimization objective function of the graph with the pose data as the graph vertices and the loop-back constraints as the graph edges to obtain the optimized pose data comprises:

And solving an optimization objective function of the graph by taking the pose data as a graph vertex, the loop constraint condition and the relative pose relation between the pose data of adjacent frames as the edges of the graph, so as to acquire the optimized pose data.

14. The method of claim 13, wherein the optimized pose data is found from a range of pose data; the searching range of the optimized pose data is set according to the strength of a positioning signal for positioning the movable platform and/or the confidence of the optimized pose data.

15. The method of claim 1, wherein said constructing a three-dimensional point cloud map based on a plurality of frames of said initial point cloud data according to said optimized pose data comprises:

verifying the optimized pose data;

and constructing a three-dimensional point cloud map based on the initial point cloud data of multiple frames according to the optimized pose data successfully checked.

16. The method of claim 15, wherein the verifying the optimized pose data comprises:

calculating the average value of the optimized pose data and the optimized pose data of a plurality of adjacent frames;

Calculating a difference between the optimized pose data and the average value;

and verifying the optimized pose data according to the difference value.

17. The method of claim 16, wherein the verifying the optimized pose data from the difference comprises:

if the difference value is larger than a preset value, judging that the optimized pose data is successfully checked, and/or

And if the difference value is smaller than or equal to the preset value, judging that the verification of the optimized pose data fails.

18. The method of claim 1, wherein said constructing a three-dimensional point cloud map based on a plurality of frames of said initial point cloud data according to said optimized pose data comprises:

registering a plurality of frames of initial point cloud data according to the optimized pose data;

and constructing the three-dimensional point cloud map according to the registered multi-frame initial point cloud data.

19. A three-dimensional point cloud map construction system, characterized in that the three-dimensional point cloud map construction system comprises:

the processing device and the laser radar system are mounted on the movable platform;

the laser radar system is used for collecting multi-frame initial point cloud data;

the processing device is used for determining at least one loop constraint condition; processing pose data when the laser radar system acquires multiple frames of initial point cloud data according to the at least one loop constraint condition to obtain optimized pose data of each frame of initial point cloud data; according to the optimized pose data, constructing a three-dimensional point cloud map based on a plurality of frames of initial point cloud data; the loop constraint condition is determined based on the pose data that the difference between two frames of spatial poses is within a first preset range and is discontinuous in time sequence;

The processing device is also used for: adjusting the number of the loop constraint conditions; wherein,

20. The three-dimensional point cloud mapping system of claim 19, further comprising:

a navigation system carried on the movable platform;

the navigation system is used for acquiring pose data when the laser radar system acquires multiple frames of initial point cloud data.

21. The three-dimensional point cloud mapping system of claim 20, wherein said navigation system comprises:

A first navigation system and a second navigation system;

the first navigation system is used for acquiring first pose data when the laser radar system acquires multiple frames of initial point cloud data;

the second navigation system is used for acquiring second pose data when the laser radar system acquires multiple frames of initial point cloud data;

the processing device is also used for:

and acquiring the pose data based on the first pose data and the second pose data.

22. The three-dimensional point cloud mapping system of claim 20, wherein said processing means is further for:

and aligning the pose data with the initial point cloud data.

23. The three-dimensional point cloud mapping system of claim 22, wherein said processing means for aligning said pose data with said initial point cloud data comprises:

24. The three-dimensional point cloud map construction system of claim 19, wherein a plurality of frames of said initial point cloud data comprise initial point cloud data collected by said lidar system at least twice through a same site of said movable platform.

25. The three-dimensional point cloud mapping system of claim 19, wherein said processing means is further for:

26. The three-dimensional point cloud map construction system of claim 25, wherein said processing means determining the accuracy of said three-dimensional point cloud map from said height difference comprises:

27. The three-dimensional point cloud mapping system of claim 19, wherein said loop-back constraint is further determined based on said pose data having a difference between two frames of spatial poses within a second preset range and facing opposite.

28. The three-dimensional point cloud mapping system of claim 19, wherein said initial point cloud data comprises registered continuous multi-frame point cloud data.

29. The three-dimensional point cloud mapping system of claim 19, wherein said processing means is further for:

30. The three-dimensional point cloud map construction system of claim 19, wherein said processing means processes said pose data according to said at least one loop constraint to obtain optimized pose data for each frame of said initial point cloud data, comprising:

31. The three-dimensional point cloud map construction system of claim 30, wherein said solving an optimization objective function of a map with said pose data as a map vertex and said loop-back constraint as a map edge to obtain said optimized pose data comprises:

32. The three-dimensional point cloud map construction system of claim 30, wherein said optimized pose data is found from a range of pose data; the searching range of the optimized pose data is set according to the strength of a positioning signal for positioning the movable platform and/or the confidence of the optimized pose data.

33. The system of claim 19, wherein the processing means constructs a three-dimensional point cloud map based on a plurality of frames of the initial point cloud data according to the optimized pose data, comprising:

verifying the optimized pose data;

34. The system of claim 33, wherein the processing means for verifying the optimized pose data comprises:

calculating a difference between the optimized pose data and the average value;

and verifying the optimized pose data according to the difference value.

35. The system of claim 34, wherein the processing means for verifying the optimized pose data based on the difference comprises:

36. The three-dimensional point cloud map construction system of claim 19, wherein said processing means constructs a three-dimensional point cloud map based on a plurality of frames of said initial point cloud data according to said optimized pose data, comprising:

37. A three-dimensional point cloud mapping apparatus comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the method of any one of claims 1 to 18 when executing the program.

38. A computer readable storage medium having stored thereon computer instructions, which when executed, implement the steps of the method of any of claims 1 to 18.