6d slam - 3d mapping outdoor environments .
Least-Squares Fitting of Two 3-D Point Sets