1932

Abstract

Many estimation, planning, and optimal control problems in robotics have an optimization problem at their core. In most of these optimization problems, the objective to be maximized or minimized is composed of many different factors or terms that are local in nature—that is, they depend only on a small subset of the variables. A particularly insightful way of modeling this locality structure is to use the concept of factor graphs, a bipartite graphical model in which factors represent functions on subsets of variables. Factor graphs can represent a wide variety of problems across robotics, expose opportunities to improve computational performance, and are beneficial in designing and thinking about how to model a problem, even aside from performance considerations. I discuss each of these three aspects in detail and review several state-of-the-art robotics applications in which factor graphs have been used with great success.

Loading

Article metrics loading...

/content/journals/10.1146/annurev-control-061520-010504
2021-05-03
2024-12-05
Loading full text...

Full text loading...

/deliver/fulltext/control/4/1/annurev-control-061520-010504.html?itemId=/content/journals/10.1146/annurev-control-061520-010504&mimeType=html&fmt=ahah

Literature Cited

  1. 1. 
    Frey B, Kschischang F, Loeliger HA, Wiberg N. 1997. Factor graphs and algorithms. Proceedings of the 35th Allerton Conference on Communications, Control, and Computing666–80 Champaign: Univ. Ill. Press
    [Google Scholar]
  2. 2. 
    Dellaert F, Kaess M. 2006. Square root SAM: simultaneous localization and mapping via square root information smoothing. Int. J. Robot. Res. 25:1181–203
    [Google Scholar]
  3. 3. 
    Dellaert F, Kaess M. 2017. Factor graphs for robot perception. Found. Trends Robot. 6:1–139
    [Google Scholar]
  4. 4. 
    Dechter R. 2003. Constraint Processing San Francisco: Morgan Kaufmann
    [Google Scholar]
  5. 5. 
    Koller D, Friedman N. 2009. Probabilistic Graphical Models: Principles and Techniques Cambridge, MA: MIT Press
    [Google Scholar]
  6. 6. 
    Triggs B, McLauchlan P, Hartley R, Fitzgibbon A 2000. Bundle adjustment—a modern synthesis. Vision Algorithms: Theory and Practice W Triggs, A Zisserman, R Szeliski 298–372 Berlin: Springer
    [Google Scholar]
  7. 7. 
    Carlone L, Kira Z, Beall C, Indelman V, Dellaert F. 2014. Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors. 2014 IEEE International Conference on Robotics and Automation4290–97 Piscataway, NJ: IEEE
    [Google Scholar]
  8. 8. 
    Davis T, Gilbert J, Larimore S, Ng E. 2004. A column approximate minimum degree ordering algorithm. ACM Trans. Math. Softw. 30:353–76
    [Google Scholar]
  9. 9. 
    Lipton R, Tarjan R. 1979. A separator theorem for planar graphs. SIAM J. Appl. Math. 36:177–89
    [Google Scholar]
  10. 10. 
    Kaess M, Johannsson H, Roberts R, Ila V, Leonard J, Dellaert F 2012. iSAM2: incremental smoothing and mapping using the Bayes tree. Int. J. Robot. Res. 31:217–36
    [Google Scholar]
  11. 11. 
    Olson E, Leonard J, Teller S 2006. Fast iterative alignment of pose graphs with poor initial estimates. Proceedings of the 2006 IEEE International Conference on Robotics and Automation2262–69 Piscataway, NJ: IEEE
    [Google Scholar]
  12. 12. 
    Björck A. 1996. Numerical Methods for Least Squares Problems Philadelphia: Soc. Ind. Appl. Math.
    [Google Scholar]
  13. 13. 
    Dellaert F, Carlson J, Ila V, Ni K, Thorpe C 2010. Subgraph-preconditioned conjugate gradient for large scale SLAM. 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems2566–571 Piscataway, NJ: IEEE
    [Google Scholar]
  14. 14. 
    Jian YD, Balcan D, Dellaert F. 2011. Generalized subgraph preconditioners for large-scale bundle adjustment. 2011 International Conference on Computer Vision295–302 Piscataway, NJ: IEEE
    [Google Scholar]
  15. 15. 
    Kushal A, Agarwal S. 2012. Visibility based preconditioning for bundle adjustment. 2012 IEEE Conference on Computer Vision and Pattern Recognition1442–49 Piscataway, NJ: IEEE
    [Google Scholar]
  16. 16. 
    Dellaert F. 2012. Factor graphs and GTSAM: a hands-on introduction Tech. Rep. GT-RIM-CP&R-2012-002 Ga. Inst. Technol Atlanta:
    [Google Scholar]
  17. 17. 
    Kuemmerle R, Grisetti G, Strasdat H, Konolige K, Burgard W. 2011. g2o: a general framework for graph optimization. 2011 IEEE International Conference on Robotics and Automation3607–13 Piscataway, NJ: IEEE
    [Google Scholar]
  18. 18. 
    Blanco-Claraco JL 2019. A modular optimization framework for localization and mapping. Robotics: Science and Systems XV A Bicchi, H Kress-Gazit, S Hutchinson, pap. 43 N.p: Robot. Sci. Syst. Found.
    [Google Scholar]
  19. 19. 
    Agarwal S, Mierle K et al. 2013. Ceres Solver http://ceres-solver.org
    [Google Scholar]
  20. 20. 
    Ni K, Dellaert F. 2010. Multi-level submap based SLAM using nested dissection. 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems2558–65 Piscataway, NJ: IEEE
    [Google Scholar]
  21. 21. 
    Guivant J, Nebot E. 2001. Optimization of the simultaneous localization and map building algorithm for real time implementation. IEEE Trans. Robot. Automat. 17:242–57
    [Google Scholar]
  22. 22. 
    Karypis G, Kumar V. 1998. Multilevel algorithms for multi-constraint graph partitioning. SC'98 : Proceedings of the 1998 ACM/IEEE Conference on Supercomputing28 Piscataway, NJ: IEEE
    [Google Scholar]
  23. 23. 
    Ni K, Dellaert F. 2012. HyperSfM. 2012 Second International Conference on 3D Imaging, Modeling, Processing, Visualization and Transmission144–51 Piscataway, NJ: IEEE
    [Google Scholar]
  24. 24. 
    Cunningham A, Wurm K, Burgard W, Dellaert F. 2012. Fully distributed scalable smoothing and mapping with robust multi-robot data association. 2012 IEEE International Conference on Robotics and Automation1093–100 Piscataway, NJ: IEEE
    [Google Scholar]
  25. 25. 
    Johannsson H, Kaess M, Fallon M, Leonard J 2013. Temporally scalable visual SLAM using a reduced pose graph. 2013 IEEE International Conference on Robotics and Automation54–61 Piscataway, NJ: IEEE
    [Google Scholar]
  26. 26. 
    Schneider J, Eling C, Klingbeil L, Kuhlmann H, Förstner W, Stachniss C. 2016. Fast and effective online pose estimation and mapping for UAVs. 2016 IEEE International Conference on Robotics and Automation4784–91 Piscataway, NJ: IEEE
    [Google Scholar]
  27. 27. 
    Paskin M. 2003. Thin junction tree filters for simultaneous localization and mapping. IJCAI' 03: Proceedings of the 18th International Joint Conference on Artificial Intelligence1157–64 San Francisco: Morgan Kaufmann
    [Google Scholar]
  28. 28. 
    Dellaert F, Kipp A, Krauthausen P 2005. A multifrontal QR factorization approach to distributed inference applied to multi-robot localization and mapping. AAAI' 05: Proceedings of the 20th National Conference on Artificial Intelligence, Vol. 31261–66 Palo Alto, CA: AAAI Press
    [Google Scholar]
  29. 29. 
    Pinies P, Paz P, Haner S, Heyden A. 2012. Decomposable bundle adjustment using a junction tree. 2012 IEEE International Conference on Robotics and Automation1246–53 Piscataway, NJ: IEEE
    [Google Scholar]
  30. 30. 
    Forster C, Carlone L, Dellaert F, Scaramuzza D. 2017. On-manifold preintegration theory for fast and accurate visual-inertial navigation. IEEE Trans. Robot. 33:1–21
    [Google Scholar]
  31. 31. 
    Lupton T, Sukkarieh S. 2012. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions. IEEE Trans. Robot. 28:61–76
    [Google Scholar]
  32. 32. 
    Hartley R, Mangelson J, Gan L, Ghaffari Jadidi M, Walls JM et al. 2018. Legged robot state-estimation through combined forward kinematic and preintegrated contact factors. 2018 IEEE International Conference on Robotics and Automation4422–29 Piscataway, NJ: IEEE
    [Google Scholar]
  33. 33. 
    Hartley R, Ghaffari Jadidi M, Gan L, Huang JK, Grizzle JW, Eustice RM 2018. Hybrid contact preintegration for visual-inertial-contact state estimation using factor graphs. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems3783–90 Piscataway, NJ: IEEE
    [Google Scholar]
  34. 34. 
    Wisth D, Camurri M, Fallon M. 2019. Robust legged robot state estimation using factor graph optimization. IEEE Robot. Autom. Lett. 4:4507–14
    [Google Scholar]
  35. 35. 
    Bloesch M, Burri M, Sommer H, Siegwart R, Hutter M. 2017. The two-state implicit filter – recursive estimation for mobile robots. IEEE Robot. Autom. Lett. 3:573–80
    [Google Scholar]
  36. 36. 
    Wisth D, Camurri M, Fallon M. 2020. Preintegrated velocity bias estimation to overcome contact nonlinearities in legged robot odometry. 2020 IEEE International Conference on Robotics and Automation Piscataway, NJ: IEEE Forthcoming
    [Google Scholar]
  37. 37. 
    Ratliff N, Zucker M, Bagnell J, Srinivasa S. 2009. CHOMP: gradient optimization techniques for efficient motion planning. 2009 IEEE International Conference on Robotics and Automation489–94 Piscataway, NJ: IEEE
    [Google Scholar]
  38. 38. 
    Schulman J, Duan Y, Ho J, Lee A, Awwal I et al. 2014. Motion planning with sequential convex optimization and convex collision checking. Int. J. Robot. Res. 33:1251–70
    [Google Scholar]
  39. 39. 
    Mukadam M, Dong J, Yan X, Dellaert F, Boots B. 2018. Continuous-time Gaussian process motion planning via probabilistic inference. Int. J. Robot. Res. 37:1319–40
    [Google Scholar]
  40. 40. 
    Donald B, Xavier P, Canny J, Reif J. 1993. Kinodynamic motion planning. J. ACM 40:1048–66
    [Google Scholar]
  41. 41. 
    Xie M, Dellaert F. 2020. Batch and incremental kinodynamic motion planning using dynamic factor graphs. arXiv:2005.12514 [cs.RO]
  42. 42. 
    Lynch K, Park F. 2017. Modern Robotics: Mechanics, Planning and Control Cambridge, UK: Cambridge Univ. Press
    [Google Scholar]
  43. 43. 
    Zhao Y, Lin H, Tomizuka M. 2018. Efficient trajectory optimization for robot motion planning. 2018 15th International Conference on Control, Automation, Robotics and Vision260–65 Piscataway, NJ: IEEE
    [Google Scholar]
  44. 44. 
    Sodhi P, Choudhury S, Mangelson J, Kaess M. 2020. ICS: incremental constrained smoothing for state estimation. 2020 IEEE International Conference on Robotics and Automationpp. 27985 Piscataway, NJ: IEEE
    [Google Scholar]
  45. 45. 
    Mukadam M, Dong J, Dellaert F, Boots B. 2018. STEAP: simultaneous trajectory estimation and planning. Auton. Robots 43:415–34
    [Google Scholar]
  46. 46. 
    Anderson S, Barfoot T, Tong C, Särkkä S. 2015. Batch nonlinear continuous-time trajectory estimation as exactly sparse Gaussian process regression. Auton. Robots 39:221–38
    [Google Scholar]
  47. 47. 
    Montemerlo M, Thrun S. 2003. Simultaneous localization and mapping with unknown data association using FastSLAM. 2003 IEEE International Conference on Robotics and Automation, Vol. 21985–91 Piscataway, NJ: IEEE
    [Google Scholar]
  48. 48. 
    Oh SM, Rehg JM, Balch T, Dellaert F. 2008. Learning and inferring motion patterns using parametric segmental switching linear dynamic systems. Int. J. Comput. Vis. 77:103–24
    [Google Scholar]
  49. 49. 
    Pasula H, Russell S, Ostland M, Ritov Y. 1999. Tracking many objects with many sensors. IJCAI' 99: Proceedings of the 18th International Joint Conference on Artificial Intelligence, Vol. 21160–67 San Francisco: Morgan Kaufmann
    [Google Scholar]
  50. 50. 
    Dellaert F, Seitz S, Thorpe C, Thrun S 2001. Feature correspondence: a Markov chain Monte Carlo approach. Advances in Neural Information Processing Systems 13 TK Leen, TG Dietterich, V Tresp 852–58 Cambridge, MA: MIT Press
    [Google Scholar]
  51. 51. 
    Hsiao M, Kaess M. 2019. MH-iSAM2: multi-hypothesis iSAM using Bayes tree and hypo-tree. 2019 International Conference on Robotics and Automation1274–80 Piscataway, NJ: IEEE
    [Google Scholar]
  52. 52. 
    Czarnowski J, Laidlow T, Clark R, Davison A 2020. DeepFactors: real-time probabilistic dense monocular SLAM. IEEE Robot. Autom. Lett. 5:721–28
    [Google Scholar]
  53. 53. 
    Bloesch M, Czarnowski J, Clark R, Leutenegger S, Davison A. 2018. CodeSLAM—learning a compact, optimisable representation for dense visual SLAM. 2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition2560–68 Piscataway, NJ: IEEE
    [Google Scholar]
  54. 54. 
    Rosinol A, Abate M, Chang Y, Carlone L. 2020. Kimera: an open-source library for real-time metric-semantic localization and mapping. 2020 IEEE International Conference on Robotics and Automationpp. 168996 Piscataway, NJ: IEEE
    [Google Scholar]
  55. 55. 
    Bhardwaj M, Boots B, Mukadam M. 2020. Differentiable Gaussian process motion planning. 2020 IEEE International Conference on Robotics and Automationpp. 10598604 Piscataway, NJ: IEEE
    [Google Scholar]
  56. 56. 
    Lv Z, Dellaert F, Rehg JM, Geiger A. 2019. Taking a deeper look at the inverse compositional algorithm. 2019 IEEE/CVF Conference on Computer Vision and Pattern Recognition4576–85 Piscataway, NJ: IEEE
    [Google Scholar]
  57. 57. 
    Davison AJ, Ortiz J. 2019. FutureMapping 2: Gaussian belief propagation for spatial AI. arXiv:1910.14139 [cs.AI]
  58. 58. 
    Ortiz J, Pupilli M, Leutenegger S, Davison AJ. 2020. Bundle adjustment on a graph processor. 2020 IEEE/CVF Conference on Computer Vision and Pattern Recognitionpp. 241322 Piscataway, NJ: IEEE
    [Google Scholar]
  59. 59. 
    Yedidia J, Freeman W, Weiss Y. 2002. Understanding belief propagation and its generalizations Tech. Rep. TR-2001-22 Mitsubishi Electr. Res. Lab Cambridge, MA:
    [Google Scholar]
  60. 60. 
    Ranganathan A, Kaess M, Dellaert F. 2007. Loopy SAM. IJCAI' 07: Proceedings of the 20th International Joint Conference on Artificial Intelligence2191–96 San Francisco: Morgan Kaufmann
    [Google Scholar]
  61. 61. 
    Rosinol A, Gupta A, Abate M, Shi J, Carlone L 2020. 3D dynamic scene graphs: actionable spatial perception with places, objects, and humans. Robotics: Science and Systems XVI M Toussaint, A Bicchi, T Hermans, pap. 79 N.p: Robot. Sci. Syst. Found.
    [Google Scholar]
/content/journals/10.1146/annurev-control-061520-010504
Loading
/content/journals/10.1146/annurev-control-061520-010504
Loading

Data & Media loading...

  • Article Type: Review Article
This is a required field
Please enter a valid email address
Approval was a Success
Invalid data
An Error Occurred
Approval was partially successful, following selected items could not be processed due to error