an improvement of fast_gicp - View it on GitHub