Depending on the map type, mapping algorithms differ. State of the art for metric maps are probabilistic methods, where the robot has probabilistic motion and uncertain perception models. By integrating of these two distributions with a Bayes filter, e.g., Kalman or particle filter, it is possible to localize the robot. Mapping is often an extension to this estimation problem. Beside the robot pose, positions of landmarks are estimated. Closed loops, i.e., a second encounter of a previously visited area of the environment, play a special role here. Once detected, they enable the algorithms to bound the error by deforming the already mapped area such that a topologically consistent model is created. However, there is no guarantee for a correct model. Several strategies exist for solving SLAM. Thrun reviews in [26] existing techniques, i.e., maximum likelihood estimation [10], expectation maximization [9,27], extended Kalman filter [6] or (sparse extended) information filter [29]. In addition to these methods, FastSLAM [28] that approximates the posterior probabilities, i.e., robot poses, by particles, and the method of Lu Milios on the basis of IDC scan matching [18] exist.
In principle, these probabilistic methods are extendable to 6D. However no reliable feature extraction nor a strategy for reducing the computational costs of multi hypothesis tracking, e.g., FastSLAM, that grows exponentially with the degrees of freedom, has been published to our knowledge.