CI-Graph Simultaneous Localization and Mapping for Three-Dimensional Reconstruction of Large and Complex Environments Using a Multicamera System

Submapping and graphical methods have been shown to be valuable approaches to simultaneous localization and mapping (SLAM), providing significant advantages over the classical extended Kalman filter (EKF) solution: they are faster and, when using local coordinates, produce more consistent estimates. The main contribution of this paper is CI-Graph SLAM, a novel algorithm that is able to efficiently map large environments by building a graph of submaps and a spanning tree of this graph with the following properties: (1) any pair of neighboring submaps in the spanning tree are conditionally independent and (2) the current submap is always up to date, containing the marginal probabilities of the submap variables given all previous measurements. Thanks to these properties, an old submap can be updated at any time by performing a single propagation from the current map to the old submap along the spanning tree. This operation is required only when a map is revisited, with a cost linear with the number of maps in the loop. At the end of the experiment the method performs a single propagation through the whole tree, recovering exactly the same marginals for all the map variables as the EKF-SLAM algorithm does, without ever needing to compute the whole covariance matrix. To evaluate CI-Graph performance in extremely loopy environments, the method was tested using a synthetic Manhattan world. The behavior of the algorithm in large real environments is shown using the public data sets from the RAWSEEDS project in which a robot equipped with a trinocular camera traversed indoor and outdoor environments with several loops and revisited areas. Loops are robustly closed using a novel technique that detects candidate loop closures using a visual vocabulary tree and filters them using temporal and geometric constraints. Our experiments show that when using frontal cameras, the technique outperforms FAB-MAP. The epipolar geometry of the loop-closing images is used to find feature matches that are imposed on the CI-Graph to correct the submap estimations along the loop. (C) 2010 Wiley Periodicals, Inc.