Motion-Aware Iterative Closest Point Estimation for Fast Visual Odometry

Iterative closest point (ICP) algorithm is a common localization method used to estimate camera poses by aligning two depth frames. Since the input depth map is easily distorted when the camera is in large motion, it might result in incorrect pose estimation and produce apparent drift for ICP-based applications. To alleviate this problem, instead of using the time-consuming graph-based optimization approach for post processing, this work aims at refining poses when detecting noisy depth maps and presents a hybrid decision mechanism to detect noisy depth maps based on the characteristic of ICP. The camera pose of the next frame is decided by referring to the last frame instead of the current frame when a noisy depth map is detected, by doing so, we can prevent the errors produced in the current frame from propagating to the next frame, thus reducing drift. Experimental results show that the relative pose error reduce to 58% in average at the time when large motion happens.