ICP stereo visual odometry for wheeled vehicles based on a 1DOF motion prior

In this paper, we propose a novel, efficient stereo visual-odometry algorithm for ground vehicles moving in outdoor environments. To avoid the drawbacks of computationally-expensive outlier-removal steps based on random-sample schemes, we use a single-degree-of-freedom kinematic model of the vehicle to initialize an Iterative Closest Point (ICP) algorithm that is utilized to select high-quality inliers. The motion is then computed incrementally from the inliers using a standard linear 3D-to-2D pose-estimation method without any additional batch optimization. The performance of the approach is evaluated against state-of-the-art methods on both synthetic data and publicly-available datasets (e.g., KITTI and Devon Island) collected over several kilometers in both urban environments and challenging off-road terrains. Experiments show that the our algorithm outperforms state-of-the-art approaches in accuracy, runtime, and ease of implementation.