Point cloud registration method, smart device, and storage medium

Abstract

A point cloud registration method, a smart device, and a storage medium. The method comprises: acquiring a source point cloud, a target point cloud, and an initial pose between the source point cloud and the target point cloud (S101); on the basis of the initial pose, converting the source point cloud to a coordinate system where the target point cloud is located (S102); constructing a cost function on the basis of the Gaussian distribution of the source point cloud and the Gaussian distribution of the target point cloud under the coordinate system where the target point cloud is located (S103); calculating a Jacobian matrix and a Hessenberg matrix on the basis of the cost function (S104); and performing optimization iteration on the cost function on the basis of the Jacobian matrix and the Hessenberg matrix to obtain a pose increment (S105).

Description

Point cloud registration method, intelligent device and storage medium

[0001] This application claims priority to Chinese patent application No. 202410158809.0, filed on February 4, 2024, with the invention name “Point cloud registration method, intelligent device and storage medium”. The entire contents of the above Chinese patent application are incorporated into this application by reference. Technical Field

[0002] The present application relates to the field of autonomous driving technology, and in particular to a point cloud registration method, an intelligent device, and a storage medium. Background Art

[0003] Point cloud registration is a computational method for solving the relative pose between two frames of point clouds. The relative pose between two frames of point clouds can be obtained based on the two matched frames of point cloud computing. According to the different forms of constraints in the calculation process, it can be divided into point-to-point registration, point-to-distribution (P2D) registration, and distribution-to-distribution (D2D) registration. Registration with different constraint forms has differences in pose solution accuracy and computing power. The distribution here is generally expressed as the Gaussian distribution of points within the resolution grid of the point cloud. Among them, the D2D method takes into account the constraint of local shape consistency between the two frames of point clouds, and the accuracy will be higher. Therefore, the industry usually uses D2D constraints to iteratively optimize the initial pose to improve the accuracy of point cloud registration.

[0004] However, the D2D constrained point cloud registration method used in the industry generally uses a third-party nonlinear optimization library (such as ceres developed by Google) for derivative calculations in order to avoid directly calculating the Jacobian matrix and the Heisenberg matrix. At the same time, a general nonlinear optimization library like ceres has some additional calculations for the sake of versatility. As a result, the computing power required to solve the problem using a third-party nonlinear optimization library such as ceres is generally relatively large.

[0005] It can be seen that the point cloud registration method in the existing technology has the defects of low accuracy or high computing power requirements. Summary of the Invention

[0006] In order to overcome the defects in the prior art, this application proposes a lightweight, high-precision point cloud registration solution.

[0007] In a first aspect, the present application provides a point cloud registration method, the method comprising:

[0008] Obtain the source point cloud, the target point cloud, and the initial pose between the source point cloud and the target point cloud;

[0009] Converting the source point cloud to the coordinate system of the target point cloud according to the initial pose;

[0010] Constructing a cost function based on the Gaussian distribution of the source point cloud and the Gaussian distribution of the target point cloud in the coordinate system of the target point cloud;

[0011] Calculating a Jacobian matrix and a Heisenberg matrix based on the cost function;

[0012] The cost function is optimized iteratively based on the Jacobian matrix and the Heisenberg matrix to obtain a pose increment.

[0013] The above-mentioned converting the source point cloud to the coordinate system of the target point cloud according to the initial relative pose specifically includes:

[0014] The source point cloud is grid-divided to obtain a plurality of source grids. The source grids are projected onto the coordinate system of the target point cloud according to the initial pose, and a target grid corresponding to the source grid is determined in the target point cloud.

[0015] The above-mentioned construction of the cost function based on the Gaussian distribution of the source point cloud and the Gaussian distribution of the target point cloud in the coordinate system of the target point cloud specifically includes:

[0016] Determining the covariance of the source grid and the covariance of the target grid according to the Gaussian distribution of the source grid and the Gaussian distribution of the target grid respectively;

[0017] Constructing a matching relationship function between the source grid and the target grid based on posture parameters; the posture parameters include a rotation matrix and a translation vector;

[0018] A cost function for posture is constructed according to the matching relationship function, the covariance of the source grid, and the covariance of the target grid.

[0019] The above-mentioned optimization iteration of the cost function based on the Jacobian matrix and the Heisenberg matrix to obtain the pose increment is specifically:

[0020] Based on the Jacobian matrix and the Heisenberg matrix, the cost function is iteratively solved using Newton's method until a convergence condition is met to obtain a pose increment.

[0021] Furthermore, the iterative solution process also includes: determining the pose covariance based on the Heisenberg matrix, the Jacobian matrix, and the pre-set matching grid point pair noise.

[0022] Furthermore, the iterative solution process also includes: when the convergence condition is not met, determining whether the pose increment is greater than a preset threshold; if so, updating the initial pose based on the pose increment, and re-matching the source point cloud and the target point cloud based on the updated initial pose; otherwise, recalculating the Heisenberg matrix and Jacobian matrix based on the pose increment, and performing the next iteration.

[0023] The above-mentioned recalculation of the Heisenberg matrix and the Jacobian matrix based on the pose increment specifically includes:

[0024] Update Gaussian distribution parameters based on the pose increment, wherein the Gaussian distribution parameters include mean and variance;

[0025] Recompute the Heisenberg and Jacobian matrices based on the updated Gaussian distribution parameters.

[0026] The above method also includes: judging whether the Heisenberg matrix conforms to a positive definite matrix, and revising the Heisenberg matrix if the Heisenberg matrix does not conform to a positive definite matrix.

[0027] Furthermore, the step of determining whether the Heisenberg matrix conforms to a positive definite matrix and correcting the Heisenberg matrix when the Heisenberg matrix does not conform to a positive definite matrix specifically includes:

[0028] performing singular value decomposition on the Heisenberg matrix to obtain a plurality of singular values, selecting a minimum singular value from the plurality of singular values, and determining if the minimum singular value is less than a preset value, if so, the Heisenberg matrix is ​​a non-positive definite matrix, otherwise, the Heisenberg matrix is ​​a positive definite matrix;

[0029] When the Heisenberg matrix is ​​a non-positive definite matrix, each of the plurality of singular values ​​is increased by the preset value to obtain a plurality of modified singular values, and the Heisenberg matrix is ​​re-determined based on the plurality of modified singular values.

[0030] The above-mentioned source point cloud is specifically real-time point cloud data acquired based on lidar collection; the target point cloud is specifically the same real-time point cloud data form as the source point cloud, or is specifically in the form of an offline point cloud image sequence divided into multiple target grids.

[0031] The above initial position and posture are specifically obtained based on signals collected by a global satellite navigation system, or based on signals collected by one or more vehicle sensors.

[0032] In a second aspect, the present application provides an intelligent device comprising a processor and a storage device, wherein the storage device is suitable for storing multiple program codes, and the program codes are suitable for being loaded and run by the processor to execute the point cloud registration method described in any one of the technical solutions of the above-mentioned point cloud registration method.

[0033] In a third aspect, the present application provides a computer-readable storage medium storing a plurality of program codes, wherein the program codes are suitable for being loaded and run by a processor to execute the point cloud registration method described in any one of the technical solutions of the point cloud registration method.

[0034] The above one or more technical solutions of the present application have at least one or more of the following beneficial effects: in implementing the technical solutions of the present application, the source point cloud is first converted to the coordinate system of the target point cloud according to the initial pose, and then a cost function is constructed based on the Gaussian distribution of the source point cloud and the Gaussian distribution of the target point cloud in the coordinate system of the target point cloud, and the Jacobian matrix and the Heisenberg matrix are calculated based on the cost function; the cost function is optimized and iterated based on the Jacobian matrix and the Heisenberg matrix to obtain the pose increment. The present application uses the Jacobian matrix and the Heisenberg matrix to implement the entire nonlinear optimization iterative process based on the Gaussian distribution of the source point cloud and the Gaussian distribution of the target point cloud, which greatly reduces the computing power. In addition, the point cloud registration method of the present application takes into account the noise of the matching grid point pairs in the pose covariance calculation, which can more accurately describe the uncertainty of the pose calculation results. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] The disclosure of this application will be more easily understood with reference to the accompanying drawings. Those skilled in the art will readily appreciate that these drawings are for illustrative purposes only and are not intended to limit the scope of protection of this application. Furthermore, similar numbers in the figures represent similar components, where:

[0036] FIG1 is a schematic flow chart of main steps of a point cloud registration method according to an embodiment of the present application;

[0037] FIG2 is a flowchart showing a specific implementation of step S103 in FIG1 ;

[0038] FIG3 is a flowchart of specific implementation steps of a point cloud registration method according to an embodiment of the present application;

[0039] FIG4 is a schematic block diagram of a point cloud registration device according to an embodiment of the present application;

[0040] FIG5 is a schematic diagram of the effect achieved by the point cloud registration method according to an embodiment of the present application. DETAILED DESCRIPTION

[0041] Some embodiments of the present application are described below with reference to the accompanying drawings. Those skilled in the art should understand that these embodiments are only used to explain the technical principles of the present application and are not intended to limit the scope of protection of the present application.

[0042] In the description of this application, "module" and "processor" may include hardware, software, or a combination of both. A module may include hardware circuitry, various suitable sensors, communication ports, and memory. It may also include software components, such as program code, or a combination of software and hardware. A processor may be a central processing unit, a microprocessor, an image processor, a digital signal processor, or any other suitable processor. A processor has data and / or signal processing capabilities. A processor may be implemented in software, hardware, or a combination of both. Non-transitory computer-readable storage media include any suitable medium capable of storing program code, such as magnetic disks, hard disks, optical disks, flash memory, read-only memory, random access memory, etc. The term "A and / or B" refers to all possible combinations of A and B, such as only A, only B, or both A and B. The terms "at least one of A or B" or "at least one of A and B" have similar meanings to "A and / or B" and may include only A, only B, or both A and B. The singular forms "one" and "the" may also include the plural forms.

[0043] The relevant user personal information that may be involved in the various embodiments of this application is strictly in accordance with the requirements of laws and regulations, following the principles of legality, legitimacy and necessity, and based on the reasonable purposes of business scenarios, to process the personal information that users actively provide during the use of products / services or generated due to the use of products / services, as well as the personal information obtained with the user's authorization.

[0044] The user personal information processed in this application will vary depending on the specific product / service scenario and will be based on the specific scenario in which the user uses the product / service. This may involve the user's account information, device information, driving information, vehicle information, or other related information. The applicant will treat the user's personal information and its processing with a high degree of diligence.

[0045] This application attaches great importance to the security of user personal information and has taken reasonable and feasible security protection measures that comply with industry standards to protect user information and prevent personal information from being accessed, disclosed, used, modified, damaged or lost without authorization.

[0046] The following first introduces the application scenarios of the point cloud registration method provided in the embodiment of the present application.

[0047] The point cloud registration algorithm provided in this embodiment is implemented using the Normal Distribution Transform (NDT) framework. The NDT algorithm is a point cloud registration method based on a normal distribution. Its principle is to convert the point cloud into a Gaussian distribution function and then perform point cloud registration by calculating the matches between different Gaussian distribution functions. For example, if the input is two matched point cloud frames, the output is the pose between the two frames.

[0048] The NDT algorithm is an algorithm for matching point clouds in three-dimensional space. It can be applied to both the construction and localization of 3D scenes using point cloud data. During the 3D scene construction phase, the NDT algorithm's key principle is to first segment the point cloud data into multiple small cubes, calculate the Gaussian distribution parameters for each small cube, and then construct the 3D scene based on these Gaussian distribution parameters. During the localization phase, the NDT algorithm uses a Gaussian distribution to describe the distribution characteristics of the point cloud and finds the optimal registration between point clouds by maximizing a similarity metric.

[0049] The basic idea of ​​the NDT algorithm is to first determine the initial pose of the source point cloud and the target point cloud in the initial state, and then perform point cloud registration based on the initial pose and the iterative algorithm. However, this method is more dependent on the initial pose. If there is a large error in the initial relative pose, there will be a problem of low point cloud registration accuracy. For this reason, the industry usually adopts constraints in the form of distribution to distribution (D2D), that is, iteratively optimize the initial relative pose based on the constraints of local shape consistency between the two frames of point clouds, so as to improve the accuracy of point cloud registration. However, the NDT algorithm based on D2D constraints used in the industry generally uses a general nonlinear optimization library such as ceres for automatic differentiation, which involves some additional calculations, so the computing power for solving the problem is generally relatively large.

[0050] Therefore, this application implements a D2D pose solution method that does not rely on third-party nonlinear optimization libraries. It explicitly provides the basic matrices of D2D constraints, namely the Jacobi matrix and the Hessian matrix, and directly implements the entire nonlinear optimization process using basic matrix operations, significantly reducing computing power. At the same time, a method for calculating the pose covariance matrix that takes into account matching point pair noise is provided, which more accurately describes the uncertainty of the pose calculation results.

[0051] The following describes in detail an embodiment of the point cloud registration method provided in this application with reference to the accompanying drawings.

[0052] Referring to FIG1 , FIG1 is a schematic flow chart of the main steps of a point cloud registration method according to an embodiment of the present application. As shown in FIG1 , the point cloud registration method in the embodiment of the present application mainly includes the following steps S101 to S104.

[0053] Step S101: Acquire a source point cloud, a target point cloud, and an initial pose between the source point cloud and the target point cloud;

[0054] Specifically, the source point cloud and the target point cloud are point cloud pairs to be matched. The source point cloud is real-time point cloud data acquired through lidar. The target point cloud is in the same real-time point cloud data format as the source point cloud, or in the form of an offline point cloud image sequence divided into multiple target grids. The initial pose is obtained based on signals collected by a global satellite navigation system or one or more vehicle sensors. In practical applications, the initial pose can also be a custom initial value.

[0055] One implementation method of this step is to obtain real-time point cloud data collected by a lidar as the source point cloud, obtain an offline point cloud image sequence composed of multiple target grids as the target point cloud, and obtain the relative initial pose between the source point cloud and the target point cloud based on the signal collected by the global satellite navigation system.

[0056] Step S102: transforming the source point cloud into the coordinate system of the target point cloud according to the initial pose;

[0057] Specifically, the source point cloud is point cloud data acquired based on LiDAR. The coordinate system of the source point cloud is the LiDAR coordinate system, while the coordinate system of the target point cloud is the world coordinate system. The initial pose includes a rotation matrix and a translation vector. Based on the initial pose, the source point cloud can be transformed to the coordinate system of the target point cloud through coordinate system transformation.

[0058] One implementation of this step involves gridding the source point cloud to obtain multiple source grids, projecting the source grids onto the target point cloud's coordinate system based on the initial pose, and determining target grids corresponding to the source grids in the target point cloud. Specifically, the corresponding target grid is determined by taking the minimum matching value based on the projected distance between the source grid and the target grid, forming a matching grid point pair between the source grid and the target grid.

[0059] Step S103: constructing a cost function based on the Gaussian distribution of the source point cloud and the Gaussian distribution of the target point cloud in the coordinate system of the target point cloud;

[0060] Specifically, in this embodiment, it is assumed that the source grid of the source point cloud and the target grid of the target point cloud in the coordinate system where the target point cloud is located are normally distributed, then the Gaussian distribution parameters of the source grid and the Gaussian distribution parameters of the target grid can be determined, and the Gaussian distribution parameters include mean and variance. According to the Gaussian distribution parameters, the expression of the Gaussian distribution, that is, the probability density function, can be determined, that is, the Gaussian distribution of the source grid and the Gaussian distribution of the target grid are obtained.

[0061] The embodiment of the present application implements the Gaussian distribution calculation of point cloud registration using the source grid and the target grid as matching grid point pairs, which greatly reduces the computing power compared to the Gaussian distribution calculation using the points in the grid as matching point pairs.

[0062] An implementation of this step is shown in FIG2 , and includes the following steps S1031 to S1033 .

[0063] Step S1031: determining the covariance of the source grid and the covariance of the target grid according to the Gaussian distribution of the source grid and the Gaussian distribution of the target grid respectively;

[0064] Specifically, the covariance of the source grid can be calculated based on the Gaussian distribution parameters of the source grid, which is denoted as ∑ i , the covariance of the target grid is calculated according to the Gaussian distribution of the target grid, denoted as ∑ j .

[0065] Step S1032: constructing a matching relationship function between the source grid and the target grid based on posture parameters; the posture parameters include a rotation matrix and a translation vector;

[0066] Specifically, the vector form of the posture parameter is represented by p, the rotation matrix is ​​represented by R, the translation adjacent is represented by t, and the matching relationship function constructed based on the posture parameter in this embodiment is represented by μ ij (p)=Rμ i +t-μ i ; where i represents the source grid, j represents the target grid, and μ represents the mean.

[0067] Step S1033: constructing a cost function for posture according to the matching relationship function, the covariance of the source grid, and the covariance of the target grid.

[0068] Specifically, the constructed cost function for posture is expressed as follows:

[0069] where n M Indicates the number of source grids, n F Indicates the number of target grids, d1 and d2 are constants, ∑ i is the covariance of the source grid, ∑ j is the covariance of the target grid, μ ij is the matching relationship function.

[0070] Step S104: Calculating the Jacobian matrix and the Heisenberg matrix based on the cost function;

[0071] Specifically, based on obtaining a gradient vector by deriving the cost function, a Jacobian matrix and a Heisenberg matrix can be calculated based on the gradient vector.

[0072] Step S105: Optimizing the cost function iteratively based on the Jacobian matrix and the Heisenberg matrix to obtain a pose increment.

[0073] One implementation of this step is to iteratively solve the cost function based on the Jacobian matrix and the Heisenberg matrix using Newton's method until a convergence condition is satisfied, thereby obtaining a pose increment. For example, the convergence condition is that the number of iterations reaches a preset number, which can be set to be equal to the number of source grids.

[0074] The iterative solution process also includes: determining the pose covariance based on the Heisenberg matrix, the Jacobian matrix, and the pre-set matching grid point pair noise. Accordingly, when the convergence condition is met, the pose increment and the pose covariance can be obtained simultaneously. The pose covariance calculation method provided in the embodiment of the present application takes into account the influence of the matching grid point pair noise and more accurately describes the uncertainty of the pose calculation result.

[0075] The pose covariance It can be expressed as follows:

[0076] Among them, p is the vector representation of the pose parameters, z is the pre-set matching grid point pair noise, is the Heisenberg matrix, is the Jacobian matrix, is the derivative of the Jacobian matrix with respect to z; in the embodiment of the present application, assuming σ s represents the noise of the matching grid point pair in a certain dimension, and σ s is a constant, select I 3×3 is a random matrix vector.

[0077] The above steps S101 to S105 are described in detail below with reference to FIG. 3 .

[0078] As shown in FIG3 , a point cloud registration method provided in this embodiment includes the following steps 201 to 211 .

[0079] Step 201: inputting a source point cloud, a target point cloud, and the initial poses of the source point cloud and the target point cloud;

[0080] Specifically, the source point cloud is real-time point cloud data collected by the lidar; the target point cloud is an offline point cloud image sequence composed of multiple target grids; the input initial pose is obtained by fusing data collected by the vehicle's wheel speed sensor and inertial sensor (IMU).

[0081] Step 202: Based on the initial pose, the source point cloud is converted to the coordinate system of the target point cloud, and the Gaussian distribution of the source grid of the source point cloud and the target grid of the matching target point cloud are calculated respectively, and a cost function is constructed according to the Gaussian distribution;

[0082] Specifically, the coordinate system of the target point cloud is the world coordinate system, the source point cloud is grid-divided to obtain multiple source grids, the source point cloud is converted to the world coordinate system based on the initial pose, and the target grid matching the source grid is determined in the target point cloud. The source grid and the target grid form a matching grid point pair, and the source point cloud and the target point cloud are converted into NDT form, that is, the Gaussian distribution of the source grid and the Gaussian distribution of the target grid are distributed and calculated.

[0083] It can be understood that the cost function can be constructed based on the probability density function of the Gaussian distribution.

[0084] Step 203: Calculate the Jacobian matrix and the Heisenberg matrix;

[0085] Specifically, the Jacobian matrix and the Heisenberg matrix can be calculated in the process of iteratively solving the linear equation function based on the cost function.

[0086] Step 204: Determine whether the Heisenberg matrix satisfies a positive definite matrix. If yes, execute step 206; otherwise, execute step 205.

[0087] Specifically, a singular value decomposition (SVD) is performed on the Heisenberg matrix to obtain multiple singular values, a minimum singular value is selected from the multiple singular values, and it is determined whether the minimum singular value is less than 1e-10. If so, the Heisenberg matrix is ​​a non-positive definite matrix, otherwise, the Heisenberg matrix is ​​a positive definite matrix.

[0088] Step 205: Modifying the Heisenberg matrix;

[0089] Specifically, each of the plurality of singular values ​​is increased by the preset value to obtain a plurality of modified singular values, and the Heisenberg matrix is ​​re-determined based on the plurality of modified singular values.

[0090] Step 206: Based on the Jacobian matrix and the Heisenberg matrix, iteratively solve the cost function using Newton's method;

[0091] Step 207: Determine whether the convergence condition is met, if yes, execute step 211, otherwise execute step 208;

[0092] For example, the convergence condition is set to that the number of iterations reaches a preset number.

[0093] Step 208: Determine whether the pose increment obtained by the current solution is greater than a preset threshold. If yes, execute step 209; otherwise, execute step 210.

[0094] Step 209: updating the initial pose based on the pose increment, and then returning to step 202;

[0095] Step 210: Update the intermediate variables of each iteration, recalculate the Heisenberg matrix and the Jacobian matrix based on the pose increment, and then return to step 204;

[0096] Specifically, the intermediate variable refers to a matrix variable that changes every iteration during the multiple iterative calculation process. For example, the number of iterations is recorded as k. n , then the first iteration is recorded as k0, and the initial value of the intermediate variable is a pre-set value, recorded as the prior variance After one iteration, the prior variance becomes the posterior validation variance. For example, after the iteration, the posterior variance can be obtained.

[0097] The updating of the intermediate variables in this embodiment, i.e., changing the prior variance to the posterior variance, is achieved using the following formula:

[0098] in, is the prior variance, H represents the Heisenberg matrix, z represents the pre-set matching grid point noise, and H pp is a symmetric matrix whose inverse of the transpose is equal to its own inverse;

[0099] Step 211: Output the pose increment and pose covariance.

[0100] Specifically, the output pose increment is recorded as Δp, which can be obtained according to the Heisenberg matrix H and the Jacobian matrix J: HΔp = -J T .

[0101] In this embodiment, the point cloud registration method also outputs a description of the uncertainty of the pose calculation, namely the pose covariance, to ensure that the downstream user can judge the credibility of the current result. This uncertainty is generally expressed using a 6x6 covariance matrix. The description method commonly used in prior art is to take the inverse of the Heisenberg matrix calculated in the point cloud registration process as the covariance matrix, but the disadvantage of this method is that it only considers the numerical error of the point cloud registration equation, and does not consider the uncertainty caused by the matching point cloud error due to the noise of the matching point pairs. Therefore, the description accuracy provided by this method is low. The point cloud registration method of the present application takes into account the noise of the matching point pairs, namely the matching grid point pair noise z in the above calculation process. Therefore, the point cloud registration method of the embodiment of the present application can accurately describe the uncertainty of the pose calculation.

[0102] As shown in Figure 5, it is a schematic diagram of the effect that can be achieved by performing point cloud registration using the point cloud registration method provided in an embodiment of the present application. As shown in the figure, the light-colored image part is used to represent the triple result of the covariance, and the dark-colored image part is used to represent the posture error, that is, the difference between the posture result obtained by the method of the present application and the true value used to evaluate the accuracy. It can be seen from the changing relationship between the dark and light parts in the figure that the method provided in an embodiment of the present application can achieve a more accurate description of the posture error.

[0103] Furthermore, the present application also provides a point cloud registration device.

[0104] 4 , which is a block diagram of the main structure of a point cloud configuration device according to an embodiment of the present application, includes an input module 301 , an iterative calculation module 302 , and an output module 303 . The functions of each module are as follows:

[0105] The input and matching module 301 is configured to receive the source point cloud, the target point cloud, and the initial position between the source point cloud and the target point cloud, and perform point cloud matching based on the initial position. In this embodiment, the input module 301 specifically includes an acquisition module 3011 and a conversion module 3012, wherein:

[0106] The acquisition module 3011 is specifically used to acquire the source point cloud, the target point cloud, and the initial position between the source point cloud and the target point cloud;

[0107] The conversion module 3012 is specifically configured to convert the source point cloud into the coordinate system of the target point cloud according to the initial pose.

[0108] The iterative calculation module 302 is configured to perform iterative calculations based on the information obtained by the input module 301 to obtain a posture increment. In this embodiment, the iterative calculation module 302 specifically includes a first calculation module 3021 and a second calculation module 3022 .

[0109] The first calculation module 3021 is specifically configured to construct a cost function based on the Gaussian distribution of the source point cloud and the Gaussian distribution of the target point cloud in the coordinate system of the target point cloud;

[0110] The second calculation module 3022 is specifically used to calculate the Jacobian matrix and the Heisenberg matrix based on the cost function, and iteratively optimize the cost function based on the Jacobian matrix and the Heisenberg matrix to obtain the pose increment.

[0111] The output module 303 is configured to output the posture increment calculated by the iterative calculation module.

[0112] For ease of explanation, the introduction to the above-mentioned point cloud registration device only shows the part related to the embodiment of the present application. For specific technical details not disclosed, please refer to the method part of the embodiment of the present application.

[0113] It will be understood by those skilled in the art that all or part of the processes in the method for implementing the above embodiment of the present application can also be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, it can implement the steps of the above-mentioned various method embodiments. The computer program includes computer program code, which can be in source code form, object code form, executable file or some intermediate form. The computer-readable storage medium may include: any entity or device, medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory, random access memory, electric carrier signal, telecommunication signal and software distribution medium, etc. that can carry the computer program code.

[0114] Furthermore, the present application provides an intelligent device, which may include at least one processor and a memory in communication with the at least one processor. The memory stores a computer program, which, when executed by the at least one processor, implements the method described in any of the above embodiments. The intelligent device described in the present application may include a driving device, a smart car, a robot, or other devices.

[0115] In some embodiments of the present application, the smart device further includes at least one sensor configured to sense information. The sensor is communicatively coupled to any of the processors described herein. Furthermore, the smart device further includes an autonomous driving system configured to guide the smart device to autonomously drive or provide assisted driving. The processor communicates with the sensor and / or autonomous driving system to perform the method described in any of the above embodiments.

[0116] In the embodiment of the present application, the smart device may be a control device formed by various electronic devices. In some possible implementations, the smart device may include multiple storage devices and multiple processors. The program for executing the point cloud registration method of the above method embodiment can be divided into multiple subroutines, each of which can be loaded and run by the processor to execute different steps of the point cloud registration method of the above method embodiment. Specifically, each subroutine can be stored in a different storage device, and each processor can be configured to execute the program in one or more storage devices to jointly implement the point cloud registration method of the above method embodiment.

[0117] The multiple processors may be processors deployed on the same device. For example, the smart device may be a high-performance device composed of multiple processors, and the multiple processors may be processors configured on the high-performance device. Furthermore, the multiple processors may be processors deployed on different devices. For example, the smart device may be a server cluster, and the multiple processors may be processors on different servers in the server cluster.

[0118] Furthermore, the present application also provides a computer-readable storage medium.

[0119] In one embodiment of a computer-readable storage medium according to the present application, the computer-readable storage medium can be configured to store a program for executing the point cloud registration method of the above-described method embodiment. The program can be loaded and executed by a processor to implement the above-described point cloud registration method. For ease of explanation, only the portion related to the embodiment of the present application is shown. For specific technical details not disclosed, please refer to the method section of the embodiment of the present application. The computer-readable storage medium can be a storage device formed by various electronic devices. In the embodiment of the present application, the computer-readable storage medium is a non-transitory computer-readable storage medium.

[0120] Thus far, the technical solutions of the present application have been described in conjunction with the specific embodiments shown in the accompanying drawings. However, it is readily understood by those skilled in the art that the scope of protection of the present application is obviously not limited to these specific embodiments. Without departing from the principles of the present application, those skilled in the art may make equivalent changes or substitutions to the relevant technical features, and the technical solutions after such changes or substitutions will fall within the scope of protection of the present application.

Claims

1. A point cloud registration method, characterized in that: The method comprises: Obtain the source point cloud, the target point cloud, and the initial pose between the source point cloud and the target point cloud; Converting the source point cloud to the coordinate system of the target point cloud according to the initial pose; Constructing a cost function based on the Gaussian distribution of the source point cloud and the Gaussian distribution of the target point cloud in the coordinate system of the target point cloud; Calculating a Jacobian matrix and a Heisenberg matrix based on the cost function; The cost function is optimized iteratively based on the Jacobian matrix and the Heisenberg matrix to obtain a pose increment.

2. The method according to claim 1, characterized in that The converting the source point cloud to the coordinate system of the target point cloud according to the initial relative pose specifically includes: The source point cloud is grid-divided to obtain a plurality of source grids. The source grids are projected onto the coordinate system of the target point cloud according to the initial pose, and a target grid corresponding to the source grid is determined in the target point cloud.

3. The method according to claim 2, wherein: The constructing of the cost function based on the Gaussian distribution of the source point cloud and the Gaussian distribution of the target point cloud in the coordinate system of the target point cloud specifically includes: Determining the covariance of the source grid and the covariance of the target grid according to the Gaussian distribution of the source grid and the Gaussian distribution of the target grid respectively; Constructing a matching relationship function between the source grid and the target grid based on posture parameters; the posture parameters include a rotation matrix and a translation vector; A cost function for posture is constructed according to the matching relationship function, the covariance of the source grid, and the covariance of the target grid.

4. The method according to claim 1, wherein: The optimization iteration of the cost function based on the Jacobian matrix and the Heisenberg matrix to obtain the pose increment is specifically as follows: Based on the Jacobian matrix and the Heisenberg matrix, the cost function is iteratively solved using Newton's method until a convergence condition is met to obtain a pose increment.

5. The method according to claim 4, characterized in that: The iterative solution process also includes: determining the pose covariance based on the Heisenberg matrix, the Jacobian matrix, and the preset matching grid point pair noise.

6. The method according to claim 4 or 5, characterized in that The iterative solution process also includes: When the convergence condition is not met, determining whether the posture increment is greater than a preset threshold; If yes, updating the initial pose based on the pose increment, and re-matching the source point cloud and the target point cloud based on the updated initial pose; Otherwise, the Heisenberg matrix and Jacobian matrix are recalculated based on the pose increment and the next iteration is performed.

7. The method according to claim 6, characterized in that The recalculation of the Heisenberg matrix and the Jacobian matrix based on the pose increment specifically includes: Update Gaussian distribution parameters based on the pose increment, wherein the Gaussian distribution parameters include mean and variance; Recompute the Heisenberg and Jacobian matrices based on the updated Gaussian distribution parameters.

8. The method according to claim 1, characterized in that The method further includes the steps of determining whether the Heisenberg matrix conforms to a positive definite matrix, and revising the Heisenberg matrix if the Heisenberg matrix does not conform to a positive definite matrix.

9. The method according to claim 8, characterized in that The step of determining whether the Heisenberg matrix conforms to a positive definite matrix and correcting the Heisenberg matrix if the Heisenberg matrix does not conform to a positive definite matrix specifically includes: performing singular value decomposition on the Heisenberg matrix to obtain a plurality of singular values, selecting a minimum singular value from the plurality of singular values, and determining if the minimum singular value is less than a preset value, if so, the Heisenberg matrix is a non-positive definite matrix, otherwise, the Heisenberg matrix is a positive definite matrix; When the Heisenberg matrix is a non-positive definite matrix, each of the plurality of singular values is increased by the preset value to obtain a plurality of modified singular values, and the Heisenberg matrix is re-determined based on the plurality of modified singular values.

10. The method according to claim 1, wherein: The source point cloud is specifically real-time point cloud data acquired based on lidar collection; the target point cloud is specifically in the same real-time point cloud data form as the source point cloud, or is specifically in the form of an offline point cloud image sequence divided into multiple target grids.

11. The method according to claim 1, wherein: The initial position and posture is specifically obtained based on signals collected by a global satellite navigation system, or based on signals collected by one or more vehicle sensors.

12. A smart device, characterized in that: include: at least one processor; and, a memory communicatively coupled to the at least one processor; Wherein, a computer program is stored in the memory, and when the computer program is executed by the at least one processor, the point cloud registration method according to any one of claims 1 to 11 is implemented.

13. A computer-readable storage medium storing a plurality of program codes, characterized in that: The program code is suitable for being loaded and run by a processor to execute the point cloud registration method according to any one of claims 1 to 11.

Images