This paper presents the extension and experimental validation of the widely used EKF1-based SLAM2 algorithm to 3D space. It uses planar features extracted probabilistically from dense three-dimensional point clouds generated by a rotating 2D laser scanner. These features are represented in compliance with the Symmetries and Perturbation model (SPmodel) in a stochastic map. As the robot moves, this map is updated incrementally while its pose is tracked by using an Extended Kalman Filter. After showing how three-dimensional data can be generated, the probabilistic feature extraction method is described, capable of robustly extracting (infinite) planes from structured environments. The SLAM algorithm is then used to track a robot moving through an indoor environment and its capabilities in terms of 3D reconstruction are analyzed
