Factor Graph based Incremental Smoothing

In this DARPA-funded project, we collaborate with SRI International ltd. to develop a plug and play framework for navigation. The goal is to produce the best possible solution in real time based on different multi-rate and asynchronous sensors that may become inactive and/or resurrected at any time. A factor graph formulation is used as a representation of the joint probability function, and an efficient inference algorithm is used to calculate the MAP estimate given measurements from different sensors. Following a recently-developed IMU pre-integration theory, an equivalent IMU factor is introduced to summarize consecutive IMU measurements into a non-linear factor, which can be re-linearized if required. This factor is then incorporated into the optimization whenever measurements from other sensors are received, while high-rate navigation solution is contentiously obtained by composing the last navigation state in the factor graph with the current summarized IMU measurements. This is in contrast to the commonly used navigation-aiding approach where IMU measurements are processed outside of the estimator, without being able to perform re-linearization of past IMU measurements. We also present a parallelized navigation architecture (right image above) that is capable of running in real-time and incorporating long-term loop closure constraints while producing the optimal Bayesian solution. This architecture splits the inference problem into a low-latency update that incorporates new measurements using just the most recent states (filter), and a high-latency update that is capable of closing long loops and smooths using all past states (smoother).

Related Publications:

Journal Articles

  1. O. Shelly and V. Indelman, “Hypotheses Disambiguation in Retrospective,” IEEE Robotics and Automation Letters (RA-L), no. 2, Apr. 2022.
    Shelly22ral.pdf DOI: 10.1109/LRA.2022.3143298 Shelly22ral.supplementary Shelly22ral.poster
  2. K. Elimelech and V. Indelman, “Efficient Modification of the Upper Triangular Square Root Matrix on Variable Reordering,” IEEE Robotics and Automation Letters (RA-L), no. 2, Apr. 2021.
    Elimelech21ral.pdf Elimelech21ral.slides DOI: 10.1109/LRA.2020.3048663 Elimelech21ral.video
  3. S. Williams, V. Indelman, M. Kaess, R. Roberts, J. Leonard, and F. Dellaert, “Concurrent Filtering and Smoothing: A Parallel Architecture for Real-Time Navigation and Full Smoothing,” International Journal of Robotics Research (IJRR), Oct. 2014.
    Williams14ijrr.pdf URL: http://ijr.sagepub.com/content/33/12/1544
  4. V. Indelman, S. Wiliams, M. Kaess, and F. Dellaert, “Information Fusion in Navigation Systems via Factor Graph Based Incremental Smoothing,” Robotics and Autonomous Systems, no. 8, Aug. 2013.
    Indelman13ras.pdf DOI: 10.1016/j.robot.2013.05.001

Conference Articles

  1. M. Shienman, O. Levy-Or, M. Kaess, and V. Indelman, “A Slices Perspective for Incremental Nonparametric Inference in High Dimensional State Spaces,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2024.
    Shienman24iros.pdf Shienman24iros.slides
  2. Y. Ben-Elisha and V. Indelman, “Active Online Visual-Inertial Navigation and Sensor Calibration via Belief Space Planning and Factor Graph Based Incremental Smoothing,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Sep. 2017.
    BenElisha17iros.pdf BenElisha17iros.slides BenElisha17iros.supplementary
  3. V. Indelman, A. Melim, and F. Dellaert, “Incremental Light Bundle Adjustment for Robotics Navigation,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Nov. 2013.
    Indelman13iros.pdf Indelman13iros.slides
  4. V. Indelman, S. Wiliams, M. Kaess, and F. Dellaert, “Factor Graph Based Incremental Smoothing in Inertial Navigation Systems,” in International Conference on Information Fusion, Jul. 2012.
    Indelman12fusion.pdf Indelman12fusion.slides
  5. M. Kaess, S. Williams, V. Indelman, R. Roberts, J. J. Leonard, and F. Dellaert, “Concurrent Filtering and Smoothing,” in International Conference on Information Fusion, Jul. 2012.
    Kaess12fusion.pdf Kaess12fusion.slides

Theses

  1. O. Shelly, “Hypotheses disambiguation in retrospective for robust perception in ambiguous environments,” Master's thesis, Technion - Israel Institute of Technology, 2022.
    Shelly22thesis.pdf Shelly22thesis.slides Shelly22thesis.video
  2. Y. Ben-Elisha, “Cooperative Multi-Robot Belief Space Planning for Visual-Inertial Navigation and Online Sensor Calibration,” Master's thesis, Technion - Israel Institute of Technology, 2017.
    BenElisha17thesis.pdf BenElisha17thesis.slides