Graph Optimization with Unstructured Covariance: Fast, Accurate, Linear Approximation

  • Luca Carlone
  • Jingchun Yin
  • Stefano Rosa
  • Zehui Yuan
Part of the Lecture Notes in Computer Science book series (LNCS, volume 7628)


This manuscript addresses the problem of optimization- based Simultaneous Localization and Mapping (SLAM), which is of concern when a robot, traveling in an unknown environment, has to build a world model, exploiting sensor measurements. Although the optimization problem underlying SLAM is nonlinear and nonconvex, related work showed that it is possible to compute an accurate linear approximation of the optimal solution for the case in which measurement covariance matrices have a block diagonal structure. In this paper we relax this hypothesis on the structure of measurement covariance and we propose a linear approximation that can deal with the general unstructured case. After presenting our theoretical derivation, we report an experimental evaluation of the proposed technique. The outcome confirms that the technique has remarkable advantages over state-of-the-art approaches and it is a promising solution for large-scale mapping.


Pose graph optimization Simultaneous Localization and Mapping Mobile robotics 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.


  1. 1.
    Barooah, P., Hespanha, J.P.: Estimation on graphs from relative measurements. IEEE Control Systems Magazine 27(4), 57–74 (2007)CrossRefGoogle Scholar
  2. 2.
    Carlone, L., Aragues, R., Castellanos, J.A., Bona, B.: A first-order solution to simultaneous localization and mapping with graphical models. In: Proc. of the IEEE lnternational Conf. on Robotics and Automation (2011)Google Scholar
  3. 3.
    Carlone, L., Aragues, R., Castellanos, J.A., Bona, B.: A linear approximation for graph-based simultaneous localization and mapping. In: Proc. of Robotics: Science and Systems (2011)Google Scholar
  4. 4.
    Carlone, L., Rosa, S., Yin, J.: Robotics research group: Resources – graph optimization with unstructured covariance (2012),
  5. 5.
    Davis, T.A.: Direct Methods for Sparse Linear Systems. Fundamentals of Algorithms, vol. 2. Society for Industrial and Applied Mathematics, Philadelphia (2006) ISBN 0898716136MATHCrossRefGoogle Scholar
  6. 6.
    Dellaert, F., Carlson, J., Ila, V., Ni, K., Thorpe, C.: Subgraph-preconditioned conjugate gradients for large scale SLAM. In: Proc. of the IEEE-RSJ Int. Conf. on Intelligent Robots and Systems (2010)Google Scholar
  7. 7.
    Frese, U., Larsson, P., Duckett, T.: A multilevel relaxation algorithm for simultaneous localization and mapping. IEEE Trans. on Robotics 21(2), 196–207 (2005)CrossRefGoogle Scholar
  8. 8.
    Grisetti, G., Stachniss, C., Burgard, W.: Non-linear constraint network optimization for efficient map learning. IEEE Trans. on Intelligent Transportation Systems 10(3), 428–439 (2009)CrossRefGoogle Scholar
  9. 9.
    Konolige, K.: Large-scale map-making. In: Proc. of the AAAI National Conf. on Artificial Intelligence (2004)Google Scholar
  10. 10.
    Kümmerle, R., Steder, B., Dornhege, C., Ruhnke, M., Grisetti, G., Stachniss, C., Kleiner, A.: Slam benchmarking webpage (2009),
  11. 11.
    Kummerle, R., Grisetti, G., Strasdat, H., Konolige, K., Burgard, W.: G2o: A general framework for graph optimization. In: 2011 IEEE International Conference on Robotics and Automation (ICRA), pp. 3607–3613 (May 2011), doi:10.1109/ICRA.2011.5979949Google Scholar
  12. 12.
    Lu, F., Milios, E.: Globally consistent range scan alignment for environment mapping. Autonomous Robots 4, 333–349 (1997)CrossRefGoogle Scholar
  13. 13.
    Nocedal, J., Wright, S.J.: Numerical Optimization. Springer (2006)Google Scholar
  14. 14.
    Olson, E., Leonard, J.J., Teller, S.: Fast iterative optimization of pose graphs with poor initial estimates. In: Proc. of the IEEE Int. Conf. on Robotics and Automation, pp. 2262–2269 (2006)Google Scholar
  15. 15.
    Stachniss, C., Frese, U., Grisetti, G.: Open SLAM webpage (2007),
  16. 16.
    Sunderhauf, N., Protzel, N.: Towards a robust back-end for pose graph slam. In: Proc. of IEEE International Conference on Robotics and Automation, ICRA (2012)Google Scholar
  17. 17.
    Thrun, S., Montemerlo, M.: The GraphSLAM algorithm with applications to large-scale mapping of urban structures. Int. J. Robot. Res. 25, 403–429 (2006)CrossRefGoogle Scholar
  18. 18.
    Thrun, S., Burgard, W., Fox, D.: Probabilistic robotics. MIT Press (2005)Google Scholar

Copyright information

© Springer-Verlag Berlin Heidelberg 2012

Authors and Affiliations

  • Luca Carlone
    • 1
  • Jingchun Yin
    • 1
  • Stefano Rosa
    • 2
  • Zehui Yuan
    • 1
  1. 1.Dipartimento di Automatica e InformaticaPolitecnico di TorinoItaly
  2. 2.Italian Institute of Technology (IIT)TorinoItaly

Personalised recommendations