阅读说明：本技术 基于点云配准的3d地图构建方法、装置、计算机设备及存储介质 (Point cloud registration-based 3D map construction method and device, computer equipment and storage medium ) 是由 高国清 莫松文 任仲超 凌云志 张业楚 于 2020-04-27 设计创作，主要内容包括：本发明公开了一种基于点云配准的3D地图构建方法、装置、计算机及存储介质,方法包括以下步骤：获取当前地图的帧点云,并将当前的帧点云作为源点云；获取最新地图的帧点云,并将最新的帧点云作为目标点云；计算从源点云变换至目标点云的变换矩阵；根据变换矩阵,将地图上的其他作为源点云的帧点云均转换至目标点云；将所有目标点云进行拼接,以构建3D地图。与现有技术相比,本发明的实施例提供了一种基于点云配准的3D地图构建方法、装置、计算机设备及存储介质,其通过ICP或NDT算法获得从源点云到目标点云的变换矩阵,从而3D地图的构建,提高了定位精度。(The invention discloses a point cloud registration-based 3D map construction method, a point cloud registration-based 3D map construction device, a point cloud registration-based 3D map construction computer and a storage medium, wherein the method comprises the following steps of: acquiring a frame point cloud of a current map, and taking the current frame point cloud as a source point cloud; acquiring a frame point cloud of a latest map, and taking the latest frame point cloud as a target point cloud; calculating a transformation matrix transformed from a source point cloud to a target point cloud; converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix; and splicing all the target point clouds to construct a 3D map. Compared with the prior art, the embodiment of the invention provides a point cloud registration-based 3D map construction method, a point cloud registration-based 3D map construction device, computer equipment and a storage medium, wherein a transformation matrix from a source point cloud to a target point cloud is obtained through an ICP (inductively coupled plasma) or NDT (non-dispersive Transmission test) algorithm, so that a 3D map is constructed, and the positioning accuracy is improved.)

1. A3D map construction method based on point cloud registration is characterized by comprising the following steps:

acquiring a frame point cloud of a current map, and taking the current frame point cloud as a source point cloud;

acquiring a frame point cloud of a latest map, and taking the latest frame point cloud as a target point cloud;

calculating a transformation matrix transformed from a source point cloud to a target point cloud;

converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix;

and splicing all the target point clouds to construct a 3D map.

2. The point cloud registration-based 3D map construction method according to claim 1, wherein the steps between "acquiring a frame point cloud of a current map and taking the current frame point cloud as a source point cloud" and "acquiring a frame point cloud of a latest map and taking the latest frame point cloud as a target point cloud" further comprise: and setting the currently obtained source point cloud as a reference frame.

3. The point cloud registration-based 3D map construction method according to claim 1, wherein the calculation method of the transformation matrix in the step of calculating the transformation matrix from the source point cloud to the target point cloud is an IPC algorithm, and the IPC algorithm comprises the following steps:

point cloud preprocessing to filter and clean the data;

carrying out point cloud registration, and solving a transformation matrix to find a closest point;

weighting processing for adjusting the weight of the corresponding point;

rejecting unreasonable registration point clouds to reduce noise effects;

and calculating a LOSS function, and obtaining an optimal change matrix according to the LOSS function.

4. The point cloud registration-based 3D mapping method according to claim 1, wherein the step of calculating the transformation matrix from the source point cloud transformation to the transformation matrix of the target point cloud is an NDT algorithm, and the NDT algorithm comprises the following steps:

subdividing the space occupied by scanning into unit grids, and calculating PDF of each unit based on point cloud distribution in the unit grids;

performing transfer matrix transformation according to each point of PDF scanning point cloud of each unit;

calculating a probability distribution function of the response according to the matrix transformation result;

and obtaining an optimal transformation matrix according to the probability distribution function.

5. A3D map building device based on point cloud registration is characterized by comprising:

the source point cloud obtaining unit is used for obtaining the frame point cloud of the current map and taking the current frame point cloud as the source point cloud;

the target point cloud obtaining unit is used for obtaining the latest map frame point cloud and taking the latest map frame point cloud as the target point cloud;

a transformation matrix calculation unit for calculating a transformation matrix transformed from a source point cloud to a target point cloud;

the point cloud registration unit is used for converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix;

and the map generation unit is used for splicing all the target point clouds to construct a 3D map.

6. The point cloud registration-based 3D map construction apparatus according to claim 5, further comprising a reference frame setting unit configured to set a currently obtained source point cloud as a reference frame.

7. The point cloud registration-based 3D map construction apparatus according to claim 5, wherein the transformation matrix calculation unit includes:

the preprocessing unit is used for point cloud preprocessing so as to filter and clean data;

the point cloud registration unit is used for carrying out point cloud registration and solving a transformation matrix to find the closest point;

a weighting unit for weighting processing for adjusting the weight of the corresponding point;

the rejecting unit is used for rejecting unreasonable registration point clouds to reduce noise influence;

and the function calculation unit is used for calculating the LOSS function and obtaining the optimal change matrix according to the LOSS function.

8. The point cloud registration-based 3D map construction apparatus according to claim 5, wherein the transformation matrix calculation unit includes:

a PDF calculation unit for subdividing the space occupied by the scanning into unit grids and calculating the PDF of each unit based on the point cloud distribution in the unit grids;

the transfer matrix transformation unit is used for carrying out transfer matrix transformation according to each point of the point cloud scanned by the PDF of each unit;

a distribution function calculation unit for calculating a probability distribution function of the response according to the matrix transformation result;

and the optimal matrix calculation unit is used for obtaining an optimal transformation matrix according to the probability distribution function.

9. A computer device, characterized in that the computer device comprises a memory on which a computer program is stored and a processor which, when executing the computer program, implements the point cloud registration-based 3D map construction method according to any one of claims 1-4.

10. A storage medium, characterized in that the storage medium stores a computer program comprising program instructions which, when executed by a processor, may implement the point cloud registration-based 3D mapping method of any one of claims 1-4.

Technical Field

The invention relates to the technical field of map construction methods, in particular to a point cloud registration-based 3D map construction method and device, computer equipment and a storage medium.

Background

The traditional map is a 2D grid map, the positioning accuracy is low, the requirement of high-accuracy positioning cannot be met, for example, automatic driving and the like, and a 3D map needs to be designed in turn to improve the positioning accuracy.

Disclosure of Invention

The embodiment of the invention provides a point cloud registration-based 3D map construction method, a point cloud registration-based 3D map construction device, computer equipment and a storage medium, and aims to solve the problem of low positioning accuracy of the existing 2D map.

In order to achieve the purpose, the technical scheme provided by the invention is as follows:

in a first aspect, the invention provides a point cloud registration-based 3D map construction method, which includes the following steps:

acquiring a frame point cloud of a current map, and taking the current frame point cloud as a source point cloud;

acquiring a frame point cloud of a latest map, and taking the latest frame point cloud as a target point cloud;

calculating a transformation matrix transformed from a source point cloud to a target point cloud;

converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix;

and splicing all the target point clouds to construct a 3D map.

The steps of obtaining the frame point cloud of the current map and taking the current frame point cloud as the source point cloud and the steps of obtaining the frame point cloud of the latest map and taking the latest frame point cloud as the target point cloud further comprise: and setting the currently obtained source point cloud as a reference frame.

Wherein, the calculation method of the transformation matrix in the step of calculating the transformation matrix transformed from the source point cloud to the target point cloud is an IPC algorithm, and the IPC algorithm comprises the following steps:

point cloud preprocessing to filter and clean the data;

carrying out point cloud registration, and solving a transformation matrix to find a closest point;

weighting processing for adjusting the weight of the corresponding point;

rejecting unreasonable registration point clouds to reduce noise effects;

and calculating a LOSS function, and obtaining an optimal change matrix according to the LOSS function.

The method for calculating the transformation matrix in the step of calculating the transformation matrix transformed from the source point cloud to the target point cloud is an NDT algorithm, and the NDT algorithm comprises the following steps of:

subdividing the space occupied by scanning into unit grids, and calculating PDF of each unit based on point cloud distribution in the unit grids;

performing transfer matrix transformation according to each point of the point cloud scanned by the PDF of each unit;

calculating a probability distribution function of the response according to the matrix transformation result;

and obtaining an optimal transformation matrix according to the probability distribution function.

In a second aspect, an embodiment of the present invention further provides a 3D map building apparatus based on point cloud registration, including:

the source point cloud obtaining unit is used for obtaining the frame point cloud of the current map and taking the current frame point cloud as the source point cloud;

the target point cloud obtaining unit is used for obtaining the latest map frame point cloud and taking the latest map frame point cloud as the target point cloud;

a transformation matrix calculation unit for calculating a transformation matrix transformed from a source point cloud to a target point cloud;

the point cloud registration unit is used for converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix;

and the map generation unit is used for splicing all the target point clouds to construct a 3D map.

The device also comprises a reference frame setting unit, wherein the reference frame setting unit is used for setting the currently obtained source point cloud as a reference frame.

Wherein the transformation matrix calculation unit includes:

the preprocessing unit is used for point cloud preprocessing so as to filter and clean data;

the point cloud registration unit is used for carrying out point cloud registration and solving a transformation matrix to find the closest point;

a weighting unit for weighting processing for adjusting the weight of the corresponding point;

the rejecting unit is used for rejecting unreasonable registration point clouds to reduce noise influence;

and the function calculation unit is used for calculating the LOSS function and obtaining the optimal change matrix according to the LOSS function.

Wherein the transformation matrix calculation unit includes:

a PDF calculation unit for subdividing the space occupied by the scanning into unit grids and calculating the PDF of each unit based on the point cloud distribution in the unit grids;

the transfer matrix transformation unit is used for carrying out transfer matrix transformation according to each point of the point cloud scanned by the PDF of each unit;

a distribution function calculation unit for calculating a probability distribution function of the response according to the matrix transformation result;

and the optimal matrix calculation unit is used for obtaining an optimal transformation matrix according to the probability distribution function.

In a third aspect, the present invention further provides a computer device, which includes a memory and a processor, where the memory stores a computer program, and the processor implements the method described above when executing the computer program.

In a fourth aspect, the present invention also provides a storage medium storing a computer program comprising program instructions which, when executed by a processor, implement the method as described above.

Compared with the prior art, the embodiment of the invention provides a point cloud registration-based 3D map construction method, a point cloud registration-based 3D map construction device, computer equipment and a storage medium, wherein a transformation matrix from a source point cloud to a target point cloud is obtained through an ICP (inductively coupled plasma) or NDT (non-dispersive Transmission test) algorithm, so that a 3D map is constructed, and the positioning accuracy is improved.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.

Fig. 1 is a main flow chart of a 3D map construction method based on point cloud registration according to an embodiment of the present invention;

fig. 2 is a sub-flowchart of a 3D map construction method based on point cloud registration according to an embodiment of the present invention;

fig. 3 is a sub-flowchart of a 3D map construction method based on point cloud registration according to an embodiment of the present invention;

FIG. 4 is a schematic diagram of a 3D map building apparatus based on point cloud registration according to an embodiment of the present invention;

FIG. 5 is a schematic diagram of another embodiment of a 3D mapping apparatus based on point cloud registration according to an embodiment of the present invention; and

FIG. 6 is a schematic block diagram of a computer device provided by an embodiment of the present invention.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

It will be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.

It is also to be understood that the terminology used in the description of the invention herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. As used in the specification of the present invention 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 be further understood that the term "and/or" as used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.

Referring to fig. 1, fig. 1 is a flowchart of a 3D map construction algorithm based on point cloud registration according to the present invention, and the 3D map construction method based on point cloud registration includes the following steps:

step S100, acquiring a frame point cloud of a current map, and taking the current frame point cloud as a source point cloud; the frame point cloud of the current map can be acquired through an influence unit such as a camera or a laser detector, and the frame point cloud is original information data constructed by the 3D map.

Step S200, setting the currently obtained source point cloud as a reference frame; and taking the frame point cloud information obtained for the first time as a basis for calculating a transformation matrix for the first time.

Step S300, acquiring a frame point cloud of a latest map, and taking the latest frame point cloud as a target point cloud; the target point cloud is also a data object when point cloud splicing is required to be subsequently performed in the same coordinate system, specifically, the angle of shooting or detecting the same target can be changed, so that the same target can be accurately described.

Step S400, calculating a transformation matrix for transforming from a source point cloud to a target point cloud; the transformation process from the source point cloud to the target point cloud is usually three-dimensional transformation, so that transformation matrices for transformation between the two are required to be calculated, including a direction matrix and a translation matrix, and an ICP algorithm and an NDT algorithm, as well as a modified ICP algorithm and a modified NDT algorithm, are usually adopted.

Step S500, converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix; and acquiring the frame point cloud information of other landforms on the map again, taking the acquired frame point cloud information for the first time as a reference, and performing transformation matrix operation on other point clouds to acquire corresponding target point clouds.

And S600, splicing all the target point clouds to construct a 3D map. And (3) target point cloud splicing, namely all the target point clouds obtained in the step (S500) are combined in the same coordinate system by taking the frame point cloud obtained for the first time as a basis reference, and finally a 3D map is formed.

Referring to fig. 2, fig. 2 is a sub-flowchart of the 3D map construction based on point cloud registration in this embodiment, which specifically includes icp (iterative Closest point) algorithm steps, also referred to as iterative Closest points, and an IPC algorithm, where the IPC algorithm includes the following steps:

step S411, point cloud preprocessing is carried out so as to filter and clean data;

step S412, point cloud registration is carried out, and a transformation matrix is solved to find the closest point;

step S413, weighting processing for adjusting the weight of the corresponding point;

step S414, unreasonable registration point clouds are removed to reduce noise influence;

in step S415, a LOSS function (i.e., LOSS function) is calculated, and an optimal change matrix is obtained according to the LOSS function.

Please refer to fig. 3, which is a subroutine diagram of a 3D map building method based on point cloud registration, and particularly is a flow chart of an NDT (normal Distribution transform) algorithm, i.e. a normal Distribution transform algorithm, where the NDT algorithm includes the following steps:

step S421, subdividing the space occupied by scanning into unit grids, and calculating PDF (probability density function) of each unit based on point cloud distribution in the unit grids; the ICP algorithm is often directed to environment invariance, but in reality there are often situations where the environment does not hear changes, and the NDT algorithm can solve these problems well. The main purpose of this step is to grid the reference point cloud and then calculate the multi-dimensional normal distribution function of each grid.

Step S422, transfer matrix transformation is carried out according to each point of the point cloud scanned by the PDF of each unit; the matrix change here is solved by newton's method.

Step S423, calculating a probability distribution function of the response according to the matrix transformation result; namely, according to the calculation result, the gradient matrix g and the Hessian matrix H in the Newton solution are updated, and a new step length is calculated.

Step S424, obtaining an optimal transformation matrix according to the probability distribution function. Specifically, whether the result converges or the iteration number is reached is mainly determined, if so, the optimal matrix is obtained, otherwise, the step S423 is continuously executed, and the solution is continuously performed until the preset condition is met.

Referring to fig. 4, fig. 4 is a schematic structural diagram of a point cloud registration-based 3D map building apparatus 100 according to the present invention, which includes:

a source point cloud obtaining unit 101, configured to obtain a frame point cloud of a current map, and use the current frame point cloud as a source point cloud;

a reference frame setting unit 102, configured to set a currently obtained source point cloud as a reference frame.

A target point cloud obtaining unit 103, configured to obtain a frame point cloud of a latest map, and use the latest frame point cloud as a target point cloud;

a transformation matrix calculation unit 104 for calculating a transformation matrix transformed from the source point cloud to the target point cloud;

the point cloud registration unit 105 is used for converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix;

and the map generating unit 106 is configured to splice all the target point clouds to construct a 3D map.

In one embodiment, the transformation matrix calculation unit 104 includes:

a preprocessing unit 1041 for point cloud preprocessing for filtering and cleaning data;

the point cloud registration unit 1042 is used for carrying out point cloud registration and solving a transformation matrix to find a closest point;

a weighting unit 1043, configured to perform weighting processing, where the weighting processing is used to adjust the weight of the corresponding point;

the rejecting unit 1044 is used for rejecting unreasonable registration point clouds to reduce noise influence;

and a function calculating unit 1045, configured to calculate a LOSS function, and obtain an optimal change matrix according to the LOSS function.

In another embodiment, referring to fig. 5, a 3D mapping apparatus based on cloud point registration includes:

a source point cloud obtaining unit 101, configured to obtain a frame point cloud of a current map, and use the current frame point cloud as a source point cloud;

a reference frame setting unit 102, configured to set a currently obtained source point cloud as a reference frame.

A target point cloud obtaining unit 103, configured to obtain a frame point cloud of a latest map, and use the latest frame point cloud as a target point cloud;

a transformation matrix calculation unit 104 for calculating a transformation matrix transformed from the source point cloud to the target point cloud;

the point cloud registration unit 105 is used for converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix;

and the map generating unit 106 is configured to splice all the target point clouds to construct a 3D map.

Wherein the transformation matrix calculation unit 104 includes:

a PDF calculation unit 141, configured to subdivide a space occupied by scanning into unit grids, and calculate a PDF of each unit based on point cloud distribution in the unit grids;

a transfer matrix transformation unit 142 for performing transfer matrix transformation according to each point of the point cloud scanned by the PDF of each unit;

a distribution function calculation unit 143 for calculating a probability distribution function of the response according to the matrix transformation result;

and an optimal matrix calculation unit 144, configured to obtain an optimal transformation matrix according to the probability distribution function.

Referring to fig. 6, the embodiment further provides a computer device, the computer device 500 includes a processor 502, a memory and a network interface 505 connected by a system bus 501, wherein the memory may include a nonvolatile storage medium 503 and an internal memory 504.

The network interface 505 is used for network communication with other devices. Those skilled in the art will appreciate that the configuration shown in fig. 4 is a block diagram of only a portion of the configuration associated with the present application and does not constitute a limitation of the computer device 500 to which the present application may be applied, and that a particular computer device 500 may include more or less components than those shown, or may combine certain components, or have a different arrangement of components.

The non-volatile storage medium 503 may store an operating system 5031 and a computer program 5032. The computer program 5032 comprises program instructions that, when executed, may cause the processor 502 to perform a method of 3D map construction based on point cloud registration.

The processor 502 is used to provide computing and control capabilities to support the operation of the overall computer device 500.

The internal memory 504 provides an environment for the operation of the computer program 5032 in the non-volatile storage medium 503, and when the computer program 5032 is executed by the processor 502, the processor 502 can perform the following steps:

step S100, acquiring a frame point cloud of a current map, and taking the current frame point cloud as a source point cloud; the frame point cloud of the current map can be acquired through an influence unit such as a camera or a laser detector, and the frame point cloud is original information data constructed by the 3D map.

Step S200, setting the currently obtained source point cloud as a reference frame; and taking the frame point cloud information obtained for the first time as a basis for calculating a transformation matrix for the first time.

Step S300, acquiring a frame point cloud of a latest map, and taking the latest frame point cloud as a target point cloud; the target point cloud is also a data object when point cloud splicing is required to be subsequently performed in the same coordinate system, specifically, the angle of shooting or detecting the same target can be changed, so that the same target can be accurately described.

Step S400, calculating a transformation matrix for transforming from a source point cloud to a target point cloud; the transformation process from the source point cloud to the target point cloud is usually three-dimensional transformation, so that transformation matrices for transformation between the two are required to be calculated, including a direction matrix and a translation matrix, and an ICP algorithm and an NDT algorithm, as well as a modified ICP algorithm and a modified NDT algorithm, are usually adopted.

Step S500, converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix; and acquiring the frame point cloud information of other landforms on the map again, taking the acquired frame point cloud information for the first time as a reference, and performing transformation matrix operation on other point clouds to acquire corresponding target point clouds.

And S600, splicing all the target point clouds to construct a 3D map. And (3) target point cloud splicing, namely all the target point clouds obtained in the step (S500) are combined in the same coordinate system by taking the frame point cloud obtained for the first time as a basis reference, and finally a 3D map is formed.

The present invention also provides a storage medium storing a computer program comprising program instructions which, when executed by a processor, may implement a method of 3D mapping based on point cloud registration as follows: step S100, acquiring a frame point cloud of a current map, and taking the current frame point cloud as a source point cloud; step S200, setting the currently obtained source point cloud as a reference frame; step S300, acquiring a frame point cloud of a latest map, and taking the latest frame point cloud as a target point cloud; step S400, calculating a transformation matrix for transforming from a source point cloud to a target point cloud; step S500, converting other frame point clouds serving as source point clouds on the map into target point clouds according to the transformation matrix; and S600, splicing all the target point clouds to construct a 3D map.

The storage medium may be a usb disk, a removable hard disk, a Read-Only Memory (ROM), a magnetic disk, or an optical disk, which can store various computer readable storage media.

Compared with the prior art, the embodiment of the invention provides a point cloud registration-based 3D map construction method, a point cloud registration-based 3D map construction device, computer equipment and a storage medium, wherein a transformation matrix from a source point cloud to a target point cloud is obtained through an ICP (inductively coupled plasma) or NDT (non-dispersive Transmission test) algorithm, so that a 3D map is constructed, and the positioning accuracy is improved.

Those of ordinary skill in the art will appreciate that the elements and algorithm steps of the examples described in connection with the embodiments disclosed herein may be embodied in electronic hardware, computer software, or combinations of both, and that the components and steps of the examples have been described in a functional general in the foregoing description for the purpose of illustrating clearly the interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.

In the embodiments provided in the present invention, it should be understood that the disclosed apparatus and method may be implemented in other ways. For example, the above-described apparatus embodiments are merely illustrative. For example, the division of each unit is only one logic function division, and there may be another division manner in actual implementation. For example, various elements or components may be combined or may be integrated into another system, or some features may be omitted, or not implemented.

The steps in the method of the embodiment of the invention can be sequentially adjusted, combined and deleted according to actual needs. The units in the device of the embodiment of the invention can be merged, divided and deleted according to actual needs. In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit.

The integrated unit, if implemented in the form of a software functional unit and sold or used as a stand-alone product, may be stored in a storage medium. Based on such understanding, the technical solution of the present invention essentially or partially contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a terminal, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention.

The above-mentioned embodiments are merely preferred examples of the present invention, and not intended to limit the present invention, and those skilled in the art can easily make various changes and modifications according to the main concept and spirit of the present invention, so that the protection scope of the present invention shall be subject to the protection scope of the claims.