The Past, Present and Future

of SLAM
Dr. John J. Leonard
MIT CSAIL and MechE
July 2021
 C S A I L M R G
marinerobotics.mit.edu John J. Leonard, MIT
Outline
• My Personal Research Journey: SLAM, AUVs and Self-Driving

• History and Challenges of Robot Navigation and Mapping

• Current Research: Inference and Representations for Spatial AI

• Open Challenges and Future Prospects

• Q&A
Outline
• My Personal Research Journey: SLAM, AUVs and Self-Driving

• History and Challenges of Robot Navigation and Mapping

• Current Research: Inference and Representations for Spatial AI

• Open Challenges and Future Prospects

• Q&A
 My Background

Autonomous Underwater Vehicles Mapping and Localization Self-Driving Vehicles

Education:
• University of Pennsylvania, BSEE (1987)
• University of Oxford, DPhil (1991)
History of MIT Positions:
• MIT Sea Grant AUV Lab (1991-1996)
• Dept. of Ocean Engineering (1996-2004)
• Dept of Mechanical Engineering 2005-present
• Artificial Intelligence Laboratory (2002-2004) and CSAIL (2005-present)
Teaching: Measurement and Instrumentation, Robotics, Electronics, …
Research Interests: Self-Driving Vehicles; Mapping and Localization; AUVs
Service: Faculty Search; MIT Task Force on Work of the Future (2018-2020)
Industry: Toyota Research Institute (2016-present; currently Technical Advisor)
MIT (since 1991): I Have Truly Found Paradise
Autonomous Underwater Vehicle Navigation

“Toward Lifelong Visual Localization and Mapping”, H. Johannsson, PhD Thesis, MIT/WHOI Joint
Program, June 2013 (REMUS AUV data courtesy of Doug Horner, NPS)
Amazing Students

Jacob Feder Rick Rikoski Michael Bosse Ryan Eustice

Ed Olson Matt Walter Alex Bahr Hordur Johannsson

Amazing Postdocs

Paul Newman Mike Benjamin John Folkesson Luke Fletcher

Maurice Fallon Michael Kaess Paul Huang Liam Paull

Outline
• My Personal Research Journey: SLAM, AUVs and Self-Driving

• History and Challenges of Robot Navigation and Mapping

• Current Research: Inference and Representations for Spatial AI

• Open Challenges and Future Prospects

• Q&A
https://global.oup.com/academic/product/how-to-get-your-phd-9780198866923?cc=us&lang=en&
Hugh Durrant-Whyte
Grasp Lab PhD
1986

Advisor: Lou Paul

Mentors: Ruzena
Bacjsy and Max
Mintz
Hugh Durrant-Whyte
Grasp Lab PhD
1986

Advisor: Lou Paul

Mentors: Ruzena University of Sydney, March 1999 (ISER)
Bacjsy and Max
Mintz
 Hans Moravec, 1970’s

Hans Moravec in 1977

"There were things, that although they looked might be easier, because human
beings do them more easily, that in fact, turn out to be heartbreakingly difficult.
And those were things like looking at the world and seeing what objects there
were in front of the camera that was connected to the computer, and moving
around competently in the world -- the typical robot tasks.”
Hans Moravec, “The Future Fantastic”, 1996 (BBC Television)
 Why is Mobile Robot Navigation and Mapping Difficult?
Inference
State Estimation
Data Association
Learning

Systems & Autonomy

Coupling Perception to
Planning and Control Representation
Going From Demo to Metric vs. Topological
Deployment Dense
Objects
Semantic
Allocentric vs.
Egocentric
 “The key idea is to use a relational map, which is
rubbery and stretchy, rather than to try to place
observations in a 2-D coordinate frame.

Rodney Brooks
“These problems are far
from solved.”
“much remains to be done
to produce complete
solutions
Representation: Occupancy Grids

ICRA 1985
 Position Referencing and Consistent World 1985

Modeling for Mobile Robots, ICRA 1985

Raja Chatila

French Television, 1982 Chatila and Laumond, ICRA 1985

The First Complete SLAM Implementation! 1991
Philippe Moutarlier, LAAS

Raja Chatila
Consistent Integration and Propagation of
Disparate Sensor Observations
Hugh Durrant-Whyte

The International journal of robotics

research 6 (3), 3-24 1987 Hugh, circa 1992
U. of Oxford, Jenkin Building Basement, Circa 1989
 1986
A Stochastic Map for Spatial Relationships
Smith, Self and Cheesemen
Proceedings of the Second Conference on Uncertainty in
Artificial Intelligence, 1986

“Rather than treat spatial uncertainty as a side

issue in geometrical reasoning, we believe it
must be an intrinsic part of spatial
representations. In this paper, we describe a
representation for spatial information, called the
stochastic map, and associated procedures for
building it, reading information from it, and
revising it incrementally as new information is
obtained.”
Tracking and Data Association 1988
Bar-Shalom and Fortmann, Academic Press, 1988
“The goal of our research is to achieve comparable performance to artificial
beacon systems … by sensing the naturally-occurring geometry of typical indoor
scenes, and comparing these observations with a map of the environment. This
competence of localization would provide a mobile robot with the ability to
determine its position …. While a limited form of this competence might use hand-
measured maps of the environment provided a priori to the robot, completely
autonomous operation will require that the robot construct and maintain its own
map in a changing environment.” (Leonard and Durrant-Whyte, Kluwer, 1992)
Sonar Mapping from Known Locations 1990
Incremental Map Making

O3

O1 O2

observation

R1
R0
motion
 1998
Visual SLAM –
Andrew Davison
Feature-based SLAM – Error
Consistency and Data Association

The SPmap: A Probabilistic Framework for Simultaneous

Localization and Map Building Jose A. Castellanos, J. M. M.
Montiel, J. Neira, and J. D. Tardós, IEEE TRA, 1999
 1997
Consistent Pose Estimation (Lu and Milios)
Our approach is to maintain all the local frames of data as well as the relative
spatial relationships between local frames. These spatial relationships are
modeled as random variables and are derived from matching pairwise scans or
from odometry. Then we formulate a procedure based on the maximum likelihood
criterion to optimally combine all the spatial relations. Consistency is achieved by
using all the spatial relations as constraints to solve for the data frame poses
simultaneously.
 1999
Loop-Closing – Gutmann and Konolige
``A map is represented as an undirected graph: nodes are robot poses with
associated scans and links are constraints between poses obtained from
dead-reckoning, scan-matching, or correlation (Gutmann and Konolige, CIRA,
1999)”

J.-S. Gutmann and K. Konolige. Incremental Mapping of Large Cyclic Environments, in: International
Symposium on Computational Intelligence in Robotics and Automation (CIRA'99), Monterey,
November 1999.
First SLAM Summer School Stockholm, 2002
First SLAM Summer School Stockholm, 2002
TartanSLAM CMU, 2021
TartanSLAM CMU, 2021
Outline
• My Personal Research Journey: SLAM, AUVs and Self-Driving

• History and Challenges of Robot Navigation and Mapping

• Current Research: Inference and Representations for Spatial AI

• Open Challenges and Future Prospects

• Q&A
Recent Research in My Group

Goal: Semantic representations for robust and

efficient navigation
Activities
• Inference and representations for spatial intelligence
• Where am I? and What objects are around me?
• Object-based mapping for situations with high ambiguity
• Existing approaches encounter difficult with ambiguous objects
– how to handle multimodal (non-Gaussian) uncertainty?
• Seeking generic approaches to handle ambiguities that can
generalize to difficult environments
Our Trend: Back to Basics

O3

O1 O2

observation

R1
R0
motion
Kevin Doherty, MIT
 Combining Objects and Locations for Spatial Navigation

building, car, …

Robot Poses Detections + Odometry Data Association Landmarks

aka Objects
“Probabilistic Data Association via Mixture Models for Robust Semantic SLAM,”
K. Doherty, D. Baxter*, E. Schneeweiss*, J. Leonard. In IEEE International Conference on Robotics and Automation 2020.
The hypothesis set grows exponentially

Hypotheses

… and so on

See Michael Kaess’s TartanSLAM Talk

Related Work
Classical Approaches
Maximum-likelihood data association
Brittle when measurements are error-
prone
Multi-hypothesis methods
Representing all possible combinations
of discrete hypotheses is often
intractable
Robust SLAM
Max-Mixtures
(Olson and Agarwal, 2013)
Switchable Constraints (Sünderhauf
and Protzel 2012)
Detect and remove outlier
measurements
Represent discrete decisions in terms
of continuous optimization problems
Probabilistic Data
Association
Expectation-Maximization (Bowman et
al. 2017)
Multimodal Semantic SLAM
(Doherty et al. 2019)
MH-iSAM2 (Hsaio and Kaess, 2019)
Make soft data association decisions to
multiple semantic landmarks
 Probabilistic Data Association via Mixture
Models for Robust Semantic SLAM
• Avoids exponential complexity typically
associated with multi-hypothesis methods
• Permits efficient optimization using nonlinear
least-squares methods
• Leverages the benefits of learned perception
models, while offering improved robustness to
their failures

See our project page on Github:

https://github.com/MarineRoboticsGroup/mixtures_semantic_slam https://www.youtube.com/watch?v=Eq_w8zOXCF4
“Probabilistic Data Association via Mixture Models for Robust Semantic SLAM,”
K. Doherty, D. Baxter*, E. Schneeweiss*, J. Leonard. In IEEE International Conference on Robotics and Automation 2020.
 Our semantic SLAM approach
Landmarks

New landmark

Known landmark

“Probabilistic Data Association via Mixture Models for Robust Semantic SLAM,”
K. Doherty, D. Baxter*, E. Schneeweiss*, J. Leonard. In IEEE International Conference on Robotics and Automation 2020.
 Our semantic SLAM approach
Landmarks

New landmark

“Probabilistic Data Association via Mixture Models for Robust Semantic SLAM,”
K. Doherty, D. Baxter*, E. Schneeweiss*, J. Leonard. In IEEE International Conference on Robotics and Automation 2020.
 Our semantic SLAM approach
Landmarks

Known landmark

“Probabilistic Data Association via Mixture Models for Robust Semantic SLAM,”
K. Doherty, D. Baxter*, E. Schneeweiss*, J. Leonard. In IEEE International Conference on Robotics and Automation 2020.
 We implicitly represent multiple hypotheses
1)

Hypotheses

2)

“Probabilistic Data Association via Mixture Models for Robust Semantic SLAM,”
K. Doherty, D. Baxter*, E. Schneeweiss*, J. Leonard. In IEEE International Conference on Robotics and Automation 2020.
 We implicitly represent multiple hypotheses
1)

Hypotheses

2)

akin to max-mixtures (Olson and Agarwal 2013)

“Probabilistic Data Association via Mixture Models for Robust Semantic SLAM,”
K. Doherty, D. Baxter*, E. Schneeweiss*, J. Leonard. In IEEE International Conference on Robotics and Automation 2020.
We implicitly represent multiple
hypotheses
1)

Hypotheses

This representation permits optimization using off-

2) the-shelf tools, like GTSAM (Dellaert 2012)

akin to max-mixtures (Olson and Agarwal 2013)

 Experiment with AprilTags in MIT
Corridors
MIT RACECAR robot

~ 200 AprilTags

~ 30 minutes of data

2 class semantic SLAM

10% misclassification rate

10% additional odometry error

Full video available at:

https://youtu.be/4crAh0bheJ4

“Probabilistic Data Association via Mixture Models for Robust Semantic SLAM,”
K. Doherty, D. Baxter*, E. Schneeweiss*, J. Leonard. In IEEE International Conference on Robotics and Automation 2020.
 Experiment with AprilTags in MIT
Corridors
MIT RACECAR robot

~ 200 AprilTags

~ 30 minutes of data

2 class semantic SLAM

10% misclassification rate

10% additional odometry error

Full video available at:

https://youtu.be/4crAh0bheJ4

“Probabilistic Data Association via Mixture Models for Robust Semantic SLAM,”
K. Doherty, D. Baxter*, E. Schneeweiss*, J. Leonard. In IEEE International Conference on Robotics and Automation 2020.
 Ambiguity-Aware Object-
Based Navigation
Ziqi Chad Kevin

Hypothesis
Decisions

Ziqi Lu*, Qiangqiang Huang*, Kevin Doherty, and John J. Leonard. Consensus-Informed
Optimization Over Mixtures for Ambiguity-Aware Object SLAM. To Appear in IROS 2021
https://arxiv.org/abs/2107.09265
Idea for testing ambiguity-aware object mapping during Covid …playing cards

56
Object Pose Ambiguity
Robot fails to perceive the full 6D pose of an object

Possible causes: object symmetry, occlusions, perception model failures...

Unexpected uncertainties hamper long-term robustness of navigation system

Especially important in sparse environments (e.g. underwater)

57
Object Pose Ambiguity

- Multiple-hypothesis pose representation

Hypo 2
Hypo 1

58
 Long-Term Goal: Ambiguity-Aware Object SLAM

Traditional SLAM system

Fails w/ measurements with ambiguity

Ambiguity-aware object-based SLAM

Account for and actively minimize
uncertainties in object poses

1. Cadena, Cesar, et al. "Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age." IEEE Transactions on robotics 32.6 (2016): 1309-1332.
59
2. Salas-Moreno, Renato F., et al. "Slam++: Simultaneous localisation and mapping at the level of objects." CVPR. 2013.
Object SLAM Back-End: MAP Estimation

Robot Poses Landmark Poses Landmark Measurements & Odometry

66
1. Wong, Jay M., et al. "Segicp: Integrated deep semantic segmentation and pose estimation." IROS. IEEE, 2017.
Foundation: Factor Graph Optimization

Robot poses

Prior factor

Measurement factor

Odometry factor

Landmark poses Gaussian measurements if no ambiguity

Nonlinear least-squares (NLS) optimization

Solved by NLS optimizers in SLAM libraries: GTSAM, iSAM, ...

67
1. Dellaert, Frank, and Michael Kaess. "Factor graphs for robot perception." Foundations and Trends® in Robotics 6.1-2 (2017): 1-139.
2. Kaess, Michael, Ananth Ranganathan, and Frank Dellaert. "iSAM: Incremental smoothing and mapping." IEEE Transactions on Robotics 24.6 (2008): 1365-1378.
Discrete-Continuous Optimization w/ Pose Ambiguity

(Non-Gaussian)

Hypothesis
Decisions

Robot Poses Landmark Poses Landmark Measurements & Odometry

68
 Discrete-Continuous Optimization

Time 1

Time 2

Time 3

69
Existing Method: Maximum Likelihood Estimation

Maximum Likelihood
Estimation (MLE)

Make hard decisions on discrete

variables

✔
❌
Brittle with noisy or outlier
measurements

❌
✔

❌
✔
70
Existing Method: Multi-Hypothesis Tracking

Multi-Hypothesis Tracking
(MHT)

Explicitly hypotheses tracking and

pruning

Solve a set of independent

❌
inference problems

Aggressive pruning can cause

suboptimality
❌ ❌
MH-iSAM2

Incremental optimization +
efficient multi-hypothesis inference

71
1. Hsiao, Ming, and Michael Kaess. "MH-iSAM2: Multi-hypothesis iSAM using Bayes Tree and Hypo-tree." 2019 ICRA. IEEE, 2019.
Existing Method: Gaussian Mixture Models

Gaussian Mixtures Models

(GMM)

Hypotheses as Gaussian modes

Max-mixtures

Max-product variable elimination

preserves Gaussian posterior

Efficient inference w/ nonlinear

least-square solvers
Our Work
Potential local optimality

Mode shifting w/ Dynamic Re-initialization

72
1. Olson, Edwin, and Pratik Agarwal. "Inference on networks of mixtures for robust robot mapping." The International Journal of Robotics Research 32.7 (2013): 826-840.
Max-Mixture Factor

Locally Gaussian Gaussian Hypothesis weights

Ambiguous Max-mixture
Measurements factors

Max-mixture factor
Ambiguous
objects

● Pros: Implicit hypothesis tracking → Efficient

● Cons: Potential local optimality → Not Robust
1. Olson, Edwin, and Pratik Agarwal. "Inference on networks of mixtures for robust robot mapping." The International Journal of Robotics Research 32.7 (2013): 826-840.
73
2. Doherty, Kevin J., et al. "Probabilistic data association via mixture models for robust semantic SLAM." 2020 ICRA. IEEE, 2020.
Local Optimality with Pose Ambiguity

“Up” hypothesis is a Marginal posterior

little more likely

Oops, “Down” hypothesis

is much more probable 74
 I can efficiently keep
Max-Mixture
Dynamic Re-Initialization track of multiple
pose hypotheses

● Step 1: Infer dominant pose hypothesis I will try to track the

Step 2: Landmark re-initialization Dynamic
● correct mode for
Re-init
your solution

Time 1

Time 2

Time 3 75
Landmark Re-Initialization

● Based on incremental SLAM solver[1]

● Minimum computational cost with only one variable edited

76
1. Kaess, Michael, et al. "iSAM2: Incremental smoothing and mapping using the Bayes tree." The International Journal of Robotics Research 31.2 (2012): 216-235.
Infer Dominant Pose Hypothesis

● When to re-init and what is the new initial value?

● Time-invariance of absolute poses for static landmarks
● Extract the most consistent pose hypothesis

Outlier-robust pose averaging over time

77
 Spatial Navigation with Ambiguous Objects
- Images: Blackfly camera
- Odometry: ZED camera visual odometry
- Cards poses: feature-based pose estimator Blackfly

Cards

MIT RACECAR: mit-racecar.github.io (Karaman et al.) 78

79
Avg. Traj. Error (m)

2 Odom. Single-hypo. Max-mix.

1.5

1
1st card re-detection (JC)
0.5

0
0 100 200 300 400 500 80
t (s)
Summary

Pose ambiguity awareness in object-based SLAM

Back-end Optimizatio

Max-mixtures + Dynamic re-init I can efficiently keep

Max-Mixture
track of multiple
pose hypotheses

I will try to track the

Dynamic
correct mode for
Re-init
your solution 81
On Inference, see Luca’s talk and also one of David Rosen’s talks, e.g.
https://robotics.utoronto.ca/news/seminar-david-rosen-of-mit-on-an-
algorithm-that-is-capable-of-efficiently-recovering-slam-solutions/
Recent Survey Paper
Outline
• My Personal Research Journey: SLAM, AUVs and Self-Driving

• History and Challenges of Robot Navigation and Mapping

• Current Research: Inference and Representations for Spatial AI

• Open Challenges and Future Prospects

• Q&A
Discussion
• Robust Semantic SLAM is not just an “engineering” problem

• The library of objects for available for Semantic SLAM is too small

• Long-term operations in dynamic environments breaks the

assumptions of our models

• Can we match the elegance and robustness of biological systems

• Many applications require shared autonomy with people

Conclusion
● I remain an optimistic for basic university research in robot navigation and
mapping
○ The questions “Where am I?” and “What is where in the environment?”
are timeless research questions
○ “Much remains to be done to produce complete solutions” (Brooks, 1986)

● Many fundamental research challenges remain to be investigated

○ What representations and inference techniques enable robust, long-term
autonomy?
○ How can we enable life-long self-learning in dynamic environments?

○ Role of machine Learning?

○ Symbolic representations that can enable human-robot interaction?
Outline
• My Personal Research Journey: SLAM, AUVs and Self-Driving

• History and Challenges of Robot Navigation and Mapping

• Current Research: Inference and Representations for Spatial AI

• Open Challenges and Future Prospects

• Q&A