Relative Motion Threshold for Rejection in ICP Registration

Abstract Simultaneous Localization and Mapping (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 Mo-tion Threshold (RMT). In combination with multiple pairing rejection, RMT identifies outliers based on error produced by paired points instead of a dis-tance measurement, which makes it more applicable to point-to-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 better than former techniques with outliers created by dy-namical obstacles. Those results were achieved without reducing convergence speed of the overall ICP algorithm