Improved ICP-based Pose Estimation by Distance-aware 3D Mapping