A point cloud three-dimensional reconstruction method based on sub-neighbor frame constraint pose graph optimization

Abstract

The application discloses a laser point cloud three-dimensional reconstruction method based on a sub-adjacent frame constraint pose graph optimization, which comprises two parts of point cloud registration and pose graph optimization. The point cloud registration calculates a rigid body transformation pose matrix between two frames of point clouds through coarse registration and fine registration, comprises an adjacent frame transformation matrix and a sub-adjacent frame transformation matrix, and provides a basis for pose graph establishment; the pose graph comprises graph nodes and constraint edges, the adjacent frame transformation matrix is used to calculate the pose of each frame of point clouds, the initialization pose graph node is formed, the adjacent frame relative pose error and the loop error are calculated according to the adjacent frame transformation matrix, the adjacent frame constraint edge and the loop constraint edge are added, and the cumulative error is suppressed; the sub-adjacent frame relative pose error is calculated according to the sub-adjacent frame transformation matrix, the sub-adjacent frame constraint edge is added, and the robustness of the pose graph is improved; the LM algorithm is used to optimize a cost function of the initial pose graph, the optimal pose meeting the constraint is obtained, all point clouds are transformed into a unified coordinate system according to the optimized point cloud pose, and a target reconstruction model is obtained.

Description

Technical Field

[0001] This invention relates to the field of 3D point cloud processing, and in particular to a method for 3D point cloud reconstruction based on next-neighbor-frame constrained pose graph optimization. This method, based on a truncated least squares point cloud registration method and pose graph optimization 3D reconstruction, processes multi-viewpoint point clouds acquired by active sensors to achieve high-precision 3D reconstruction of the target. Background Art

[0002] 3D reconstruction, a point cloud processing technology, is widely used in urban planning, cultural heritage preservation, industrial manufacturing, and space rendezvous and docking due to its advantages such as non-contact measurement, minimal impact from ambient lighting, and ability to reconstruct 3D models of targets. 3D reconstruction uses active sensors to acquire point clouds from multiple viewpoints of a target. Point cloud registration techniques are then used to calculate the rigid body transformation matrix between the point clouds. These point clouds from different viewpoints are then transformed into a unified coordinate system to obtain a 3D model of the target. Errors in point cloud registration can lead to cumulative overall errors during reconstruction. This accumulated error can cause distortion, deformation, or misalignment in the reconstructed point cloud model. To mitigate the impact of this accumulated error, pose graph optimization methods are used to redistribute the error across frames, avoiding large cumulative errors and improving 3D reconstruction accuracy. A pose graph is a graph structure composed of nodes and edges. Nodes represent point cloud poses and are the variables to be optimized, while edges represent the transformation relationships between point cloud poses and are the constraints of the optimization problem. Pose graph optimization improves the overall accuracy of point cloud poses and 3D reconstruction. Due to issues such as sensor noise, long-distance point cloud sparsification, occlusion, and low overlap, point cloud registration can produce significant errors, impacting the accuracy of pose graph optimization 3D reconstruction. Therefore, designing the pose graph structure to minimize the impact of registration errors on pose graph optimization accuracy is an urgent problem to be solved.

[0003] In order to overcome the influence of the registration error on the optimization accuracy of the pose graph, there are currently two commonly used processing methods. One is to construct a pose graph based on full graph reweighting, that is, to exhaustively register the multi-view point clouds so that there are edges connecting every two nodes of the pose graph, and then iteratively reweight all edges to gradually reduce the weights of the edges corresponding to the large error registration results. Among them, the iterative reweighting mainly determines the weights of the edges through the iterative method of iteratively determining the weights of the edges by the point cloud overlap rate and the number of inliers of the point cloud registration results, thereby reducing the influence of abnormal edges on the global pose estimation. The full graph reweighting method can effectively improve the accuracy of three-dimensional reconstruction by exhaustively registering and iteratively reducing the weights of abnormal edges, but it also greatly increases the amount of calculation. Since the time complexity of the exhaustive registration algorithm is O(n 2), and each iterative reweighting requires recalculating the overlap ratio and number of inliers for all registration results, increasing computational cost. Another approach is based on sparse pose graph construction, which uses selective registration and optimization to reduce computational complexity while maintaining high 3D reconstruction accuracy. This sparse pose graph construction typically selects point cloud pairs for registration based on feature matching or point cloud overlap, and adds corresponding edges to the pose graph. Feature matching selection constructs a sparse pose graph by detecting and matching local feature points in the point clouds and selecting pairs of frames for registration based on the number or quality of feature point matches. This feature matching method is sensitive to noise and occlusion. When point clouds are noisy and sparsity is present, the reliability of feature point extraction and matching is significantly reduced. In areas with less texture or many repetitive patterns, the extracted feature points may be sparse or non-unique, leading to difficulties and errors in matching. The point cloud overlap ratio measures the degree of commonality between two point clouds and determines the size of the overlap area between them. Generally, the higher the overlap ratio between two point clouds, the higher the point cloud registration accuracy. The overlap ratio calculation is relatively simple, highly efficient, insensitive to local variations, and can effectively handle noise and partial occlusion, ensuring reliability in overlapping areas. The point cloud overlap ratio selection improves point cloud registration accuracy and pose graph optimization accuracy by selecting frame pairs with high point cloud overlap ratios.

[0004] The pose graph construction method based on overlap rate is divided into the overlap rate of unordered point clouds and the overlap rate of ordered point clouds. For the unordered point cloud, the time sequence and position sequence between point clouds are unknown, and the overlap rate between all point clouds needs to be exhaustively calculated; for the ordered point cloud, the time sequence and position sequence between point clouds are known, and usually the adjacent frames and the next adjacent frames are the point cloud pairs with the highest overlap rate. In the three-dimensional reconstruction application scenario described in the present invention, the sensor moves around the target for one circle, obtains point clouds from multiple perspectives of the target, and finally the sensor returns to the vicinity of the initial position, so that loop constraints can be constructed. The multi-perspective point cloud obtained belongs to the three-dimensional reconstruction technology of the overlap rate of ordered point clouds, and its time sequence and position sequence are known. Therefore, the method based on overlap rate mainly selects the adjacent frame point cloud and the next adjacent frame for registration, and adds adjacent frame constraint edges, next adjacent frame constraint edges and loop constraint edges to the pose graph.

[0005] According to the patent review provided by the existing patent office, the existing patent application selects the adjacent frame point cloud to construct a pose graph based on the point cloud overlap rate, obtains the initial value of the pose graph node through the adjacent frame point cloud registration result, and obtains the target three-dimensional reconstruction model based on the pose after the graph optimization. The above patent includes the "three-dimensional reconstruction method and system" disclosed in the Chinese patent CN117911617A. The above patent does not consider the problem of large adjacent frame registration error. When the adjacent frame registration result error is too large, the accuracy of the pose graph is reduced. The difference between the present application and the above patents includes: First, the point cloud registration method is different. The above patent calculates the rough pose of the point cloud by integrating the angular velocity and acceleration information collected from the inertial measurement unit, and calculates the pose of the adjacent point cloud based on the rough pose of the point cloud. The adjacent point cloud precise registration algorithm such as iterative nearest neighbor point (ICP) and normal distribution transform (NDT) is used to calculate the pose of the adjacent point cloud. The present patent uses truncated least squares estimation and semidefinite relaxation. The patent uses the TEASER (Teaser) algorithm for coarse registration, suppresses the influence of matching pairs with large errors by truncating the least squares function, and performs fine registration by the point-to-surface ICP algorithm; second, the types of pose graph constraint edges are different. The above-mentioned patent only adds neighboring frame constraint edges in the pose graph based on the neighboring frame registration results. In addition to the neighboring frame constraint edges, this patent also adds sub-neighboring frame constraint edges in the pose graph based on the sub-neighboring frame registration results; third, the constraint edge cost function is different. The above-mentioned patent calculates the root mean square error or reprojection error of the neighboring frame feature matching pairs as the cost function, while this patent calculates the error between the relative pose of the neighboring frame and the point cloud registration result, and uses its Lie algebra form as the cost function.

[0006] Another type of patent application for three-dimensional reconstruction is based on the construction of a fully weighted pose graph, performing feature matching on the data of any two scanning stations, determining weights based on the number of feature matches, constructing a fully connected weighted graph, and generating a minimum spanning tree of the graph to obtain the registration path between any two stations. The feature matching pairs are used to calculate the transformation matrix between any two connected stations, and then the transformation matrix of each station relative to the fixed station is obtained through the tree path to complete the global registration of all stations. The ICP registration principle is used to perform precise registration on each registration station in the path to obtain a refined transformation matrix. The above patent includes "A method for multi-view automatic registration of multi-station ground laser point cloud data" disclosed in Chinese patent CN201510833425. The above patent does not take into account the possibility of erroneous matching pairs between two frames of point clouds. When there are erroneous matching pairs between two frames of point clouds, the point cloud registration accuracy is reduced, and it is easy to assign a larger weight to the registration result with larger errors, resulting in pose errors. Figure 3The accuracy of dimensional reconstruction is reduced. The differences between this patent application and the above patents include: First, the feature matching method is different. The above patent obtains geometric features such as straight lines and cylinders through the point cloud clustering method, and assigns semantic information to the feature points, and matches the semantic feature points by constructing triangular geometric constraints and semantic constraints. The present patent application describes the local geometric information of the point cloud by calculating the feature descriptor, and searches for the points with the smallest Euclidean distance between the feature descriptors between the two frames of point clouds as matching pairs; Second, the point cloud registration method is different. After obtaining the matching pairs, the above patent uses a random sampling consistency algorithm to eliminate the wrong matching pairs, and uses singular value decomposition to calculate the transformation matrix for the screened matching pairs. The present patent application uses a truncated least squares function to suppress the influence of the wrong matching pairs, achieve high-precision coarse registration, and input the coarse registration results into the point-to-surface ICP algorithm to further improve the point cloud registration accuracy; Third, the pose graph construction is different. The above patent requires pairwise registration of multi-view point clouds to construct a fully connected complete graph. If the number of point cloud frames is N, the time complexity of the pose graph construction algorithm is O(N 2 ), this paper uses adjacent frames and next adjacent frames to construct the pose graph, and the time complexity of the algorithm is O(N).

[0007] To improve the accuracy of three-dimensional reconstruction of laser point clouds, reduce the computational cost and complexity of three-dimensional reconstruction algorithms, and achieve high-precision three-dimensional model reconstruction of a target from multi-view laser point clouds based on point cloud registration and pose graph optimization, the present invention discloses a method for three-dimensional reconstruction of laser point clouds based on optimization of a pose graph with next-neighboring frame constraints. The method collects multi-view point clouds of a target using an active sensor, calculates the transformation matrix through TEASER coarse registration and point-to-surface ICP fine registration, constructs a pose graph based on adjacent frame constraint edges, next-neighboring frame constraint edges, and loop constraint edges, performs pose graph optimization, and completes three-dimensional reconstruction of the target. The present invention has broad application prospects in the field of three-dimensional reconstruction of laser point clouds. Summary of the Invention

[0008] This paper proposes a method for point cloud 3D reconstruction based on pose graph optimization with constraints of next-neighboring frames, which is characterized in that the pose estimation accuracy is enhanced by the constraints of next-neighboring frames, so as to realize high-precision 3D reconstruction; the method for point cloud 3D reconstruction based on pose graph optimization with constraints of next-neighboring frames includes point cloud registration and pose graph optimization; the point cloud registration provides a basis for establishing a pose graph by calculating the rigid body transformation pose matrix between two frames of point clouds; the point cloud registration step includes feature descriptor calculation, matching pair calculation, and transformation matrix estimation; the pose graph includes graph nodes and constraint edges, the graph nodes are the pose matrices of single-frame point clouds, the constraint edges are the rigid body transformation matrices between two frames of point clouds, and the The constraint edges include adjacent frame constraint edges, sub-adjacent frame constraint edges, and loop constraint edges; the adjacent frame constraint edges are the transformation matrix constraints between two adjacent frame point clouds in the pose graph; the sub-adjacent frame constraint edges are the transformation matrix constraints between two frame point clouds separated by one frame in the pose graph, which are used to improve the robustness of the pose graph; the loop constraint edges are the transformation matrix constraints constructed by the closed loop of the last frame point cloud and the first frame point cloud, which are used to improve the accuracy of pose estimation of continuous multi-frame point clouds; through the pose graph optimization method, the cumulative error of adjacent frame point cloud registration is reduced, the overall point cloud pose error is reduced, and the optimized point cloud pose is used to transform all point clouds into a unified coordinate system to obtain a reconstructed target model;

[0009] The point cloud pose matrix T is a 4×4 matrix consisting of the rotation matrix R and translation vector t of the point cloud relative to the reference coordinate system; the pose error includes the rotation error e rot and translation error e tra , which is defined as:

[0010]

[0011] Among them, T is the pose matrix estimate, T G is the true value of the pose matrix, ΔR is the residual of the rotation matrix R, Δt is the residual of the translation vector t, tr(·) represents the trace of the matrix, and ||·|| represents the bi-norm of the vector; the cumulative error refers to the existence of small errors in the registration of adjacent frames of the point cloud, which leads to the gradual accumulation of errors during reconstruction and the gradual increase of the pose error;

[0012] The point cloud 3D reconstruction method comprises the following steps:

[0013] (1) Multi-view target point cloud acquisition: An active three-dimensional imaging sensor is used to acquire multi-view target point clouds around the target. The active sensor moves in a circular motion around the target, scanning a frame of point cloud every M°. The sensor moves at least one circle, and a total of N frames of point cloud are acquired.

[0014] (2) Feature descriptor calculation: voxel downsampling is performed on the multi-view point cloud to compress the data volume and improve the calculation speed; the feature descriptor is calculated for each point in the downsampled point cloud; the feature descriptor describes the feature point by establishing a local coordinate system at the feature point and combining the spatial position information or geometric feature statistics of the neighboring points. The basic idea is to establish a local coordinate system based on the neighboring points, and then encode the geometric features of each point into the histogram to obtain a three-dimensional descriptor. The point cloud feature descriptor is essentially a vector.

[0015] (3) Matching pair determination: Based on the point cloud feature descriptor, a matching pair is calculated using a mutual search method; the two frames of point clouds to be registered are respectively called the source point cloud and the target point cloud; for a point in the source point cloud, the point with the smallest Euclidean distance to the feature descriptor of the point is searched in the target point cloud; for the corresponding point found in the target point cloud, the same search is performed again in the source point cloud; if a pair of spatial points in the source point cloud and the target point cloud are the same after the two searches, then this pair of spatial points is considered a matching pair;

[0016] (4) Estimation of transformation matrices of adjacent frames and next adjacent frames: Based on the calculation results of matching pairs, the transformation matrix is ​​estimated by combining the truncated least squares cost function and the semidefinite relaxation method; the truncated least squares cost function enhances the robustness to erroneous matching pairs, and the semidefinite relaxation is used to rewrite the non-convex problem of rotation estimation into a convex optimization problem, thereby improving the accuracy of 3D point cloud registration; the non-convex problem refers to the existence of multiple local extreme points in the objective function or constraint conditions during the optimization process, which makes it difficult to find the global optimal solution and easily falls into the local extreme point points; the convex optimization problem refers to the existence of a unique global optimal solution for the objective function and constraint conditions during the optimization process, and attempts to find the global optimal solution;

[0017] (5) Pose graph construction: Based on the estimation results of the adjacent frame transformation matrix, the pose matrix of each frame point cloud is calculated, and the pose graph nodes are initialized according to the pose matrix; based on the adjacent frame transformation matrix, the adjacent frame constraint edge is added; based on the next adjacent frame transformation matrix, the next adjacent frame constraint edge is added; for the last frame point cloud and the first frame point cloud, a loop constraint edge is added, that is, the transformation matrix between the last frame point cloud and the first frame point cloud is the unit matrix, which means that the sensor returns to the initial position after the circular motion;

[0018] (6) Pose graph optimization: Calculate the cost function for the pose graph. Assuming that the pose graph has N nodes, define the relative pose Lie group expression of adjacent frames as follows:

[0019]

[0020] Among them, T i and T i+1 are the poses of the i-th frame and the i+1-th frame respectively, T i,i+1is the relative pose between the i-th frame and the i+1-th frame. In the graph optimization, the relative pose T of the adjacent frames i,i+1 It has been obtained through the point cloud registration algorithm, so the error can be calculated by the following formula:

[0021]

[0022] Similarly, the next-neighbor frame error can be calculated as follows:

[0023]

[0024] Among them, T i,i+2 is the relative pose of the next adjacent frame; the loop error is:

[0025]

[0026] Among them, T N,1 is the relative pose of the last frame and the first frame; the cost function F is the sum of these three errors; the Levenberg-Marquardt algorithm is used to optimize and iteratively solve until the cost function converges to obtain the optimized point cloud pose T i O ,i=1,2,…,N;

[0027] (7) Three-dimensional reconstruction: According to the optimized point cloud pose of the graph, all point clouds are aligned to the coordinate system of the first frame point cloud through rigid body transformation to obtain the reconstructed target model.

[0028] The point cloud 3D reconstruction method based on optimization of the constrained pose graph of the next adjacent frame is characterized in that the active sensor can be a lidar or a ToF camera; the multi-view target point cloud is acquired by causing the active sensor to move in a circular motion around the target and finally return to near the initial position, so that the last frame point cloud and the first frame point cloud can form a closed loop through point cloud registration; the output of this part is point clouds under N frames of different view angles.

[0029] The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization is characterized in that the feature descriptor calculation can select a Signature of Histogram of Orientation (SHOT) descriptor, a Point Feature Histogram (FPH) descriptor, or a Fast Point Feature Histogram (FPFH) descriptor; the feature descriptor calculation steps are as follows:

[0030] 1) The point in the point cloud where the feature descriptor is to be calculated is called a query point. For each query point p in the point cloud, the covariance matrix M of the point neighborhood is constructed:

[0031]

[0032] Among them, r represents the neighborhood radius, p is the query point, and p i is a point in the neighborhood, d i For p i The distance between it and p;

[0033] 2) By performing singular value decomposition on the covariance matrix, its eigenvalues ​​and corresponding eigenvectors are obtained, and the eigenvalues ​​are arranged in descending order. The corresponding eigenvectors are the x, y, and z axes of the local coordinate system;

[0034] 3) Calculate the geometric information such as the normal of the neighborhood point and the cosine value of the angle between the normal and the query point, and encode the geometric features of the neighborhood points into the histogram to form a feature descriptor, which is essentially a K-dimensional vector. Assume that the set of query points in the point cloud is P = {p i ,i=1,2,…,m}, the calculation result is the feature descriptor set The output of this part is the feature descriptor calculation result D, which will be used as the input for the next step of matching pair determination.

[0035] The point cloud 3D reconstruction method based on the optimization of the pose graph of the next adjacent frame constraint is characterized in that the matching pair is determined based on the calculation result of the feature descriptor, assuming that the source point cloud is P = {p i ,i=1,2,…,m}, the corresponding feature descriptor is The target point cloud is Q = {q i ,i=1,2,…,m}, the corresponding feature descriptor is For a point p in the source point cloud i , and its corresponding feature descriptor is In the target point cloud feature descriptor D Q Inquiry and The feature descriptor with the smallest Euclidean distance The corresponding point q j For p i To improve the matching accuracy, a mutual search method is used, that is, for the preliminary corresponding point q j , in the source point cloud feature descriptor D P Perform the same search in , if the corresponding point found is also p i , then p i and q j Considered as a matching pair; traversing the feature descriptor D P, and finally n matching pairs are obtained; in order to avoid the high time consumption caused by traversal search, a KD tree is established for the feature descriptors of the source point cloud and the target point cloud to accelerate the mutual search; the output of this part is n matching pairs between point clouds, which will be used as the input for the next step of coarse registration.

[0036] The point cloud 3D reconstruction method based on the optimization of the pose graph of the next adjacent frame constraint is characterized in that, assuming that the source point cloud P = {p i ,i=1,2,…,n} and target point cloud Q={q i , i=1,2,…,n}, there are scale, translation, and rotation transformations between them, and the truncated least squares cost function is:

[0037]

[0038] Where s is the scale transformation factor, R is the rotation matrix, t is the translation matrix, n is the matching number, β i is the maximum value of the measurement residual that can be tolerated in the i-th measurement, is a constant; through the truncated least squares cost function, a constant cost is assigned to the measurement value with a large residual, so that the transformation matrix estimation is insensitive to the measurement value with a large residual. The output of this part is the coarse alignment result, that is, the rotation matrix R and the translation matrix t, which will be used as the input for the next step of fine alignment.

[0039] The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization is characterized in that the truncated least squares and semidefinite relaxation registration results are used as the initial estimate of the transformation matrix and input into a fine registration algorithm for further fine registration; the fine registration algorithm can select an iterative closest point (ICP) algorithm or a point-to-plane ICP (ICP) algorithm; the ICP algorithm cost function is:

[0040]

[0041] Wherein, T is the transformation matrix, n is the number of matching pairs, p i is a point in the source point cloud, q closest The target point cloud and p i The point with the smallest Euclidean distance, ||·|| represents the vector's two-norm;

[0042] The cost function of the point-surface ICP algorithm is:

[0043]

[0044] Among them, v iis the normal vector of the tangent plane of the target point; the output of this part is the rigid body transformation matrix T of the adjacent frame i,i+1 , i=1,2,…,N and the rigid body transformation matrix T of the adjacent frame i,i+2 ,i=1,2,…,N, which will be used as input for the next step of pose graph construction.

[0045] The point cloud 3D reconstruction method based on the optimization of the pose graph of the adjacent frame constraint is characterized in that the pose graph is constructed, and the initial value of the pose node is determined based on the adjacent frame registration result; the adjacent frame registration result is the adjacent frame rigid body transformation matrix T i,i+1 ,i=1,2,…,N, the pose node is the pose matrix T i ,i=1,2,…,N, the calculation relationship between the two is:

[0046] T1=I4 (11)

[0047] T i =T i-1 ·T i-1,i ,i=2,3,…,N (12)

[0048] Where I4 is the 4th-order identity matrix, and formula (11) indicates that the first pose node is set to the identity matrix as the benchmark; formula (12) indicates that the initial value of the pose node is the initial value of the previous pose node multiplied by the rigid body transformation matrix from the previous pose node to the pose node; the output of this part is the pose graph node T i ,i=1,2,…,N。

[0049] The point cloud 3D reconstruction method based on the optimization of the constrained pose graph of the next adjacent frame is characterized in that the pose graph is constructed based on the rigid body transformation matrix T of the adjacent frame. i,i+1 , i=1,2,…,N add adjacent frame constraint edges to the pose graph; based on the rigid body transformation matrix T of the adjacent frame i,i+2 ,i=1,2,…,N add the next adjacent frame constraint edge; add the loop constraint edge between the last pose node and the first pose node;

[0050] The neighboring frame constraint edge is essentially the error term shown in formula (4), and its Lie algebraic form is:

[0051]

[0052] Among them, ξ i and ξ i+1 T i and T i+1 The corresponding Lie algebra is the optimization variable in the subsequent optimization, and has T i =exp(ξ i ) ^ , Ti+1 =exp(ξ i+1 ) ^ , Indicates T i -1 The corresponding Lie algebra, and T i -1 =exp((-ξ i ) ^ );in· ^ The symbol represents the conversion of a vector into an antisymmetric matrix, ∨ The symbol represents the conversion of the antisymmetric matrix into a vector; in the graph optimization, the relative pose T i,i+1 It has been obtained through the point cloud registration algorithm, and its corresponding Lie algebra is denoted as ξ i,i+1 , so the error expression is for:

[0053] e i,i+1 =ln(exp((-ξ i,i+1 ) ∧ )·exp((-ξ i ) ∧ )exp(ξ i+1 ) ∧ ) ∨ (14)

[0054] Similarly, the next-neighbor frame error expression is for:

[0055] e i,i+2 =ln(exp((-ξ i,i+2 ) ∧ )·exp((-ξ i ) ∧ )exp(ξ i+2 ) ∧ ) ∨ (15)

[0056] Loop closure constraint error expression for:

[0057] e N,1 =ln(exp((-ξ N,1 ) ∧ )·exp((-ξ N ) ∧ )exp(ξ1) ∧ ) ∨ (16)

[0058] The output of this part is the three constraint edge error expressions e described in the pose graph. i,i+1 、e i,i+2 and e N,1, which is convenient for the next step to calculate the pose graph cost function.

[0059] The point cloud 3D reconstruction method based on the optimization of the pose graph with constraints on the next adjacent frame is characterized in that the cost function of the pose graph is defined as:

[0060]

[0061] The cost function F is a 6-dimensional vector, where the first three dimensions represent translation errors, the last three dimensions represent rotation errors, and Ω i is a preset information matrix, which represents the prediction of the correlation of the error terms corresponding to each element in e, and assigns different weights to each dimension of the error; the information matrix Ω i It is a 6×6 diagonal matrix, and the main diagonal elements represent the optimization variables (t x ,t y ,t z ,r x ,r y ,r z ) weight, the larger the diagonal element value, the larger the weight of the corresponding optimization variable; the cost function is iteratively solved until it converges; the output of this part is the point cloud pose T after graph optimization i O ,i=1,2,…,N, which will be used as the input for the next step of 3D reconstruction.

[0062] The point cloud 3D reconstruction method based on the optimization of the constrained pose graph of the next adjacent frame is characterized in that the pose of the point cloud of the first frame is fixed to the unit matrix I as a reference; the pose obtained after the pose graph optimization is T i O ,i=2,3,…,N, which is actually the transformation matrix T of each point cloud relative to the first frame point cloud 1,i , i=2,3,…,N;The three-dimensional reconstruction transforms all point clouds into the first frame point cloud coordinate system, that is, for each frame point cloud P i , i=2,3,…,N make the following transformations:

[0063]

[0064] Among them, P i T is the transformed point cloud; the output of this part is the set P of transformed point clouds i T ,i=2,3,…,N, that is, the three-dimensional reconstructed model. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 Workflow diagram of the point cloud 3D reconstruction method based on the optimization of the pose graph constrained by the next neighboring frame

[0066] Figure 2 Schematic diagram of the pose graph structure

[0067] Figure 3 Schematic diagram of sensor data collection

[0068] Figure 4 Constructing a schematic diagram for the pose graph

[0069] Figure 5 Schematic diagram of target trajectory before and after image optimization

[0070] Figure 6 3D reconstruction result DETAILED DESCRIPTION

[0071] The following is a detailed description of the specific implementation of the present invention in conjunction with the accompanying drawings. The present invention discloses an online ranging method for laser radar saturation signals based on waveform compensation. The working process of an embodiment of the method is as follows: Figure 1 This embodiment consists of two parts: point cloud registration and pose graph optimization. The point cloud registration provides a basis for establishing a pose graph by calculating the rigid body transformation pose matrix between two frames of point cloud. The point cloud registration step includes feature descriptor calculation, matching pair calculation, and transformation matrix estimation. The pose graph contains graph nodes and constraint edges, such as Figure 2 As shown; the graph nodes are the pose matrices of each point cloud, the constraint edges are the rigid body transformation matrices between two frames of point clouds, and the constraint edges include adjacent frame constraint edges, sub-adjacent frame constraint edges, and loop constraint edges; the adjacent frame constraint edges are the transformation matrix constraints between two adjacent frames of point clouds in the pose graph; the sub-adjacent frame constraint edges are the transformation matrix constraints between two frames of point clouds separated by one frame in the pose graph, which are used to improve the robustness of the pose graph; the loop constraint edges are the transformation matrix constraints constructed by the closed loop of the last frame of point cloud and the first frame of point cloud, which are used to improve the accuracy of pose estimation of continuous multi-frame point clouds; through the pose graph optimization method, the cumulative error of adjacent frame point cloud registration is reduced, the overall point cloud pose error is reduced, and the optimized point cloud pose is used to transform all point clouds into a unified coordinate system to obtain a reconstructed target model;

[0072] The point cloud pose matrix T is a 4×4 matrix consisting of the rotation matrix R and translation vector t of the point cloud relative to the reference coordinate system; the pose error includes the rotation error e rot and translation error e tra , which is defined as:

[0073]

[0074] Among them, T is the pose matrix estimate, T Gis the true value of the pose matrix, ΔR is the residual of the rotation matrix R, Δt is the residual of the translation vector t, tr(·) represents the trace of the matrix, and ||·|| represents the bi-norm of the vector; the cumulative error refers to the existence of small errors in the registration of adjacent frames of the point cloud, which leads to the gradual accumulation of errors during reconstruction and the gradual increase of the pose error;

[0075] The point cloud 3D reconstruction method comprises the following steps:

[0076] (1) Multi-view target point cloud acquisition: An active three-dimensional imaging sensor is used to acquire multi-view target point clouds around the target. The active sensor moves in a circular motion around the target, scanning a frame of point cloud every M°. The sensor moves at least one circle, and a total of N frames of point cloud are acquired.

[0077] (2) Feature descriptor calculation: voxel downsampling is performed on the multi-view point cloud to compress the data volume and improve the calculation speed; the feature descriptor is calculated for each point in the downsampled point cloud; the feature descriptor describes the feature point by establishing a local coordinate system at the feature point and combining the spatial position information or geometric feature statistics of the neighboring points. The basic idea is to establish a local coordinate system based on the neighboring points, and then encode the geometric features of each point into the histogram to obtain a three-dimensional descriptor. The point cloud feature descriptor is essentially a vector.

[0078] (3) Matching pair determination: Based on the point cloud feature descriptor, a matching pair is calculated using a mutual search method; the two frames of point clouds to be registered are respectively called the source point cloud and the target point cloud; for a point in the source point cloud, the point with the smallest Euclidean distance to the feature descriptor of the point is searched in the target point cloud; for the corresponding point found in the target point cloud, the same search is performed again in the source point cloud; if a pair of spatial points in the source point cloud and the target point cloud are the same after the two searches, then this pair of spatial points is considered a matching pair;

[0079] (4) Estimation of transformation matrices of adjacent frames and next adjacent frames: Based on the calculation results of matching pairs, the transformation matrix is ​​estimated by combining the truncated least squares cost function and the semidefinite relaxation method; the truncated least squares cost function enhances the robustness to erroneous matching pairs, and the semidefinite relaxation is used to rewrite the non-convex problem of rotation estimation into a convex optimization problem, thereby improving the accuracy of 3D point cloud registration; the non-convex problem refers to the existence of multiple local extreme points in the objective function or constraint conditions during the optimization process, which makes it difficult to find the global optimal solution and easily falls into the local extreme point points; the convex optimization problem refers to the existence of a unique global optimal solution for the objective function and constraint conditions during the optimization process, and attempts to find the global optimal solution;

[0080] (5) Pose graph construction: The pose graph construction process is as follows Figure 4As shown, based on the estimation results of the adjacent frame transformation matrix, the pose matrix of each frame point cloud is calculated, and the pose graph nodes are initialized according to the pose matrix; based on the adjacent frame transformation matrix, the adjacent frame constraint edge is added; based on the next adjacent frame transformation matrix, the next adjacent frame constraint edge is added; for the last frame point cloud and the first frame point cloud, a loop constraint edge is added, that is, the transformation matrix between the last frame point cloud and the first frame point cloud is the unit matrix, which means that the sensor returns to the initial position after the circular motion;

[0081] (6) Pose graph optimization: Calculate the cost function for the pose graph. Assuming that the pose graph has N nodes, define the relative pose Lie group expression of adjacent frames as follows:

[0082] T i,i+1 =T i -1 T i+1 (twenty one)

[0083] Among them, T i and T i+1 are the poses of the i-th frame and the i+1-th frame respectively, T i,i+1 is the relative pose between the i-th frame and the i+1-th frame. In the graph optimization, the relative pose T of the adjacent frames i,i+1 It has been obtained through the point cloud registration algorithm, so the error can be calculated by the following formula:

[0084]

[0085] Similarly, the next-neighbor frame error can be calculated as follows:

[0086]

[0087] Among them, T i,i+2 is the relative pose of the next adjacent frame; the loop error is:

[0088]

[0089] Among them, T N,1 is the relative pose of the last frame and the first frame; the cost function F is the sum of these three errors; the Levenberg-Marquardt algorithm is used to optimize and iteratively solve until the cost function converges to obtain the optimized point cloud pose T i O ,i=1,2,…,N;

[0090] (7) Three-dimensional reconstruction: According to the optimized point cloud pose of the graph, all point clouds are aligned to the coordinate system of the first frame point cloud through rigid body transformation to obtain the reconstructed target model.

[0091] In step (1) of acquiring multi-view target point cloud, this embodiment selects the laser radar as the active sensor, and makes the laser radar move in a circle around the target and finally returns to the vicinity of the initial position, such as Figure 3 As shown, the last frame of point cloud and the first frame of point cloud can form a closed loop through point cloud registration; in this embodiment, when the sensor performs circular motion, a frame of point cloud is collected every 25°, and a total of 15 frames of point cloud are collected.

[0092] In step (2) of calculating the feature descriptor, this embodiment selects the SHOT feature descriptor. For the query point, a local coordinate system is established based on its neighborhood points. The neighborhood space of the point is divided into multiple subspaces. The normal feature statistics of each point in the subspace are then encoded into a histogram. The histograms of each subspace are then combined to obtain a three-dimensional descriptor. The calculation steps are as follows:

[0093] 1) For each query point p in the point cloud, construct the covariance matrix M of the point neighborhood:

[0094]

[0095] Where r represents the neighborhood radius. In this embodiment, r is taken as 300 times the point cloud resolution. The point cloud resolution used in this embodiment is 2.08 cm, so r = 624.00 cm; p is the query point, p i is a point in the neighborhood, d i For p i The distance between it and p;

[0096] 2) By performing singular value decomposition on the covariance matrix, its eigenvalues ​​and corresponding eigenvectors can be obtained. The eigenvalues ​​are arranged in descending order, and the corresponding eigenvectors are the x, y, and z axes of the local coordinate system;

[0097] 3) With the query point as the center, a spherical neighborhood with a radius of r is established, and the sphere is divided into 8, 2, and 2 parts according to the longitude, latitude, and radial directions. In this way, the spherical neighborhood is divided into 32 subspaces;

[0098] 4) Calculate the cosine of the angle between the normal of the neighboring points distributed in each subspace and the normal of the query point, then divide the cosine values ​​in each space into 11 equal parts and perform histogram statistics. Finally, combine the histograms of all subspaces together to form a 352-dimensional high-dimensional histogram;

[0099] Assume that the set of query points in the point cloud is P = {p i ,i=1,2,…,m}, the calculation result is the feature descriptor set The output of this part is the feature descriptor calculation result D, which will be used as the input for the next step of matching pair determination.

[0100] In step (3) matching pair determination, based on the feature descriptor calculation results, the source point cloud is P = {p i ,i=1,2,…,m}, the corresponding feature descriptor is The target point cloud is Q = {q i ,i=1,2,…,m}, the corresponding feature descriptor is For a point p in the source point cloud i , and its corresponding feature descriptor is In the target point cloud feature descriptor D Q Inquiry and The feature descriptor with the smallest Euclidean distance The corresponding point q j For p i To improve the matching accuracy, a mutual search method is used, that is, for the preliminary corresponding point q j , in the source point cloud feature descriptor D P Perform the same search in , if the corresponding point found is also p i , then p i and q j Considered as a matching pair; traversing the feature descriptor D P , and finally n matching pairs are obtained; in order to avoid the high time consumption caused by traversal search, a KD tree is established for the feature descriptors of the source point cloud and the target point cloud to accelerate the mutual search; the output of this part is n matching pairs between point clouds, which will be used as the input for the next step of coarse registration.

[0101] In step (4) of estimating the transformation matrix of adjacent frames and next adjacent frames, the source point cloud P = {p i ,i=1,2,…,n} and target point cloud Q={q i ,i=1,2,…,n}, there are scale, translation, and rotation transformations between them, and the truncated least squares function is used as the cost function:

[0102]

[0103] Where s is the scale transformation factor, R is the rotation matrix, t is the translation matrix, n is the matching number, β i is the maximum value of the measurement residual that can be tolerated in the i-th measurement, is a constant; by using the truncated least squares cost function, a constant cost is assigned to the measurement value with a large residual, so that the transformation matrix estimation is insensitive to the measurement value with a large residual; after rewriting, formula (26) still belongs to the nonlinear least squares problem and is non-convex, and the time complexity of exhaustive search for the global optimal solution is O(2 n); In order to solve the problem in polynomial time, the complex problem is decoupled according to the translation-invariant measurement assumption and the translation-rotation-invariant measurement assumption, that is, Equation (26) is decomposed into three independent problems of scale, translation, and rotation transformation estimation, and solved separately; the translation-invariant measurement assumption means that the direction of the vector composed of any two points in the point cloud remains unchanged during the translation process; assuming that the two point clouds to be registered P = {p i ,i=1,2,…,n}、Q={q i ,i=1,2,…,n} conforms to the following generative model:

[0104] q i =sRp i +t+o i +ε i (27)

[0105] Among them, ε i is the measurement noise and conforms to the Gaussian distribution, that is, ||ε i ||≤β i If (p i ,q i ) constitutes an interior point, then o i is a zero vector, otherwise it is an arbitrary value; according to the above model, let q i Subtract p i have to:

[0106]

[0107] in, o ij =o j -o i , ε ij =ε j -ε i ; by ||ε i ||≤β i It can be seen that: ε i is also bounded, then the model (27) only depends on the unknown scale transformation s and rotation transformation R;

[0108] The translation and rotation invariant measurement assumption is that the modulus of the vector formed by any two points in the point cloud remains unchanged during the translation and rotation process. Taking the modulus of equation (28) yields:

[0109]

[0110] Since the noise is bounded: ||ε i ||≤β i 、||ε j ||≤β j ,||ε ij ||≤βi +β j =δ ij , then we can approximately get:

[0111]

[0112] If (p i ,q i ) constitutes an interior point, then is a zero vector, otherwise it is an arbitrary value; divide both sides of the above formula by We can get:

[0113]

[0114] From formula (31), we can see that after using the translation and rotation invariant measurement assumption, model (27) is only related to the scale transformation factor s. Based on the above two assumptions, we can see that we only need to first use the translation and rotation invariant measurement to obtain the scale estimate, then obtain the rotation estimate based on the translation invariant measurement and the scale estimate, and finally substitute the scale estimate and the rotation estimate into formula (26) to obtain the translation estimate, thereby completing the solution of the point cloud registration problem based on truncated least squares. The output of this part is the coarse registration result, that is, the rotation matrix R and the translation matrix t, which will be used as the input for the next step of fine registration.

[0115] In step (4) of estimating the transformation matrices of adjacent frames and next adjacent frames, the truncated least squares and semidefinite relaxation registration results are used as the initial estimation of the transformation matrix and input into the fine registration algorithm for further fine registration. In this embodiment, the point-to-plane iterative closest point (ICP) algorithm is selected as the fine registration algorithm, and the cost function of the point-to-plane ICP algorithm is:

[0116]

[0117] Among them, v i is the normal vector of the tangent plane of the target point; the output of this part is the rigid body transformation matrix T of the adjacent frame i,i+1 , i=1,2,…,N and the rigid body transformation matrix T of the adjacent frame i,i+2 ,i=1,2,…,N, which will be used as input for the next step of pose graph construction.

[0118] In step (5) pose graph construction, the initial value of the pose node is determined based on the adjacent frame registration result; the adjacent frame registration result is the adjacent frame rigid body transformation matrix T i,i+1 ,i=1,2,…,N, the pose node is the pose matrix T of each point cloud i ,i=1,2,…,N, the calculation relationship between the two is:

[0119] T1=I4 (33)

[0120] T i =T i-1 ·T i-1,i ,i=2,3,…,N (34)

[0121] Where I4 is the 4th-order unit matrix. Formula (33) indicates that the first pose node is set to the unit matrix as the benchmark. Formula (34) indicates that the initial value of the pose node is the initial value of the previous pose node multiplied by the rigid body transformation matrix from the previous pose node to the pose node. The output of this part is the pose graph node T i ,i=1,2,…,N。

[0122] In step (6) pose graph optimization, based on the adjacent frame rigid body transformation matrix T i,i+1 , i=1,2,…,N add adjacent frame constraint edges in the pose graph; add next adjacent frame constraint edges T based on the next adjacent frame rigid body transformation matrix i,i+2 ,i=1,2,…,N; add a loop constraint edge between the last pose node and the first pose node;

[0123] The essence of the adjacent frame constraint edge is the error term shown in the following formula:

[0124]

[0125] Among them, T i and T i+1 are the poses of the i-th frame and the i+1-th frame respectively, T i,i+1 is the relative pose between the i-th frame and the i+1-th frame. In the graph optimization, the relative pose T of the adjacent frames i,i+1 has been obtained through the point cloud registration algorithm; T i -1 T i+1 The Lie algebraic form of the term is:

[0126]

[0127] Among them, ξ i and ξ i+1 T i and T i+1 The corresponding Lie algebra is the optimization variable in the subsequent optimization, and has T i =exp(ξ i ) ∧ , T i+1 =exp(ξ i+1 ) ∧ , Indicates T i -1 The corresponding Lie algebra, and T i -1=exp((-ξ i ) ∧ );where ·^ symbol indicates that the vector is converted into an antisymmetric matrix, · ∨ The symbol represents the conversion of the antisymmetric matrix into a vector; in the graph optimization, the relative pose T i,i+1 It has been obtained through the point cloud registration algorithm, and its corresponding Lie algebra is denoted as ξ i,i+1 , so the error expression is for:

[0128] e i,i+1 =ln(exp((-ξ i,i+1 ) ∧ )·exp((-ξ i ) ∧ )exp(ξ i+1 ) ∧ ) ∨ (37)

[0129] Similarly, the next-neighbor frame error expression is for:

[0130]

[0131] Loop closure constraint error expression for:

[0132]

[0133] The output of this part is the three constraint edge error expressions e described in the pose graph. i,i+1 、e i,i+2 and e N,1 , which is convenient for the next step to calculate the pose graph cost function.

[0134] In step (6) pose graph optimization, the cost function of the pose graph is defined as:

[0135]

[0136] The cost function F is a 6-dimensional vector, where the first three dimensions represent translation errors, the last three dimensions represent rotation errors, and Ω i is a preset information matrix, which represents the prediction of the correlation of the error terms corresponding to each element in e, and assigns different weights to each dimension of the error; the information matrix Ω i It is a 6×6 diagonal matrix, and the main diagonal elements represent the optimization variables (t x ,t y ,t z ,r x ,r y ,r z), the larger the diagonal element value, the larger the weight of the corresponding optimization variable; in this embodiment, the information matrix is ​​determined according to the noise size of the translation vector and the rotation vector. Assume that the noise of the translation vector is The noise of the rotation vector is Then the information matrix is:

[0137]

[0138] The cost function is iteratively solved until it converges to obtain the optimized pose T i O , i=2,3,…,N; the estimated laser radar position trajectory before and after the optimization is as follows Figure 5 As shown in the figure, the results show that graph optimization can effectively suppress the cumulative error of adjacent frame point cloud registration and improve the accuracy of pose estimation; the output of this part is the point cloud pose T after graph optimization i O ,i=2,3,…,N, which will be used as the input for the next step of 3D reconstruction.

[0139] In step (7) 3D reconstruction, the first frame point cloud pose is fixed to the unit matrix T1 O =I4, as a benchmark; the pose obtained after the pose graph is optimized is T i O ,i=2,3,…,N, which is actually the transformation matrix T of each point cloud relative to the first frame point cloud 1,i , i=2,3,…,N;the three-dimensional reconstruction is based on T 1,i , i=2,3,…,N transform all point clouds into the first frame point cloud coordinate system, that is, for each frame point cloud P i , i=2,3,…,N make the following transformations:

[0140]

[0141] Among them, P i T is the transformed point cloud; the output of this part is the set P of transformed point clouds i T , i=1,2,3,…,N, i.e. 3D reconstruction model; In this embodiment, the tower and the transport vehicle are respectively used as targets for 3D reconstruction. The collected multi-view point clouds and the corresponding 3D reconstruction results are shown as follows: Figure 6 shown.

[0142] In this example, Blensor software was used to generate a simulated point cloud. CAD models of the transport vehicle and tower were imported into Blensor. A Velodyne HDL 64E2 lidar was used as the sensor. The distance between the lidar and the target was 30 m, the lidar resolution was 264×216, and the field of view was 43.6°×34.6°. The lidar was moved in a circular motion around the target, collecting a point cloud frame every 25°. 15 frames of point cloud were collected for each target. To verify the accuracy of the proposed 3D reconstruction method under different sensor noise conditions, this example tested the 3D reconstruction accuracy of point clouds under 20 different noise standard deviations. Gaussian noise was added to the point cloud using Blensor software. The Gaussian noise had a mean of 0 and a standard deviation ranging from 0 to 20 cm in 1 cm increments. 15 frames of point cloud were generated for each noise standard deviation as input to the 3D reconstruction algorithm. The SHOT feature descriptor was calculated for the input multi-view point cloud sequence, with a feature descriptor calculation radius 300 times the point cloud resolution. The point cloud resolution calculation formula is:

[0143]

[0144] Where n represents the number of points in the point cloud, d i Represents the distance between each point in the point cloud and its nearest neighbor. According to formula (43), the average resolution of the 15-frame point cloud used in this embodiment is 2.08 cm, so the feature descriptor radius r is 624.00 cm. The mutual search method is used to determine the matching pair.

[0145] The TEASER coarse registration is combined with the point-to-surface ICP fine registration method to calculate the adjacent frame transformation matrix and the next adjacent frame transformation matrix; based on the adjacent frame transformation matrix, the point cloud pose of each frame is calculated according to equations (33) and (34), the pose graph nodes are initialized, and adjacent frame constraint edges are added; loop constraint edges are added between the point clouds of the 15th frame and the 1st frame; next adjacent frame constraint edges are added according to the next adjacent frame transformation matrix; the cost function is calculated for the pose graph, and the LM algorithm is used to optimize it to obtain the optimized point cloud pose; the pose provided by Blenser is taken as the true value, respectively According to formula (19) and formula (20), the pose errors before and after optimization are calculated. When the noise standard deviation is 0, the pose errors of each frame point cloud are shown in Table 1. As the frame number increases, the translation error before optimization continues to increase, and the rotation error also shows an overall growth trend, indicating that there is a cumulative error in the registration of adjacent frame point clouds before image optimization; after image optimization, the cumulative error is suppressed, the registration error of the point cloud with a large frame number is significantly reduced, and the error of the registration results of some frame numbers with a small frame number is slightly increased, indicating that the image optimization distributes the cumulative error evenly to each frame point cloud, reducing the overall pose error;

[0146] Table 1 Pose error of noise-free point cloud before and after optimization

[0147]

[0148] Table 2 shows the pose errors before and after pose graph optimization under different noise conditions. When the sensor noise standard deviation ranges from 0 cm to 20 cm, both the rotation error and the translation error are reduced after optimization, indicating that pose graph optimization significantly improves pose accuracy and is robust to point cloud noise.

[0149] Table 2 Different noise standard deviations Figure 2 Posture errors before and after optimization

[0150]

[0151] According to the optimized pose, all point clouds are transformed into the coordinate system of the first frame to achieve 3D reconstruction of the target multi-view point cloud. In this embodiment, the chamfer distance is used to evaluate the accuracy of the 3D reconstructed model, which is defined as:

[0152]

[0153] Among them, S R For 3D reconstruction of point cloud, S G is the ground-truth point cloud of the model, || represents the number of points in the point cloud, and ||||2 represents the vector norm. The smaller the chamfer distance, the smaller the distance between the 3D reconstructed model and the true model, and the better the reconstruction effect;

[0154] The reconstructed chamfer distances before and after optimization under different noise standard deviations are shown in Table 3. The test results show that under different noise standard deviations, graph optimization can improve the reconstruction accuracy. When the noise standard deviation is less than or equal to 12 cm, the average improvement is 1.40×10 - 2 cm; when the noise standard deviation is between 13cm and 18cm, the average improvement is 0.21cm, which verifies the effectiveness of the proposed 3D reconstruction method.

[0155] Table 3 3D reconstruction accuracy under different noise standard deviations

[0156]

[0157] The above is merely a basic scheme for implementing the present invention, but the scope of protection of the present invention is not limited thereto. Any changes and substitutions that can be imagined by anyone skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims. All changes that fall within the meaning and scope of equivalents of the claims are intended to be included within the scope of the claims.

Claims

1. A point cloud 3D reconstruction method based on pose graph optimization with next-neighboring frame constraints, characterized in that: The pose estimation accuracy is enhanced by the next-neighboring frame constraint to achieve high-precision 3D reconstruction; the point cloud 3D reconstruction method with the next-neighboring frame constraint pose graph optimization includes point cloud registration and pose graph optimization; the point cloud registration provides a basis for the establishment of the pose graph by calculating the rigid body transformation pose matrix between the two frames of point clouds; the point cloud registration step includes feature descriptor calculation, matching pair calculation, and transformation matrix estimation; the pose graph contains graph nodes and constraint edges, the graph nodes are single-frame point cloud pose matrices, the constraint edges are rigid body transformation matrices between two frames of point clouds, and the constraint edges include adjacent frame constraint edges and next-neighboring frame constraint edges , loop constraint edge; the adjacent frame constraint edge is the transformation matrix constraint between two adjacent frames of point clouds in the pose graph; the sub-adjacent frame constraint edge is the transformation matrix constraint between two frames of point clouds separated by one frame in the pose graph, which is used to improve the robustness of the pose graph; the loop constraint edge is the transformation matrix constraint constructed by the closed loop of the last frame point cloud and the first frame point cloud, which is used to improve the pose estimation accuracy of continuous multi-frame point clouds; through the pose graph optimization method, the cumulative error of the adjacent frame point cloud registration is reduced, the overall point cloud pose error is reduced, and the optimized point cloud pose is used to transform all point clouds into a unified coordinate system to obtain the reconstructed target model; The point cloud pose matrix T is a 4×4 matrix consisting of the rotation matrix R and translation vector t of the point cloud relative to the reference coordinate system; the pose error includes the rotation error e rot and translation error e tra , which is defined as: Among them, T is the pose matrix estimate, T G is the true value of the pose matrix, ΔR is the residual of the rotation matrix R, Δt is the residual of the translation vector t, tr(·) represents the trace of the matrix, and ||·|| represents the bi-norm of the vector; the cumulative error refers to the existence of small errors in the registration of adjacent frames of the point cloud, which leads to the gradual accumulation of errors during reconstruction and the gradual increase of the pose error; The point cloud 3D reconstruction method comprises the following steps: (1) Multi-view target point cloud acquisition: An active three-dimensional imaging sensor is used to acquire multi-view target point clouds around the target. The active sensor moves in a circular motion around the target, scanning a frame of point cloud every M°. The sensor moves at least one circle, and a total of N frames of point cloud are acquired. (2) Feature descriptor calculation: voxel downsampling is performed on the multi-view point cloud to compress the data volume and improve the calculation speed; the feature descriptor is calculated for each point in the downsampled point cloud; the feature descriptor describes the feature point by establishing a local coordinate system at the feature point and combining the spatial position information or geometric feature statistics of the neighboring points. The basic idea is to establish a local coordinate system based on the neighboring points, and then encode the geometric features of each point into the histogram to obtain a three-dimensional descriptor. The point cloud feature descriptor is essentially a vector. (3) Matching pair determination: Based on the point cloud feature descriptor, a matching pair is calculated using a mutual search method; the two frames of point clouds to be registered are respectively called the source point cloud and the target point cloud; for a point in the source point cloud, the point with the smallest Euclidean distance to the feature descriptor of the point is searched in the target point cloud; for the corresponding point found in the target point cloud, the same search is performed again in the source point cloud; if a pair of spatial points in the source point cloud and the target point cloud are the same after the two searches, then this pair of spatial points is considered a matching pair; (4) Estimation of transformation matrices of adjacent frames and next adjacent frames: Based on the calculation results of matching pairs, the transformation matrix is ​​estimated by combining the truncated least squares cost function and the semidefinite relaxation method; the truncated least squares cost function enhances the robustness to erroneous matching pairs, and the semidefinite relaxation is used to rewrite the non-convex problem of rotation estimation into a convex optimization problem, thereby improving the accuracy of 3D point cloud registration; the non-convex problem refers to the existence of multiple local extreme points in the objective function or constraint conditions during the optimization process, which makes it difficult to find the global optimal solution and easily falls into the local extreme point points; the convex optimization problem refers to the existence of a unique global optimal solution for the objective function and constraint conditions during the optimization process, and attempts to find the global optimal solution; (5) Pose graph construction: Based on the estimation results of the adjacent frame transformation matrix, the pose matrix of each frame point cloud is calculated, and the pose graph nodes are initialized according to the pose matrix; based on the adjacent frame transformation matrix, the adjacent frame constraint edge is added; based on the next adjacent frame transformation matrix, the next adjacent frame constraint edge is added; for the last frame point cloud and the first frame point cloud, a loop constraint edge is added, that is, the transformation matrix between the last frame point cloud and the first frame point cloud is the unit matrix, which means that the sensor returns to the initial position after the circular motion; (6) Pose graph optimization: Calculate the cost function for the pose graph. Assuming that the pose graph has N nodes, define the relative pose Lie group expression of adjacent frames as follows: Among them, T i and T i+1 are the poses of the i-th frame and the i+1-th frame respectively, T i,i+1 is the relative pose between the i-th frame and the i+1-th frame. In the graph optimization, the relative pose T of the adjacent frames i,i+1 It has been obtained through the point cloud registration algorithm, so the error can be calculated by the following formula: Similarly, the next-neighbor frame error can be calculated as follows: Among them, T i,i+2 is the relative pose of the next adjacent frame; the loop error is: Among them, T N,1 is the relative pose of the last frame and the first frame; the cost function F is the sum of these three errors; the Levenberg-Marquardt algorithm is used to optimize and iteratively solve until the cost function converges to obtain the optimized point cloud pose T i O ,i=1,2,…,N; (7) Three-dimensional reconstruction: According to the optimized point cloud pose of the graph, all point clouds are aligned to the coordinate system of the first frame point cloud through rigid body transformation to obtain the reconstructed target model.

2. The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization according to claim 1, characterized in that: The active sensor can be a lidar or a ToF camera. The multi-view target point cloud acquisition is performed by making the active sensor move in a circular motion around the target and finally return to the vicinity of the initial position, so that the last frame of the point cloud and the first frame of the point cloud can form a closed loop through point cloud registration. The output of this part is N frames of point clouds at different viewpoints.

3. The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization according to claim 1, characterized in that: The feature descriptor calculation may select a Signature of Histogram of Orientation (SHOT) descriptor, a Point Feature Histogram (FPH) descriptor, or a Fast Point Feature Histogram (FPFH) descriptor; the feature descriptor calculation steps are as follows: 1) The point in the point cloud where the feature descriptor is to be calculated is called a query point. For each query point p in the point cloud, the covariance matrix M of the point neighborhood is constructed: Among them, r represents the neighborhood radius, p is the query point, and p i is a point in the neighborhood, d i For p i The distance between it and p; 2) By performing singular value decomposition on the covariance matrix, its eigenvalues ​​and corresponding eigenvectors are obtained, and the eigenvalues ​​are arranged in descending order. The corresponding eigenvectors are the x, y, and z axes of the local coordinate system; 3) Calculate the geometric information such as the normal of the neighborhood point and the cosine value of the angle between the normal and the query point, and encode the geometric features of the neighborhood points into the histogram to form a feature descriptor, which is essentially a K-dimensional vector. Assume that the set of query points in the point cloud is P = {p i ,i=1,2,…,m}, the calculation result is the feature descriptor set The output of this part is the feature descriptor calculation result D, which will be used as the input for the next step of matching pair determination.

4. The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization according to claim 1, characterized in that: The matching pair is determined based on the feature descriptor calculation result, assuming that the source point cloud is P = {p i ,i=1,2,…,m}, the corresponding feature descriptor is The target point cloud is Q = {q i ,i=1,2,…,m}, the corresponding feature descriptor is For a point p in the source point cloud i , and its corresponding feature descriptor is In the target point cloud feature descriptor D Q Inquiry and The feature descriptor with the smallest Euclidean distance The corresponding point q j For p i To improve the matching accuracy, a mutual search method is used, that is, for the preliminary corresponding point q j , in the source point cloud feature descriptor D P Perform the same search in , if the corresponding point found is also p i , then p i and q j Considered as a matching pair; traversing the feature descriptor D P , and finally n matching pairs are obtained; in order to avoid the high time consumption caused by traversal search, a KD tree is established for the feature descriptors of the source point cloud and the target point cloud to accelerate the mutual search; the output of this part is n matching pairs between point clouds, which will be used as the input for the next step of coarse registration.

5. The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization according to claim 1, characterized in that: Assume that the source point cloud P = {p i ,i=1,2,…,n} and target point cloud Q={q i , i=1,2,…,n}, there are scale, translation, and rotation transformations between them, and the truncated least squares cost function is: Where s is the scale transformation factor, R is the rotation matrix, t is the translation matrix, n is the matching number, β i is the maximum value of the measurement residual that can be tolerated in the i-th measurement, is a constant; through the truncated least squares cost function, a constant cost is assigned to the measurement value with a large residual, so that the transformation matrix estimation is insensitive to the measurement value with a large residual. The output of this part is the coarse alignment result, that is, the rotation matrix R and the translation matrix t, which will be used as the input for the next step of fine alignment.

6. The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization according to claim 1, characterized in that: The truncated least squares and semidefinite relaxation registration results are used as the initial estimate of the transformation matrix and input into the fine registration algorithm for further fine registration; the fine registration algorithm can be an iterative closest point (ICP) algorithm or a point-to-plane ICP (ICP) algorithm; the cost function of the ICP algorithm is: Wherein, T is the transformation matrix, n is the number of matching pairs, p i is a point in the source point cloud, q closest The target point cloud and p i The point with the smallest Euclidean distance, ||·|| represents the vector's two-norm; The cost function of the point-surface ICP algorithm is: Among them, v i is the normal vector of the tangent plane of the target point; the output of this part is the rigid body transformation matrix T of the adjacent frame i,i+1 , i=1,2,…,N and the rigid body transformation matrix T of the adjacent frame i,i+2 ,i=1,2,…,N, which will be used as input for the next step of pose graph construction.

7. The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization according to claim 1, characterized in that: The pose graph is constructed, and the initial value of the pose node is determined based on the adjacent frame registration result; the adjacent frame registration result is the adjacent frame rigid body transformation matrix T i,i+1 ,i=1,2,…,N, the pose node is the pose matrix T i ,i=1,2,…,N, the calculation relationship between the two is: T1=I4 (11) T i =T i-1 ·T i-1,i ,i=2,3,…,N (12) Where I4 is the 4th-order identity matrix, and formula (11) indicates that the first pose node is set to the identity matrix as the benchmark; formula (12) indicates that the initial value of the pose node is the initial value of the previous pose node multiplied by the rigid body transformation matrix from the previous pose node to the pose node; the output of this part is the pose graph node T i ,i=1,2,…,N。 8. The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization according to claim 1, characterized in that: The pose graph is constructed based on the rigid body transformation matrix T of the adjacent frame i,i+1 , i=1,2,…,N add adjacent frame constraint edges in the pose graph; add next adjacent frame constraint edges T based on the next adjacent frame rigid body transformation matrix i,i+2 ,i=1,2,…,N; add a loop constraint edge between the last pose node and the first pose node; The neighbor frame constraint edge is essentially the error term shown in formula (4), where T i -1 T i+1 The Lie algebraic form of the term is: Among them, ξ i and ξ i+1 T i and T i+1 The corresponding Lie algebra is the optimization variable in the subsequent optimization, and has T i =exp(ξ i ) ∧ , T i+1 =exp(ξ i+1 ) ∧ , Indicates T i -1 The corresponding Lie algebra, and T i -1 =exp((-ξ i ) ∧ );in· ∧ The symbol represents the conversion of a vector into an antisymmetric matrix, ∨ The symbol represents the conversion of the antisymmetric matrix into a vector; in the graph optimization, the relative pose T i,i+1 It has been obtained through the point cloud registration algorithm, and its corresponding Lie algebra is denoted as ξ i,i+1 , so the error expression is for: from i,i+1 =ln(exp((-ξ i,i+1 ) ∧ )·exp((-ξ i ) ∧ )exp(ξ i+1 ) ∧ ) ∨ (14) Similarly, the next-neighbor frame error expression is for: from i,i+2 =ln(exp((-ξ i,i+2 ) ∧ )·exp((-ξ i ) ∧ )exp(ξ i+2 ) ∧ ) ∨ (15) Loop closure constraint error expression for: e N,1 =ln(exp((-ξ N,1 ) ∧ )·exp((-ξ N ) ∧ )exp(ξ1) ∧ ) ∨ (16) The output of this part is the three constraint edge error expressions e described in the pose graph. i,i+1 、e i,i+2 and e N,1 , which is convenient for the next step to calculate the pose graph cost function.

9. The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization according to claim 1, characterized in that: The cost function of the pose graph is defined as: The cost function F is a 6-dimensional vector, where the first three dimensions represent translation errors, the last three dimensions represent rotation errors, and Ω i is a preset information matrix, which represents the prediction of the correlation of the error terms corresponding to each element in e, and assigns different weights to each dimension of the error; the information matrix Ω i It is a 6×6 diagonal matrix, and the main diagonal elements represent the optimization variables (t x ,t y ,t z ,r x ,r y ,r z ) weight, the larger the diagonal element value, the larger the weight of the corresponding optimization variable; the cost function is iteratively solved until it converges; the output of this part is the point cloud pose T after graph optimization i O ,i=1,2,…,N, which will be used as the input for the next step of 3D reconstruction.

10. The point cloud 3D reconstruction method based on next-neighboring frame constrained pose graph optimization according to claim 1, characterized in that: The pose of the first frame point cloud is fixed to the unit matrix I4 as a reference; the pose obtained after the pose graph is optimized is T i O ,i=2,3,…,N, which is actually the transformation matrix T of each point cloud relative to the first frame point cloud 1,i , i=2,3,…,N;The three-dimensional reconstruction transforms all point clouds into the first frame point cloud coordinate system, that is, for each frame point cloud P i , i=2,3,…,N make the following transformations: Among them, P i T is the transformed point cloud; the output of this part is the set P of transformed point clouds i T ,i=2,3,…,N, that is, the three-dimensional reconstructed model.

Images