Multi-view point cloud registration method based on high-precision map

Abstract

The present invention discloses a multi-view point cloud registration method based on high-precision map, including: obtaining point cloud data to be registered and high-precision map data of different radars; converting the coordinate information of the point cloud data to be registered into the reference coordinate system of the high-precision map; extracting the high-precision map point cloud according to the height and the distance between the point cloud data to be registered; extracting super points from the obtained high-precision map point cloud and calculating its geometric information embedding, and at the same time, each point in the point cloud to be registered is counted as a super point and its geometric information embedding is calculated, and then the best matching super point pair is calculated by combining the geometric information embedding of the two, and a one-to-many buffer point pair between the point cloud to be registered and the high-precision map point cloud is obtained according to the best matching super point pair expansion; performing precise registration on the buffer point pairs between the point cloud to be registered and the high-precision map point cloud; and completing the registration between the point cloud data to be registered based on the precise registration result. The present invention solves the problem of low registration accuracy between point cloud data with low overlap.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle-road collaboration, and specifically to a multi-view point cloud registration method based on a high-precision map. Background Art

[0002] The vehicle-road cooperative system is an innovative technology that obtains vehicle and road information based on technologies such as wireless communication and sensor detection. Through the interaction and sharing between vehicles and between vehicles and road infrastructure, intelligent coordination and cooperation between vehicles and infrastructure can be achieved to optimize the use of system resources, improve road traffic safety, and alleviate traffic congestion. Thanks to the continuous development of key technologies such as lidar, GPS, communications and wireless networks, vehicle cooperative technology has gradually evolved and matured. However, despite the broad prospects of vehicle cooperative technology, there are still some challenges in practical applications. One of them is that the data collected by different radars have different perspectives, resulting in low overlap of their data point clouds in space, making it quite difficult to directly integrate these data. Summary of the invention

[0003] The purpose of the present invention is to address the shortcomings of the prior art and provide a multi-view point cloud registration method based on high-precision maps. The method solves the problem of low registration accuracy between low-overlap point cloud data and significantly improves the efficiency of multi-view point cloud registration.

[0004] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0005] A multi-view point cloud registration method based on high-precision maps includes the following steps:

[0006] Step 1: Obtain the point cloud data to be registered and high-precision map data of different radars;

[0007] Step 2: Convert the coordinate information of the point cloud data to be registered into the reference coordinate system of the high-precision map to roughly configure the point cloud data to be registered with the high-precision map data;

[0008] Step 3: In the HD map reference coordinate system, extract the HD map point cloud according to the height and the distance between the point cloud data to be registered;

[0009] Step 4: extract super points from the HD map point cloud obtained in step 3 and calculate their geometric information embedding. At the same time, count each point in the point cloud to be registered as a super point and calculate its geometric information embedding. Then, combine the geometric information embedding between the HD map super points and the point cloud to be registered, calculate the best matching super point pair between the two, and expand the one-to-many buffer point pairs between the point cloud to be registered and the HD map original point cloud according to the best matching super point pair.

[0010] Step 5: Use the GICP algorithm to precisely align the buffer point pairs between the point cloud to be aligned obtained in step 4 and the original point cloud of the high-precision map;

[0011] Step 6: Based on the precise registration result of step 5, the registration of the point cloud data to be registered is completed in the high-precision map reference system.

[0012] Furthermore, the specific method of step 2 is:

[0013] Use the transformation matrix provided by GPS to perform matrix transformation on the point cloud to be registered, and transform the coordinate information of the point cloud data to be registered into the reference coordinate system of the high-precision map. The specific formula is as follows:

[0014]

[0015] In the formula, r l =(x l ,y l ,z l ) T Represents the coordinate information of the point in the radar coordinate system, r m =(x m ,y m ,z m ) T The coordinate information of the point converted to the high-precision map coordinate system; represents the transformation matrix from the radar coordinate system to the high-precision map coordinate system provided by GPS, where R represents the rotation matrix, r 11 ,r 12 ,r 13 Represents the component of the point on the x-axis in the high-precision map coordinate system, r 21 ,r 22 ,r 23 Represents the component of the point on the y-axis in the high-precision map coordinate system, r 31 ,r 32 ,r 33 It represents the component of the point on the z-axis in the HD map coordinate system, t represents the translation matrix, and t1, t2, and t3 represent the distance of the point from the radar coordinate system to the HD map coordinate system in the x, y, and z directions.

[0016] Furthermore, step 3 specifically includes the following implementation method:

[0017] S3.1. Calculate the height information of each point in the high-precision map and retain all points whose height is not less than the threshold;

[0018] S3.2. In the high-precision map reference coordinate system, for the high-precision map obtained in S3.1, calculate the shortest distance from each downsampled point to the point cloud to be registered;

[0019] S3.2. Set a distance threshold d max , traverse the high-precision map point cloud and extract the points whose closest distance calculated in step S3.2 is less than d max All points.

[0020] Furthermore, extracting super points in step 4 includes:

[0021] For dense high-precision map point cloud data P, downsample it to obtain sparse point cloud data Consider it as a super point, and then use the K nearest neighbor search algorithm to calculate each super point The K nearest neighboring points in the Euclidean space of the original HD map point cloud

[0022] For each superpoint Neighborhood points Encode the relative positions of the neighborhood points and superpoints, and encode the superpoints The three-dimensional coordinates of 3D coordinates of neighborhood points Relative coordinates Connect them as the feature information of the super point and get:

[0023]

[0024] Furthermore, the geometric information embedding of the superpoint in step 4 includes point pair distance embedding information and triplet angle embedding information.

[0025] Furthermore, the calculation method of the point-to-point distance embedding information of the superpoint is:

[0026] First, calculate the distance between any two hyperpoints Then according to the obtained distance d i,j The sine function is used to calculate the point-to-point distance embedding information of the superpoint, and the formula is as follows:

[0027]

[0028] Among them, σ d The sensitivity of the control point to distance changes, d t Represents the feature dimension.

[0029] Furthermore, the calculation method of the triplet angle embedding information is:

[0030] First, for each neighborhood point Calculate the angle α i,j,x =∠(Δ x,i ,Δ j,i ),in Δ x,i represents the vector representation between the i-th superpoint and the x-th superpoint, Δj,i Represents the vector representation between the i-th superpoint and the j-th superpoint, and then uses the sine function to encode the triplet angle embedding information. The formula is as follows:

[0031]

[0032] Among them, σ α Controls the sensitivity of angle changes, d t represents the feature dimension, α i,j,x represents the angle between the i-th superpoint and the x-th superpoint representation vector and the i-th superpoint and the j-th superpoint representation vector.

[0033] Furthermore, the buffer point pair is obtained by:

[0034] Calculate the distance of geometric information between every two super points of the HD map and the point cloud to be registered, select the super point pair with a distance less than the threshold as the best matching super point pair, and then expand the best matching super point obtained in the HD map back to the original point to form a one-to-many buffer point pair A={a i} i＝1,...N and B = {b i,j} i＝1,...N,j＝1,...K .

[0035] Furthermore, step 5 specifically includes the following steps:

[0036] S51, set a maximum matching threshold, remove incorrect corresponding relationships in the one-to-many buffer point pairs between the radar point cloud and the high-precision map point cloud according to the maximum matching threshold, and obtain a radar point cloud A with a correct corresponding relationship = {a i} i＝1,...N and high-precision map point cloud B = {b i} i＝1,...N ;

[0037] S52, radar point cloud A = {a i} i＝1,...N and high-precision map point cloud B = {b i} i＝1,...N Perform registration calculation to obtain a transformation matrix that minimizes the distance between corresponding point clouds;

[0038] S53, repeat the iteration step S52 until the desired transformation matrix is ​​converged or the maximum number of iterations is reached.

[0039] Furthermore, step 52 is specifically implemented as follows:

[0040] S521. Assume that there is a set of potential points. and And according to and Generate A and B, where and represents the covariance matrix associated with the measurement points;

[0041] S522, assuming the correct change matrix is ​​T * ,have For any rigid transformation, we have

[0042] S523, due to a i and b i,j Assuming they are independent and identically distributed under Gaussian distribution, we have:

[0043]

[0044] S524, use the maximum likelihood estimation method to iteratively calculate T, that is:

[0045]

[0046] In the formula, ω i,j Represents the weight of the buffer point pair.

[0047] Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention is based on high-precision map data, introduces buffer point pairs, and combines GPS information with GICP technology to achieve point cloud registration of multiple views, solving the problem of low registration accuracy under low overlap conditions; the present invention can be applied to data integration work for urban vehicle-road collaboration, which helps to achieve a safer and more efficient road traffic system. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 This is a flow chart of a multi-view point cloud registration method based on a high-precision map according to an embodiment of the present invention;

[0049] Figure 2 The present invention provides a flowchart for extracting high-precision map data according to an embodiment of the present invention. DETAILED DESCRIPTION

[0050] The technical solutions in the embodiments of the present invention will be described clearly and completely below in combination with the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0051] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0052] The present invention will be further described below in conjunction with specific embodiments, but the present invention is not limited thereto.

[0053] like Figure 1 As shown, the embodiment of the present invention discloses a multi-view point cloud registration method based on a high-precision map, comprising the following steps:

[0054] Step 1: Obtain the point cloud data to be registered and high-precision map data of different radars;

[0055] This embodiment uses radar data collected from different directions at the same intersection and high-precision map data, and combines them with the location information provided by GPS to perform multi-view point cloud registration, where the point cloud data to be registered includes three-dimensional point cloud information of multiple views, and the high-precision map data includes dense point cloud information within the entire radar measurement range. GPS provides the initial transformation matrix from radar data to high-precision map data.

[0056] Step 2: Convert the coordinate information of the point cloud data to be registered into the reference coordinate system of the high-precision map to roughly configure the point cloud data to be registered with the high-precision map data;

[0057] In this embodiment, the transformation matrix provided by GPS is used to directly perform matrix transformation on the point cloud to be registered, and the coordinate information of the point cloud data to be registered is converted into the reference coordinate system of the high-precision map. The specific formula is as follows:

[0058]

[0059] In the above formula, r l =(x l ,y l ,z l ) T Represents the coordinate information of the point in the radar coordinate system, r m =(x m ,y m ,z m ) T The coordinate information of the point converted to the high-precision map coordinate system;

[0060] Represents the transformation matrix from the radar coordinate system to the high-precision map coordinate system provided by GPS, where R represents the rotation matrix, r 11 ,r 12 ,r 13 Represents the component of the point on the x-axis in the high-precision map coordinate system, r 21 ,r 22 ,r 23 Represents the component of the point on the y-axis in the high-precision map coordinate system, r 31 ,r 32 ,r 33It represents the component of the point on the z-axis in the HD map coordinate system, t represents the translation matrix, and t1, t2, and t3 represent the distance of the point from the radar coordinate system to the HD map coordinate system in the x, y, and z directions.

[0061] Step 3: In the HD map reference coordinate system, extract the HD map point cloud according to the height and the distance between the point cloud data to be registered; see Figure 2 , the steps include:

[0062] 3.1 Screening ground point cloud data

[0063] Calculate the height information of each point in the high-precision map, and then remove all points whose height is lower than the threshold to obtain a filtered high-precision map, which makes the retained points have more information;

[0064] 3.2 Determine the distance relationship between the point cloud to be registered and the high-precision map point cloud

[0065] In the high-precision map reference coordinate system, for the high-precision map obtained in step 3.1, calculate the closest distance between each downsampled point and the point cloud to be registered. The distance calculation formula is as follows:

[0066]

[0067] Where d represents the distance between two points, x1, y1, and z1 represent the 3D coordinate information of the downsampled high-precision map point cloud, and x2, y2, and z2 represent the 3D coordinate information of the midpoint of the point cloud to be registered.

[0068] 3.3 Region Cropping

[0069] Set a distance threshold d max , traverse the entire high-precision map, and find all points whose distance to the point cloud to be registered is less than d max The points are added into the cropping frame, and the corresponding point cloud information is extracted from the high-precision map according to the content of the cropping frame.

[0070] Step 4: extract super points from the high-precision map point cloud obtained in step 3 and calculate their geometric information embedding. At the same time, count each point in the point cloud to be registered as a super point and calculate its geometric information embedding. Then, combine the geometric information embedding of the high-precision map point cloud and the point cloud to be registered, calculate the best matching super point pair between the two, and obtain a one-to-many buffer point pair between the point cloud to be registered and the high-precision map point cloud according to the best matching super point pair. This step is specifically as follows:

[0071] 4.1 Extracting Superpoints

[0072] For dense high-precision map point cloud data P, downsample it to obtain sparser point cloud data In the point cloud In the process, N points are obtained by uniform sampling, and these N points are regarded as super points. Then, the K nearest neighbor search algorithm is used to calculate the distance from each point in the original high-precision map point cloud P to each super point. Distance, find the K nearest neighboring points of each superpoint in Euclidean space For each superpoint Neighborhood points The relative position between the neighborhood point and the super point is encoded as the feature information of the super point. The specific encoding is: The three-dimensional coordinates of 3D coordinates of neighborhood points Relative coordinates Connect together as the feature information of the super point, and we get:

[0073]

[0074] 4.2 Calculate the distance between super points and embed information

[0075] First calculate the distance between any two super points Then, the feature dimension is calculated and the sensitivity parameter is introduced for control. Finally, the sine function is used to calculate the point pair distance embedding information. The formula is as follows:

[0076]

[0077] Among them, σ d The sensitivity of the control point to distance changes, d t represents the feature dimension, d i,j represents the distance between the i-th superpoint and the j-th superpoint.

[0078] 4.3 Calculating triplet angle embedding information

[0079] First, for each Calculate the angle α i,j,x =∠(Δ x,i ,Δ j,i ),in Δ x,i represents the vector representation between the i-th superpoint and the x-th superpoint, Δ j,i Represents the vector representation between the i-th superpoint and the j-th superpoint, and then uses the sine function to encode the triplet angle embedding information. The formula is as follows:

[0080]

[0081] Among them, σ α Controls the sensitivity of angle changes, d t represents the feature dimension, α i,j,xrepresents the angle between the vector representing the i-th superpoint and the x-th superpoint and the vector representing the i-th superpoint and the j-th superpoint;

[0082] 4.4 Geometric Information Embedding

[0083] Embed geometric information for each superpoint, including point-to-point distance information and triplet angle information. The specific formula is as follows:

[0084]

[0085] Among them, W D The projection matrix representing the distance information of the point pair, Represents the point pair distance embedding information, represents the triplet angle embedding information, W A Projection matrix representing the angular embedding information of the triplet.

[0086] 4.5 Calculate buffer point pairs

[0087] Each point in the radar point cloud is counted as a superpoint, and its geometric information embedding is calculated in the same way as the HD map. Combining the distance information and embedded geometric information between the HD map point cloud and the radar point cloud, the distance of the geometric information between each two superpoints is calculated, and the superpoint pair with a distance less than the threshold is selected as the best matching superpoint pair. According to the encoding method of the superpoint in 4.1, the best matching superpoint obtained in the HD map is expanded back to the original point, forming a one-to-many buffer point pair A={a i} i＝1,...N and B = {b i,j} i＝1,...N,j＝1,...K , and calculate the weight of each buffer point pair according to the distance between the original point in the high-precision map and its corresponding super point W = {ω i,j} i＝1,...N,j＝1,...K .

[0088] Step 5: Use the GICP algorithm to accurately align the buffer point pairs between the point cloud to be registered obtained in step 4 and the high-precision map point cloud; this step specifically includes:

[0089] 5.1 Removing Outlier Pairs

[0090] Add a minimum matching threshold, compare the weight of the buffer point pair calculated in 4.5 with the minimum threshold, remove all buffer point pairs with weights less than the maximum threshold, and obtain the point cloud A with the correct correspondence = {a i} i＝1,...N And point cloud B = {b i,j} i＝1,...N,j＝1,...K ;

[0091] 5.2 Calculate the transformation that minimizes the distance between corresponding points on the point cloud with correct correspondence

[0092] Assume there is a set of potential points, and For each point a i ,b i , all satisfy the Gaussian distribution, that is, and Based on this, A and B can be generated, where and represents the covariance matrix associated with the measurement points;

[0093] Assume that the correct transformation matrix is ​​T * ,have For any rigid transformation, we have It represents the distance between the i-th point pair after the original point cloud is changed by T, and it is considered that The distribution of is known.

[0094] Due to a i and b i,j Assuming they are independent and identically distributed under Gaussian distribution, we have:

[0095]

[0096] Embed the weight information and use the maximum likelihood estimation method to iteratively calculate T, that is:

[0097]

[0098] Considering any high-dimensional Gaussian distribution x~N(μ,Σ), x represents a random variable that satisfies the high-dimensional Gaussian distribution, μ,Σ represent the mean and variance of x respectively, and the probability density function of x can be expanded into the following form:

[0099]

[0100] Taking the negative logarithm, it becomes:

[0101]

[0102] Maximizing the original distribution is equivalent to minimizing the negative logarithm. Therefore, when minimizing x in the above formula, the first term is independent of x and can be omitted. Therefore, as long as the quadratic term on the right side is minimized, the maximum likelihood estimate of the state is obtained. Substituting it into the solution equation of the transformation matrix, it can be simplified to obtain:

[0103]

[0104] 5.3 Iterative Optimization

[0105] Repeat step 5.2 until the desired transformation matrix is ​​converged or the maximum number of iterations is reached, and the final transformation matrix T is obtained.

[0106] Step 6: Complete the registration between the point cloud data to be registered

[0107] Based on the obtained transformation matrix T between the point cloud to be registered and the high-precision map, the registration between the point cloud data to be registered is completed directly on the high-precision map reference system.

[0108] The above are only preferred embodiments of the present invention, and are not intended to limit the implementation methods and protection scope of the present invention. Those skilled in the art should be aware that all solutions obtained by equivalent substitutions and obvious changes made using the contents of the specification of the present invention should be included in the protection scope of the present invention.

Claims

1. A multi-view point cloud registration method based on high-precision map, characterized in that: The steps include: Step 1: Obtain the point cloud data to be registered and high-precision map data of different radars; Step 2: Convert the coordinate information of the point cloud data to be registered into the reference coordinate system of the high-precision map to roughly configure the point cloud data to be registered with the high-precision map data; Step 3: In the HD map reference coordinate system, extract the HD map point cloud according to the height and the distance between the point cloud data to be registered; Step 4: extract super points from the high-precision map point cloud obtained in step 3 and calculate their geometric information embedding. At the same time, count each point in the point cloud to be registered as a super point and calculate its geometric information embedding. Then, combine the geometric information embedding between the super points of the high-precision map and the super points of the point cloud to be registered, calculate the best matching super point pair between the two, and expand the one-to-many buffer point pairs between the point cloud to be registered and the original point cloud of the high-precision map according to the best matching super point pair. Step 5: Use the GICP algorithm to accurately align the point cloud to be registered obtained in step 4 with the buffered point pairs of the original point cloud of the high-precision map; Step 6: Based on the precise registration result of step 5, the registration between the point cloud data to be registered is completed in the high-precision map reference system; wherein, The method for obtaining a one-to-many buffer point pair in step 4 includes: according to the encoding method of the super point, the best matching super point obtained in the high-precision map is expanded back to the original point to form a one-to-many buffer point pair A between the radar point cloud and the original point cloud of the high-precision map. i } i＝1,...N and B = {b i,j } i＝1,...N,j＝1,...K , and calculate the weight of each buffer point pair according to the distance between the original point in the high-precision map and its corresponding super point W = {ω i,j } i＝1,...N,j＝1,...K ; Among them, the encoding method of the super point is: The three-dimensional coordinates of 3D coordinates of neighborhood points Relative coordinates Connect them as the feature information of the super point and get: Step 5 specifically includes the following steps: S51, set a maximum matching threshold, and remove incorrect correspondences between the radar point cloud and the original point cloud in the high-precision map according to the maximum matching threshold, and obtain the radar point cloud A with the correct correspondence. i } i＝1,...N and high-precision map point cloud B = {b i,j } i＝1,...N ; S52, radar point cloud A = {a i } i＝1,...N and high-precision map point cloud B = {b i,j } i＝1,...N Perform registration calculation to obtain a transformation matrix that minimizes the distance between corresponding point clouds; S53, repeat the iteration step S52 until the desired transformation matrix is ​​converged or the maximum number of iterations is reached.

2. The multi-view point cloud registration method based on high-precision map according to claim 1 is characterized in that: Step 2: Use the transformation matrix provided by GPS to perform matrix transformation on the point cloud to be registered, and transform the coordinate information of the point cloud data to be registered into the reference coordinate system of the high-precision map. The specific formula is as follows: In the formula, r l =(x l ,y l ,z l ) T Represents the coordinate information of the point in the radar coordinate system, r m =(x m ,y m ,z m ) T The coordinate information of the point converted to the high-precision map coordinate system; Represents the transformation matrix from the radar coordinate system to the high-precision map coordinate system provided by GPS, where R represents the rotation matrix, r 11 ,r 12 ,r 13 Represents the component of the point on the x-axis in the high-precision map coordinate system, r 21 ,r 22 ,r 23 Represents the component of the point on the y-axis in the high-precision map coordinate system, r 31 ,r 32 ,r 33 It represents the component of the point on the z-axis in the HD map coordinate system, t represents the translation matrix, and t1, t2, and t3 represent the distance of the point from the radar coordinate system to the HD map coordinate system in the x, y, and z directions.

3. The multi-view point cloud registration method based on high-precision map according to claim 1 is characterized in that: Step 3 specifically includes the following implementation methods: S3.

1. Calculate the height information of each point in the high-precision map and retain all points whose height is not less than the threshold; S3.

2. In the high-precision map reference coordinate system, for the high-precision map obtained in S3.1, calculate the shortest distance from each downsampled point to the point cloud to be registered; S3.

2. Set a distance threshold d max , traverse the high-precision map point cloud and extract the points whose closest distance calculated in step S3.2 is less than d max All points.

4. The multi-view point cloud registration method based on high-precision map according to claim 1 is characterized in that: Extracting super points in step 4 includes: For dense high-precision map point cloud data P, downsample it to obtain sparse point cloud data Consider it as a super point, and then use the K nearest neighbor search algorithm to calculate each super point The K nearest neighboring points in the Euclidean space of the original HD map point cloud For each superpoint Neighborhood points Encode the relative position of the neighborhood points and the superpoints. The three-dimensional coordinates of 3D coordinates of neighborhood points Relative coordinates Connect them as the feature information of the super point and get:

5. The multi-view point cloud registration method based on high-precision map according to claim 4 is characterized in that: The geometric information embedding of the superpoint in step 4 includes the point pair distance embedding information and the triplet angle embedding information.

6. The multi-view point cloud registration method based on high-precision map according to claim 5 is characterized in that: The calculation method of the point-to-point distance embedding information of the super point is: First, calculate the distance between any two hyperpoints Then according to the obtained distance d i,j The sine function is used to calculate the point-to-point distance embedding information of the superpoint, and the formula is as follows: Among them, σ d The sensitivity of the control point to distance changes, d t Represents the feature dimension.

7. The multi-view point cloud registration method based on high-precision map according to claim 5 is characterized in that: The calculation method of the triplet angle embedding information is: First, for each neighborhood point Calculate the angle α i,j,x =∠(Δ x,i ,Δ j,i ),in Δ x,i represents the vector representation between the i-th superpoint and the x-th superpoint, Δ j,i Represents the vector representation between the i-th superpoint and the j-th superpoint, and then uses the sine function to encode the triplet angle embedding information. The formula is as follows: Among them, σ α Controls the sensitivity of angle changes, d t represents the feature dimension, α i,j,x represents the angle between the i-th superpoint and the x-th superpoint representation vector and the i-th superpoint and the j-th superpoint representation vector.

8. The multi-view point cloud registration method based on high-precision map according to claim 5, characterized in that: The method for obtaining the buffer point pair is: Calculate the distance of geometric information between every two super points of the HD map and the point cloud to be registered, select the super point pair with a distance less than the threshold as the best matching super point pair, and then expand the best matching super point obtained in the HD map back to the original point to form a one-to-many buffer point pair A={a i } i＝1,...N and B = {b i,j } i＝1,...N,j＝1,...K .

9. The multi-view point cloud registration method based on high-precision map according to claim 1, characterized in that: The specific implementation of step 52 is as follows: S521. Assume that there is a set of potential points. and And according to and Generate A and B, where and represents the covariance matrix associated with the measurement points; S522, assuming the correct change matrix is ​​T * ,have For any rigid transformation, we have It represents the distance between the i-th point pair after the original point cloud is changed by T, and it is considered that The distribution of is known; S523, due to a i and b i,j Assuming they are independent and identically distributed under Gaussian distribution, we have: S524, use the maximum likelihood estimation method to iteratively calculate T, that is: In the formula, ω i,j Represents the weight of the buffer point pair.

Images