Fast and robust point cloud matching based on EM-ICP prepositioning
Closed-form solution of absolute orientation using unit quaternions