Improved ICP-based pose estimation by distance-aware 3D mapping

In this paper, we propose and evaluate various distance-aware weighting strategies to increase the accuracy of pose estimation by improving the accuracy of a voxel-based model, generated from the data obtained by low-cost depth sensors. We investigate two strategies: (a) weight definition to prioritize prominence of the sensed data according to the data accuracy, and (b) model updating to determine the influential level of the newly captured data on the existing synthetic 3D model. Specifically, we propose Distance-Aware (DA) and Distance-Aware Slow-Saturation (DASS) updating methods to intelligently integrate the depth data into the 3D model, according to the distance-sensitivity metric of a low-cost depth sensor. We validate the proposed methods by applying them to a benchmark of datasets and comparing the resulting pose trajectories to the corresponding ground-truth. The obtained improvements are measured in terms of Absolute Trajectory Error (ATE) and Relative Pose Error (RPE) and compared against the performance of the original Kinfu. The validation shows that on the average, our most promising method called DASS, leads to a pose estimation improvement in terms of ATE and RPE by 43.40% and 48.29%, respectively. The method shows robust performance for all datasets, with best-case improvement reaching 90% of pose-error reduction