Ground truth evaluation of large urban 6D SLAM
Closed-form solution of absolute orientation using unit quaternions