introducing slambench , a performance and accuracy benchmarking methodology for slam .
A method for registration of 3d shapes