A Fast Egomotion Estimation Method based on Visual Feature Tracking and Iterative Closest Point

This paper presents a method to speed up ICP (Iterative Closest Point) refinement of the ego motion estimation of a visual tracker using a 3D time-of-flight camera-SwissRanger SR 4000. The ICP algorithm may be used to reduce the ego motion estimation error of a visual tracker in a feature-sparse environment. However, the ICP refinement is a computationally expensive. To address this problem, we propose a new ICP refinement method that uses 3D convex hull to reduce the number of data points for ICP computation and thus its computational time. The 3D convex hull is generated using the visual feature correspondences between two camera views. It represents an approximate overlap between the two views. Using the data points within this region for ICP refinement does not affect the ICP refinement results but its ICP computational time. The efficacy of the proposed method is validated with multiple datasets collected in feature-sparse environments and from real-world navigation scenarios.