Three-Dimensional Sensors Lecture 6: Point-Cloud Registration

Transcription

1 Three-Dimensional Sensors Lecture 6: Point-Cloud Registration Radu Horaud INRIA Grenoble Rhone-Alpes, France

2 Point-Cloud Registration Methods Fuse data gathered with several 3D sensors or with a moving 3D sensor. Each sensor provides a point cloud in a sensor-centered coordinate frame. There is only a partial overlap between the two point clouds. The data are corrupted both by noise and by outliers (or gross errors). The two point clouds must be registered or represented in the same coordinate frame. The registration process requires point-to-point correspondences which is a difficult problem.

3 Estimating the Transformation Between Two Point Clouds We start by assuming that the two point clouds have an equal number of points and that the point-to-point correspondences are known in advance. Notations: Each point cloud has N points {X n } n=n n=1 and {Y n } n=n n=1. X and Y are the centers of the two point clouds. X n = X n X and Ŷ n = Y n Y are the centered coordinates. The transformation between the two clouds is determined by a scale factor s, a rotation matrix R and a translation vector t. A point in one cloud is the transformed of its corresponding point in the other cloud: Y n = srx n + t

4 Least-Square Minimization The transformation parameters can be determined by minimization of: E = N Y n srx n t 2 n=1 Let s develop the quadratic error E (using R R = I): E = (Y n srx n t) (Y n srx n t) ( ) ( ) = s 2 X n X n + 2s X n R t n n ( ) ( ) 2s X n R Y n 2 Y n t + n n n Y n Y n + Nt t

5 The Optimal Translation t = argmin t E The derivative of the error with respect to t writes: de dt = 2sR n X n 2 n Y n + 2Nt Setting the derivative with respect to the translation vector equal to 0 t = Y srx

6 A Simplified Criterion By substitution of the optimal translation in the error function, we obtain: N E = Ŷ n sr X n 2 n=1 ( ) ( ) = s 2 X X n n 2s X n R Ŷ n + n n n Ŷ n Ŷ n

7 The Optimal Scale Factor s = argmin s E Which yields: s = n (R X n ) Ŷ n n X n X n Using a symmetric treatment of the scale factor [Horn 87] shows that the error: E = N ( s) 1 Ŷ n sr X n 2 n=1 yields the following solution: s = n Ŷ n Ŷ n n X n X n

8 The Optimal Rotation From the expansion of the error function we obtain that: ( ) R = argmin E = argmax X n R Ŷ n R R and: Q = n Ŷ ( n R X n = tr R X n Ŷ ) n n = tr(rc XY ) Solution: Let C XY = USV be the singular value decomposition (SVD) of the data co-variance matrix. The optimal rotation matrix is (see [ArunHuangBlostein 87] for more details): R = VU

9 Point Registration We assume now that the point-to-point correspondences are not known. The minimization criterion becomes: E = M m=1 n=1 N α mn Y m srx n t 2 Possible representations for the assignment variables α mn : 1 Binary assignment variables: α mn = 1 if Y m is in correspondence with X n and α mn = 0 otherwise. 2 Soft assignment variables: They are subject to the constraints m α mn = n α mn = 1, 0 α mn 1. 3 Probabilistic assignment variables: α mn = P (Y m = n Y m ) or the posterior probability of assigning point m to the point n.

10 The ICP Algorithm This is the most used point-registration method: iterative closest point (ICP) proposed by [BeslMcKay 92]: 1 Initialize the scale, rotation and translation parameters; 2 Transform each point X n and search its closest point Y k = argmin m Y m srx n t 2 and set α kn = 1; 3 Estimate the parameters minimizing: E = M α nk Y k srx n t 2 k=1 4 Repeat steps 2 and 3 until convergence (the parameters do not change any more).

11 Drawbacks of ICP It is very sensitive to the initial transformation parameters (it is trapped in a local solution). It is not robust to gross errors in the data. It cannot deal with anisotropic noise (the noise has the same variance along all the coordinates).

12 Accelerating ICP If the point clouds contain a large number of points, each iteration of the algorithm is rather slow. One can either sample the data, or better, apply K-means clustering to each point cloud. The cluster centers are then used in ICP. Step 2 is the most time-consuming process of the algorithm and needs an efficient implementation of searching for the nearest point. One can use KD-trees to structure one set of points as a binary tree and to rapidly perform a query into this tree in order to retrieve the closest point. Many variants of ICP were proposed in the vision/graphics literature.

13 Probabilistic Point Registration The minimization criterion is now generalized to: E = M m=1 n=1 N ( α mn Y m srx n t 2 C n + log C n ) The posterior probabilities are defined by: α mn = C n 1 2 exp ( 1 2 Y m srx n t 2 C n ) N k=1 C k 1 2 exp ( 1 2 Y m srx k t 2 C k ) + 3D Mahalanobis distance: a b 2 C = (a b) C 1 (a b)

14 Point Registration with Expectation-Maximization (EM) 1 Initialize the transformation parameters s, R, t and the covariance matrices {C n } N n=1. 2 Estimate the posterior probabilities {α mn } m=m,n=n m=1,n=1. 3 Estimate the transformation parameters s, R, t. 4 Estimate the covariance matrices. 5 Repeat steps 2, 3, and 4 until convergence

15 Discussion The EM algorithm for point registration uses a Gaussian mixture model (GMM) augmented with a uniform-distribution component that captures the outliers. The estimation of the transformation parameters do not need explicit point-to-point correspondences. However, such correspondences can be easily established using: if α mk 0.5 accept the assignment Y m X k with α mk = argmax n α mn. The algorithm is robust to initialization, Gaussian noise and uniformly distributed gross errors (outliers), it may however be trapped in local minima. It is less efficient than ICP because of the computation of the probabilities α mn. See [Horaud et al 2011] for more details.

16 An Example The first set: 15 points (blue circles) The second set: 25 points of which 15 inliers corrupted with Gaussian noise (green squares) and 10 outliers drawn from a uniform distribution (red squares). Initialization: R = I and t = 0. Result: 12 point-to-point correspondences found correctly and 7 point-to-outlier assignments.

17 The 2-nd Iteration

18 The 6-th Iteration

19 The 35-th and Final Iteration

20 Projects Using 3-D Sensors and Point Registration The Great Budha project (University of Tokyo): KinectFusion project: en-us/projects/surfacerecon/ The MixCam project: