An Improved ICP with Heuristic Initial Pose for Point Cloud Alignment

This paper proposes a speed-up approach to find an initial transformation for ICP (Iterative Closest Point) algorithm to improve its performance significantly. The proposed method uses 2D features of bearing angle images to find the corresponding point pairs which speeds up the registration significantly. The proposed method consists of five steps: (1) transforming 3D scans into 2D Bearing Angle Images, (2) extracting features from the 2D images by SURF (Speeded-up robust features), (3) finding the corresponding 3D point pairs respective to the 2D corresponding pixel pairs by the reversed mapping function of bearing image, (4) calculating translation matrices of the corresponding points and (5) finding the optimal transformation between two point clouds by SVD and adopting the optimal transformation as the initial pose of ICP. In simulation results, the proposed algorithm not only greatly decreases the RMSE (Root Mean Square Error) of initial poses but reduces 75% of the iteration times of ICP to a stable state. Furthermore, taking 2D features on bearing angle images as the initial pose of ICP also increases the robustness for larger view angle diversity up to 48 degrees.