MLESAC and SVD-based coordinate transformation correction method for 3D point cloud space volume identification

Abstract

The invention relates to a coordinate transformation correction method for 3D point cloud space volume identification based on MLESAC and SVD. The coordinate transformation correction method comprises the following steps: preprocessing original point cloud data; extracting a reference plane, and constructing a mixed probability model through an MLESAC algorithm; calculating an optimal rotation matrix and an optimal translation vector by using the normal vector on the reference plane and the normal vector on the three-dimensional coordinate system and adopting an SVD (Singular Value Decomposition) algorithm; according to the obtained optimal rotation matrix and translation vector, rotation transformation and translation transformation are carried out on all point sources in the source point set P, and the point sources are made to fall on a target plane; and outputting the corrected point cloud, verifying the standard deviation of the distance from the corrected reference plane to the target plane, and ensuring that the standard deviation is close to zero. The beneficial effects of the invention are that through robust registration and accurate transformation, the problem of point cloud space coordinate offset caused by installation deviation of 3D scanning equipment is solved, and the accuracy and efficiency of cargo volume measurement are significantly improved.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of cargo volume measurement, and particularly relates to a coordinate transformation and correction method for 3D point cloud space volume recognition based on a MLESAC (Maximum Likelihood Estimation Sample and Consensus) algorithm and an SVD (Singular Value Decomposition) algorithm. BACKGROUND

[0002] In the fields of warehouse management, logistics transportation and inventory management, it is crucial to quickly and accurately measure the volume of goods. The traditional manual measurement method is inefficient and prone to human error, which cannot meet the efficient and accurate management needs of modern enterprises.

[0003] The 3D point cloud technology can effectively improve work efficiency and measurement accuracy and provide better solutions for enterprises due to its non-contact acquisition of three-dimensional data of goods. The working principle of the 3D point cloud technology is as follows: a light beam is emitted to the goods through laser scanning, structured light scanning and other scanning methods; the light beam is reflected after contacting the surface of the object, and the device accurately calculates the three-dimensional coordinates of each point on the surface of the object according to the time difference and phase difference of the reflected light; a large number of coordinate points form point cloud data, which intuitively presents the three-dimensional profile of the goods. However, the 3D point cloud technology still has the following defects: in the actual application process, the 3D scanning device is installed at the top of the warehouse to scan the goods from a top-down perspective and obtain three-dimensional data of the goods (x, y, z) point cloud data with the 3D scanning device as the origin coordinate system). When the installation of the 3D scanning device deviates, the origin of the coordinate system will deviate, resulting in deviation of all 3D point cloud data in the coordinate system and reduction of data accuracy.

[0004] However, the installation deviation of the 3D scanning device cannot be avoided, and therefore a deviation correction method is needed to correct the point cloud data in the coordinate system and obtain accurate and reliable 3D point cloud data. SUMMARY

[0005] The purpose of the present application is to overcome the deficiencies in the prior art and provide a coordinate transformation and correction method for 3D point cloud space volume recognition based on MLESAC and SVD, which solves the problem of point cloud space coordinate deviation caused by installation deviation of the 3D scanning device through robust registration and accurate transformation, and significantly improves the accuracy and efficiency of cargo volume measurement.

[0006] A coordinate transformation and correction method for 3D point cloud space volume recognition based on MLESAC and SVD, comprising the following steps:

[0007] Step 1: Preprocess the raw point cloud data;

[0008] Step 2: Extract the reference plane, establish corresponding point sets for the reference plane and the target plane, construct a mixed probability model using the MLESAC algorithm (Maximum Likelihood Estimation Sampling and Consensus Algorithm), maximize the total likelihood value of all source points, and calculate the probability that the current source point belongs to an interior point:

[0009] Step 3: Using the normal vectors on the reference plane and the normal vectors on the three-dimensional coordinate system, the SVD algorithm (Singular Value Decomposition) is used to calculate the optimal rotation matrix and translation vector;

[0010] Step 4: Based on the obtained optimal rotation matrix and translation vector, perform rotation and translation transformations on all points in the source point set P to make them fall on the target plane. The purpose of transforming the points in the source point set is to minimize the alignment error between the reference plane and the target plane Z = h. This application transforms the alignment problem between the reference plane and the target plane into a registration problem between the source point set and the target point set (i.e., during registration, the normal vector of the reference plane is no longer made to coincide with the normal vector of the target plane, but rather each source point p on the reference plane is made to...). i All points are moved as far as possible to the target height h directly below the source point (this is more intuitive and better aligns with the ultimate goal of "correction"); thus obtaining the corrected point cloud.

[0011] Step 5: Output the corrected point cloud and verify the standard deviation of the distance from the reference plane to the target plane after correction, ensuring that the standard deviation is close to zero.

[0012] Preferably, performing median filtering on the original point cloud data in step 1 can suppress reflection noise in the calibration board point cloud and reduce the error of SVD in solving the rotation matrix.

[0013] As a preferred option, step 2 specifically involves:

[0014] A plane (e.g., ground A) is obtained using a 3D scanning device. Points on the plane are fitted to obtain the surface function of the plane. The plane is used as a reference plane and aligned with the horizontal plane of the three-dimensional coordinate system with the 3D scanning device as the origin. The plane containing all the corrected point clouds is set as the target plane Z = h.

[0015] Establish corresponding point sets for the reference plane and the target plane respectively: use the extracted point cloud of the reference plane as the source point set P = {p1, p2, ..., p...} n}, where the i-th source point p i ={x i ,y i ,z i}; for each source point p i Define a target point q iThe target point q i It is the source point p i The vertical projection point q on the target plane Z = h i ={x i ,y i ,h};

[0016] The MLESAC algorithm transforms the bias correction problem into a maximum likelihood estimation problem. The MLESAC algorithm assumes that the point cloud data in the source point set P consists of inliers that follow a Gaussian distribution and outliers that follow a uniform distribution.

[0017] Construct a mixture probability model to find the mixture probability P that is most likely to generate the current source point. i Model parameters γ and σ:

[0018]

[0019] In the above formula, r i σ is the residual (distance), γ is the inlier rate (i.e. the parameter to be estimated), σ is the standard deviation of the Gaussian distribution followed by the inliers, and v is the interval size of the uniform distribution followed by the outliers.

[0020] Maximize the total likelihood of all source points according to the following formula:

[0021]

[0022] (Or minimize the negative log-likelihood); in the above formula, θ is the rotation angle;

[0023] Based on the most likely source point p to be generated i Given the model parameters γ and σ for the mixed probability, calculate the probability P that the current source point belongs to an interior point. i ;

[0024]

[0025] As a preferred option, step 3 specifically involves:

[0026] The centroids of the source and target point sets are calculated using the following formula:

[0027]

[0028] In the above formula, n represents the number of point clouds, and μ p μ represents the centroid (geometric center) of the source point set. q Represents the centroid (geometric center) of the target point set;

[0029] The centered source and target point sets are calculated using the following formula:

[0030] p′ i =pi -μ p

[0031] q′ i =q i -μ q

[0032] In the above formula, p′ i Represented by the centroid μ p Let q′ be the relative coordinates of the origin. i Indicated by μ q The relative coordinates with the origin (by subtracting the centroid, the problem is transformed from an "absolute coordinate system" to a "relative coordinate system with the centroid as the origin". The purpose of doing this is to decouple rotation and translation: in the relative coordinate system, only the rotation needs to be solved, and the translation problem is solved by the difference of the centroids).

[0033] The covariance matrix H is calculated using the following formula:

[0034]

[0035] The singular value decomposition of the covariance matrix H is performed using the SVD algorithm:

[0036] H=U∑V T

[0037] In the above formula, U represents a 3×3 orthogonal matrix, and U contains the left singular vector of H; ∑ represents a 3×3 diagonal matrix, and ∑ contains the left singular vector of H; V represents a 3×3 orthogonal matrix, and V contains the right singular vector of H.

[0038] The optimal rotation matrix is ​​obtained by calculating the following formula:

[0039] R = VU T

[0040] The optimal translation vector is obtained by calculating the following formula:

[0041] t = μ q -Rμ p ;

[0042] The optimal rotation matrix and optimal translation vector mentioned above minimize the mean square error between the source point set and the target point set in the following equation (i.e., the points in the source point set and the target point set can be aligned):

[0043]

[0044] As a preferred method, the specific way to obtain the corrected point cloud in step 4 is as follows:

[0045] p corrected =R·p original +t

[0046] In the above formula, p original Let p represent the original point. corrected Indicates the correction point.

[0047] The beneficial effects of this invention are:

[0048] This invention employs median filtering preprocessing to quickly suppress impulse noise in the original point cloud data, reducing the computational load of subsequent steps; it uses the MLESAC algorithm to construct a hybrid probability model, which reduces invalid iterations; by maximizing the total likelihood value to adaptively select interior points, it can effectively resist scanning noise and reduce the impact of outliers on registration; it also combines SVD decomposition to obtain the optimal rigid body transformation, providing a closed-form solution, which can avoid iteration convergence delay; it directly outputs the optimal solutions of the rotation matrix and translation vector, ensuring the accuracy of coordinate transformation; this invention can guarantee that the standard deviation of the distance from the reference plane to the target plane after correction approaches zero.

[0049] The method of this invention is applicable to the installation deviation correction of top-down scanning in warehousing and logistics scenarios. It can directly process the original point cloud with the equipment as the origin. It transforms the plane alignment problem into a point set registration problem and intuitively achieves height correction by vertically projecting the target point. It does not rely on the strict coincidence of normal vectors and can adapt to the actual installation tilt angle. It can output the correction point cloud to maintain the original shape of the object and ensure the reliability of the identified volume. Attached Figure Description

[0050] Figure 1 This is a flowchart of the correction method of the present invention;

[0051] Figure 2 This is a schematic diagram illustrating the acquisition of point cloud data of an object under test using a 3D scanning device (radar).

[0052] Figure 3 These are point cloud data images before and after correction; light color represents the original point cloud data, and dark color represents the point cloud data after correction.

[0053] Figure 4 This is a schematic diagram illustrating the correction effect of the correction method of the present invention;

[0054] Figure 5 This is a schematic diagram for comparing the stacked volume of items within the scanned area.

[0055] Figure 6 This is a schematic diagram of the point cloud distribution for case 1;

[0056] Figure 7 This is a schematic diagram of the point cloud distribution in case 2;

[0057] Figure 8 This is a schematic diagram of the point cloud distribution in case 3;

[0058] Figure 9 This is a schematic diagram of the point cloud distribution in case 4.

[0059] Explanation of the attached diagram labels: Radar 1, Object under test 2. Detailed Implementation

[0060] The present invention will be further described below with reference to embodiments. The description of the embodiments below is only for the purpose of helping to understand the present invention. It should be noted that those skilled in the art can make several modifications to the present invention without departing from the principle of the present invention, and these improvements and modifications also fall within the protection scope of the claims of the present invention.

[0061] As one example, such as Figure 1 As shown, a coordinate transformation and correction method for 3D point cloud spatial volume recognition based on MLESAC and SVD is presented, with the following specific steps:

[0062] Step 1: Median filtering is applied to the original point cloud data to suppress reflection noise in the calibration board point cloud, thereby reducing the error in solving the rotation matrix using SVD.

[0063] Step 2: Extract the reference plane, establish corresponding point sets for the reference plane and the target plane, construct a mixed probability model using the MLESAC algorithm (Maximum Likelihood Estimation Sampling and Consensus Algorithm), maximize the total likelihood value of all source points, and calculate the probability that the current source point belongs to an interior point:

[0064] like Figure 2 As shown, a plane (e.g., ground A) is obtained through a 3D scanning device. The points on the plane are fitted to obtain the surface function of the plane. The plane is used as a reference plane and aligned with the horizontal plane of the three-dimensional coordinate system with the 3D scanning device as the origin. The plane containing all the corrected point clouds is set as the target plane Z = h.

[0065] Establish corresponding point sets for the reference plane and the target plane respectively: use the extracted point cloud of the reference plane as the source point set P = {p1, p2, ..., p...} n}, where the i-th source point p i ={x i ,y i ,z i}; for each source point p i Define a target point q i The target point q i It is the source point p i The vertical projection point q on the target plane Z = h i ={x i ,y i ,h};

[0066] The MLESAC algorithm transforms the bias correction problem into a maximum likelihood estimation problem. The MLESAC algorithm assumes that the point cloud data in the source point set P consists of inliers that follow a Gaussian distribution and outliers that follow a uniform distribution.

[0067] Construct a mixture probability model to find the mixture probability P that is most likely to generate the current source point. i Model parameters γ and σ:

[0068]

[0069] In the above formula, r i σ is the residual (distance), γ is the inlier rate (i.e. the parameter to be estimated), σ is the standard deviation of the Gaussian distribution followed by the inliers, and v is the interval size of the uniform distribution followed by the outliers.

[0070] Maximize the total likelihood of all source points according to the following formula:

[0071]

[0072] (Or minimize the negative log-likelihood); in the above formula, θ is the rotation angle;

[0073] Based on the most likely source point p to be generated i Given the model parameters γ and σ for the mixed probability, calculate the probability P that the current source point belongs to an interior point. i ;

[0074]

[0075] Step 3: Using the normal vectors on the reference plane and the normal vectors on the three-dimensional coordinate system, the SVD algorithm (Singular Value Decomposition) is used to calculate the optimal rotation matrix and translation vector;

[0076] The centroids of the source and target point sets are calculated using the following formula:

[0077]

[0078] In the above formula, n represents the number of point clouds, and μ p μ represents the centroid (geometric center) of the source point set. q Represents the centroid (geometric center) of the target point set;

[0079] The centered source and target point sets are calculated using the following formula:

[0080] p′ i =p i -μ p

[0081] q′ i =q i -μq

[0082] In the above formula, p′ i Represented by the centroid μ p Let q′ be the relative coordinates of the origin. i Indicated by μ q The relative coordinates with the centroid as the origin (by subtracting the centroid, the problem is transformed from an "absolute coordinate system" to a "relative coordinate system with the centroid as the origin". The purpose of doing this is to decouple rotation and translation: in the relative coordinate system, only the rotation needs to be solved, and the translation problem is solved by the difference in centroids).

[0083] The covariance matrix H is calculated using the following formula:

[0084]

[0085] The singular value decomposition of the covariance matrix H is performed using the SVD algorithm:

[0086] H=U∑V T

[0087] In the above formula, U represents a 3×3 orthogonal matrix, and U contains the left singular vector of H; ∑ represents a 3×3 diagonal matrix, and ∑ contains the left singular vector of H; V represents a 3×3 orthogonal matrix, and V contains the right singular vector of H.

[0088] The optimal rotation matrix is ​​obtained by calculating the following formula:

[0089] R = VU T

[0090] The optimal translation vector is obtained by calculating the following formula:

[0091]

[0092] The optimal rotation matrix and optimal translation vector mentioned above minimize the mean square error between the source point set and the target point set in the following equation (i.e., the points in the source point set and the target point set can be aligned):

[0093]

[0094] Step 4: Based on the obtained optimal rotation matrix and translation vector, perform rotation and translation transformations on all points in the source point set P, so that they fall on the target plane Z = h. The purpose of transforming the points in the source point set is to minimize the alignment error between the reference plane and the target plane Z = h. This application transforms the alignment problem between the reference plane and the target plane into a registration problem between the source point set and the target point set (i.e., during registration, the normal vector of the reference plane is no longer made to coincide with the normal vector of the target plane, but rather each source point p on the reference plane is made to...). iAll points are moved as far as possible to the target height h directly below the source point (this is more intuitive and better aligns with the ultimate goal of "correction"); the corrected point cloud is obtained:

[0095] p corrected =R·p original +t

[0096] Step 5: Output the corrected point cloud and verify the standard deviation of the distance from the reference plane to the target plane, ensuring the standard deviation is close to zero; Figure 3 It can be seen that the point cloud data has been corrected; according to Figure 4 As can be seen, after the correction, the object retains its original shape, and the coordinate system is corrected to the standard coordinates under the assembly. The data can be applied based on the corrected data.

[0097] The coordinate transformation and correction method for 3D point cloud spatial volume recognition based on MLESAC and SVD in the above embodiments was used to conduct targeted experiments. The experimental process and results are as follows:

[0098] like Figure 5 As shown, a comparative test was conducted on the stacked volume of items within the scanned area:

[0099] 1. Adjust the radar installation position to ensure it is level and without deviation. Collect and measure volume data multiple times at the same installation position. The volume to be measured manually is approximately 30m³. 3 The specific details of the measured volume data are shown in Table 1 below:

[0100] Table 1. Actual test volume data

[0101] Serial number 1 2 3 4 5 6 7 8 Measured volume (m 3 )]]> 30.23 30.19 30.26 30.31 30.34 30.21 30.18 30.29

[0102] The mean value of the measured volume data in Table 1 above is 30.25125 m³. 3 The variance is 0.0034125m. 3 The maximum value of the measured volume data is 30.34, the minimum value is 30.18, and the maximum fluctuation value is 30.34-30.18=0.16; the ratio of the maximum fluctuation value to the mean is: 0.16 / 30.25125=0.52%.

[0103] 2. Adjust different installation angles (installation deviation approximately 3° to 5°, direction random) to introduce a certain deviation in the point cloud scanned by the radar; for example... Figures 6 to 9Ten point cloud acquisitions were performed for each of the four scenarios shown. Volume was calculated using source and target points. The coordinate transformation and correction method for 3D point cloud spatial volume recognition based on MLESAC and SVD in this embodiment was applied to calculate and statistically analyze the measurement data, which includes the mean and the ratio of the maximum fluctuation to the mean. The statistical results are shown in Table 2 below.

[0104] Table 2. Data measurement table calculated under four different conditions.

[0105] Case 1 Case 2 Case 3 Case 4 Mean value after origin calculation 28.26133 28.76133 26.76133 24.76133 Maximum fluctuation proportion after origin calculation 0.48％ 0.61％ 0.57％ 0.59％ Corrected mean value 30.19625 30.22362 30.41221 30.01667 Maximum fluctuation proportion after correction 0.53％ 0.59％ 0.52％ 0.49％

[0106] Take scenario 2 as an example:

[0107] After multiple samplings, the calculation was performed using the origin. The maximum fluctuation in the measured volume was approximately 0.61%, and the fluctuation was very small after multiple samplings, which is consistent with the measurement error.

[0108] The mean value of the origin measurement is 28.76133, and the error compared with the horizontal installation in Experiment 1 is approximately 30.25125 - 28.76133 = 1.48992; the error rate is (30.25125 - 28.76133) / 30.25125 = 4.925%. After correction, the calculated value is 30.22362, and the error compared with the mean value of 30.25125 is approximately 30.25125 - 30.22362 = 0.02763; the error rate is (30.25125 - 30.22362) / 30.25125 = 0.0912%.

[0109] Therefore, it can be concluded that the point cloud obtained by applying the correction method proposed in this invention can improve the calculation accuracy and better reflect the actual situation of cargo stacking. The correction method proposed in this invention has practical significance and good application effect.

Claims

1. A coordinate transformation and correction method for 3D point cloud spatial volume recognition based on MLESAC and SVD, characterized in that, Includes the following steps: Step 1: Preprocess the raw point cloud data; Step 2: Extract the reference plane, establish corresponding point sets for the reference plane and the target plane, construct a mixture probability model using the MLESAC algorithm, maximize the total likelihood of all source points, and calculate the probability that the current source point belongs to an interior point: Step 3: Using the normal vectors on the reference plane and the normal vectors on the three-dimensional coordinate system, the SVD algorithm is used to calculate the optimal rotation matrix and translation vector; Step 4: Based on the obtained optimal rotation matrix and translation vector, perform rotation and translation transformations on all point sources in the source point set P, so that they fall on the target plane; thus obtaining the corrected point cloud. Step 5: Output the corrected point cloud, verify the standard deviation of the distance from the reference plane to the target plane after correction, and ensure that the standard deviation is close to zero.

2. The coordinate transformation and correction method for 3D point cloud spatial volume recognition based on MLESAC and SVD according to claim 1, characterized in that, In step 1, the original point cloud data is subjected to median filtering.

3. The coordinate transformation and correction method for 3D point cloud spatial volume recognition based on MLESAC and SVD according to claim 1, characterized in that, Step 2 specifically involves: A plane is obtained by using a 3D scanning device. Points on the plane are fitted to obtain the surface function of the plane. The plane is used as a reference plane and aligned with the horizontal plane of the three-dimensional coordinate system with the 3D scanning device as the origin. The plane containing all the corrected point clouds is set as the target plane Z = h. Establish corresponding point sets for the reference plane and the target plane respectively: use the extracted point cloud of the reference plane as the source point set P = {p1, p2, ..., p...} n }, where the i-th source point p i ={x i ,y i ,z i }; for each source point p i Define a target point q i The target point q i The source point p i The vertical projection point q on the target plane Z = h i ={x i ,y i ,h}; The MLESAC algorithm assumes that the point cloud data in the source point set P consists of inliers that follow a Gaussian distribution and outliers that follow a uniform distribution. Construct a mixture probability model to find the mixture probability P that is most likely to generate the current source point. i Model parameters γ and σ: In the above formula, r i σ is the residual, γ is the inlier rate, σ is the standard deviation of the Gaussian distribution followed by the inliers, and v is the interval size of the uniform distribution followed by the outliers. Maximize the total likelihood of all source points according to the following formula: In the above formula, θ is the rotation angle; based on the most likely generation of the current source point p... i Given the model parameters γ and σ for the mixed probability, calculate the probability P that the current source point belongs to an interior point. i ; 4. The coordinate transformation and correction method for 3D point cloud spatial volume recognition based on MLESAC and SVD according to claim 3, characterized in that, Step 3 specifically involves: The centers of the source and target point sets are calculated using the following formula: In the above formula, n represents the number of point clouds, and μ p μ represents the centroid of the source point set. q Represents the centroid of the target point set; The centered source and target point sets are calculated using the following formula: p′ i =p i -μ p q′ i =q i -μ q In the above formula, p′ i Represented by the centroid μ p Let q′ be the relative coordinates of the origin. i Indicated by μ q The relative coordinates of the origin; The covariance matrix H is calculated using the following formula: The singular value decomposition of the covariance matrix H is performed using the SVD algorithm: H=U∪V T In the above formula, U represents a 3×3 orthogonal matrix, and U contains the left singular vector of H; ∑ represents a 3×3 diagonal matrix, and ∑ contains the left singular vector of H; V represents a 3×3 orthogonal matrix, and V contains the right singular vector of H. The optimal rotation matrix is ​​obtained by calculating the following formula: R=VU T The optimal translation vector is obtained by calculating the following formula: t=μ q -Rμ p ; The optimal rotation matrix and optimal translation vector described above minimize the mean square error between the source point set and the target point set in the following equation:

5. The coordinate transformation and correction method for 3D point cloud spatial volume recognition based on MLESAC and SVD according to claim 4, characterized in that, The specific method for obtaining the corrected point cloud in step 4 is as follows: p corrected =R·p original +t In the above formula, p original Let p represent the original point. corrected Indicates the correction point.

Images