Relative Motion Threshold for Rejection in ICP Registration

One of the most applied techniques in autonomous robotics for navigation is Simultaneous Localization and Mapping (SLAM). SLAM iteratively builds a map of the environment by putting each new observation in relation with the current map. This relation is usually done by scan matching algorithms such as Iterative Closest Point (ICP) where two sets of features are paired. However as ICP is sensitive to outliers, methods have been proposed to reject them. In this article, we present a new rejection technique called Relative Motion Threshold (RMT). In combination with multiple pairing rejection, RMT identifies outliers based on error produced by paired points instead of a distance measurement, which makes it more applicable to pointto-plane error. The rejection threshold is calculated with a simulated annealing ratio which follows the convergence rate of the algorithm. Experiments demonstrate that RMT performs 20% better than former techniques for outliers created by dynamical obstacles. Those results were achieved without reducing convergence speed of the overall ICP algorithm.