Balanced ICP for precise lidar odometry from non bilateral correspondences

In the field of lidar odometry for autonomous navigation, the Iterative Closest Point (ICP) algorithm is a prevalent choice for estimating robot motion by comparing point clouds. However, ICP accuracy is strictly dependent on the nature of the features involved, but also on the directional choice of the extraction and matching, either from the current to the reference point cloud or vice-versa. Point-to-line or point-to-plane correspondences have been proven to provide the more accurate odometry results. The matching is generally done in a mono-directional framework: extract the features (lines or planes) in the current point cloud and match them to points in the reference point cloud. This paper introduces a novel formulation, named Balanced ICP, that performs feature extraction (lines or planes) in both point clouds and consequent matching in both the directions. Therefore, the cost function is designed to perform a simultaneous optimization of all available data balancing the noise and extraction errors. The experiments, conducted both on simulated and real data from the KITTI dataset, reveal that our method outperform the classical mono-directional formulations, in terms of robustness, accuracy and stability.