Point Cloud Registration (PCR)

General Purpose in SLAM

Whenever our robot updates its pose, it takes wheel odometry data to infer how far the robot moved and where, and combines it with the most recent pose to estimate a new pose. This newly estimated pose is then added to the pose graph, and used for the next update.

Robot estimates its movement (red arrow) and adds that to its previous pose (green) to compute its current pose (blue) in its computed map.

However, wheel odometry inaccuracies lead to incorrect pose estimation. This can be mitigated by also using LiDAR data. The robot additionally saves LiDAR scans for each pose, and compares subsequent scans to verify/update its original pose estimation. This process is called point cloud registration, and is also used in loop closure to compare two non-subsequent poses and check whether the robot is in a familiar location.

PCR Overview

Mathematically, the following two questions are equivalent:

  1. How did the robot move from pose A to B?
  2. How can we map scan B to scan A?

Point cloud registration, also called scan-matching, aims to answer the second question. (Note: the comparison direction between A/B is flipped because as the robot moves forward, the scans move backward, and etc.) We explored two algorithms, ICP and NDT, and ultimately used ICP for our SLAM implementation. We provide a high level overview of both below.

Mathematically, the two tasks are equivalent.

Iterative Closest Point (ICP)

We call the scan we want to map (B) the "source", and the scan we want to match (A) as the "destination". ICP iteratively shifts the source to the destination by iterating through each source point and finding its nearest destination point. It then uses homogenous coordinates and SVD computation to deconstruct the covariance matrix of both points and derive the transformation that minimizes these points.

We can combine all individual point differences (small arrows) into one transformation that minimizes these differences (big red arrow).

This transformation is not perfect, since some points select neighbors that should actually be further away. However, we still move our source closer to the destination. We thus repeat this process, selecting new neighbors, until the source converges to the destination. The final transformation matrix is our desired robot pose transformation.

In practice, ICP is not robust, and often requires similar scans to correctly find the optimum transformation. (This is why our robot updates its pose every 5cm.) When ICP works, it is very accurate, though convergence is slow.

Normal Distribution Transform (NDT)

NDT is also an iterative algorithm, designed to give coarse but fast estimates. The local map is first divided into cells (roughly 50cm by 50cm), and the destination scan is turned into a series of 2D normal distributions. We then run Newton's method of unconstrained optimization to find the direction of transformation that maximizes the source points' overlap with the normal distribution.

The destination scan is transformed into a collection of 2D normal distributions, and we compute the direction that maximizes the overlap.
In practice, NDT is more robust than ICP and often converges much faster, but to a more inaccurate optimum. There exists a method to combine the best of both worlds, called NDT-ICP, that runs NDT for half of its iterations to quickly obtain a course estimate, and runs ICP for the remaining iterations to accurately refine the estimate.

Works Cited

Arun, K.S, Thomas S Huang, and Steven D Blostein. 1987. “Least-Squares Fitting of Two 3-D Point Sets.” IEEE Transactions on Pattern Analysis and Machine Intelligence PAMI-9 (5): 698–700. https://doi.org/10.1109/tpami.1987.4767965.
Biber, P., and W. Strasser. n.d. “The Normal Distributions Transform: A New Approach to Laser Scan Matching.” Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453). https://doi.org/10.1109/iros.2003.1249285.
Challis, John H. 1995. “A Procedure for Determining Rigid Body Transformation Parameters.” Journal of Biomechanics 28 (6): 733–37. https://doi.org/10.1016/0021-9290(94)00116-l.
Dennis, J E, and Robert B Schnabel. 1996. Numerical Methods for Unconstrained Optimization and Nonlinear Equations. Philadelphia: Society For Industrial And Applied Mathematics.
Shi, Xiuying, Jianjun Peng, Jiping Li, Pitao Yan, and Hangyu Gong. 2019. “The Iterative Closest Point Registration Algorithm Based on the Normal Distribution Transformation.” Procedia Computer Science 147: 181–90. .https://doi.org/10.1016/j.procs.2019.01.219.
Sorkine-Hornung, Olga, and Michael Rabinovich. 2017. “Least-Squares Rigid Motion Using SVD.” https://igl.ethz.ch/projects/ARAP/svd_rot.pdf.
Van Den Boomgaard, Rein. 2016. “Homogeneous Coordinates — Image Processing and Computer Vision 2.0 Documentation.” Staff.fnwi.uva.nl. 2016. https://staff.fnwi.uva.nl/r.vandenboomgaard/IPCV20162017/LectureNotes/MATH/homogenous.html.