Continuous Probabilistic SLAM Solved via Iterated Conditional Modes

  • J. Gimenez
  • A. Amicarelli
  • J. M. ToiberoEmail author
  • F. di Sciascio
  • R. Carelli
Research Article


This article proposes a simultaneous localization and mapping (SLAM) version with continuous probabilistic mapping (CP-SLAM), i.e., an algorithm of simultaneous localization and mapping that avoids the use of grids, and thus, does not require a discretized environment. A Markov random field (MRF) is considered to model this SLAM version with high spatial resolution maps. The mapping methodology is based on a point cloud generated by successive observations of the environment, which is kept bounded and representative by including a novel recursive subsampling method. The CP-SLAM problem is solved via iterated conditional modes (ICM), which is a classic algorithm with theoretical convergence over any MRF. The probabilistic maps are the most appropriate to represent dynamic environments, and can be easily implemented in other versions of the SLAM problem, such as the multi-robot version. Simulations and real experiments show the flexibility and excellent performance of this proposal.


Probabilistic simultaneous localization and mapping (SLAM) dynamic obstacles Markov random fields (MRF) iterated conditional modes (ICM) kernel estimator 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.



This research was financed by Argentinean National Council for Scientific Research (CONICET) and the National University of San Juan (UNSJ) of Argentina. We also thank NVIDIA Corporation for their support.


  1. [1]
    H. Durrant-Whyte, T. Bailey. Simultaneous localization and mapping: Part I. IEEE Robotics & Automation Magazine, vol. 13, no. 2, pp. 99–110, 2006. DOI: Scholar
  2. [2]
    T. S. Ho, Y. C. Fai, E. S. Lee Ming. Simultaneous localization and mapping survey based on filtering techniques. In Proceedings of the 10th Asian Control Conference, IEEE, Kota Kinabalu, Malaysia, pp. 1–6, 2015. DOI: Scholar
  3. [3]
    Y. Yang, F. Qiu, H. Li, L. Zhang, M. L. Wang, M. Y. Fu. Large-scale 3D semantic mapping using stereo vision. International Journal of Automation and Computing, vol. 15, no. 2, pp. 194–206, 2018. DOI: Scholar
  4. [4]
    A. Yassin, Y. Nasser, M. Awad, A. Al-Dubai, R. Liu, C. Yuen, R. Raulefs, E. Aboutanios. Recent advances in indoor localization: A survey on theoretical approaches and applications. IEEE Communications Surveys & Tutorials, vol. 19, no. 2, pp. 1327–1346, 2017. DOI: Scholar
  5. [5]
    X. Yuan, J. F. Martínez-Ortega, J. A. Sánchez Fernández, M. Eckert. AEKF-SLAM: A new algorithm for robotic underwater navigation. Sensors, vol. 17, no. 5, Article number 1174, 2017. DOI:
  6. [6]
    H. Roh, J. Jeong, A. Kim. Aerial image based heading correction for large scale SLAM in an urban canyon. IEEE Robotics and Automation Letters, vol. 2, no. 4, pp. 2232–2239, 2017. DOI: Scholar
  7. [7]
    P. Kim, J. D. Chen, Y. K. Choc. SLAM-driven robotic mapping and registration of 3D point clouds. Automation in Construction, vol. 89, pp. 38–48, 2018. DOI: Scholar
  8. [8]
    S. Thrun, W. Burgard, D. Fox. Probabilistic Robotics (Intelligent Robotics and Autonomous Agents), Cambridge, USA: The MIT Press, 2005.zbMATHGoogle Scholar
  9. [9]
    P. Xu, G. Dherbomez, E. Hery, A. Abidli, P. Bonnifait. System architecture of a driverless electric car in the grand cooperative driving challenge. IEEE Intelligent Transportation Systems Magazine, vol. 10, no. 1, pp. 47–59, 2018. DOI: Scholar
  10. [10]
    J. C. Trujillo, R. Munguia, E. Guerra, A. Grau. Cooperative monocular-based SLAM for multi-UAV systems in GPS-denied environments. Sensors, vol. 18, no. 5, Article number 1351, 2018. DOI:
  11. [11]
    J. Li, M. Kaess, R. M. Eustice, M. Johnson-Roberson. Pose-graph SLAM using forward-looking sonar. IEEE Robotics and Automation Letters, vol. 3, no. 3, pp. 2330–2337, 2018. DOI: Scholar
  12. [12]
    R. Mur-Artal, J. D. Tardós. ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras. IEEE Transactions on Robotics, vol. 33, no. 5, pp. 1255–1262, 2017. DOI: Scholar
  13. [13]
    L. T. Hsu, Y. Wada, Y. L. Gu, S. Kamijo. Rectification of 3D building models based on GPS signal collected by vehicle. In Proceedings of IEEE International Conference on Vehicular Electronics and Safety, IEEE, Yokohama, Japan, 2015. DOI: Scholar
  14. [14]
    F. Auat Cheein, G. Steiner, G. P. Paina, R. Carelli. Optimized EIF-SLAM algorithm for precision agriculture mapping based on stems detection. Computers and Electronics in Agriculture, vol. 78, no. 2, pp. 195–207, 2011. DOI: Scholar
  15. [15]
    J. A. Hesch, D. G. Kottas, S. L. Bowman, S. I. Roumeliotis. Consistency analysis and improvement of vision-aided inertial navigation. IEEE Transactions on Robotics, vol. 30, no. 1, pp. 158–176, 2014. DOI: Scholar
  16. [16]
    G. P. Huang, A. I. Mourikis, S. I. Roumeliotis. A quadratic-complexity observability-constrained unscented Kalman filter for SLAM. IEEE Transactions on Robotics, vol. 29, no. 5, pp. 1226–1243, 2013. DOI: Scholar
  17. [17]
    J. Gimenez, D. Herrera, S. Tosetti, R. Carelli. Optimization methodology to fruit grove mapping in precision agriculture. Computers and Electronics in Agriculture, vol. 116, pp. 88–100, 2015. DOI: Scholar
  18. [18]
    Y. Xu, T. Shen, X. Y. Chen, L. L. Bu, N. Feng. Predictive adaptive Kalman filter and its application to INS/UWB-integrated human localization with missing UWB-based measurements. International Journal of Automation and Computing, to be published. DOI:
  19. [19]
    P. J. Huber. Robust Statistics, New York, USA: Wiley-Interscience, 1981.zbMATHGoogle Scholar
  20. [20]
    D. M. Rosen, C. DuHadway, J. J. Leonard. A convex relaxation for approximate global optimization in simultaneous localization and mapping. In Proceedings of IEEE International Conference on Robotics and Automation, IEEE, Seattle, USA, 2015. DOI: Scholar
  21. [21]
    I. J. Cox. A review of statistical data association techniques for motion correspondence. International Journal of Computer Vision, vol. 10, no. 1, pp. 53–66, 1993. DOI: Scholar
  22. [22]
    M. Adams, B. N. Vo, R. Mahler, J. Mullane. SLAM gets a PHD: New concepts in map estimation. IEEE Robotics & Automation Magazine, vol. 21, no. 2, pp. 26–37, 2014. DOI: Scholar
  23. [23]
    J. Folkesson, H. I. Christensen. Graphical SLAM - a self-correcting map. In Proceedings of IEEE International Conference on Robotics and Automation, IEEE, New Orleans, USA, pp. 383–389, 2004. DOI: Scholar
  24. [24]
    R. G. Cowell, A. P. Dawid, S. L. Lauritzen, D. J. Spiegelhalter, Probabilistic Networks and Expert Systems, New York, USA: Springer, 1999. DOI: Scholar
  25. [25]
    J. M. Hammersley, P. Clifford. Markov Field on Finite Graphs and Lattices, Technical Report, Department of Statistics, Oxford University, Oxford, UK, 1971.Google Scholar
  26. [26]
    R. Kindermann, J. L. Snell. Markov Random Fields and Their Applications, Providence, USA: American Mathematical Society, 1980.zbMATHGoogle Scholar
  27. [27]
    S. Z. Li. Markov Random Field Modeling in Image Analysis, 3rd ed., London, USA: Springer, 2009. DOI: Scholar
  28. [28]
    F. Dellaert, M. Kaess. Square root SAM: Simultaneous localization and mapping via square root information smoothing. International Journal of Robotics Research, vol. 25, no. 12, pp. 1181–1203, 2006. DOI: Scholar
  29. [29]
    M. Jadaliha, J. Choi. Fully Bayesian simultaneous localization and spatial prediction using Gaussian Markov random fields (GMRFs). In Proceedings of American Control Conference, IEEE, Washington, USA, pp. 4592–4597, 2013. DOI: Scholar
  30. [30]
    Y. Latif, C. Cadena, J. Neira. Robust graph SLAM back-ends: A comparative analysis. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, Chicago, USA, pp. 2683–2690, 2014. DOI: Scholar
  31. [31]
    N. Carlevaris-Bianco, M. Kaess, R. M. Eustice. Generic node removal for factor-graph SLAM. IEEE Transactions on Robotics, vol. 30, no. 6, pp. 1371–1385, 2014. DOI: Scholar
  32. [32]
    B. Suger, G. D. Tipaldi, L. Spinello, W. Burgard. An approach to solving large-scale slam problems with a small memory footprint. In Proceedings of IEEE International Conference on Robotics and Automation, IEEE, Hong Kong, China, pp. 3632–3637, 2014. DOI: Scholar
  33. [33]
    M. Mazuran, G. D. Tipaldi, L. Spinello, W. Burgard. Non-linear graph sparsification for SLAM. In Proceedings of Robotics: Science and Systems, RSS, Berkeley, USA, 2014. DOI: Scholar
  34. [34]
    S. Choudhary, V. Indelman, H. I. Christensen, F. Dellaert. Information-based reduced landmark SLAM. In Proceedings of IEEE International Conference on Robotics and Automation, IEEE, Seattle, USA, 2015. DOI: Scholar
  35. [35]
    G. Dissanayake, S. B. Williams, H. Durrant-Whyte, T. Bailey. Map management for efficient simultaneous localization and mapping (SLAM). Autonomous Robots, vol. 12, no. 3, pp. 267–286, 2002. DOI: Scholar
  36. [36]
    J. Besag. On the statistical analysis of dirty pictures. Journal of the Royal Statistical Society, Series B, vol. 48, no. 3, pp. 259–302, 1986.MathSciNetzbMATHGoogle Scholar
  37. [37]
    J. Gimenez, A. Amicarelli, J. M. Toibero, F. di Sciascio, R. Carelli. Iterated conditional modes to solve simultaneous localization and mapping in Markov random fields context. International Journal of Automation and Computing, vol. 15, no. 3, pp. 310–324, 2018. DOI: Scholar
  38. [38]
    S. Kullback, R. A. Leibler. On information and sufficiency. Annals of Mathematical Statistics, vol. 22, no. 1, pp. 79–86, 1951. DOI: Scholar
  39. [39]
    Z. I. Botev, J. F. Grotowski, D. P. Kroese. Kernel density estimation via diffusion. The Annals of Statistics, vol. 38, no. 5, pp. 2916–2957, 2010. DOI: Scholar
  40. [40]
    M. P. Wand, M. C. Jones. Comparison of smoothing parameterizations in bivariate kernel density estimation. Journal of the American Statistical Association, vol. 88, no. 422, pp. 520–528, 1993. DOI: Scholar
  41. [41]
    J. Gimenez, S. Tosetti, L. Salinas, R. Carelli. Bounded memory probabilistic mapping of out-of-structure objects in fruit crops environments. Computers and Electronics in Agriculture, vol. 115, pp. 11–20, 2018. DOI: Scholar
  42. [42]
    Z. I. Botev. Kernel density estimation using Matlab, 2007. [Online], Available:, 2007.Google Scholar
  43. [43]
    V. H. Andaluz, F. Roberti, J. M. Toibero, R. Carelli, B. Wagner. Adaptive dynamic path following control of an unicycle-like mobile robot. In Proceedings of the 4th International Conference on Intelligent Robotics and Applications, Springer, Aachen, Germany, pp. 563–574, 2011. DOI: Scholar
  44. [44]
    H. Secchi, R. Carelli, V. Mut. An experience on stable control of mobile robots. Latin American Applied Research, vol. 33, no. 4, pp. 379–385, 2003.Google Scholar

Copyright information

© Institute of Automation, Chinese Academy of Sciences and Springer-Verlag GmbH Germany, part of Springer Nature 2019

Authors and Affiliations

  1. 1.Automatics Institute (INAUT)National University of San Juan - CONICETSan JuanArgentina

Personalised recommendations