Using Augmented Measurements to Improve the Convergence of ICP. Jacopo Serafin and Giorgio Grisetti

Transcription

1 Jacopo Serafin and Giorgio Grisetti

2 Point Cloud Registration We want to find the rotation and the translation that maximize the overlap between two point clouds Page 2

3 Point Cloud Registration We want to find the rotation and the translation that maximize the overlap between two point clouds Page 3

4 Point Cloud Registration We want to find the rotation and the translation that maximize the overlap between two point clouds Page 4

5 Point Cloud Registration We want to find the rotation and the translation that maximize the overlap between two point clouds Page 5

6 Point Cloud Registration We want to find the rotation and the translation that maximize the overlap between two point clouds Page 6

7 Point Cloud Registration We want to find the rotation and the translation that maximize the overlap between two point clouds Page 7

8 Practical Applications Point cloud registration is a crucial building block for many applications: SLAM Object detection and recognition Augmented reality Page 8

9 Iterative Closest Point (ICP) 1 Algorithm ICP iteratively refine an initial transform T by alternating: search of correspondences between the two point clouds and the optimization step to update the current transform 1. Besl, P.J., McKay, N.D.: A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence (1992) Page 9

10 Iterative Closest Point (ICP) 1 Algorithm ICP iteratively refine an initial transform T by alternating: search of correspondences between the two point clouds and the optimization step to update the current transform 1. Besl, P.J., McKay, N.D.: A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence (1992) Page 10

11 ICP Optimization Function T = argmin T C (p i T p j ) T (p i T p j ) At each iteration, ICP minimizes the squared Euclidean norm starting from the current transform T A point is a vector representing the coordinates in R 3 Page 11

12 ICP Variant: Generalized ICP (GICP) 2 T = argmin T Assumption: points are sampled from a locally continuous and smooth surface It minimizes a Mahalanobis distance As for standard ICP, a point is a vector representing the coordinates in R 3 C (p i T p j ) T Ω ij (p i T p j ) Information Matrix 2. Segal, A.V., Haehnel, D., Thrun, S.: Generalized-ICP. In: Proc. of Robotics: Science ad Systems (SS) (2009) Page 12

13 How to Improve ICP and its Variants The optimization and correspondences search are not independent An optimization step more robust to outliers and with a smoother behavior improves the overall results of the algorithm We need something more than a point to describe a surface element. The normal is a good candidate Page 13

14 Augmented Measurements Our point is a 6D vector representing the Euclidean coordinates and the surface normal p = (p x p y p z n x n y n z ) T Page 14

15 How to Compute the Information Matrix The goal is to have an Ω ij that scales the error so that: The normals of the points align The distance along the tangent plane is neglected The distance along the normal direction is penalized Page 15

16 Point Cloud Registration Examples Page 16

17 Convergence Speed Example Page 17

18 Robustness Example Page 18

19 Experiments We evaluated our algorithm against ICP and Generalized ICP We compared the evolution of the translational and rotational part of the error We performed experiments both on simulated data and on real standard benchmarking datasets 3 Simulated data plots show the evolution of the error under different percentage of outliers in the correspondences search Real data plots show the evolution of the error at different frame skipping rate 3. Sturm, J., Engelhard, N., Endres, F., Burgard, W., Cremers, D.: A benchmark for the evaluation of rgb-d slam systems. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS) (2012) Page 19

20 Real Data Experiments Page 20

21 Simulated Data Experiments Page 21

22 Conclusions We achieved the following results: Increased robustness versus outliers and to frame skipping Faster convergence speed Better point cloud alignment Open source implementation available at: Page 22

23 Bibliography 1. Besl, P.J., McKay, N.D.: A method for registration of 3- D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence (1992) 2. Segal, A.V., Haehnel, D., Thrun, S.: Generalized-ICP. In: Proc. of Robotics: Science ad Systems (SS) (2009) 3. Sturm, J., Engelhard, N., Endres, F., Burgard, W., Cremers, D.: A benchmark for the evaluation of rgb-d slam systems. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS) (2012) Page 23