A Non-Linear Least Squares Approach to SLAM using a Dynamic Likelihood Field

  • Eurico Pedrosa
  • Artur Pereira
  • Nuno Lau


This paper presents a fast scan matching approach to online SLAM supported by a dynamic likelihood field. The dynamic likelihood field plays a central role in the approach: it avoids the necessity to establish direct correspondences; it is the connection link between scan matching and the online SLAM; and it has a low computational complexity. Scan matching is formulated as a non-linear least squares problem that allows us to solve it using Gauss-Newton or Levenberg-Marquardt methods. Furthermore, to reduce the influence of outliers during optimization, a loss function is introduced. The proposed solution was evaluated using an objective benchmark designed to compare different SLAM solutions. Additionally, the execution times of our proposal were also analyzed. The obtained results show that the proposed approach provides a fast and accurate online SLAM, suitable for real-time operation.


SLAM Scan matching Likelihood field Least squares optimization 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.



This research is supported by: National Funds through FCT - Foundation for Science and Technology, in the context of the project UID/CEC/00127/2013; and by European Union’s FP7 under EuRoC grant agreement CP-IP 608849.


  1. 1.
    Bennewitz, M., Stachniss, C., Behnke, S., Burgard, W.: Utilizing reflection properties of surfaces to improve mobile robot localization. In: 2009 IEEE International Conference on Robotics and Automation, pp. 4287–4292. Kobe, Japan (2009)Google Scholar
  2. 2.
    Biber, P., Strasser, W.: The normal distributions transform: a new approach to laser scan matching. In: Intelligent Robots and Systems, 2003. (IROS 2003)., pp. 2743–2748 vol. 3. IEEE, Las Vegas, USA (2003)Google Scholar
  3. 3.
    Bresenham, J.E.: Algorithm for computer control of a digital plotter. IBM Syst. J. 4(1), 25–30 (1965)CrossRefGoogle Scholar
  4. 4.
    Burguera, A., González, Y., Oliver, G.: On the use of likelihood fields to perform sonar scan matching localization. Auton. Robot. 26(4), 203–222 (2009)CrossRefGoogle Scholar
  5. 5.
    Castellanos, J., Martinez-Cantin, R., Tardós, J., Neira, J.: Robocentric map joining: improving the consistency of EKF-SLAM. Robot. Auton. Syst. 55(1), 21–29 (2007)CrossRefGoogle Scholar
  6. 6.
    Castellanos, J.A., Tardos, J.D., Schmidt, G.: Building a Global Map of the Environment of a Mobile Robot: The Importance of Correlations. In: Proceedings of International Conference on Robotics and Automation, vol. 2, pp. 1053–1059. New Mexico, USA (1997)Google Scholar
  7. 7.
    Censi, A.: An ICP variant using a point-to-line metric. In: Proceedings of the IEEE International Conference on Robotics & Automation (ICRA), pp. 19–25. Pasadena, CA, USA (2008)Google Scholar
  8. 8.
    Felzenszwalb, P., Huttenlocher, D.: Distance transforms of sampled functions. Tech. Rep. Cornell University, New York (2004)zbMATHGoogle Scholar
  9. 9.
    Grisetti, G., Stachniss, C., Burgard, W.: Improved techniques for grid mapping with rao-blackwellized particle filters. IEEE Trans. Robot. 23(1), 34–46 (2007)CrossRefGoogle Scholar
  10. 10.
    Grisetti, G., Stachniss, C., Grzonka, S., Burgard, W.: A tree parameterization for efficiently computing maximum likelihood maps using gradient descent. In: Proceedings of Robotics: Science and Systems (RSS). Atlanta, USA (2007)Google Scholar
  11. 11.
    Hall, B.: Lie groups, lie algebras, and representations: an elementary introduction, vol. 222. Springer, Berlin (2015)CrossRefzbMATHGoogle Scholar
  12. 12.
    Hertzberg, C.: A Framework for Sparse, Non-Linear Least Squares Problems on Manifolds. Master’s thesis. Universität Bremen, Germany (2008)Google Scholar
  13. 13.
    Holz, D., Behnke, S.: Sancta Simplicitas - On the efficiency and achievable results of SLAM using ICP-based Incremental Registration. In: Proceedings of the IEEE International Conference on Robotics & Automation (ICRA), pp. 1380–1387. Alaska, USA (2010)Google Scholar
  14. 14.
    Howard, A., Roy, N.: The robotics data set repository (Radish). (2003)
  15. 15.
    Jesus, F., Ventura, R.: Combining Monocular And Stereo Vision In 6D-SLAM for The Localization of a Tracked Wheel Robot. In: 2012 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), pp. 1–6. Texas, USA (2012)Google Scholar
  16. 16.
    Kohlbrecher, S., von Stryk, O., Meyer, J., Klingauf, U.: A Flexible and Scalable SLAM System with Full 3D Motion Estimation. In: Proceedings of the IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), pp. 155–160. Kyoto, Japan (2011)Google Scholar
  17. 17.
    Kümmerle, R., Steder, B., Dornhege, C., Ruhnke, M., Grisetti, G., Stachniss, C., Kleiner, A.: On measuring the accuracy of SLAM algorithms. Auton. Robot. 27(4), 387–407 (2009)CrossRefGoogle Scholar
  18. 18.
    Lau, B., Sprunk, C., Burgard, W.: Efficient grid-based spatial representations for robot navigation in dynamic environments. Robot. Auton. Syst. 61(10), 1116–1130 (2013)CrossRefGoogle Scholar
  19. 19.
    Lee, J.M.: Smooth manifolds. In: Introduction to Smooth Manifolds, pp. 1–29. Springer (2003)Google Scholar
  20. 20.
    Lu, F., Milios, E.: Globally consistent range scan alignment for environment mapping. Auton. Robot. 4 (4), 333–349 (1997)CrossRefGoogle Scholar
  21. 21.
    Lu, F., Milios, E.: Robot pose estimation in unknown environments by matching 2D range scans. J. Intell. Robot. Syst. 18(3), 249–275 (1997)CrossRefGoogle Scholar
  22. 22.
    Madsen, K., Bruun, H., Tingleff, O.: Methods for non-linear least squares problems. Tech. rep., Informatics and Mathematical Modelling. Technical University of Denmark, Denmark (2004)Google Scholar
  23. 23.
    Montemerlo, M., Thrun, S.: Simultaneous Localization and Mapping With Unknown Data Association Using FastSLAM. In: Proceedings of the IEEE International Conference on Robotics & Automation (ICRA), vol. 2, pp. 1985–1991 (2003)Google Scholar
  24. 24.
    Montesano, L., Minguez, J., Montano, L.: Probabilistic scan matching for motion estimation in unstructured environments. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3499–3504 (2005)Google Scholar
  25. 25.
    Moravec, H.P.: Sensor fusion in certainty grids for mobile robots. AI Mag. 9(2), 61 (1988)Google Scholar
  26. 26.
    Neira, J., Tardos, J.: Data association in stochastic mapping using the joint compatibility test. IEEE Trans. Robot. Autom. 17(6), 890–897 (2001)CrossRefGoogle Scholar
  27. 27.
    Olson, E.B.: Real-Time Correlative Scan Matching. In: Proceedings of the IEEE International Conference on Robotics & Automation (ICRA), pp. 4387–4393. Kobe, Japan (2009)Google Scholar
  28. 28.
    Pfister, S.T., Kriechbaum, K.L., Roumeliotis, S.I., Burdick, J.W.: Weighted range sensor matching algorithms for mobile robot displacement estimation. In: Proceedings of the IEEE International Conference on Robotics & Automation (ICRA), vol. 2, pp. 1667–1674. Washington, DC, USA (2002)Google Scholar
  29. 29.
    Platinsky, L., Davison, A.J., Leutenegger, S.: Monocular Visual Odometry: Sparse Joint Optimisation or Dense Alternation?. In: 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 5126–5133 (2017)Google Scholar
  30. 30.
    Smith, R.C., Cheeseman, P.: On the representation and estimation of spatial uncertainty. Int. J. Robot. Res. 5(4), 56–68 (1986)CrossRefGoogle Scholar
  31. 31.
    Stachniss, C., Leonard, J.J., Thrun, S.: Simultaneous Localization and Mapping. In: Siciliano, B., Khatib, O. (eds.) Springer Handproceedings of Robotics, pp. 1153–1176. Springer International Publishing, Cham (2016)Google Scholar
  32. 32.
    Thrun, S.: A probabilistic on-line mapping algorithm for teams of mobile robots. Int. J. Robot. Res. 20(5), 335–363 (2001)CrossRefGoogle Scholar
  33. 33.
    Thrun, S., Burgard, W., Fox, D.: Probabilistic robotics, vol. 1. MIT Press, Cambridge (2005)Google Scholar
  34. 34.
    Triggs, B., McLauchlan, P.F., Hartley, R.I., Fitzgibbon, A.W.: Bundle adjustment — a modern synthesis. In: Triggs, B., Zisserman, A., Szeliski, R. (eds.) Vision Algorithms: Theory and Practice, Lecture Notes in Computer Science, vol. 1883, pp. 298–372. Springer Berlin Heidelberg (2000)Google Scholar

Copyright information

© Springer Science+Business Media B.V., part of Springer Nature 2017

Authors and Affiliations

  1. 1.Department of Electronics, Telecommunications and Informatics (DETI), Intelligent Robotics and Intelligent Systems Laboratory (IRIS), Institute of Electronics and Informatics Engineering of Aveiro (IEETA)University of AveiroAveiroPortugal

Personalised recommendations