Method, apparatus, device and storage medium for point cloud registration

Abstract

The application relates to the technical field of automatic driving, and discloses a point cloud registration method, device, equipment and storage medium, which are used for improving the accuracy of point cloud registration. The point cloud registration method comprises the following steps: obtaining to-be-registered point cloud data, the to-be-registered point cloud data comprising target point cloud and source point cloud; performing pre-registration processing based on the target point cloud and the source point cloud to obtain a candidate translation rotation matrix, the candidate translation rotation matrix comprising a candidate rotation component and a candidate translation component; performing conversion processing on the candidate rotation component to obtain a target rotation component; and performing optimization processing based on the candidate translation component and the target rotation component to obtain a target rotation translation matrix.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a method, apparatus, device, and storage medium for point cloud registration. Background Technology

[0002] The high-precision maps required for autonomous driving are created from data collected by vehicles equipped with data acquisition devices as they drive on roads. LiDAR is a commonly used data acquisition device. During the high-precision map creation process, the point cloud data collected by the vehicle needs to be transformed from the LiDAR measurement coordinate system to the vehicle's body coordinate system using a rotation and translation matrix. This rotation and translation matrix is ​​obtained through point cloud registration.

[0003] Currently, the Normal Distribution Transform (NDT) algorithm is commonly used for point cloud data registration. It is applied to the statistical model of 3D points and uses standard optimization methods to determine the optimal match between two frames of point cloud data.

[0004] However, due to the enormous number of points in a real point cloud, each point loses information about its distribution. Furthermore, using Euler angles to represent the rotation matrix in the rotation and translation matrix can lead to gimbal lock, resulting in low accuracy of point cloud registration. Summary of the Invention

[0005] This invention provides a method, apparatus, device, and storage medium for point cloud registration, which can improve the accuracy of point cloud registration.

[0006] The first aspect of the present invention provides a method for point cloud registration, comprising: acquiring point cloud data to be registered, the point cloud data including a target point cloud and a source point cloud; performing pre-registration processing based on the target point cloud and the source point cloud to obtain a candidate translation-rotation matrix, the candidate translation-rotation matrix including a candidate rotation component and a candidate translation component; performing transformation processing on the candidate rotation component to obtain a target rotation component; and performing optimization processing based on the candidate translation component and the target rotation component to obtain a target rotation-translation matrix.

[0007] In one feasible implementation, the pre-registration process based on the target point cloud and the source point cloud to obtain a candidate translation and rotation matrix includes: converting the target point cloud to an initial 3D mesh of a preset resolution to obtain a target 3D mesh, each mesh in the target 3D mesh including multiple 3D points used to indicate feature points of the target point cloud; calculating the mean and covariance corresponding to each mesh based on the multiple 3D points of each mesh; obtaining the target 3D points in the target 3D mesh, as well as the target mean and target covariance of the target mesh, wherein the target 3D points are feature points of the source point cloud transformed into feature points of the target point cloud based on the initial rotation and translation matrix; obtaining the number of feature points of the source point cloud, and obtaining a candidate translation and rotation matrix based on the number of feature points, the target 3D points, the target mean, the target covariance, and the initial rotation and translation matrix.

[0008] In one feasible implementation, the step of transforming the candidate rotation component to obtain the target rotation component includes: performing a Lie group transformation on the candidate rotation component to obtain the corresponding Lie group function; and performing differentiation on the Lie group function to obtain the target rotation component.

[0009] In one feasible implementation, the step of obtaining the target rotation component by performing differentiation based on the Lie group function includes: constructing a function of the three-dimensional vector based on the Lie group function to obtain an initial three-dimensional function; performing differentiation based on the initial three-dimensional function to obtain a candidate three-dimensional function; and obtaining the target rotation component based on the candidate three-dimensional function and the three-dimensional vector.

[0010] In one feasible implementation, the optimization process based on the candidate translation components and the target rotation components to obtain the target rotation translation matrix includes: selecting an initial trust region radius based on the candidate translation components and the target rotation components; calculating an objective function based on the initial trust region radius to obtain a first value, wherein the objective function represents the rotation translation matrix to be optimized corresponding to the candidate translation components and the target rotation components; if a first difference between the first value and a preset minimum value is less than or equal to a first preset threshold, the initial trust region radius is increased in multiple levels, and the above steps are repeated until the objective function satisfies a preset condition, wherein the preset condition indicates that the first difference is less than or equal to a second preset threshold, and the first preset threshold is greater than the second preset threshold; if the objective function satisfies the preset condition, the rotation translation matrix to be optimized is determined as the target rotation translation matrix.

[0011] In one feasible implementation, after calculating the objective function based on the initial trust region radius to obtain a first value, the method further includes: if the first difference between the first value and the preset minimum value is greater than the first preset threshold, then the initial trust region radius is reduced in multiple levels; the above steps are repeated until the objective function satisfies the preset condition; if the objective function satisfies the preset condition, then the rotation and translation matrix to be optimized is determined as the target rotation and translation matrix.

[0012] In one feasible implementation, the step of transforming the candidate rotation components to obtain the target rotation component includes: performing quaternion transformation on the candidate rotation components to obtain corresponding scalar components and vector components; and performing matrix transformation on the scalar components and the vector components to obtain the target rotation component.

[0013] A second aspect of the present invention provides a point cloud registration apparatus, comprising: an acquisition module for acquiring point cloud data to be registered, the point cloud data including a target point cloud and a source point cloud; a pre-registration module for performing pre-registration processing based on the target point cloud and the source point cloud to obtain a candidate translation-rotation matrix, the candidate translation-rotation matrix including a candidate rotation component and a candidate translation component; a conversion module for performing conversion processing on the candidate rotation component to obtain a target rotation component; and an optimization module for performing optimization processing based on the candidate translation component and the target rotation component to obtain a target rotation-translation matrix.

[0014] In one feasible implementation, the pre-registration module is specifically used for: converting the target point cloud to an initial 3D mesh of a preset resolution to obtain a target 3D mesh, wherein each mesh in the target 3D mesh includes multiple 3D points, which are used to indicate feature points of the target point cloud; calculating the mean and covariance of each mesh based on the multiple 3D points of each mesh; obtaining the target 3D points in the target 3D mesh, as well as the target mean and target covariance of the target mesh, wherein the target 3D points are feature points of the source point cloud transformed into feature points of the target point cloud based on an initial rotation and translation matrix; obtaining the number of feature points of the source point cloud, and obtaining a candidate translation and rotation matrix based on the number of feature points, the target 3D points, the target mean, the target covariance, and the initial rotation and translation matrix.

[0015] In one feasible implementation, the conversion module includes: a conversion unit for performing Lie group transformation on the candidate rotation component to obtain the corresponding Lie group function; and a differentiation unit for performing differentiation based on the Lie group function to obtain the target rotation component.

[0016] In one feasible implementation, the differentiation unit is specifically used to: construct a functional expression of the three-dimensional vector based on the Lie group function to obtain an initial three-dimensional functional expression; perform differentiation processing based on the initial three-dimensional functional expression to obtain a candidate three-dimensional functional expression; and obtain the target rotation component based on the candidate three-dimensional functional expression and the three-dimensional vector.

[0017] In one feasible implementation, the optimization module includes: a selection unit, configured to select an initial trust region radius based on the candidate translation component and the target rotation component; a calculation unit, configured to calculate an objective function based on the initial trust region radius to obtain a first value, wherein the objective function represents the rotation-translation matrix to be optimized corresponding to the candidate translation component and the target rotation component; an addition unit, configured to increase the initial trust region radius in multiple levels if the first difference between the first value and a preset minimum value is less than or equal to a first preset threshold, and repeat the above steps until the objective function satisfies a preset condition, wherein the preset condition indicates that the first difference is less than or equal to a second preset threshold, and the first preset threshold is greater than the second preset threshold; and a first determination unit, configured to determine the rotation-translation matrix to be optimized as the target rotation-translation matrix if the objective function satisfies the preset condition.

[0018] In one feasible implementation, the optimization module further includes: a reduction unit, configured to reduce the initial trust region radius in multiple levels if the first difference between the first value and the preset minimum value is greater than the first preset threshold; an execution unit, configured to repeatedly execute the above steps until the objective function satisfies the preset condition; and a second determination unit, configured to determine the rotation and translation matrix to be optimized as the target rotation and translation matrix if the objective function satisfies the preset condition.

[0019] In one feasible implementation, the conversion module is specifically used to: perform quaternion conversion on the candidate rotation component to obtain the corresponding scalar component and vector component; and perform matrix conversion on the scalar component and the vector component to obtain the target rotation component.

[0020] A third aspect of the present invention provides a point cloud registration device, comprising: a memory and at least one processor, wherein the memory stores instructions; the at least one processor invokes the instructions in the memory to cause the point cloud registration device to perform the point cloud registration method described above.

[0021] A fourth aspect of the present invention provides a computer-readable storage medium storing instructions that, when executed on a computer, cause the computer to perform the point cloud registration method described above.

[0022] The technical solution provided by this invention involves acquiring point cloud data to be registered, including a target point cloud and a source point cloud; performing pre-registration processing based on the target and source point clouds to obtain a candidate translation-rotation matrix, which includes a candidate rotation component and a candidate translation component; transforming the candidate rotation component to obtain a target rotation component; and performing optimization processing based on the candidate translation component and the target rotation component to obtain the target rotation-translation matrix. In this embodiment, by performing pre-registration processing on the target and source point clouds to obtain a candidate translation-rotation matrix, and then transforming the candidate rotation components in the candidate translation-rotation matrix to obtain the target rotation component, and finally performing optimization processing based on the candidate translation component and the target rotation component to obtain the target rotation-translation matrix, the invention avoids the need to use Euler angles to represent the rotation matrix in the rotation-translation matrix, thus avoiding the gimbal lock problem and improving the accuracy of point cloud registration. Attached Figure Description

[0023] Figure 1 This is a schematic diagram of one embodiment of the point cloud registration method in this invention;

[0024] Figure 2 This is a schematic diagram of another embodiment of the point cloud registration method in this invention;

[0025] Figure 3 This is a schematic diagram of another embodiment of the point cloud registration method in this invention;

[0026] Figure 4 This is a schematic diagram of one embodiment of the point cloud registration device in this invention;

[0027] Figure 5 This is a schematic diagram of another embodiment of the point cloud registration device in this invention;

[0028] Figure 6 This is a schematic diagram of one embodiment of the point cloud registration device in this invention. Detailed Implementation

[0029] This invention provides a method, apparatus, device, and storage medium for point cloud registration, which can improve the accuracy of point cloud registration.

[0030] The terms “first,” “second,” “third,” “fourth,” etc. (if present) in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms “comprising” or “having,” and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0031] For ease of understanding, the specific process of the embodiments of the present invention is described below. Please refer to [link / reference]. Figure 1 One embodiment of the point cloud registration method in this invention includes:

[0032] 101. Obtain the point cloud data to be registered, which includes the target point cloud and the source point cloud;

[0033] It is understood that the execution subject of this invention can be a point cloud registration device or a terminal, and no specific limitation is made here. This embodiment of the invention will be described using a terminal as an example.

[0034] Point cloud registration refers to taking two point cloud frames as input: source point cloud Ps and target point cloud Pt, and outputting a rotation and translation matrix T to improve the overlap between T(Ps) and target point cloud Pt.

[0035] In the process of creating high-precision maps, the point cloud data collected by the vehicle needs to be transformed from the lidar measurement coordinate system to the vehicle's body coordinate system based on the rotation and translation matrix T, thereby generating a high-precision map.

[0036] In this embodiment, the terminal acquires point cloud data to be registered, which includes a target point cloud and a source point cloud, and then performs further point cloud registration based on the target point cloud and the source point cloud.

[0037] 102. Based on the target point cloud and the source point cloud, perform pre-registration processing to obtain the candidate translation and rotation matrix, which includes the candidate rotation component and the candidate translation component;

[0038] This embodiment uses the Normal Distribution Transform (NDT) algorithm for point cloud registration.

[0039] In this embodiment, the terminal performs pre-registration processing based on the target point cloud and the source point cloud to obtain a candidate translation and rotation matrix, which includes a candidate rotation component and a candidate translation component.

[0040] 103. Transform the candidate rotation components to obtain the target rotation component;

[0041] This embodiment uses a Lie group to transform the candidate rotation component. A Lie group is a continuous group, which is a smooth manifold with a group structure. For example, if the candidate rotation component is R, the terminal performs a Lie group transformation on R to obtain the target rotation component: R(t)∈R. 3*3 The target rotation component is used to represent that R belongs to a 3*3 rotation matrix and changes continuously with time t.

[0042] Among them, the Lie algebra is the tangent space of the Lie group at the identity element ε, a linear vector space, which describes the local properties of the Lie group.

[0043] 104. Optimize the candidate translation components and the target rotation components to obtain the target rotation and translation matrix.

[0044] It is understandable that the candidate translation component and the target rotation component are combined to form the rotation and translation matrix to be optimized.

[0045] This embodiment uses the trust region algorithm to optimize the rotation and translation matrix to be optimized, thereby obtaining the target rotation and translation matrix. The trust region algorithm is a numerical algorithm for solving nonlinear optimization problems. The trust region algorithm is an iterative algorithm, that is, starting from a given initial solution, it iterates step by step to continuously improve until a satisfactory approximate optimal solution is obtained, that is, the target rotation and translation matrix is ​​obtained.

[0046] In this embodiment of the invention, a candidate translation and rotation matrix is ​​obtained by pre-registering the target point cloud and the source point cloud. Then, the candidate rotation components in the candidate translation and rotation matrix are transformed to obtain the target rotation component. Based on the candidate translation component and the target rotation component, the target rotation and translation matrix is ​​obtained. It is not necessary to use Euler angles to represent the rotation matrix in the rotation and translation matrix, thus avoiding the gimbal lock problem and improving the accuracy of point cloud registration.

[0047] Please see Figure 2 Another embodiment of the point cloud registration method in this invention includes:

[0048] 201. Obtain the point cloud data to be registered, which includes the target point cloud and the source point cloud;

[0049] The execution process of step 201 is the same as that of step 101, and will not be described again here.

[0050] 202. Based on the target point cloud and the source point cloud, perform pre-registration processing to obtain the candidate translation and rotation matrix, which includes the candidate rotation component and the candidate translation component;

[0051] This embodiment uses the Normal Distribution Transform (NDT) algorithm for point cloud registration.

[0052] In this embodiment, the terminal performs pre-registration processing based on the target point cloud and the source point cloud to obtain a candidate translation and rotation matrix, which includes a candidate rotation component and a candidate translation component.

[0053] Understandably, the candidate translation / rotation matrix is ​​a 4x4 matrix, where the candidate rotation component is a 3x3 matrix R and the candidate translation component is a 3x1 matrix t. 3*1 The candidate translation and rotation matrix is:

[0054] 203. Perform Lie group transformation on the candidate rotation components to obtain the corresponding Lie group function;

[0055] For example, if the candidate rotation component is R, the terminal performs a Lie group transformation on the candidate rotation component R, resulting in the corresponding Lie group function: SO = {R ∈ R}. 3*3 |RR T =I, det(R)=1}, where R∈R 3*3 Used to indicate that R is a 3x3 rotation matrix, R T R is used to represent the transpose of R, and I is used to represent the identity matrix, i.e. det(R) is used to denote the determinant of R, that is, when the determinant of R is equal to 1, the Lie group function corresponding to the candidate translation and rotation matrix T is: in, The matrix T is used to indicate that the candidate translation / rotation matrix T belongs to a 4x4 matrix, and R∈SO is used to indicate that the candidate rotation component R belongs to the Lie group function corresponding to the candidate rotation component R, and T∈R 3 This is used to indicate that the candidate translation and rotation matrix T belongs to a three-dimensional vector.

[0056] 204. Based on the differentiation of the Lie group function, the rotation components of the target are obtained;

[0057] Specifically, (1) the terminal constructs the function of the three-dimensional vector based on the Lie group function to obtain the initial three-dimensional function; (2) the terminal performs differentiation based on the initial three-dimensional function to obtain the candidate three-dimensional function; (3) the terminal obtains the target rotation component based on the candidate three-dimensional function and the three-dimensional vector.

[0058] For example, based on the example in step 203, the time function of the candidate rotation component R that changes continuously with time is: R(t), therefore, RR T =I transforms into: R(t)R(t) T =I, the terminal differentiates with respect to time, and obtains: in, Used to represent the derivative of R(t), Used to represent R(t) T The derivative of the terminal After rearranging the terms, we get: Obviously, It is an antisymmetric matrix, characterized by elements on the main diagonal being 0, and elements symmetric about the main diagonal being opposites of each other. Therefore, for any antisymmetric matrix, there can always be a unique corresponding vector, where... It can be represented as: The "^" in a^ represents a three-dimensional matrix; that is, a^ represents a three-dimensional matrix with element a. Therefore, the terminal constructs a three-dimensional vector φ(t)∈R based on the Lie group function. 3 The function expression is obtained by deriving the initial three-dimensional function expression. The terminal then performs differentiation based on the initial three-dimensional function expression to obtain the candidate three-dimensional function expression: The terminal multiplies both sides of the candidate three-dimensional function by R(t) on the right. Since R(t) = R(t) T =I, where I represents the identity matrix, i.e., R(t)R(t) T =1, therefore, we get: Therefore, the target rotation component is: Here, "-1" is used to represent the reciprocal.

[0059] 205. Select the initial trust region radius based on the candidate translation components and the target rotation components;

[0060] The specific initial trust region radius can be set according to the actual application scenario. As an example rather than a limitation, the initial trust region radius can be 5 or 6.

[0061] 206. The objective function is calculated based on the initial trust region radius to obtain the first value. The objective function is used to represent the rotation and translation matrices to be optimized corresponding to the candidate translation components and the target rotation components.

[0062] For example, based on the examples from steps 202 to 204, the candidate translation component is: a 3*1 matrix t 3*1 The target rotation component is: The rotation and translation matrix to be optimized is:

[0063] For example, Δ k Used to represent the initial trust region radius, denoted by x. k Centered on the circle, based on the initial trust region radius Δ k Draw a circle to obtain the circular trust region, where "k" represents the iteration number, i.e., x. k f is used to represent the position of the k-th iteration. k Used to represent the objective function, i.e., the terminal at x. k The objective function is calculated at this point, and the corresponding formula is: Here, p is a variable vector used to represent the direction of iteration, that is, the first value m is determined by p within the trust region. k (p), Used to represent the objective function f k The transpose matrix f k T After taking the derivative in each orthogonal direction, multiply it by the unit vector in each direction, p T B is the transpose matrix used to represent the variable vector p. k Used to represent x k The Hessian matrix, or approximate Hessian matrix, is a block matrix composed of the second-order partial derivatives of real-valued functions whose independent variables are vectors.

[0064] 207. If the first difference between the first value and the preset minimum value is less than or equal to the first preset threshold, the initial trust region radius is increased in multiple levels, and the above steps are repeated until the objective function meets the preset conditions. The preset conditions are used to indicate that the first difference is less than or equal to the second preset threshold, and the first preset threshold is greater than the second preset threshold.

[0065] The specific preset minimum value can be set according to the actual application scenario. As an example rather than a limitation, the preset minimum value can be 7 or 8.

[0066] The specific first preset threshold can be set according to the actual application scenario. As an example rather than a limitation, the first preset threshold can be 15 or 20.

[0067] The specific second preset threshold can be set according to the actual application scenario. As an example rather than a limitation, the second preset threshold can be 9 or 10.

[0068] The specific number of levels can be set according to the actual application scenario. As an example rather than a limitation, the number of levels can be multiples or rounds.

[0069] For example, if the first difference between the first value and the preset minimum value is less than or equal to the first preset threshold, the initial trust region radius is increased in multiple levels, and the above step 206 is repeated until the objective function meets the preset condition. The preset condition is used to indicate that the first difference is less than or equal to the second preset threshold, and the first preset threshold is greater than the second preset threshold.

[0070] 208. If the objective function satisfies the preset conditions, then the rotation and translation matrix to be optimized is determined as the target rotation and translation matrix.

[0071] For example, based on the examples in steps 206 and 207, if the objective function satisfies the preset conditions, then the rotation and translation matrix to be optimized will be... The target rotation and translation matrix is ​​determined.

[0072] In this embodiment of the invention, a candidate translation and rotation matrix is ​​obtained by pre-registering the target point cloud and the source point cloud. Then, the candidate rotation components in the candidate translation and rotation matrix are transformed to obtain the target rotation component. Based on the candidate translation component and the target rotation component, the target rotation and translation matrix is ​​obtained. It is not necessary to use Euler angles to represent the rotation matrix in the rotation and translation matrix, thus avoiding the gimbal lock problem and improving the accuracy of point cloud registration.

[0073] Please see Figure 3 Another embodiment of the point cloud registration method in this invention includes:

[0074] 301. Obtain the point cloud data to be registered, which includes the target point cloud and the source point cloud;

[0075] The execution process of step 301 is the same as that of step 101, and will not be described again here.

[0076] 302. Convert the target point cloud to an initial 3D mesh with a preset resolution to obtain the target 3D mesh. Each mesh in the target 3D mesh includes multiple 3D points, which are used to indicate the feature points of the target point cloud.

[0077] The specific preset resolution can be set according to the actual application scenario. As an example rather than a limitation, the preset resolution can be 0.1m*0.2m*0.3m or 0.2m*0.3m*0.4m.

[0078] For example, with a preset resolution of 0.1m*0.2m*0.3m, the terminal converts the target point cloud into an initial 3D mesh of 0.1m*0.2m*0.3m to obtain the target 3D mesh. Each mesh in the target 3D mesh includes multiple 3D points, which are used to indicate the feature points of the target point cloud.

[0079] 303. Calculate the mean and covariance of each grid based on multiple 3D points in each grid;

[0080] For example, based on the example in step 302, the terminal calculates the mean and covariance of each grid based on multiple 3D points of each grid, where the mean is the 3D variable M. Np The number of multiple 3D points in each grid, x i y i and z i Let x, y, and z be the x, y, and z values ​​of each 3D point, respectively, and let C be a 3x3 matrix with covariance. Among them, P i Used to represent each 3D point in each grid, each 3D point has x, y, and z values, (P i -M) T The "T" in the upper right corner is used to indicate (P) i The matrix transpose of -M).

[0081] 304. Obtain the target 3D points in the target 3D mesh, as well as the target mean and target covariance of the target mesh. The target 3D points are the feature points of the source point cloud transformed into feature points of the target point cloud based on the initial rotation and translation matrix.

[0082] For example, based on the example in step 303, suppose the initial rotation and translation matrix is ​​T_cs, and T_cs is composed of an initial rotation matrix R. cs and an initial translation vector t cs The terminal acquires the target 3D point x in the target 3D mesh. k And the target mean M of the target grid k tk and target covariance C tk Target 3D point x k It is the feature point x of the source point cloud k _src is transformed into feature points of the target point cloud based on the initial rotation and translation matrix T_cs, and the target 3D point x k It has x-values, y-values, and z-values.

[0083] 305. Obtain the number of feature points in the source point cloud, and based on the number of feature points, the target 3D points, the target mean, the target covariance, and the initial rotation and translation matrix, obtain the candidate translation and rotation matrix;

[0084] For example, based on the example in step 304, the terminal obtains the number of feature points Nsrc of the source point cloud, and based on the number of feature points Nsrc and the target 3D point x... k Target mean M tk Target covariance C tk Given an initial rotation and translation matrix T_cs, construct the error function as follows: Among them, (x k -M tk ) T The "T" in the upper right corner is used to indicate (x k -M tk The matrix transpose of x k =R cs *x k _src+t cs , In det(Σ), "Σ" represents the covariance C, and det(Σ) represents the determinant of the covariance C. p0 is the signal-to-noise ratio of the point cloud data to be registered, which is the ratio of noise points to all feature points in the point cloud data to be registered. h is the preset resolution, d3 = -log(c2), d1 = -log(c1 + c2) - d3. "exp" represents an exponential function with the natural constant e as the base, and "log" represents the logarithm. The terminal optimizes the initial rotation and translation matrix T_cs based on the error function to generate the objective function value err_function. When the objective function value is less than or equal to a preset value, the candidate translation and rotation matrix is ​​obtained as follows: Wherein, the initial rotation matrix R cs After optimization, we obtain R, and the initial translation vector t. cs After optimization, t is obtained 3*1 The specific preset value can be set according to the actual application scenario. As an example rather than a limitation, the preset value can be 0.1 or 0.2.

[0085] 306. Perform quaternion transformation on the candidate rotation components to obtain the corresponding scalar components and vector components;

[0086] For example, based on the example in step 305, the terminal performs quaternion transformation on the candidate rotation component R to obtain the corresponding scalar component and vector component, where w represents the scalar component, and the vector component includes x1, y1, and z1, i.e., R = [w, (x1, y1, z1)]. R is defined as a complex number, i.e., R = w + i*x1 + j*y1 + k*z1, where i, j, and k are the imaginary parts, and the corresponding relationships are as follows: i*j = k, j*i = -k; j*k = i k*j = -i; k*i = j, i*k = -j. The geometric meaning of i, j, and k can be understood as a kind of rotation. Among them, i rotation represents the rotation from the positive X-axis to the positive Y-axis in the plane where the X-axis and Y-axis intersect, j rotation represents the rotation from the positive Z-axis to the positive X-axis in the plane where the Z-axis and X-axis intersect, and k rotation represents the rotation from the positive Y-axis to the positive Z-axis in the plane where the Y-axis and Z-axis intersect. -i, -j, and -k represent the opposite rotations of i, j, and k, respectively.

[0087] 307. Perform matrix transformation based on scalar and vector components to obtain the target rotation components;

[0088] For example, based on the example in step 306, the terminal performs matrix transformation based on scalar and vector components, and the target rotation component is:

[0089] 308. Select the initial trust region radius based on the candidate translation components and the target rotation components;

[0090] The specific initial trust region radius can be set according to the actual application scenario. As an example rather than a limitation, the initial trust region radius can be 5 or 6.

[0091] 309. Calculate the objective function based on the initial trust region radius to obtain the first value. The objective function is used to represent the rotation and translation matrices to be optimized corresponding to the candidate translation components and the target rotation components.

[0092] For example, based on the examples from steps 305 to 308, the candidate translation component is: a 3*1 matrix t 3*1 ,Right now Therefore, the rotation and translation matrix to be optimized is:

[0093] For example, Δ k Used to represent the initial trust region radius, denoted by x. k Centered on the circle, based on the initial trust region radius Δ k Draw a circle to obtain the circular trust region, where "k" represents the iteration number, i.e., x. k f is used to represent the position of the k-th iteration. k Used to represent the objective function, i.e., the terminal at x. kThe objective function is calculated at this point, and the corresponding formula is: Here, p is a variable vector used to represent the direction of iteration, that is, the first value m is determined by p within the trust region. k (p), Used to represent the objective function f k The transpose matrix f k T After taking the derivative in each orthogonal direction, multiply it by the unit vector in each direction, p T B is the transpose matrix used to represent the variable vector p. k Used to represent x k The Hessian matrix, or approximate Hessian matrix, is a block matrix composed of the second-order partial derivatives of real-valued functions whose independent variables are vectors.

[0094] 310. If the first difference between the first value and the preset minimum value is greater than the first preset threshold, then the initial trust region radius is reduced in multiple levels.

[0095] For example, if the first value is 10, the preset minimum value is 5, the first preset threshold is 3, and the multi-level is twice, then the first difference between the first value and the preset minimum value is greater than the first preset threshold, and the terminal reduces the initial trust region radius by a factor of two.

[0096] 311. Repeat the above steps until the objective function meets the preset conditions;

[0097] For example, the terminal repeatedly executes steps 309 and / or 310 until the objective function meets the preset conditions, which are used to indicate that the first difference is less than or equal to the second preset threshold.

[0098] 312. If the objective function satisfies the preset conditions, then the rotation and translation matrix to be optimized is determined as the target rotation and translation matrix.

[0099] For example, based on the examples from steps 309 to 311, if the objective function meets the preset conditions, the terminal will select the rotation and translation matrix to be optimized. The target rotation and translation matrix is ​​determined.

[0100] In this embodiment of the invention, a candidate translation and rotation matrix is ​​obtained by pre-registering the target point cloud and the source point cloud. Then, the candidate rotation components in the candidate translation and rotation matrix are transformed to obtain the target rotation component. Based on the candidate translation component and the target rotation component, the target rotation and translation matrix is ​​obtained. It is not necessary to use Euler angles to represent the rotation matrix in the rotation and translation matrix, thus avoiding the gimbal lock problem and improving the accuracy of point cloud registration.

[0101] The point cloud registration method in the embodiments of the present invention has been described above. The apparatus for point cloud registration in the embodiments of the present invention is described below. Please refer to [link / reference]. Figure 4 One embodiment of the point cloud registration device in this invention includes:

[0102] The acquisition module 401 is used to acquire the point cloud data to be registered, which includes the target point cloud and the source point cloud.

[0103] The pre-registration module 402 is used to perform pre-registration processing based on the target point cloud and the source point cloud to obtain a candidate translation and rotation matrix, which includes a candidate rotation component and a candidate translation component.

[0104] The conversion module 403 is used to convert the candidate rotation components to obtain the target rotation component;

[0105] The optimization module 404 is used to perform optimization processing based on the candidate translation components and the target rotation components to obtain the target rotation and translation matrix.

[0106] In this embodiment of the invention, a candidate translation and rotation matrix is ​​obtained by pre-registering the target point cloud and the source point cloud. Then, the candidate rotation components in the candidate translation and rotation matrix are transformed to obtain the target rotation component. Based on the candidate translation component and the target rotation component, the target rotation and translation matrix is ​​obtained. It is not necessary to use Euler angles to represent the rotation matrix in the rotation and translation matrix, thus avoiding the gimbal lock problem and improving the accuracy of point cloud registration.

[0107] Please see Figure 5 Another embodiment of the point cloud registration device in this invention includes:

[0108] The acquisition module 401 is used to acquire the point cloud data to be registered, which includes the target point cloud and the source point cloud.

[0109] The pre-registration module 402 is used to perform pre-registration processing based on the target point cloud and the source point cloud to obtain a candidate translation and rotation matrix, which includes a candidate rotation component and a candidate translation component.

[0110] The conversion module 403 is used to convert the candidate rotation components to obtain the target rotation component;

[0111] The optimization module 404 is used to perform optimization processing based on the candidate translation components and the target rotation components to obtain the target rotation and translation matrix.

[0112] Optionally, the pre-registration module 402 is specifically used for:

[0113] The target point cloud is converted to an initial 3D mesh of a preset resolution to obtain the target 3D mesh. Each mesh in the target 3D mesh includes multiple 3D points, which are used to indicate the feature points of the target point cloud.

[0114] The mean and covariance of each grid are calculated based on multiple 3D points in each grid.

[0115] Obtain the target 3D points in the target 3D mesh, as well as the target mean and target covariance of the target mesh. The target 3D points are the feature points of the source point cloud transformed into feature points of the target point cloud based on the initial rotation and translation matrix.

[0116] Obtain the number of feature points in the source point cloud, and based on the number of feature points, the target 3D points, the target mean, the target covariance, and the initial rotation and translation matrix, obtain the candidate translation and rotation matrix.

[0117] Optionally, the conversion module 403 includes:

[0118] Transformation unit 4031 is used to perform Lie group transformation on candidate rotation components to obtain the corresponding Lie group function.

[0119] The differentiation unit 4032 is used to perform differentiation based on the Lie group function to obtain the target rotation component.

[0120] Optionally, the derivative unit 4032 is specifically used for:

[0121] Based on the Lie group function, construct the function of the three-dimensional vector to obtain the initial three-dimensional function;

[0122] By performing differentiation on the initial three-dimensional function expression, candidate three-dimensional function expressions are obtained.

[0123] Based on the candidate 3D function and 3D vector, the target rotation component is obtained.

[0124] Optionally, optimization module 404 includes:

[0125] Selection unit 4041 is used to select the initial trust region radius based on the candidate translation component and the target rotation component;

[0126] The calculation unit 4042 is used to calculate the objective function based on the initial trust region radius to obtain the first value. The objective function is used to represent the rotation and translation matrix to be optimized corresponding to the candidate translation component and the target rotation component.

[0127] Add unit 4043, which is used to increase the initial trust region radius in multiple levels if the first difference between the first value and the preset minimum value is less than or equal to the first preset threshold, and repeat the above steps until the objective function meets the preset condition. The preset condition is used to indicate that the first difference is less than or equal to the second preset threshold and the first preset threshold is greater than the second preset threshold.

[0128] The first determining unit 4044 is used to determine the rotation and translation matrix to be optimized as the target rotation and translation matrix if the objective function meets the preset conditions.

[0129] Optionally, optimization module 404 also includes:

[0130] The reduction unit 4045 is used to reduce the initial trust region radius in multiple levels if the first difference between the first value and the preset minimum value is greater than the first preset threshold.

[0131] Execution unit 4046 is used to repeatedly execute the above steps until the objective function meets the preset conditions;

[0132] The second determining unit 4047 is used to determine the rotation and translation matrix to be optimized as the target rotation and translation matrix if the objective function meets the preset conditions.

[0133] Optionally, conversion module 403 is specifically used for:

[0134] The candidate rotation components are processed by quaternion transformation to obtain the corresponding scalar components and vector components;

[0135] The target rotation component is obtained by performing matrix transformation based on scalar and vector components.

[0136] In this embodiment of the invention, a candidate translation and rotation matrix is ​​obtained by pre-registering the target point cloud and the source point cloud. Then, the candidate rotation components in the candidate translation and rotation matrix are transformed to obtain the target rotation component. Based on the candidate translation component and the target rotation component, the target rotation and translation matrix is ​​obtained. It is not necessary to use Euler angles to represent the rotation matrix in the rotation and translation matrix, thus avoiding the gimbal lock problem and improving the accuracy of point cloud registration.

[0137] above Figure 4 and Figure 5 The point cloud registration apparatus in this embodiment of the invention will be described in detail from the perspective of modular functional entities. The point cloud registration device in this embodiment of the invention will be described in detail from the perspective of hardware processing.

[0138] Figure 6This is a schematic diagram of the structure of a point cloud registration device 600 provided in an embodiment of the present invention. The point cloud registration device 600 can vary significantly due to different configurations or performance. It may include one or more central processing units (CPUs) 610 (e.g., one or more processors) and a memory 620, and one or more storage media 630 (e.g., one or more mass storage devices) for storing application programs 633 or data 632. The memory 620 and storage media 630 can be temporary or persistent storage. The program stored in the storage media 630 may include one or more modules (not shown in the diagram), each module may include a series of instruction operations on the point cloud registration device 600. Furthermore, the processor 610 may be configured to communicate with the storage media 630 and execute the series of instruction operations in the storage media 630 on the point cloud registration device 600.

[0139] The point cloud registration device 600 may also include one or more power supplies 640, one or more wired or wireless network interfaces 650, one or more input / output interfaces 660, and / or one or more operating systems 631, such as Windows Server, Mac OS X, Unix, Linux, FreeBSD, etc. Those skilled in the art will understand that... Figure 6 The device structure shown for point cloud registration does not constitute a limitation on the device for point cloud registration. It may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0140] The present invention also provides a point cloud registration device, the point cloud registration device including a memory and a processor, the memory storing computer-readable instructions, and when the computer-readable instructions are executed by the processor, causing the processor to perform the steps of the point cloud registration method in the above embodiments.

[0141] The present invention also provides a computer-readable storage medium, which may be a non-volatile computer-readable storage medium or a volatile computer-readable storage medium, wherein the computer-readable storage medium stores instructions that, when the instructions are executed on a computer, cause the computer to perform the steps of the point cloud registration method.

[0142] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0143] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0144] The above-described embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for point cloud registration, characterized in that, The point cloud registration method includes: Acquire point cloud data to be registered, which includes target point cloud and source point cloud; Pre-registration processing is performed based on the target point cloud and the source point cloud to obtain a candidate translation and rotation matrix, which includes a candidate rotation component and a candidate translation component. The candidate rotation components are transformed to obtain the target rotation component; The target rotation and translation matrix is ​​obtained by performing optimization processing based on the candidate translation components and the target rotation components. The optimization process based on the candidate translation components and the target rotation components to obtain the target rotation and translation matrix includes: The initial trust region radius is selected based on the candidate translation component and the target rotation component; The objective function is calculated based on the initial trust region radius to obtain a first value. The objective function is used to represent the rotation and translation matrices to be optimized corresponding to the candidate translation component and the target rotation component. If the first difference between the first value and the preset minimum value is less than or equal to the first preset threshold, the initial trust region radius is increased in multiple levels, and the above steps are repeated until the objective function satisfies the preset condition. The preset condition is used to indicate that the first difference is less than or equal to the second preset threshold, and the first preset threshold is greater than the second preset threshold. If the objective function satisfies the preset condition, then the rotation and translation matrix to be optimized is determined as the target rotation and translation matrix.

2. The point cloud registration method according to claim 1, characterized in that, The pre-registration process based on the target point cloud and the source point cloud to obtain the candidate translation and rotation matrix includes: The target point cloud is converted to an initial 3D mesh of a preset resolution to obtain a target 3D mesh. Each mesh in the target 3D mesh includes multiple 3D points, which are used to indicate feature points of the target point cloud. The mean and covariance of each grid are calculated based on multiple 3D points in each grid. Obtain the target 3D points in the target 3D mesh, as well as the target mean and target covariance of the target mesh. The target 3D points are transformed from the feature points of the source point cloud based on an initial rotation and translation matrix. The number of feature points in the source point cloud is obtained, and a candidate translation and rotation matrix is ​​obtained based on the number of feature points, the target 3D points, the target mean, the target covariance, and the initial rotation and translation matrix.

3. The point cloud registration method according to claim 1, characterized in that, The step of transforming the candidate rotation components to obtain the target rotation component includes: The candidate rotation components are subjected to Lie group transformation to obtain the corresponding Lie group function. The target rotation component is obtained by performing differentiation based on the Lie group function.

4. The point cloud registration method according to claim 3, characterized in that, The process of obtaining the target rotation component by performing differentiation based on the Lie group function includes: Based on the Lie group function, construct the function of the three-dimensional vector to obtain the initial three-dimensional function; Based on the initial three-dimensional function expression, the candidate three-dimensional function expression is obtained by performing differentiation. Based on the candidate three-dimensional function and the three-dimensional vector, the target rotation component is obtained.

5. The point cloud registration method according to claim 1, characterized in that, After calculating the objective function based on the initial trust region radius to obtain the first value, the method further includes: If the first difference between the first value and the preset minimum value is greater than the first preset threshold, then the initial trust region radius is reduced in multiple levels. Repeat the above steps until the objective function satisfies the preset condition; If the objective function satisfies the preset condition, then the rotation and translation matrix to be optimized is determined as the target rotation and translation matrix.

6. The point cloud registration method according to any one of claims 1-5, characterized in that, The step of transforming the candidate rotation components to obtain the target rotation component includes: The candidate rotation components are subjected to quaternion transformation to obtain the corresponding scalar components and vector components; Matrix transformation is performed based on the scalar component and the vector component to obtain the target rotation component.

7. A point cloud registration apparatus, characterized in that, The point cloud registration device includes: The acquisition module is used to acquire point cloud data to be registered, which includes target point cloud and source point cloud; The pre-registration module is used to perform pre-registration processing based on the target point cloud and the source point cloud to obtain a candidate translation and rotation matrix, wherein the candidate translation and rotation matrix includes a candidate rotation component and a candidate translation component. The conversion module is used to convert the candidate rotation components to obtain the target rotation component; The optimization module is used to perform optimization processing based on the candidate translation components and the target rotation components to obtain the target rotation and translation matrix; The optimization module includes: The selection unit is used to select an initial trust region radius based on the candidate translation component and the target rotation component; The calculation unit is used to calculate the objective function based on the initial trust region radius to obtain a first value. The objective function is used to represent the rotation and translation matrix to be optimized corresponding to the candidate translation component and the target rotation component. An additional unit is used to increase the initial trust region radius in multiple levels if the first difference between the first value and the preset minimum value is less than or equal to the first preset threshold, and repeat the above steps until the objective function satisfies a preset condition. The preset condition is used to indicate that the first difference is less than or equal to the second preset threshold, and the first preset threshold is greater than the second preset threshold. The first determining unit is used to determine the rotation and translation matrix to be optimized as the target rotation and translation matrix if the objective function satisfies the preset condition.

8. A point cloud registration device, characterized in that, The point cloud registration device includes: a memory and at least one processor, wherein the memory stores instructions; The at least one processor invokes the instructions in the memory to cause the point cloud registration device to perform the point cloud registration method as described in any one of claims 1-6.

9. A computer-readable storage medium storing instructions thereon, characterized in that, When the instructions are executed by the processor, they implement the point cloud registration method as described in any one of claims 1-6.

Images