SLAM Handbook

From Localization and Mapping to Spatial Intelligence

Edited by

Luca Carlone, Ayoung Kim, Frank Dellaert,

Timothy Barfoot, and Daniel Cremers

Together with all contributors

Henrik Andreasson Arash Asgharivaskasi

Nikolay Atanasov Timothy Barfoot
Jens Behley Wolfram Burgard
Cesar Cadena Marco Camurri
Luca Carlone Yun Chang
Margarita Chli Henrik Christensen
Giovanni Cioffi Javier Civera
Daniel Cremers Andy Davison
Frank Dellaert Jia Deng
Kevin Doherty Jacob Engel
Maurice Fallon Guillermo Gallego
Cedric Le Gentil Christoffer Heckman
Connor Holmes Paul Huang
Shoudong Huang Nathan Hughes
Krishna Murthy Jatavallabhula Michael Kaess
Kasra Khosoussi Giseop Kim
Ayoung Kim John Leonard
Stefan Leutenegger Lahav Lipson
Martin Magnusson Joshua Mangelson
Hidenobu Matsuki Matias Mattamala
José M Martı́nez Montiel Mustafa Mukadam
Paul Newman Helen Oleynikova
Lionel Ott Marc Pollefeys
Victor Reijgwart David Rosen
Davide Scaramuzza Lukas Schmid
Jingnan Shi Cyrill Stachniss
Niko Sunderhauf Zach Teed
Teresa Vidal-Calleja Chen Wang
Heng Yang Fu Zhang
Ji Zhang Shibo Zhao
 Contents

Notation page 1

PART ONE FOUNDATIONS 3

1 Prelude 5
1.1 What is SLAM? 5
1.2 Anatomy of a Modern SLAM System 7
1.3 The Role of SLAM in the Autonomy Architecture 11
1.3.1 Do we really need SLAM for robotics? 12
1.4 Past, Present, and Future of SLAM, and Scope of this Handbook 15
1.4.1 Short History and Scope of this Handbook 15
1.4.2 From SLAM to Spatial AI 18
1.5 Handbook Structure 19
2 Factor Graphs for SLAM 21
2.1 Visualizing SLAM With Factor Graphs 22
2.1.1 A Toy Example 22
2.1.2 A Factor-Graph View 23
2.1.3 Factor Graphs as a Language 25
2.2 From MAP Inference to Least Squares 26
2.2.1 Factor Graphs for MAP Inference 27
2.2.2 Specifying Probability Densities 28
2.2.3 Nonlinear Least Squares 30
2.3 Solving Linear Least Squares 30
2.3.1 Linearization 31
2.3.2 SLAM as Least-Squares 32
2.3.3 Matrix Factorization for Least-Squares 32
2.4 Nonlinear Optimization 34
2.4.1 Steepest Descent 34
2.4.2 Gauss-Newton 35
2.4.3 Levenberg-Marquardt 35
iv Contents

2.4.4 Dogleg Minimization 36

2.5 Factor Graphs and Sparsity 37
2.5.1 The Sparse Jacobian and its Factor Graph 37
2.5.2 The Sparse Information Matrix and its Graph 38
2.5.3 Sparse Factorization 39
2.6 Elimination 40
2.6.1 Variable Elimination Algorithm 40
2.6.2 Linear-Gaussian Elimination 42
2.6.3 Sparse Cholesky Factor as a Bayes Net 45
2.7 Incremental SLAM 47
2.7.1 The Bayes Tree 48
2.7.2 Updating the Bayes Tree 50
2.7.3 Incremental Smoothing and Mapping 51
3 Advanced State Variable Representations 54
3.1 Optimization on Manifolds 54
3.1.1 Rotations and Poses 55
3.1.2 Matrix Lie Groups 56
3.1.3 Lie Group Optimization 57
3.1.4 Lie Group Extras 59
3.2 Continuous-Time Trajectories 61
3.2.1 Splines 62
3.2.2 From Parametric to Nonparametric 64
3.2.3 Gaussian Processes 65
3.2.4 Spline and GPs on Lie Groups 68
4 Robustness to Incorrect Data
Association and Outliers 74
4.1 What Causes Outliers and Why Are They a Problem? 74
4.1.1 Data Association and Outliers 74
4.1.2 Least-Squares in the Presence of Outliers 76
4.2 Detecting and Rejecting Outliers in the SLAM Front-end 77
4.2.1 RANdom SAmple Consensus (RANSAC) 77
4.2.2 Graph-theoretic Outlier Rejection and
Pairwise Consistency Maximization 80
4.3 Increasing Robustness to Outliers in the SLAM Back-end 84
4.3.1 Iteratively Reweighted Least Squares 88
4.3.2 Black-Rangarajan Duality 89
4.3.3 Alternating Minimization 91
4.3.4 Graduated Non-Convexity 92
4.4 Further References and New Trends 97
5 Differentiable Optimization 100
5.1 Introduction 100
 Contents v

5.1.1 Recap on Nonlinear Least Squares 101

5.2 Differentiation Through Nonlinear Least Squares 102
5.2.1 Unrolled Differentiation 103
5.2.2 Truncated Unrolled Differentiation 105
5.2.3 Implicit Differentiation 106
5.3 Differentiation on Manifold 108
5.3.1 Derivatives on the Lie Group 108
5.3.2 Differentiation Operations on Manifold 110
5.4 Modern Libraries 112
5.4.1 Numerical Challenges of Automatic Differentiation 112
5.4.2 Implementation of Differentiable Optimization 114
5.4.3 Related Open-source Libraries 115
5.5 Final Considerations & Recent Trends 117
6 Dense Map Representations 118
6.1 Range Sensing Preliminaries 118
6.1.1 Sensor Measurement Model 119
6.1.2 Conversion to Point Cloud 120
6.2 Foundations of Mapping 121
6.2.1 Occupancy Maps 122
6.2.2 Distance Fields 123
6.2.3 Occupancy Maps or Distance Fields? 125
6.3 Map Representations 125
6.3.1 Explicitness of Target Spatial Structures 125
6.3.2 Types of Spatial Abstractions 126
6.3.3 Data Structures and Storage 131
6.4 Constructing Maps: Methods and Practices 134
6.4.1 Points 134
6.4.2 Surfels 135
6.4.3 Meshes 135
6.4.4 Voxels 136
6.4.5 GPs 140
6.4.6 Hilbert Maps 142
6.4.7 Deep Learning in Mapping 143
6.5 Usage Considerations 143
6.5.1 Environmental Aspects 144
6.5.2 Downstream Task Types 145
6.5.3 Summary of Mapping Methods 146
7 Certifiably Optimal Solvers and Theoretical Properties of SLAM 148
vi Contents

PART TWO USE CASES / APPLICATIONS / STATE OF

PRACTICE 149
8 Visual SLAM 151
9 LiDAR SLAM 152
10 Radar SLAM 153
11 Event-based SLAM 154
12 Inertial Odometry for SLAM 155
13 Leg Odometry for SLAM 156

PART THREE FROM SLAM TO SPATIAL PERCEPTION

AI 157
14 Spatial AI 159
15 Boosting SLAM with Deep Learning 160
16 NeRF and GS 161
17 Dynamic and Deformable SLAM 162
18 Metric-Semantic SLAM 163
19 Foundation Models for SLAM 164

Notes 165
References 166
Author index 187
Subject index 188
 Notation

– General Notation –
a This font is used for real scalars
a This font is used for real column vectors
A This font is used for real matrices
X This font is used for sets
I The identity matrix
0 The zero matrix
AT The transpose of matrix A
RM ×N The vector space of real M × N matrices
p(a) The probability density of a
p(a|b) The probability density of a given b
N (µ, Σ) Gaussian probability density with mean µ and covariance Σ
GP(µ(t), K(t, t′ )) Gaussian process with mean function, µ(t), and covariance function, K(t, t′ )
ˆ
(·) A posterior (estimated) quantity
ˇ
(·) A prior quantity
(·)k The value of a quantity at timestep k
(·)k1 :k2 The set of values of a quantity from timestep k1 to timestep k2 , inclusive
P
∥·∥1 L1 norm ∥x∥1 = p |xi |
P 2
∥·∥2 L2 norm ∥x∥2 = xi
 2 Notation

– 3D Geometry Notation –
Fa A reference frame in three dimensions
va The coordinates of a vector in frame F a
Rab A 3 × 3 rotation matrix (member of SO(3)) that takes points expressed in F a and
re-expresses them in (purely rotated) F b : v b = Rab v a
tb The three-dimensional position of the origin of frame F a expressed in F b
 a a
v
ṽ a = A 4 × 1 homogeneous point expressed in F a
1
 b b

b R a t a
Ta = A 4 × 4 transformation matrix (member of SE(3)) that takes homogeneous points
0 1
expressed in F a and re-expresses them in (rotated and translated) F b : ṽ b = Tab ṽ a
SO(3) The special orthogonal group, a matrix Lie group used to represent rotations
so(3) The Lie algebra associated with SO(3)
SE(3) The special Euclidean group, a matrix Lie group used to represent poses
se(3) The Lie algebra associated with SE(3)
(·)∧ An operator mapping a vector in R3 (resp. R6 ) to an element of the Lie algebra for
rotations (resp. poses); implements the cross product for three-dimensional quanti-
ties, i.e., for two vectors u, v ∈ R3 , u∧ v = u × v
(·)∨ An operator mapping an element of the Lie algebra for rotations (resp. poses) to a
vector in R3 (resp. R6 )
PART ONE
FOUNDATIONS
 1
Prelude
Luca Carlone, Ayoung Kim, Frank Dellaert, Timothy Barfoot, Daniel Cremers

This chapter introduces the Simultaneous Localization and Mapping (SLAM) prob-
lem, presents the modules that form a typical SLAM system, and explains the role
of SLAM in the architecture of an autonomous system. The chapter also provides
a short historical perspective of the topic and discusses how the traditional notion
of SLAM is evolving to fully leverage new technological trends and opportunities.
The ultimate goal of the chapter is to introduce basic terminology and motivations,
and to describe the scope and structure of this handbook.

1.1 What is SLAM?

A necessary prerequisite for a robot to operate safely and effectively in an unknown
environment is to form an internal representation of its surroundings. These type of
representations can be used to support obstacle avoidance, low-level control, plan-
ning, and, more generally, the decision-making processes required for the robot to
complete the task it has been assigned. The execution of simple tasks (e.g., fol-
lowing a lane, or maintaining a certain distance to an object in front of the robot)
may only require tracking entities of interest in the sensor data streams, and com-
plex tasks (e.g., large-scale navigation or mobile manipulation) require building and
maintaining a persistent representation (a map) of the environment. Such a map
describes the presence of obstacles, objects, and other entities of interest, and their
relative location with respect to the robot’s pose (position and orientation). For in-
stance, the map might be used instruct the robot to reach a location of interest, to
grasp a certain object, or to support the exploration of an initially unknown envir-
onment. Figure 1.1 provides some real-world examples of simultaneous localization
and mapping (SLAM) in action.
For a robot operating in an initially unknown environment, the problem of build-
ing a map of the environment, while concurrently estimating its pose with respect
to that map, is referred to as Simultaneous Localization and Mapping (SLAM).
SLAM reduces to localization if the map is given, in which case the robot only has
to estimate its pose with respect to the map. On the other hand, SLAM reduces to
mapping if the pose of the robot is already known, for instance when an absolute
6 Prelude
208 A. Kukko et al. / ISPRS Journal of Photogrammetry and Remote Sensing 132 (2017) 199–209 A. Kukko et al. / ISPRS Journal of Photogrammetry and Remote Sensing 132 (2017) 199–209 201

488 kHz, while 5 Hz satellite and 200 Hz IMU observations were

recorded with the positioning system. Thus the profile spacing in
the point cloud data was ca. 1 cm and the angular resolution
1.2 mrad (0.07!). The scan data was split into file blocks of 3000
profiles in each at the data collection to permit batch processing
for the data preparation phase. Fig. 2 represents an illustration of
the point cloud data for dataset B colored by the point elevation.
The yellow circles in the figure are 50 m in diameter and are used
as data buffer around the actual 32 m by 32 m field plots shown
in white squares.

EFM3D: A Benchmark for Measuring Progress

4. A method for trajectory optimization

arXiv:2406.10224v1 [cs.CV] 14 Jun 2024

4.1. Outline of the method

Towards 3D Egocentric Foundation Models

The proposed processing pipeline is shown in full in Fig. 3. The
steps are from data collection, georeferencing, ground classifica-
tion and other preparations for the raw point cloud data to trunk
detection, trunk feature association, generating the graph repre-
sentation of the features and the trajectory, optimization and
finally georeferencing of the point cloud using the optimized tra-
jectory. The following sections describe in detail the steps taken
to reach the final optimization result.

Julian Straubω , Daniel DeToneωω , Tianwei Shenωω , Nan Yangωω ,

4.2. Pre-processing of the MLS data

The initial trajectories (position and attitude) of the MLS system at

Warehouse Robot Using Visual SLAM to Navigate

Chris Sweeney, Richard Newcombe each site were calculated from the GNSS-IMU data with Inertial
Explorer (NovAtel Inc., Canada) post-processing software. Base station
data for differential correction of the GNSS-IMU data was downloaded
Lidar SLAM Used in the Forestry Industry to Inventory cross-track
Trees data collection up to 100 m range and the GNSS-IMU system observes
Fig. 7. Optimized point cloud for the forest plot B shows the accurately matched tree stems ready for further stem modeling and inventoryFig. 1. RoamerR2 mounted on the rear rack of the ATV. The scanner provides with
work.
from virtual data service (Trimnet, Geotrim, Finland). The recorded
(credit: Canon Inc.) (credit: Kukko et al. (2017)) the system trajectory and orientation for kinematic mapping of the surroundings laser measurements were then georeferenced based on the time syn-
into a 3D point cloud.

Meta cloud
Reality Labs Research
chronization and trajectory data using proprietary Matlab tool.
similar algorithm could possibly largely remove the remaining diameter-at-breast-height and tree height. The improved point Next the georeferenced point clouds were processed with
UIMU-LCI) is seen below the scanner and is connected to the GNSS
errors. Using the method as an initial step to correct the trajectory data permit stem curve modeling of individual trees unam- TerraScan (TerraSolid, Finland) to remove measurement noise
receiver (NovAtel Flexpak6) observing Navstar GPS and GLONASS
for the larger positional and angular errors is beneficial as iterative biguously. We could also expect more accurate object level match-
constellation satellites.
and to extract the ground points. Points with intensity value less
closest point algorithm is prone to settling for local minima instead ing to be successful after running the optimizationThe method than 1000 (full range 11 bits, 0–2047) were removed from the fur-
sensor on head is elevated to about 2 m above the ground
ther processing as experience has shown that these data points are
of finding the correct transformation. By starting from an opti- forest MLS data as the ambiguity in pairing adjacent usingtree stems segment mounted on the rear rack of the
a GlobalTruss
mostly noise. Points from dark objects are also typically worse in
mized solution close to the correct one the chances for finding con- is significantly reduced. ATV. The scanner has a 50 degree blind angle beneath (See
ranging quality compared to higher reflectance surfaces. Next, iso-
Fig. 4). The collected laser measurements are stored on 32 GB SDHC
verging fine-tune corrections to the trajectory locally using ICP are The applied feature detection is less accurate in constraining
Abstract. The advent of wearable computers enables a new source of
lated points, a result of failed ranging, were removed using two-
(10) card on the scanner and positioning data is stored using a
expected to greatly improve. the elevation direction than what is achieved in the tablet
planarcomputer
domain.(Panasonic Toughpad FZ-G1, Windows 8, 8 GB step filtering requiring each point to have at least one neighbor clo-
For correcting the elevation the features in use also require us toThe laser scanner sends TTL level timing signal ser than 10 cm to remove single noise points, and after that clus-
RAM, 1 TB SSD).
7. Conclusions context for AI that is embedded in egocentric sensor data. This new ego- trade-off some of the performance for planar corrections
to the SPAN as use
(3.3 V TTL during
of them causes a large part of the constraints to be disabled
system event logger for mutual synchronization
pulse for start of each scan). The scanner measures pre-
tered noise patterns were removed by demanding at least 30
points to be found within a 1 m point neighborhood.
Ground classification was based on triangulation method
centric
We have developed a process data comes
pipeline for equipped
MLS data from boreal
forests to correct trajectory data for errors induced by obstructed
with fine-grained 3D location information
the optimization. Use of additional features, such
batches and horizontal structures, which could better
cise ranges up to 100 m by measuring the phase difference of the
as ground
emitted amplitude modulated beam and the scattered returning
tie the cor- principle has its shortcomings in vegetation
signal. Such ranging
implemented into TerraScan (Axelsson, 2000) with parameters
set to 65! maximum terrain slope, 20! iteration angle, and 20 cm

GNSS signal underand thusforestpresents theresults

opportunity for a novel class of spatial founda-
rection for error in elevation to the optimizationand pipeline, isthea point observations from the tree stems through iteration distance. The seed points for the ground classification
mature boreal canopy and report as a result
were the lowest points within 5 m radius from the original noise
achieved using the method. The experiments were conducted on straightforward task and should enable us to remove densethecanopy
remain- is somewhat reduced. Also partial hits on thin
filtered point clouds.

tion models that are rooted in 3D space. To measure progress on what ing major errors in the near future. branches and no-return sky generate measurement noise that
data from ATV mounted RoamerR2 MLS system, which allows for needs to be removed prior to further data processing.
To enable easy finding of tree stems, a horizontal slice of the
efficient collection of 3D tree inventory data from the ground. By incorporating a separate set of features to correct for the ele- point cloud following the terrain elevation with a constant offset
vation drift, the tree features currently used could be reserved for from the ground is extracted. The assumption here was to have
we term
scanning and post-processed Egocentric
GNSS-IMU positioning. We Foundation
The point cloud data was obtained with cross-track vertical laser
present Models (EFMs) we establish EFM3D, only constraining the horizontal drift of the trajectory. Thiscloud
3.3. Point would data acquisition slices from the trees to constrain the horizontal drift, but also to
provide ability to tie the elevation uncertainty as the slices are rel-
decrease the number of disabled constraints and enable us to cap-
aofbenchmark with twothecore 3D egocentric perception tasks. EFM3D
the processing pipeline and report optimization results for the data The experimental data comprises three boreal forest test areas ative to the instantaneous local ground eliminating the effect of
from three test plots nominal size 64 m by 64 m, and analyze italize most of the detected stem features in making the optimiza- drift. Fig. 4 shows an example of the outcome of the pre-
(A, B, and C) that were captured with the RoamerR2 system in Octo-
results against tree location reference derived from TLS data geo- tion more accurate. A ground detection algorithm better
ber 2014 suited to of an extensive data collection campaign to cover
as a part processing step for a single data block.

is the
referenced to spherical
plot.
first
landmarks benchmark
with known for 3D object detection and surface regression
locations on each handle raw data with considerable elevation errors
ground patterned noise within relatively short periods
and
all the below-
forest
of time
tory snakes
study plots in the Evo research forests. The MLS trajec-
is the plots to cover all the trees from multiple
through 4.3. Stem extraction

The results showonthathigh quality

the method annotated
could vastly improve the
quality of the produced optimized point cloud. The internal consis-
egocentric data of Project Aria. We propose
needed. directions and to collect complete terrain data (as seen in Fig. 7).
Table 1 gives some plot and data collection characteristics showing
The proposed method is based on sparse features and graph
the average data collection speed to be about 1 m/s. The scanner
The horizontal slice of the point cloud at a constant offset from
the ground is used as an input for the graph optimization pipeline,

Egocentric Voxel Lifting (EVL), a baseline for 3D EFMs. EVL leverages

tency of tree stem locations is reduced to millimeter level in terms
of distance residual standard deviation: for plot A 0.008 m, for B
optimization, and is tested on MLS data on forestry scan application
frequency was set to 95 Hz and point measurement speed to
with supposedly the most challenging GNSS signal loss conditions.
which is built with the help of the Point Cloud Library (PCL) (Rusu

However, the method could be also used in any general MLS case,
all available egocentric
0.007 m and C 0.005 m. The methods ability in providing correc-
tion in absolute coordinates is found slightly weaker, but this is modalities and inherits foundational capabilities
e.g. for correcting urban and road asset point cloud data as similar
pole features are abundant in those environments. Additional fea-
view. There were from 2D foundation models. This model, trained on a large simulated
of secondary importance from individual tree modeling point of
no absolute landmarks used to constraint the tures, such as building and other structural corners, provide robust
optimization from locational drifting, and the results show that constraints for trajectory optimization in built and industrial envi-
with the larger thedataset,
initial drift theoutperforms existing methods on the EFM3D benchmark.
larger the remaining absolute ronments that often pose a challenge for the GNSS based position-
ing. Thus the proposed method is seen to provide an important
Floor-Cleaning Robot Using SLAM to Navigate Self-Driving Car Navigating withpositional offset may be expected.
High-Definition Map Created by SLAM
The attained level of correction in the internal accuracy of the
Drone of
asset for a diversity Mapping As-Built Construction via Visual SLAM
MLS data applications.
(credit: Robot Vacuum Company) (credit:
pointCruise)
cloud enables us to use all of the non noise measured points The phase-shift ranging scanner mounted
(credit:onSkydio)
RoamerR2 and
when extracting individual tree inventory parameters, such as used in the experiments for this paper generate noise in dense veg-

Multi-Camera Video

Surfaces
Semi-dense Points 3D
Egocentric
Foundation
Model
Objects

Camera Trajectory

Legged Robot Using SLAM to Map a Building Wearable Devices Using SLAM to Build Augmented Reality Scenes
(credit: Boston Dynamics) Fig. 1: 3D Egocentric Foundation Models leverage
(credit: Meta spatial priors from egocentric data
Reality Labs)

to power core 3D tasks such as 3D object detection and reconstruction.

Figure 1.1 SLAM is rapidly becoming an enabling technology in a wide array of appli-
1 Introduction
cations including warehouse robotics, forest inventories [141], floor-cleaning, self-driving
cars, drones surveillance, legged-robot mapping, and trained
Foundation models augmented reality
on Internet-scale to name
text, image and videoonly a
datasets
few. have demonstrated the potential in using large-scale self-supervised learning ap-
proaches to build backbones that are useful for numerous downstream tasks,
ω
Project lead.
ωω
Equal contribution in alphabetic order.
positioning system is used (e.g., differential GPS or motion capture), in which case
the robot only needs to model its surroundings using its sensor data.
The central role of SLAM in robotics research is due to the fact that robot poses
are rarely known in practical applications. Differential GPS and motion capture
systems are expensive and restricted to small areas, hence being unsuitable for large-
scale robot deployments. Consumer-grade GPS is much more broadly available, but
its accuracy (with errors typically in the order of meters) and its availability (which
is limited to outdoor areas with line-of-sight to satellites) often makes it unsuitable
as a source of localization; the consumer-grade GPS —when available— is typically
used as an additional source of information for SLAM, rather than a replacement
for the localization aspects of SLAM.
Similarly, in many robotics applications, the robot will not typically have access
 1.2 Anatomy of a Modern SLAM System 7

to a prior map, hence it needs to perform SLAM rather than localization. Indeed,
in certain applications, building a map is actually the goal of the robot deployment;
for instance, when robots are used to support disaster response and search-and-
rescue operations, they might be deployed to construct a map of the disaster site
to help first-responders. In other cases, the map might be stale or not have enough
detail. For instance, a domestic robot might have access to the floor plan of the
apartment it has to operate in, but such a floor plan may not describe the furniture
and objects actually present in the environment, nor the fact that these elements
can be rearranged from day to day. In a similar manner, Mars exploration rovers
have access to low-resolution satellite maps of the Martian surface, but they still
need to perform local mapping to guide obstacle avoidance and motion planning.
The importance of the SLAM problem motivates the large amount of attention
this topic has received, both within the research community and from practitioners
interested in using SLAM technologies across multiple application domains from
robotics, to virtual and augmented reality. At the same time, SLAM remains an
exciting area of research, with many open problems and new opportunities.

1.2 Anatomy of a Modern SLAM System

The ultimate goal of SLAM is to infer a map representation and robot poses (i.e.,
trajectory) from sensor data, including data from proprioceptive sensors (e.g., wheel
odometry or inertial measurement unit, IMU) and exteroceptive sensors (e.g., cam-
eras, light detection and ranging (LiDAR)s, radars). In mathematical terms this
can be understood as an inverse problem: given a set of measurements, determine
a model of the world (the map) and a set of robot poses (trajectory) that could
have produced those measurements. There exist two alternative strategies to solve
the SLAM problem: indirect and direct methods.
The vast majority of SLAM methods prefers pre-processing the raw sensory data
in order to extract “intermediate representations” that are compact and easier to
describe mathematically. Instead of using every pixel in an image, these methods
extract a few distinctive 2D point features (or keypoints) and then only model the
geometry of how these keypoints depend on the pose of the camera and the geometry
of the scene. In contrast, rather than computing an intermediate abstraction, direct
methods aim to compute localization and mapping directly from the raw sensory
data. This categorization is prominent in visual SLAM but is not limited to it as
we will see in Chapter 9 and Chapter 10. Both indirect and direct methods have
their advantages and shortcomings.
Indirect methods are often faster and more memory efficient. Rather than pro-
cessing every single pixel of each camera image, for example, they merely process a
small subset of keypoints for which the 3D location is determined. As a consequence,
real-time capable systems for indirect visual SLAM were already available around
the year 2000. To date, indirect methods are the preferred approach for real-time
8 Prelude

robot vision on platforms with limited compute. Moreover, once the intermediate
representation is determined, the subsequent computations are often mathemati-
cally simpler, making the resulting inference problems more tractable. In the case
of visual SLAM, for example, once a set of corresponding points is identified across
a set of images, the resulting problem of localization and mapping amounts to the
classical bundle adjustment problem for which a multitude of powerful solvers and
approximation methods exist.
In turn, direct methods have the potential to provide superior accuracy because
they make use of all available input information. While the processing of all avail-
able input information (for example all pixels in each image) is computationally
cumbersome and capturing the complex relationship between the quantities of in-
terest (localization and mapping) and the raw input data (e.g. the brightness of
each pixel) may create additional non-convexities in the overall loss function, there
exist efficient approximation and inference strategies with first real-time capable
methods for direct visual SLAM emerging in the 2010s. As we will see in Part II
and III, the efficient processing of huge amounts of input data can be facilitated by
using graphics processing units (GPUs) to parallelize computations.
In both direct and indirect methods the measurements are used to infer the robot
pose and map representation. There is a well-established literature in estimation
theory describing how we can infer quantities of interest (in our case, the robot
poses and the surrounding map) from observations. This book particularly focuses
on estimation theoretic tools —reviewed and tailored to the SLAM problem in
Chapter 2 and Chapter 3— that have their foundations in probabilistic inference
and that rephrase estimation in terms of solving optimization problems.

front-end back-end
raw pseudo-
measure- sensor processing measure-
ments ments
(feature extraction, estimated
IMU preintegration, pose,
downsampling) geometric SLAM map
(based on factor graph
optimization)
place recognition
(topological loop-
closure detection) map
constraints

Figure 1.2 The anatomy of a typical SLAM is made up of a front-end (to process rich
sensing data into more manageable information and to detect topological loop closures)
and a back-end (to estimate the robot’s pose and a geometric map). The back-end often has
a number of helper modules aimed at helping with robustness, computational tractability,
and map quality.
 1.2 Anatomy of a Modern SLAM System 9

Indirect methods produce a natural split in common SLAM architectures (Figure

1.2): the raw sensor data is first passed to a set of algorithms (the SLAM front-
end) in charge of extracting intermediate representations; then such intermediate
representations are passed to an estimator (the SLAM back-end), that estimates the
quantities of interest. The front-end is typically also in charge of building an initial
guess: this is an initial estimate the back-end can use for iterative optimization,
hence mitigating convergence issues due to non-convexity. Let us discuss a few
examples to clarify the difference between the SLAM front-end and back-end.

Figure 1.3 (a) In landmark-based SLAM models, the front-end produces measurements
to 3D landmarks and the back-end estimates the robot trajectory (as a set of poses) and
landmark positions. (b) In pose-graph-based SLAM models, the front-end abstracts the
raw sensor measurements in terms of odometry and loop closure measurements (these
are typically relative pose measurements) and the back-end estimates the overall robot
trajectory.

Example 1.1 (Visual SLAM: from pixels to landmarks). Visual SLAM uses camera
images to estimate the robot trajectory and a sparse 3D point cloud map. The
typical front-end of a visual SLAM system extracts 2D keypoints and matches them
across frames such that each group (a feature track) corresponds to re-observations
of the same 3D point (a landmark) across different camera views. The front-end will
also compute rough estimates of the camera poses and 3D landmark positions by
using computer vision techniques known as minimal solvers.1 Then, the back-end
is in charge of estimating (or refining) the unknown 3D position of the landmarks
and the robot poses observing them by solving an optimization problem, known
as bundle adjustment. This example leads to a landmark-based (of feature-based)
SLAM model, visualized in Figure 1.3(a). We will discuss visual SLAM at length
in Chapter 8.
Example 1.2 (Lidar SLAM: from scans to odometry and loop closures). Lidar
1
A more subtle point is that minimal solvers will also allow pruning away a large portion of outliers,
i.e., incorrect detections of a landmark. This makes the job of the back-end easier, while still
allowing it to remove any remaining outlier. We discuss outlier rejection and the related problem of
data association in Chapter 4.
10 Prelude

SLAM uses lidar scans to estimate the robot trajectory and a map. A common
front-end for lidar SLAM consists in using scan matching algorithms (e.g., the
Iterative Closest Point or ICP) to compute the relative pose between two lidar scans.
In particular, the front-end will match scans taken at consecutive time instants to
estimate the relative motion of the robot between them (the so called odometry) and
will also match scans corresponding to multiple visits to the same place (the so called
loop closures). Odometry and loop closure measurements are then passed to the
back-end that optimizes the robot trajectory by solving an optimization problem,
known as pose-graph optimization. This example leads to a pose-graph-based SLAM
model, visualized in Figure 1.3(b). We discuss LiDAR SLAM in Chapter 9.

The previous examples showcase three popular examples of “intermediate rep-

resentations” (or pseudo-measurements) that are produced by the front-end and
passed to the back-end (Figure 1.2): landmark observations, odometry, and loop
closures. In complex SLAM systems, these representations can be used in combi-
nation: for instance, in certain visual-SLAM systems one might extract keypoints
corresponding to 3D landmarks, and further process them to compute relative poses
corresponding to odometry and loop-closures, and finally use a pose-graph-based
back-end. The choice of the front-end/back-end split is about selecting a desired
trade-off between computation and accuracy. Extracting simpler representations
might lead to much faster back-end solvers (e.g., performing pose-graph optimiza-
tion is typically much faster than doing bundle adjustment); but at the same time
abstracting measurements induces approximation in how the measurements are
modeled in the back-end, hence leading to small inaccuracies (e.g., bundle adjust-
ment is typically more accurate than pose-graph optimization).
We remark that loop closures are a key aspect of SLAM. If we only use odom-
etry for trajectory estimation, the resulting estimate —obtained by accumulating
odometry motion estimates— is bound to drift over time, leading to severe distor-
sion in the trajectory estimate. Revisiting already visited places is crucial to keep
the trajectory estimation error bounded and obtain globally consistent maps. We
also remark that loop closures are implicitly captured in landmark-based SLAM,
where loop closures correspond to new observations of previously seen landmarks.
We conclude this section by observing how SLAM research cuts across multi-
ple disciplines. The SLAM front-end extracts features from raw sensor data, hence
touching disciplines ranging from signal processing, geometry, 2D computer vision,
and machine learning. The SLAM back-end performs estimation given measure-
ments from the front-end, hence touching estimation theory, optimization, and ap-
plied mathematics. This variety of ideas and influences contribute to making SLAM
a fascinating and multi-faceted problem.
 1.3 The Role of SLAM in the Autonomy Architecture 11

motion motion control

error system:
goal motion plan motion inputs
+
- robot and
planning controller
environment
odometry
sensor
SLAM data
front-end:
map and odometry
robot state SLAM
back-end
SLAM
front-end:
loop closures
SLAM

Figure 1.4 SLAM plays an important role in the overall autonomy pipeline of a robot
that interacts with the world, and provides necessary information for control and motion
planning.

1.3 The Role of SLAM in the Autonomy Architecture

The role of SLAM is to serve downstream tasks. For instance, the robot pose es-
timate can be used to control the robot to follow a desired trajectory, while the
map (in combination with the current robot pose) can be used for motion planning
(Figure 1.4). Here motion planning is used in a broad sense: while SLAM is typi-
cally used to build large-scale maps to support navigation tasks, it can also support
building local 3D maps to enable manipulation and grasping.
While it would be tempting to think about SLAM as a monolithic system that
takes sensor data in input and instantaneously outputs robot poses and map, the
actual implementation of these systems and their integration in autonomy archi-
tectures is more complicated in practice. This is due to the fact that the robot
needs to close different control and decision-making loops with different latency
requirements. For instance, with reference to Figure 1.4, the robot will need to
close low-level control loops over its trajectory (this is the standard feedback con-
trol loop at the top-right of the figure), which might require relatively high rates
and low-latency to be stable; for instance, a UAV flying at high speed might need
the front-end to produce odometry estimates with a latency of a few milliseconds.
On the other hand, closing the loop over motion planning (the outer loop in Fig-
ure 1.4) can accommodate higher latencies, since global planning typically runs at
lower rates; hence it might be acceptable for the back-end to provide global tra-
jectory and map estimates with a latency of seconds. For these reasons, a typical
implementation of a SLAM system involves multiple processes running in parallel
and in a way that slower processes (e.g., global pose and map optimization in the
back-end) do not get in the way of faster processes (e.g., odometry estimation). We
12 Prelude

also observe that the processes involved in a SLAM system have complex interac-
tions (as emphasized by the bi-directional edges in Figure 1.4): for instance, while
the front-end feeds the odometry to the back-end, the back-end periodically applies
global corrections to the odometric trajectory, which is then passed to the motion
controller; similarly, while the front-end computes loop closures that are fed to the
back-end, the back-end can also inform loop closure detection about plausible or
implausible loop closure opportunities.
The problem of visual SLAM is closely related to the problem of Structure from
Motion (SfM). While for some researchers both terms are equivalent, others distin-
guish and argue that visual SLAM systems will typically also integrate additional
sensory information (IMUs, wheel odometry, etc) and focus on an online approach
where data comes in sequentially whereas in SfM both online and offline versions
are conceivable and the input is only images.
Overall, one can distinguish two complementary challenges: There is the online
challenge, where a robot moves around, where sensory data streams in sequentially
and while the SLAM back-end might run at a slower pace, vital estimates such
as the localization of the robot must be determined in real-time, often even on
embedded hardware with limited compute. These realtime constraints are vital for
the robot to properly act in a complex environment, in particular with faster robots
such as drones. They often dictate the choice of algorithms and processing steps.
And there is the offline challenge where the input data may not exhibit any
sequential ordering (say an unordered dataset of images), where computations typ-
ically do not require real-time performance and where the compute hardware can
be (arbitrarily) large (multiple powerful GPUs), see for example [8]. In such cases,
accuracy of the estimated map and trajectories is more important than compute
time.
In most applications, however, one will face a mix of these two extreme sce-
narios where certain quantities need to be determined fast whereas others can be
determined offline. In practical applications of SLAM it is of utmost importance to
carefully analyze which quantities need to be determined at which frequency and
one may come up with an entire hierarchy of different temporal scales at which
quantities are being estimated.

1.3.1 Do we really need SLAM for robotics?

From our description above, SLAM feels like an intriguing but very challenging
problem, ranging from its complex implementation, to the need of fast runtime
on resource-constrained platforms. Therefore, a fair question to ask is whether we
can develop complex autonomous robots that do not rely on SLAM. We refine this
question into three sub-questions.
Q1. Do we need SLAM for any robotics task? We started this section
stating that SLAM is designed to support robotics tasks. Then a natural question is
 1.3 The Role of SLAM in the Autonomy Architecture 13

whether it is necessary for any robotics task. The answer is clearly: no. More reactive
tasks, for instance keeping a target in sight can be solved with simpler control
strategies (e.g., visual servoing).2 Similarly, if the robot has to operate over small
distances, relying on odometry estimates and local mapping might be acceptable.
Moreover, if the environment the robot operates in has some infrastructure for
localization, then we may not need to solve SLAM. Nevertheless, SLAM seems
an indispensable component for long-term robot operation in unstructured (i.e.,
infrastructure-free) environments: long-term operation typically requires memory
(e.g., to go back to previously seen objects or find suitable collision-free paths),
and map representations built from SLAM provide such a long-term memory.

Meeting room O ce 2
Closet O ce 2

O ce 1 O ce 3 Kitchen O ce 1

O ce 3 Kitchen
(a)
(b)
Meeting
Closet Of ce 2
room
Of ce 2

Corridor
Corridor

Of ce 1 Of ce 3 Kitchen
Of ce 1 Kitchen

ffi
fi
(c) (d)

Figure 1.5 (a) Our robot visits Office 1 in a building and then —after exploring other
areas (including Office 2 and the Kitchen)— it visits Office 3, which is just next door from
Office 1. Obstacles are shown in black and ground truth trajectory is shown in green. (b)
Odometric estimate of the trajectory, labeled with corresponding room labels. (c) Ground
truth topological map of the environment. (d) Estimated topological map in the presence
of perceptual aliasing, causing the robot to think that Office 1 and 3 are the same room.

Q2. Do we need globally consistent geometric maps for navigation? A

major focus in SLAM is to optimize the trajectory and map representations such
that they are metrically accurate (or globally consistent) – this is precisely the role
of the SLAM back-end. One might ask whether metric accuracy is actually needed.
One alternative that comes to mind is to just use odometry to get locally consis-
tent trajectory and map estimates; this circumvents the need for loop closures and
2
One could argue that while not being strictly necessary for tracking, SLAM and odometry might
still be helpful to increase its robustness, e.g., when the target gets out of sight.
14 Prelude

back-end optimization. Unfortunately, due to its drift, odometry is unsuitable to

support long-term operation: imagine that our robot visits Office 1 in a building
and then, after exploring other areas of the building it visits Office 3, which is just
next door from Office 1 (see Figure 1.5(a)). Using just odometry, the robot might
be misled to conclude that Office 1 and Office 3 are quite far from each other (due
to the odometry drift), hence being unable to realize there is a short path connect-
ing the two offices (Figure 1.5(b)). A slightly more sophisticated alternative is to
build a topological map instead. A topological map can be thought of as a graph
where nodes are places the robot visited and edges represent traversability between
the places connected by each edge (Figure 1.5(c)). The difference with the metric
SLAM lens we adopt in this handbook is that nodes and edges in a topological map
do not carry metric information (distances, bearing, positions), hence they do not
require any optimization: one can simply add edges to a topological map when the
robot traveled between two places (odometry) or when a place recognition module
recognizes the places to overlap (loop closures). While this seems a perfectly reason-
able approach, the main issue is that place recognition techniques are not perfect
and, more fundamentally, two different places might look similar (a phenomenon
known as perceptual aliasing). Therefore, going back to our example above, if Office
1 and Office 3 look very similar, a purely topological approach might be misled
to think there is a single office instead (Figure 1.5(d)). On the other hand, metric
SLAM approaches can use geometric information to conclude that the two offices
are indeed two different rooms, by giving the user access to a more powerful set of
tools to decide whether place recognition results are correct and if two observations
correspond to the same place; we will discuss these tools at length in Chapter 4.
Q3. Do we need maps? SLAM builds a map that can be directly queried,
inspected, and visualized. As we will see in Chapter 6, there are many ways to rep-
resent a map, including 3D point clouds, voxels, meshes, neural radiance fields, and
others. On the other hand, one might take a completely different approach: in order
for the robot to execute a task, the robot might be trained to translate raw sensor
data directly to actions (e.g., using Reinforcement Learning), hence circumventing
the need to build a map. In such an approach, the neural network trained from
sensor data to actions will arguably create an internal representation, but such an
internal representation cannot be directly queried, inspected, or visualized. While
the jury is still out on whether maps are indeed necessary, there is some initial
evidence that using maps as an intermediate representation is at least beneficial
in completing many visual tasks for robotics [218, 304]. Moreover, maps have the
benefit of being useful across a wide variety of tasks, while a representation that is
fully learned in the context of a single task might not be able to support new unseen
tasks. Finally, we observe that there are several applications where the goal is to
have a map that can be inspected. This is the case in search-and-rescue robotics ap-
plications where it is desirable to provide a map to help first-responders. Moreover,
it is the case for several applications beyond robotics (e.g., real-estate planning and
 1.4 Past, Present, and Future of SLAM, and Scope of this Handbook 15

visualization, construction monitoring, virtual and augmented reality), where the

goal is for a human to inspect or visualize the map.

1.4 Past, Present, and Future of SLAM, and Scope of this Handbook
The design of algorithms for spatial reasoning has been at the center-stage of
robotics and computer vision research since their inception. At the same time,
SLAM research keeps evolving and expanding to novel tools and problems.

1.4.1 Short History and Scope of this Handbook

As discussed across the various chapters of this book, SLAM has multiple facets.
As a consequence, its history is also multi-faceted with origins that can be traced
back across different scientific communities.
Creating maps of the world from observations and measurements is among the
oldest challenges in history and leads to the fields of geodesy (the science measuring
properties of the Earth) and surveying. There are many pioneers who contributed to
this field. Carl Friedrich Gauss triangulated the Kingdom of Hannover in the years
1821-1825. Sir George Everest served as Surveyor General of India 1830-1843 in the
Great Trigonometric Survey, efforts for which he was honored by having the world’s
largest mountain named after him. In 1856, Carl Maximilian von Bauernfeind pub-
lished a standard book on “Elements of Surveying” [22]. He subsequently founded
the Technical University of Munich in 1868 with a central focus on establishing
geodesy as a scientific discipline. André-Louis Cholesky developed the well-known
Cholesky matrix decomposition while surveying Crete and North Africa before the
First World War.
The problem of visual SLAM is also closely related to the field of photogrammetry
and the problems of Structure from Motion in computer vision. Its origins can be
traced back to the 19th century. See Chapter 8.
In robotics, the origin of SLAM is typically traced back to the seminal work of
Smith and Chessman [239] and Durrant-Whyte [79], as well as the parallel work
by Crowley [60] and Chatila and Laumond [48]. The acronym SLAM was coined in
1995, as part of the survey paper [80]. These early works developed two fundamental
insights. The first insight is that to avoid drift in unknown environments, one needs
to simultaneously estimate the robot poses and the position of fixed external entities
(e.g., landmarks). The second insight is that existing tools from estimation theory,
and in particular the celebrated Extended Kalman Filter (EKF), could be used
to perform estimation over an extended state describing the robot poses and the
landmark positions, leading to a family of EKF-SLAM approaches.
EKF-SLAM approaches have been extremely popular but face three main issues
in practice. The first is that they are sensitive to outliers and data association er-
rors. These errors may result from failures of place recognition or object detection,
 10
10 IEEE
IEEE TRANSACTIONS
TRANSACTIONS ON
ON ROBOTICS,
ROBOTICS, MANUSCRIPT
MANUSCRIPT SEPTEMBER
SEPTEMBER 7,7, 2008
2008

10 IEEE TRANSACTIONS ON ROBOTICS, MANUSCRIPT SEPTEMBER 7, 2008

16 Prelude
MIT Killian Court

P3

P2

(a)
(a) Original
Original noisy
noisy data
data set.
set. (b)
(b) Trajectory
Trajectory after
after incremental
incremental optimization.
optimization. (c)
(c) Final
Final RR factor
factor with
with side
side length
length 10
10500.
500.

P1 Fig.
Fig. 9.
9. iSAM
iSAM results
results for
for the
the simulated
simulated Manhattan
Manhattan world
world from
from [21]
[21] with 3500 poses
with 3500 poses and 5598 constraints.
and 5598 constraints. iSAM
iSAM takes
takes about 40ms per
about 40ms per step.
step. The
The resulting
resulting
Pn R factor
R factor has
has 187 423 entries,
187423 entries, which
which corresponds
corresponds to 0.34% or
to 0.34% or an
an average
average of 17.8 entries
of 17.8 entries per
per column.
column.

P n-1 (a) Original noisy data set. (b) Trajectory after incremental optimization. (c) Final R factor with side length 10 500.

a Fig. 9. iSAM results for the simulated Manhattan world from [21] with 3500 poses and 5598 constraints. iSAM takes about 40ms per step. The resulting
R factor has 187 423 entries, which corresponds to 0.34% or an average of 17.8 entries per column.

b
(a)
(a) Trajectory
Trajectory based
based on odometry
odometry only. (b)
(b) Final
Final trajectory and
and evidence
Early Graph SLAM Compressed EKF SLAM, Victoria Rao-Blackwellized onParticle only.
Filter Incremental trajectory
Smoothing andgrid
evidence grid map.
map.
Mapping (c)
(c) Final
Final RR factor
factor with
with side
side length
length 2730.
2730.

(F. Lu / E. Milios, 1997) Park Dataset (credit: J. Guivant / E. SLAM

Fig.
Fig. 10. (credit:
10. Results
Results from G. Grisseti
from iSAM
iSAM applied to/the
applied to C.Intel
the (iSAM)
Intel dataset.
dataset. iSAM (credit:
iSAM calculates
calculates the M.
the full Kaessfor
full solution
solution / A.
for 910 poses
910 poses and 4453 constraints
and 4453 constraints with
with an
an average
average of
of 85ms
85ms
Nebot, 2001) per step,
step, while
while reordering
perStachniss reordering the
the variables
/ W. Burgard, variables every
2007) 20 steps.
every 20 steps. The
The problem
problem has
Ranganathan has 910 ⇥
/ F.
910 2730 variables
⇥3Dellaert,
3== 2730 2008)and
variables 4453⇥
and 4453 ⇥33 =
= 13 359 measurement
13359 measurement equations.
equations. The
The R
R
factor
factor contains
contains 90 363 entries,
90363 entries, which
which corresponds
corresponds to 2.42% or
to 2.42% 33.1 entries
or 33.1 entries per
per column.
column.
refuses to initialize. PTAM initializes selecting sometimes
Our initialization method detects the ambiguity and for safety
interpretations, i.e. the twofold ambiguity described in [27].
camera trajectory with respect to the plane has two possible
(fr3 nstr tex far). This is a planar scene that because of the
all the sequences, except for fr3 nostructure texture far

16 sequences selected.
shows the median results over 5 executions in each of the
frames from which we get a good initialization. Table III
with the ground truth. For PTAM we manually selected two
fore we have discarded the first 10 keyframes when comparing
from random depth values and takes time to converge, there-
check if the scale was well recovered. LSD-SLAM initializes
tories with a rigid body transformation, but also a similarity to
(ATE) [38]. In the case of RGBD-SLAM we align the trajec-
scale is unknown, and measure the absolute trajectory error
keyframe trajectories using a similarity transformation, as
LSD-SLAM and PTAM with the ground truth, we align the
in the benchmark website. In order to compare ORB-SLAM,
SLAM [43] which are provided for some of the sequences
We compare also with the trajectories generated by RGBD-
semi-dense LSD-SLAM [10] and PTAM [4] in the benchmark.

no texture or no motion.
monocular SLAM systems, as they contain strong rotations,
those sequences that we consider that are not suitable for pure
an external motion capture system. We have discarded all
several sequences with accurate ground truth obtained with
to evaluate the accuracy of camera localization as it provides

B. Localization Accuracy in the TUM RGB-D Benchmark

of keyframes, which is a quite sparse graph.

Essential Graph includes edges around 5 times the number
the potential of bag of words for place recognition. Our
subset of images with words in common, which demonstrates
the efficient querying of the database that only compare the

closures were found, therefore they do not perfectly align.

bigger loop on the right is traversed in opposite directions and not visual loop
Fig. 6. ORB-SLAM reconstruction of the full sequence of NewCollege. The

IEEE TRANSACTIONS ON ROBOTICS

(a) Trajectory based on odometry only. (b) Final trajectory and evidence grid map. (c) Final R factor with side length 2730.
It can be seen that ORB-SLAM is able to process

For comparison we have also executed the novel, direct,

The TUM RGB-D benchmark [38] is an excellent dataset

Fig. 10. Results from iSAM applied to the Intel dataset. iSAM calculates the full solution for 910 poses and 4453 constraints with an average of 85ms
per step, while reordering the variables every 20 steps. The problem has 910 ⇥ 3 = 2730 variables and 4453 ⇥ 3 = 13 359 measurement equations. The R
factor contains 90 363 entries, which corresponds to 2.42% or 33.1 entries per column.

(a)
(a) Trajectory
Trajectorybased
basedon
onodometry
odometryonly.
only. (b)
(b) Final
Final trajectory
trajectory and
and evidence
evidence grid
grid map.
map. (c)
(c) Final
Final RR factor
factor with
with side
side length
length 5823.
5823.

Fig.
Fig. 11.
11. iSAM
iSAM results
results for
for the
the MIT
MIT Killian
Killian Court
Court dataset.
dataset. iSAM
iSAM calculates
calculates the
the full
full solution
solution for
for the 1941 poses
the 1941 poses and 2190 pose
and 2190 pose constraints
constraints with
with an
an average
average
Large-Scale Direct SLAM ORB-SLAM (credit: R. Mur- of 12.2ms per
of 12.2ms per step.
step. The
The R R factor
factor contains
contains
3D SLAM 52 414 entries
52414 thefor
entries
in for 5823 variables,
5823
DARPA SubTwhich
variables, which corresponds
corresponds to 0.31% or
to 0.31% 9.0 per
or 9.0 per column.
column.
property in conjunction with our keyframe culling procedure
points and robustly under moderate dynamic changes. This
system is able to localize in a map from very different view-

D. Lifelong Experiment in the TUM RGB-D Benchmark

our system in these experiments.

some examples of challenging relocalizations performed by
78% of the frames, as can be seen in Table IV. Fig. 8 shows
PTAM finds no relocalizations while our system relocalizes
are big occlusions due to people moving in the scene. Here
fr3 walking xyz. This is a challenging experiment as there
quence fr3 sitting xyz and try to relocalize all frames from
the second experiment we create an initial map with se-
relocalizes more than the double of frames than PTAM. In
error with respect to the ground truth. ORB-SLAM accurately
its relocalization method. Table IV shows the recall and the
which are near to the keyframes due to the little invariance of
can be seen that PTAM is only able to relocalize frames
relocalized frames and the ground truth for those frames. It
keyframes used to create the initial map, the poses of the
experiment with PTAM for comparison. Fig. 7 shows the
the accuracy of the recovered poses. We perform the same
global relocalization with every successive frame and evaluate
with the first 30 seconds of the sequence fr2 xyz and perform
RGB-D benchmark. In the first experiment we build a map

C. Relocalization in the TUM RGB-D Benchmark

were obtained, and we have been unable to reproduce them.

the paper does not give enough details on how those results
LSD-SLAM in fr2 xyz with an RMSE of 24.28cm. However,
Engel et al. [10] reported that PTAM has less accuracy than
significantly reduces the error. Finally it should be noted that
in fr2 sequences, as aligning the trajectories with 7 DoF

as seen in fr2 desk with person and fr3 walking xyz.

seems to be less robust to dynamic objects than our system
in Section IX-B. Another interesting result is that LSD-SLAM
solve structure from motion [2]. We further discuss this result
sensor measurements, which is the gold standard algorithm to
bundle adjustment and jointly optimize cameras and map over
were sensor measurements are discarded, while we perform
reduce the map optimization to a pose-graph optimization
RGBD-SLAM. One of the possible causes can be that they
SLAM are clearly more accurate than LSD-SLAM and
The most surprising results is that both PTAM and ORB-
fr3 nostructure texture near withloop (fr3 nstr tex near).
accuracy when detecting large loops as in the sequence
in open trajectories, while ORB-SLAM achieves higher

track in eight and three sequences respectively.

LSD-SLAM exhibit less robustness than our method, loosing
sequence is very high. In the rest of the sequences, PTAM and
different reconstructions from LSD-SLAM but the error in this
case the error is unacceptable. We have not noticed two
the true solution and others the corrupted one, in which

(credit: J.Engel / T. Schöps / Artal, J Montiel, J Tardós, 2015) Competition (credit: K. Ebadi et al., 2023)
Previous relocalization experiments have shown that our

We perform two relocalization experiments in the TUM

We have noticed that RGBD-SLAM has a bias in the scale

In terms of accuracy ORB-SLAM and PTAM are similar

D. Cremers, 2014)

(a) Trajectory based on odometry only. (b) Final trajectory and evidence grid map. (c) Final R factor with side length 5823.

Figure 1.6 The history of SLAM is filled with

Fig. numerous advances
11. iSAM results for the MIT Killian Court that have
dataset. iSAM led
calculates tosolution
the full modern
for the 1941 poses and 2190 pose constraints with an average
of 12.2ms per step. The R factor contains 52 414 entries for 5823 variables, which corresponds to 0.31% or 9.0 per column.
SLAM systems capable of localizing and mapping robots in challenging real-world envi-
ronments. This image shows a selection of representative highlights.

where the robot believes it is observing a given object or place, but it is actually
observing a different (but possibly similarly looking) one. If these spurious measure-
10

ments are not properly handled, EKF-SLAM produces grossly incorrect estimates.
The second issue is related to the fact that EKF relies on linearization of the equa-
tions describing the motion of the robot and sensor observations. In practice, the
linearization point is typically built from odometry and when the latter drifts, the
linearized system might be a poor approximation of the original nonlinear system.
This leads EKF-SLAM to diverge when odometry accumulates substantial drift.
The third problem is about computational complexity: a naive implementation of
the Kalman Filter leads to a computational complexity that grows quadratically in
the number of state variables, due to the need to manipulate a dense covariance
matrix. In a landmark-based SLAM problem it is not uncommon to have thousands
of landmark, which makes the naive approach prohibitive to run in real-time.
As a response to these issues, in the early 2000s, the community started focusing
 1.4 Past, Present, and Future of SLAM, and Scope of this Handbook 17

on particle-filter-based approaches [178, 238, 103], which model the robot trajectory
using a set of hypothesis (or particles), building on the theory of particle filtering
in estimation theory.3 When used in combination with landmark-based maps, these
models allowed using a large number of landmarks (breaking through the quadratic
complexity of the EKF); moreover, they allowed to more easily estimate dense map
models, such as 2D occupancy grid maps. Also, these approaches did not rely on
linearization and were less sensitive to outliers and incorrect data association. How-
ever, they still exhibited a trade-off between computation and accuracy: obtaining
accurate trajectories and maps requires using many particles (in the thousands)
but the more particles, the more computation. In particular, for a finite amount
of particles, a particle filter may still diverge when none of the sampled particles
are near the real trajectory of the robot (an issue known as particle depletion);
this issue is exacerbated in 3D problems where one needs many particles to cover
potential robot poses.
Between 2005 and 2015, a key insight pushed to the spotlight an alternative
approach to SLAM. The insight is that while the covariance matrix appearing in
the EKF is dense, its inverse (the so called Information Matrix) is very sparse and
has a very predictable sparsity pattern when past robot poses are retained in the
estimation [87]; this allows designing filtering algorithms that have close-to-linear
complexity, as opposed to the quadratic complexity of the EKF. While this insight
was initially applied to EKF-like approaches, such as EIF, it also paved the way for
optimization-based approaches. Optimization-based approaches were first proposed
in the early days of SLAM [162], but then disregarded as too slow to be practi-
cal. The sparsity structure mentioned above allowed rethinking these optimization
methods and making them more scalable and solvable in online fashion [65, 126].4
This new wave can be interpreted as a shift toward yet another estimation frame-
work: maximum likelihood and maximum a posteriori estimation. These frameworks
rephrase estimation problems in terms of optimization, while describing the struc-
ture of the problem in terms of a probabilistic graphical model, or, specifically, a
factor graph. The resulting factor-graph-based approach to SLAM is still the dom-
inant paradigm today, and has also shaped the way the community thinks about
related problems, such as visual and visual-inertial odometry. The optimization lens
is a powerful one and allows a much deeper theoretical analysis than previously
possible (see Chapter 7). Moreover, it is fairly easy to show that the EKF (with
suitable linearization points) can be understood as a single iteration of a nonlinear
optimization solver, hence making the optimization lens strictly more powerful than
its filtering-based counterpart. Finally, the optimization-based perspective appears
more suitable for recent extensions of SLAM (described in the next section and

3
The resulting algorithms are known with different names in different communities, e.g.,
Sampling/Importance Re-sampling, Monte-Carlo filter Condensation algorithm, Survival of the
fittest algorithm, and others.
4
More details are in Chapter 2.
18 Prelude

Part III of this handbook), where one wants to estimate both continuous variables
(describing the scene geometry) and discrete variables (describing semantic aspects
of the scene).
This short history review stops at 2015, while the goal of Part III of this handbook
is to discuss more modern trends, including those triggered by the “deep learning
revolution”, which started around 2012 and slowly permeated to robotics. We also
remark that the short history above mostly gravitates around what we called the
SLAM back-end (essentially, the estimation engine), while the development of the
SLAM front-end traces back to work done across multiple communities, including
computer vision, signal processing, and machine learning.
As a result of the considerations mentioned above, this handbook will primarily
focus on the factor-graph-based formulation of SLAM. This is a decision about
scope and does not detract from the value of ongoing works using other technical
tools. For instance, at the time of writing of this handbook, EKF-based tools are
still popular for visual-inertial odometry applications (building on the seminal work
from Mourikis and Roumeliotis [180]), and novel estimation formulations have been
developed, including invariant [20] and equivariant filters [90], as well as alternative
formulations based on random finite sets [181].

1.4.2 From SLAM to Spatial AI

SLAM essentially focuses on estimating geometric properties of the environment
(and the robot). For instance, the SLAM map carries information about obstacles
in the environment, distances and traversable paths between two locations, or ge-
ometric coordinates of distinctive landmarks. In this sense, SLAM is useful as a
representation for the robot to understand and execute commands such as “robot:
go to position [x, y, z]”, where [x, y, z] are the coordinates (in the map frame) of a
place or object the robot has to reach. However, specifying goals in terms of coor-
dinates is not suitable for non-expert human users and it is definitely not the way
we interact or specify goals for humans. Therefore, it would be desirable for the
next generation of robots to understand and execute high-level commands speci-
fied in natural language, such as “robot: pick up the clothes in the bathroom, and
take them to the laundry room”. Parsing these instructions requires the robot to
understand both geometry (e.g., where is the bathroom) and semantics (e.g., what
is a bathroom or laundry room, which objects are clothes) of the environment.
This realization has recently pushed the research community to think about
SLAM as an integrated component of a broader spatial perception system, that
simultaneously reasons about geometric, semantic, and possibly physical aspects of
the scene, in order to build a multi-faceted map representation (a “world model”),
that enables the robot to understand and execute complex instructions. The re-
sulting Spatial AI algorithms and systems have the potential to increase robot
autonomy and have rapidly progressed over the last decade. Intuitively, one can
 1.5 Handbook Structure 19

semantic/
high-level motion motion control
goal goal plan error inputs system:
task motion motion
+
- robot and
planning planning controller
environment

fast
odometry loop
metric medium
metric-semantic slow
map and loop
map and loop robot state sensor
odometry data
robot state
SLAM
spatial AI

Figure 1.7 Spatial AI (or spatial perception) extends the geometric reasoning capabilities
of SLAM to also perform semantic and physical reasoning. While the SLAM block is
informed by odometry and provides a geometric understanding of the scene, the Spatial
AI block is informed by the SLAM results and adds a scene understanding component,
spanning semantics, affordances, dynamics, and more. This allows closing the loop over
higher-level decision making modules, such as task planning, and allows the user to specify
higher-level goals the robot has to achieve.

think that Spatial AI has SLAM as a submodule (to handle the geometric reason-
ing part), but provides extra semantic reasoning capabilities. This allows closing
the loop over task planning, as shown in Figure 1.7, where now the robot can take
high-level semantic goals instead of coordinates of motion goals. We will discuss
Spatial AI at length in Part III of this handbook.

1.5 Handbook Structure

The chapters of this handbook are grouped into three parts.
Part I covers the foundations of SLAM, with particular focus on the estimation-
theoretic machinery used in the SLAM back-end and the different types of map
representations SLAM can produce. In particular, Chapter 2 introduces the factor-
graph formulation of SLAM and reviews how to solve it via iterative nonlinear
optimization methods. Then, Chapter 3 takes the indispensable step of extending
the formulation to allow the estimation of variables belonging to smooth manifolds,
such as rotation and poses. Chapter 4 discusses how to model and mitigate the
impact of outliers and incorrect data association in the SLAM back-end. Chap-
ter 5 reviews techniques to make the back-end optimization differentiable, a key
step towards interfacing traditional SLAM methods with more recent deep learn-
ing architectures. Chapter 6 shifts the focus from the back-end to the question of
dense map representations and discusses the most important representations used
for SLAM. Finally, Chapter 7 discusses more advanced solvers and theoretical prop-
erties of the SLAM back-end.
Part II covers the “state of practice” in SLAM by discussing key approaches and
20 Prelude

applications of SLAM using different sensing modalities. This part touches on the
SLAM front-end design (which is heavily sensor dependent) and exposes what’s
feasible with modern SLAM algorithms and systems. Chapter 8 reviews the large
body of literature on visual SLAM. Chapter 9 and Chapter 10 cover lidar-SLAM
and radar-SLAM, respectively. Chapter 11 discusses recent work on SLAM using
event-based cameras. Chapter 12 reviews how to model inertial measurements as
part of a factor-graph SLAM system and discusses fundamental limits (e.g., observ-
ability). Chapter 13 discussed how to model other sources of odometry information,
including wheel and legged odometry.
Part III provides a future-looking view of the state of the art and recent trends.
In particular, we touch on a variety of topics, ranging from computational architec-
tures, to novel problems and representations, to the role of language and Foundation
Models in SLAM. In particular, Chapter 14 focuses on future computational archi-
tectures for Spatial AI that could leverage more flexible and distributed computing
hardware and better support spatial perception across many robotic platforms.
Chapter 15 reviews recent improvements obtained by introducing deep learning
modules in conjunction with differentiable optimization in SLAM. Chapter 16 dis-
cusses opportunities and challenges in using novel map presentations, including
neural radiance fields (NeRFs) and Gaussian Splatting. Chapter 17 covers recent
work on SLAM in highly dynamic and deformable environments, touching on real
applications from mapping in crowded environments to surgical robotics. Chap-
ter 18 discusses progress in Spatial AI and metric-semantic map representations.
Finally, Chapter 19 considers new opportunities arising from the use of Founda-
tion Models (e.g., Large Vision-Language Models) and their role in creating novel
map representation for Spatial AI that allow understanding and grounding “open-
vocabulary” commands given in natural language.
 2
Factor Graphs for SLAM
Frank Dellaert, Michael Kaess, Timothy Barfoot

In this chapter we introduce factor graphs and establish the connection with maxi-
mum a posteriori (MAP) inference and least-squares for the case of Gaussian priors
and Gaussian measurement noise. We focus on the SLAM back-end, after mea-
surements have been extracted by the front-end and data association has been
accomplished. We discuss both linear and nonlinear optimization methods for the
corresponding least-squares problems, and then make the connection between spar-
sity, factor graphs, and Bayes nets more explicit. Finally, we apply this to develop
the Bayes tree and the incremental smoothing and mapping (iSAM) algorithm.

Historical Note
A smoothing approach to SLAM involves not just the most current robot location,
but the entire robot trajectory up to the current time. A number of authors consider
the problem of smoothing the robot trajectory only [48, 160, 161, 107, 140, 86],
now known as PoseSLAM. This is particularly suited to sensors such as laser-range
finders that yield pairwise constraints between nearby robot poses.
More generally, one can consider the full SLAM problem [259], i.e., the problem
of optimally estimating the entire set of sensor poses along with the parameters of
all features in the environment. This led to a flurry of work between 2000 and 2005
where these ideas were applied in the context of SLAM [76, 93, 92, 259]. From a
computational view, this optimization-based smoothing was recognized as beneficial
since (a) in contrast to the filtering-based covariance or information matrices, which
both become fully dense over time [201, 258], the information matrix associated with
smoothing is and stays sparse; (b) in typical mapping scenarios (i.e., not repeatedly
traversing a small environment) this matrix is a much more compact representation
of the map covariance structure.
Square-root smoothing and mapping (SAM), also known as the ‘factor-graph ap-
proach’, was introduced in [65, 68] based on the fact that the information matrix
or measurement Jacobian can be efficiently factorized using sparse Cholesky or QR
factorization, respectively. This yields a square-root information matrix that can
be used to immediately obtain the optimal robot trajectory and map. Factoring the
22 Factor Graphs for SLAM

information matrix is known in the sequential estimation literature as square-root

information filtering (SRIF), and was developed in 1969 for use in JPL’s Mariner
10 missions to Venus [30]. The use of square roots results in more accurate and
stable algorithms, and, quoting Maybeck [174], “a number of practitioners have
argued, with considerable logic, that square root filters should always be adopted
in preference to the standard Kalman filter recursion”.
Below we discuss in detail how factor graphs are a natural representation for
the sparsity inherent in SLAM problems, how (sparse) matrix factorization into a
matrix-square root is at the heart of solving these problems, and finally how all
this relates to the much more general variable elimination algorithm. Much of this
chapter is an abridged version of a longer article by Dellaert et al. [69].

2.1 Visualizing SLAM With Factor Graphs

In this section we introduce factor graphs as a way of intuitively visualizing the
sparse nature of the SLAM problem by first considering a toy example and its
factor graph representation. We then show how many different flavors of SLAM
can be represented as such, and how even in larger problems the sparse nature of
many sparse problems is immediately apparent.

2.1.1 A Toy Example

We begin by examining a simple SLAM scenario to illustrate how factor graphs are
constructed. Figure 2.1 shows a simple toy example illustrating the structure of the
problem graphically. A robot moving across three successive poses p1 , p2 , and p3
makes bearing observations on two landmarks ℓ1 and ℓ2 . To anchor the solution
in space, let us also assume there is an absolute position/orientation measurement
on the first pose p1 . Without this there would be no information about absolute
position, as bearing measurements are all relative.1
Because of measurement uncertainty, we cannot hope to recover the true state of
the world, but we can obtain a probabilistic description of what can be inferred from
the measurements. In the Bayesian probability framework, we use the language of
probability theory to assign a subjective degree of belief to uncertain events. We
do this using probability density functions (PDFs) p(x) over the unknown variables
x. PDFs are non-negative functions satisfying
Z
p(x) dx = 1, (2.1)

which is the axiom of total probability. In the simple example of Figure 2.1, the
1
Handling rotations properly is a bit more involved than our treatment in this first chapter lets on.
However, the next chapter will put us on a proper footing for such quantities. For now, we will
assume they are regular vector quantities and delay discussion of their subtleties.
 2.1 Visualizing SLAM With Factor Graphs 23

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

`l11 `l22

<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

p
x1 xp22 xp33

Figure 2.1 A toy simultaneous localization and mapping (SLAM) example with three
robot poses and two landmarks. Above we schematically indicate the robot motion with
arrows, while the dotted lines indicate bearing measurements.

state, x, is
 
p1
p2 
 
x= 
p3  , (2.2)
 ℓ1 
ℓ2
which is just a stacking of the individual unknowns.
In SLAM we want to characterize our knowledge about the unknowns x, in this
case robot poses and the unknown landmark positions, when given a set of observed
measurements z. Using the language of Bayesian probability, this is simply the
conditional density
p(x|z), (2.3)

and obtaining a description like this is called probabilistic inference. A prerequisite

is to first specify a probabilistic model for the variables of interest and how they
give rise to (uncertain) measurements. This is where probabilistic graphical models
enter the picture.
Probabilistic graphical models provide a mechanism to compactly describe com-
plex probability densities by exploiting the structure in them [139]. In particular,
high-dimensional probability densities can often be factorized as a product of many
factors, each of which is a probability density over a much smaller domain.

2.1.2 A Factor-Graph View

Factor graphs are probabilistic graphical models and they allow us to specify a joint
density as a product of factors. However, they are more general in that they can
be used to specify any factored function ϕ(x) over a set of variables x, not just
probability densities.
24 Factor Graphs for SLAM
<latexit sha1_base64="PqET7EFmVz4fHiCLlbz+BRucjoE=">AAACHHicbVDLSgMxFL3js9ZX1aWbwSropsxIfe0KblxWsCp0ypDJ3NpgJhmSjFCGLvwRt271H9yJW8Ff8CvMTEV8HQg5nHMPuTlRypk2nvfmTExOTc/MVuaq8wuLS8u1ldVzLTNFsUMll+oyIho5E9gxzHC8TBWSJOJ4EV0fF/7FDSrNpDgzwxR7CbkSrM8oMVYKa+tBOmBhczuIJI/1MLFXHiDno9DfCWt1r+GVcP8S/5PUWxUo0Q5r70EsaZagMJQTrbu+l5peTpRhlOOoGmQaU0KvyRV2LRUkQd3Ly0+M3C2rxG5fKnuEcUv1eyIniS4WtJMJMQP92yvE/7xuZvqHvZyJNDMo6PihfsZdI92iETdmCqnhQ0sIVczu6tIBUYQa21s1KIO5kGbc2ABJjGpULcs5KrD3VcVfcr7b8Pcb/mmz3toctwQVWIcN2AYfDqAFJ9CGDlC4hXt4gEfnznlynp2X8eiE85lZgx9wXj8A7y+ioQ==</latexit> <latexit sha1_base64="x6Vgnp8/4cA9+SDB5FaRdViLwOw=">AAACHHicbVDLSgMxFL3js9ZX1WU3g1XQTZkRn7uCG5cVrAqdMmQytzaYSYYkI5ShC3/ErVv9B3fiVvAX/AozUxFfB0IO59xDbk6UcqaN5705E5NT0zOzlbnq/MLi0nJtZfVcy0xR7FDJpbqMiEbOBHYMMxwvU4UkiTheRNfHhX9xg0ozKc7MMMVeQq4E6zNKjJXCWj1IByzc2woiyWM9TOyVB8j5KNzZDmsNr+mVcP8S/5M0WhUo0Q5r70EsaZagMJQTrbu+l5peTpRhlOOoGmQaU0KvyRV2LRUkQd3Ly0+M3E2rxG5fKnuEcUv1eyIniS4WtJMJMQP92yvE/7xuZvqHvZyJNDMo6PihfsZdI92iETdmCqnhQ0sIVczu6tIBUYQa21s1KIO5kGbc2ABJjGpULcs5KrD3VcVfcr7T9Peb/uluo7UxbgkqUId12AIfDqAFJ9CGDlC4hXt4gEfnznlynp2X8eiE85lZgx9wXj8A8n2iow==</latexit>

4 (`1 ) 5 (`2 )

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

x3 x2 x1
l`11 l`22

<latexit sha1_base64="GS5uFCPJnpEJVdcs05uLIoU++3g=">AAACLXicbVDLSgMxFL3js9ZX1aWbwSooSJkRtboruHGpYFXolCGTubXBTDIkGaEM/Ql/xK1b/QcXgrgV/AozUxFfB0IO59xDbk6UcqaN5z07Y+MTk1PTlZnq7Nz8wmJtaflcy0xRbFPJpbqMiEbOBLYNMxwvU4UkiTheRNdHhX9xg0ozKc7MIMVuQq4E6zFKjJXC2naQ9lnY3AwiyWM9SOyVp8PQ3/4uBMi51bbCWt1reCXcv8T/JPVWBUqchLX3IJY0S1AYyonWHd9LTTcnyjDKcVgNMo0podfkCjuWCpKg7ublr4buhlVityeVPcK4pfo9kZNEFwvayYSYvv7tFeJ/XiczvYNuzkSaGRR09FAv466RblGRGzOF1PCBJYQqZnd1aZ8oQo0tshqUwVxIM6qwjyRGNayW5RwW2Puq4i8532n4+w3/dLfeWh+1BBVYhTXYBB+a0IJjOIE2ULiFe3iAR+fOeXJenNfR6JjzmVmBH3DePgAlt6oF</latexit> <latexit sha1_base64="ZaMrlTNmHTsHSoROOn40UVNP6Dg=">AAACLXicbVDLSiNBFL2tjsbMqFGXbhrjgIKE7jCPuAvMxmUGJjGQDk119Y0prK5qqqqF0PRP+COzna3+g4sBcSv4FVPdCUN8HCjqcM491K0TpZxp43l/nZXVtQ/rG7XN+sdPW9s7jd29gZaZotinkks1jIhGzgT2DTMch6lCkkQcL6KrH6V/cY1KMyl+mVmK44RcCjZhlBgrhY3TIJ2ysHMcRJLHepbYK0+LsH26LATIeRH6J2Gj6bW8Cu5b4i9Is1uDCr2w8RzEkmYJCkM50Xrke6kZ50QZRjkW9SDTmBJ6RS5xZKkgCepxXv2qcD9bJXYnUtkjjFupy4mcJLpc0E4mxEz1a68U3/NGmZl0xjkTaWZQ0PlDk4y7RrplRW7MFFLDZ5YQqpjd1aVTogg1tsh6UAVzIc28wimSGFVRr8o5K/H1fxVvyaDd8r+1/J9fmt2jeUtQgwM4hGPw4Tt04Rx60AcKN/AHbuHO+e3cOw/O43x0xVlk9uEFnKd/KSqqBw==</latexit> <latexit sha1_base64="Eo/PBR3Z+fmPJNSoXw65gdCK38s=">AAACLXicbVDLSuRAFL1pXz3tq9Wlm2ArKEiT+Jpx1zAblwq2Cp0mVCq3uwsrVaGqIjQhPzE/Mlu3+g8uhMGtMF9hJS3i60BRh3PuoW6dKOVMG897dGpT0zOzc/UfjfmFxaXl5srqhZaZotilkkt1FRGNnAnsGmY4XqUKSRJxvIyuf5f+5Q0qzaQ4N+MU+wkZCjZglBgrhc3dIB2x8Hg7iCSP9TixV54W4f7ueyFAzotwbydstry2V8H9SvxX0urUocJp2PwfxJJmCQpDOdG653up6edEGUY5Fo0g05gSek2G2LNUkAR1P69+VbhbVondgVT2CONW6vtEThJdLmgnE2JG+rNXit95vcwMfvVzJtLMoKCThwYZd410y4rcmCmkho8tIVQxu6tLR0QRamyRjaAK5kKaSYUjJDGqolGVc1zi8K2Kr+Rir+0ftf2zg1Znc9IS1GEdNmAbfPgJHTiBU+gChT9wC3dw7/x1Hpx/ztNktOa8ZtbgA5znFy46qgo=</latexit>

7 (p1 , `1 ) 8 (p2 , `1 ) 9 (p3 , `2 )

l2 <latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit>

l1
xp11
<latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit>

xp22
<latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

xp33
<latexit sha1_base64="AY73GQiTB52dLlEEET5KkyWQjX0=">AAACGXicbVDLSgMxFL3js9ZX1Y3gZrAKuikz4nNXcOOyglWhU4ZM5tYGM8mQZIQy1B9x61b/wZ24deUv+BVmpiK+DoQczrmH3Jwo5Uwbz3tzxsYnJqemKzPV2bn5hcXa0vK5lpmi2KaSS3UZEY2cCWwbZjhepgpJEnG8iK6PC//iBpVmUpyZQYrdhFwJ1mOUGCuFtdUg7bPQ3woiyWM9SOyVp8PQ3w5rda/hlXD/Ev+T1JsVKNEKa+9BLGmWoDCUE607vpeabk6UYZTjsBpkGlNCr8kVdiwVJEHdzcsfDN1Nq8RuTyp7hHFL9XsiJ4kutrOTCTF9/dsrxP+8TmZ6h92ciTQzKOjooV7GXSPdog43Zgqp4QNLCFXM7urSPlGEGltaNSiDuZBmVFcfSYxqWC3LOSqw91XFX3K+0/D3G/7pbr25MWoJKrAG67AFPhxAE06gBW2gcAv38ACPzp3z5Dw7L6PRMeczswI/4Lx+AH8KoVc=</latexit> <latexit sha1_base64="hJcgXdAki0rpaFkGbybvBuSfYg4=">AAACKnicbVDLSgMxFL3js9ZX1aWbwSooSJkpPncFNy4VrAqdMmQytzaYSYYkI5Sh3+CPuHWr/+CuuHXlV5iZivg6EHI45x5yc6KUM208b+RMTE5Nz8xW5qrzC4tLy7WV1UstM0WxTSWX6joiGjkT2DbMcLxOFZIk4ngV3Z4U/tUdKs2kuDCDFLsJuRGsxygxVgprO0HaZ2FzO4gkj/UgsVeeDsPm7i/B3wlrda/hlXD/Ev+T1FsVKHEW1t6DWNIsQWEoJ1p3fC813ZwowyjHYTXINKaE3pIb7FgqSIK6m5dfGrpbVondnlT2COOW6vdEThJdbGcnE2L6+rdXiP95ncz0jro5E2lmUNDxQ72Mu0a6RT9uzBRSwweWEKqY3dWlfaIINbbFalAGcyHNuL8+khjVsFqWc1xg/6uKv+Sy2fAPGv75Xr21OW4JKrAOG7ANPhxCC07hDNpA4R4e4QmenQfnxRk5r+PRCeczswY/4Lx9AJ7sqLo=</latexit> <latexit sha1_base64="6w3K4iziOYNRVY/Dg+sA59X+blk=">AAACKnicbVDLSgMxFL3js9ZX1aWbwSooSJnx7a7gxqWCVaFThkzm1oZmkiHJCGXoN/gjbt3qP7gTt678CjNTEV8HQg7n3ENuTpRypo3nvThj4xOTU9OVmers3PzCYm1p+VLLTFFsUcmluo6IRs4EtgwzHK9ThSSJOF5F/ZPCv7pFpZkUF2aQYichN4J1GSXGSmFtK0h7LNzdDCLJYz1I7JWnw3B3+5ewsxXW6l7DK+H+Jf4nqTcrUOIsrL0HsaRZgsJQTrRu+15qOjlRhlGOw2qQaUwJ7ZMbbFsqSIK6k5dfGrobVondrlT2COOW6vdEThJdbGcnE2J6+rdXiP957cx0jzo5E2lmUNDRQ92Mu0a6RT9uzBRSwweWEKqY3dWlPaIINbbFalAGcyHNqL8ekhjVsFqWc1xg/6uKv+Ryp+EfNPzzvXpzfdQSVGAV1mATfDiEJpzCGbSAwh08wCM8OffOs/PivI5Gx5zPzAr8gPP2AaP2qL0=</latexit>

1 (p1 ) 2 (p2 , p1 ) 3 (p3 , p2 )

<latexit sha1_base64="iC0E0ySHt+M5B1pmT0FEOWjfMnQ=">AAACGXicbVDLSgMxFL3js9ZX1Y3gZrAKuikzolV3BTcuFawKnTJkMrc2mEmGJCOUof6IW7f6D+7ErSt/wa8wMxXxdSDkcM495OZEKWfaeN6bMzY+MTk1XZmpzs7NLyzWlpbPtcwUxTaVXKrLiGjkTGDbMMPxMlVIkojjRXR9VPgXN6g0k+LMDFLsJuRKsB6jxFgprK0GaZ+Fza0gkjzWg8ReeToM/e2wVvcaXgn3L/E/Sb1VgRInYe09iCXNEhSGcqJ1x/dS082JMoxyHFaDTGNK6DW5wo6lgiSou3n5g6G7aZXY7UlljzBuqX5P5CTRxXZ2MiGmr397hfif18lM76CbM5FmBgUdPdTLuGukW9ThxkwhNXxgCaGK2V1d2ieKUGNLqwZlMBfSjOrqI4lRDatlOYcF9r6q+EvOdxp+s+Gf7tZbG6OWoAJrsA5b4MM+tOAYTqANFG7hHh7g0blznpxn52U0OuZ8ZlbgB5zXD4dwoVw=</latexit>

6 (p1 )

Figure 2.2 Factor graph resulting from the example in Figure 2.1.

To motivate this, consider performing inference for the toy SLAM example. The
posterior p(x|z) can be re-written using Bayes’ law, p(x|z) ∝ p(z|x)p(x), as

p(x|z) ∝ p(p1 ) p(p2 |p1 ) p(p3 |p2 ) (2.4a)

× p(ℓ1 ) p(ℓ2 ) (2.4b)
× p(z1 |p1 ) (2.4c)
× p(z2 |p1 , ℓ1 ) p(z3 |p2 , ℓ1 ) p(z4 |p3 , ℓ2 ). (2.4d)

where we assumed a typical Markov chain generative model for the pose trajectory.
Each of the factors represents one piece of information about the unknowns, x.
To visualize this factorization, we use a factor graph. Figure 2.2 introduces the
corresponding factor graph by example: all unknown states x, both poses and land-
marks, have a node associated with them. Measurements are not represented ex-
plicitly as they are known, and hence not of interest. In factor graphs we explicitly
introduce an additional node type to represent every factor in the posterior p(x|z).
In the figure, each small black node represents a factor, and—importantly—is con-
nected to only those state variables of which it is a function. For example, the factor
ϕ9 (p3 , ℓ2 ) is connected only to the variable nodes p3 and ℓ2 . In more detail, we have

ϕ(p1 , p2 , p3 , ℓ1 , ℓ2 ) = ϕ1 (p1 ) ϕ2 (p2 , p1 ) ϕ3 (p3 , p2 ) (2.5a)

× ϕ4 (ℓ1 ) ϕ5 (ℓ2 ) (2.5b)
× ϕ6 (p1 ) (2.5c)
× ϕ7 (p1 , ℓ1 ) ϕ8 (p2 , ℓ1 ) ϕ9 (p3 , ℓ2 ), (2.5d)
 2.1 Visualizing SLAM With Factor Graphs 25

where the correspondence between the factors and the original probability densities
in (2.4a)-(2.4d) should be obvious.
The factor values need only be proportional to the corresponding probability
densities: any normalization constants that do not depend on the state variables
may be omitted without consequence. Also, in this example, all factors above came
either from a prior, e.g., ϕ1 (p1 ) ∝ p(p1 ) or from a measurement, e.g., ϕ9 (p3 , ℓ2 ) ∝
p(z4 |p3 , ℓ2 ). Although the measurement variables z1 ..z4 are not explicitly shown
in the factor graph, those factors are implicitly conditioned on them. Sometimes,
when it helps to make this more explicit, factors can be written as (for example)
ϕ9 (p3 , ℓ2 ; z4 ) or even ϕz4 (p3 , ℓ2 ).

2.1.3 Factor Graphs as a Language

In addition to providing a formal basis for inference, factor graphs help visualize
SLAM problems of many different flavors, give insight into the structure of the
problem, and serve as a lingua franca that can help practitioners align across team
boundaries. Each factor in a factor graph, such as those in Fig 2.2, can be thought
of as an equation involving the variables it is connected to. There are typically
many more equations than unknowns, which is why we need to quantify the uncer-
tainty in both prior information and measurements. This will lead to a least-squares
formulation, appropriately fusing the information from multiple sources.
Many different flavors of the SLAM problem are all easily represented as factor
graphs. Figure 2.1 is an example of landmark-based SLAM because it involves both
pose and landmark variables. Figure 2.3 illustrates several other variants includ-
ing bundle adjustment (BA) (same as landmark-based SLAM but without motion
model), pose-graph optimization (PGO) (no landmark variables but includes loop
closures), and simultaneous trajectory estimation and mapping (STEAM) (poses
are augmented to include derivatives such as velocity).
The factor graph for a more realistic landmark-based SLAM problem than the
toy example could look something like Figure 2.4. This graph was created by sim-
ulating a 2D robot, moving in the plane for about 100 time steps, as it observes
landmarks. For visualization purposes, each robot pose and landmark is rendered at
its ground-truth position in 2D. With this, we see that the odometry factors form
a prominent, chain-like backbone, whereas off to the sides binary likelihood factors
are connected to the 20 or so landmarks. All factors in such SLAM problems are
typically nonlinear, except for priors.
Examining the factor graph reveals a great deal of structure by which we can gain
insight into a particular instance of the SLAM problem. First, there are landmarks
with a great deal of measurements, which we expect to be pinned down very well.
Others have only a tenuous connection to the graph, and hence we expect them to
be less well determined. For example, the lone landmark near the bottom right has
only a single measurement associated with it: if this is a bearing-only measurement,
26 Factor Graphs for SLAM
<latexit sha1_base64="7iyXCYXvtnf7LlcEQO+3L706C+s=">AAACFHicbVDLSsNAFL3xWeur6tJNsAhdlaT43BXcuKxgq9CUMpnctkMnM2FmIpTQ33DrVv/Bnbh17y/4FU5SEbUeGOZw7j3cwwkTzrTxvHdnYXFpeWW1tFZe39jc2q7s7Ha0TBXFNpVcqtuQaORMYNsww/E2UUjikONNOL7I5zd3qDST4tpMEuzFZCjYgFFirNSv7AWh5JGexPbLAuR82m/0K1Wv7hVw54n/RarNEhRo9SsfQSRpGqMwlBOtu76XmF5GlGGU47QcpBoTQsdkiF1LBYlR97Ii/NQ9tErkDqSyTxi3UH86MhLrPJ/djIkZ6b+zXPxv1k3N4KyXMZGkBgWdHRqk3DXSzZtwI6aQGj6xhFDFbFaXjogi1Ni+ykFhzIQ0s6ZGSCJU03JRznmO4+8q5kmnUfdP6v7VUbVZm7UEJdiHA6iBD6fQhEtoQRsoTOABHuHJuXeenRfndba64Hx59uAXnLdPgS+f1Q==</latexit> <latexit sha1_base64="7iyXCYXvtnf7LlcEQO+3L706C+s=">AAACFHicbVDLSsNAFL3xWeur6tJNsAhdlaT43BXcuKxgq9CUMpnctkMnM2FmIpTQ33DrVv/Bnbh17y/4FU5SEbUeGOZw7j3cwwkTzrTxvHdnYXFpeWW1tFZe39jc2q7s7Ha0TBXFNpVcqtuQaORMYNsww/E2UUjikONNOL7I5zd3qDST4tpMEuzFZCjYgFFirNSv7AWh5JGexPbLAuR82m/0K1Wv7hVw54n/RarNEhRo9SsfQSRpGqMwlBOtu76XmF5GlGGU47QcpBoTQsdkiF1LBYlR97Ii/NQ9tErkDqSyTxi3UH86MhLrPJ/djIkZ6b+zXPxv1k3N4KyXMZGkBgWdHRqk3DXSzZtwI6aQGj6xhFDFbFaXjogi1Ni+ykFhzIQ0s6ZGSCJU03JRznmO4+8q5kmnUfdP6v7VUbVZm7UEJdiHA6iBD6fQhEtoQRsoTOABHuHJuXeenRfndba64Hx59uAXnLdPgS+f1Q==</latexit>

<latexit sha1_base64="b5MWbhCtoIcoBuQZk/2t5aCElIc=">AAACFHicbVDLSsNAFL2pr1pfVZdugkXoqiTic1dw47KCbYWmlMnkth06mQkzE6GE/oZbt/oP7sSte3/Br3CSivg6MMzh3Hu4hxMmnGnjeW9OaWFxaXmlvFpZW9/Y3Kpu73S0TBXFNpVcqpuQaORMYNsww/EmUUjikGM3nFzk8+4tKs2kuDbTBPsxGQk2ZJQYKw2qu0EoeaSnsf2yADmfDfxBteY1vALuX+J/klqzDAVag+p7EEmaxigM5UTrnu8lpp8RZRjlOKsEqcaE0AkZYc9SQWLU/awIP3MPrBK5Q6nsE8Yt1O+OjMQ6z2c3Y2LG+vcsF/+b9VIzPOtnTCSpQUHnh4Ypd4108ybciCmkhk8tIVQxm9WlY6IINbavSlAYMyHNvKkxkgjVrFKUc57j+KuKv6Rz2PBPGv7VUa1Zn7cEZdiDfaiDD6fQhEtoQRsoTOEeHuDRuXOenGfnZb5acj49u/ADzusHf5Of1A==</latexit>
`2 <latexit sha1_base64="b5MWbhCtoIcoBuQZk/2t5aCElIc=">AAACFHicbVDLSsNAFL2pr1pfVZdugkXoqiTic1dw47KCbYWmlMnkth06mQkzE6GE/oZbt/oP7sSte3/Br3CSivg6MMzh3Hu4hxMmnGnjeW9OaWFxaXmlvFpZW9/Y3Kpu73S0TBXFNpVcqpuQaORMYNsww/EmUUjikGM3nFzk8+4tKs2kuDbTBPsxGQk2ZJQYKw2qu0EoeaSnsf2yADmfDfxBteY1vALuX+J/klqzDAVag+p7EEmaxigM5UTrnu8lpp8RZRjlOKsEqcaE0AkZYc9SQWLU/awIP3MPrBK5Q6nsE8Yt1O+OjMQ6z2c3Y2LG+vcsF/+b9VIzPOtnTCSpQUHnh4Ypd4108ybciCmkhk8tIVQxm9WlY6IINbavSlAYMyHNvKkxkgjVrFKUc57j+KuKv6Rz2PBPGv7VUa1Zn7cEZdiDfaiDD6fQhEtoQRsoTOEeHuDRuXOenGfnZb5acj49u/ADzusHf5Of1A==</latexit>
`2
`1 `1

p3
<latexit sha1_base64="A2NNUUAtUlazpp7Hib/5Qu5OzT8=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSuLbXcGNSwVbC20pk8ltO3QyE2YmQgn9Crdu9R/ciVu/wF/wK5ykRdR6YJjDufdwDydMONPG9z+cufmFxaXl0kp5dW19Y9Pd2m5omSqKdSq5VM2QaORMYN0ww7GZKCRxyPEuHF7m87t7VJpJcWtGCXZi0hesxygxVuq6bjuUPNKj2H5ZMu4edd2KX/ULeLMkmJJKrQQFrrvuZzuSNI1RGMqJ1q3AT0wnI8owynFcbqcaE0KHpI8tSwWJUXeyIvnY27dK5PWksk8Yr1B/OjIS6zyc3YyJGei/s1z8b9ZKTe+8kzGRpAYFnRzqpdwz0str8CKmkBo+soRQxWxWjw6IItTYssrtwpgJaSY1DZBEqMblopyLHCffVcySxmE1OK0GN8eV2sGkJSjBLuzBAQRwBjW4gmuoA4V7eIQneHYenBfn1XmbrM45U88O/ILz/gUgEZ6P</latexit>

p3
<latexit sha1_base64="A2NNUUAtUlazpp7Hib/5Qu5OzT8=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSuLbXcGNSwVbC20pk8ltO3QyE2YmQgn9Crdu9R/ciVu/wF/wK5ykRdR6YJjDufdwDydMONPG9z+cufmFxaXl0kp5dW19Y9Pd2m5omSqKdSq5VM2QaORMYN0ww7GZKCRxyPEuHF7m87t7VJpJcWtGCXZi0hesxygxVuq6bjuUPNKj2H5ZMu4edd2KX/ULeLMkmJJKrQQFrrvuZzuSNI1RGMqJ1q3AT0wnI8owynFcbqcaE0KHpI8tSwWJUXeyIvnY27dK5PWksk8Yr1B/OjIS6zyc3YyJGei/s1z8b9ZKTe+8kzGRpAYFnRzqpdwz0str8CKmkBo+soRQxWxWjw6IItTYssrtwpgJaSY1DZBEqMblopyLHCffVcySxmE1OK0GN8eV2sGkJSjBLuzBAQRwBjW4gmuoA4V7eIQneHYenBfn1XmbrM45U88O/ILz/gUgEZ6P</latexit>

<latexit sha1_base64="ce4uuLkjfKZLQAC8vzdJb3ctrho=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSiI+dwU3LivYB7SlTCa3dnAyE2YmhRL6FW7d6j+4E7d+gb/gVzhJi6j1wDCHc+/hHk6YcKaN7384C4tLyyurpbXy+sbm1ra7s9vUMlUUG1Ryqdoh0ciZwIZhhmM7UUjikGMrvL/K560RKs2kuDXjBHsxuRNswCgxVuq7bjeUPNLj2H5ZMumf9N2KX/ULePMkmJFKrQQF6n33sxtJmsYoDOVE607gJ6aXEWUY5Tgpd1ONCaH35A47lgoSo+5lRfKJd2iVyBtIZZ8wXqH+dGQk1nk4uxkTM9R/Z7n436yTmsFFL2MiSQ0KOj00SLlnpJfX4EVMITV8bAmhitmsHh0SRaixZZW7hTET0kxrGiKJUE3KRTmXOU6/q5gnzeNqcFYNbk4qtaNpS1CCfTiAIwjgHGpwDXVoAIURPMITPDsPzovz6rxNVxecmWcPfsF5/wIhrZ6Q</latexit> <latexit sha1_base64="ce4uuLkjfKZLQAC8vzdJb3ctrho=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSiI+dwU3LivYB7SlTCa3dnAyE2YmhRL6FW7d6j+4E7d+gb/gVzhJi6j1wDCHc+/hHk6YcKaN7384C4tLyyurpbXy+sbm1ra7s9vUMlUUG1Ryqdoh0ciZwIZhhmM7UUjikGMrvL/K560RKs2kuDXjBHsxuRNswCgxVuq7bjeUPNLj2H5ZMumf9N2KX/ULePMkmJFKrQQF6n33sxtJmsYoDOVE607gJ6aXEWUY5Tgpd1ONCaH35A47lgoSo+5lRfKJd2iVyBtIZZ8wXqH+dGQk1nk4uxkTM9R/Z7n436yTmsFFL2MiSQ0KOj00SLlnpJfX4EVMITV8bAmhitmsHh0SRaixZZW7hTET0kxrGiKJUE3KRTmXOU6/q5gnzeNqcFYNbk4qtaNpS1CCfTiAIwjgHGpwDXVoAIURPMITPDsPzovz6rxNVxecmWcPfsF5/wIhrZ6Q</latexit>

p4 p4
<latexit sha1_base64="PyPWDAiJqaev1QY37Ip8vMe49yM=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSiI+dwU3LivYB7SlTCa3dnAyE2YmhRL6FW7d6j+4E7d+gb/gVzhJi6j1wDCHc+/hHk6YcKaN7384C4tLyyurpbXy+sbm1ra7s9vUMlUUG1Ryqdoh0ciZwIZhhmM7UUjikGMrvL/K560RKs2kuDXjBHsxuRNswCgxVuq7bjeUPNLj2H5ZMukf992KX/ULePMkmJFKrQQF6n33sxtJmsYoDOVE607gJ6aXEWUY5Tgpd1ONCaH35A47lgoSo+5lRfKJd2iVyBtIZZ8wXqH+dGQk1nk4uxkTM9R/Z7n436yTmsFFL2MiSQ0KOj00SLlnpJfX4EVMITV8bAmhitmsHh0SRaixZZW7hTET0kxrGiKJUE3KRTmXOU6/q5gnzeNqcFYNbk4qtaNpS1CCfTiAIwjgHGpwDXVoAIURPMITPDsPzovz6rxNVxecmWcPfsF5/wIedZ6O</latexit>

p2
<latexit sha1_base64="PyPWDAiJqaev1QY37Ip8vMe49yM=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSiI+dwU3LivYB7SlTCa3dnAyE2YmhRL6FW7d6j+4E7d+gb/gVzhJi6j1wDCHc+/hHk6YcKaN7384C4tLyyurpbXy+sbm1ra7s9vUMlUUG1Ryqdoh0ciZwIZhhmM7UUjikGMrvL/K560RKs2kuDXjBHsxuRNswCgxVuq7bjeUPNLj2H5ZMukf992KX/ULePMkmJFKrQQF6n33sxtJmsYoDOVE607gJ6aXEWUY5Tgpd1ONCaH35A47lgoSo+5lRfKJd2iVyBtIZZ8wXqH+dGQk1nk4uxkTM9R/Z7n436yTmsFFL2MiSQ0KOj00SLlnpJfX4EVMITV8bAmhitmsHh0SRaixZZW7hTET0kxrGiKJUE3KRTmXOU6/q5gnzeNqcFYNbk4qtaNpS1CCfTiAIwjgHGpwDXVoAIURPMITPDsPzovz6rxNVxecmWcPfsF5/wIedZ6O</latexit>

p2
<latexit sha1_base64="tAUSgPttDVlGAhAf5tUO9UjLf+4=">AAACEXicbVDLSsNAFL2pr1pfUZdugkXoqiTic1dw41LBtkJbymRy2w6dzISZSaGEfoVbt/oP7sStX+Av+BVOUhFfB4Y5nHsP93DChDNtfP/NKS0sLi2vlFcra+sbm1vu9k5Ly1RRbFLJpboNiUbOBDYNMxxvE4UkDjm2w/FFPm9PUGkmxY2ZJtiLyVCwAaPEWKnvut1Q8khPY/tlyawf9N2qX/cLeH9J8EmqjTIUuOq7791I0jRGYSgnWncCPzG9jCjDKMdZpZtqTAgdkyF2LBUkRt3LiuQz78AqkTeQyj5hvEL97shIrPNwdjMmZqR/z3Lxv1knNYOzXsZEkhoUdH5okHLPSC+vwYuYQmr41BJCFbNZPToiilBjy6p0C2MmpJnXNEISoZpVinLOcxx/VfGXtA7rwUk9uD6qNmrzlqAMe7APNQjgFBpwCVfQBAoTuIcHeHTunCfn2XmZr5acT88u/IDz+gEc2Z6N</latexit>

p1
<latexit sha1_base64="tAUSgPttDVlGAhAf5tUO9UjLf+4=">AAACEXicbVDLSsNAFL2pr1pfUZdugkXoqiTic1dw41LBtkJbymRy2w6dzISZSaGEfoVbt/oP7sStX+Av+BVOUhFfB4Y5nHsP93DChDNtfP/NKS0sLi2vlFcra+sbm1vu9k5Ly1RRbFLJpboNiUbOBDYNMxxvE4UkDjm2w/FFPm9PUGkmxY2ZJtiLyVCwAaPEWKnvut1Q8khPY/tlyawf9N2qX/cLeH9J8EmqjTIUuOq7791I0jRGYSgnWncCPzG9jCjDKMdZpZtqTAgdkyF2LBUkRt3LiuQz78AqkTeQyj5hvEL97shIrPNwdjMmZqR/z3Lxv1knNYOzXsZEkhoUdH5okHLPSC+vwYuYQmr41BJCFbNZPToiilBjy6p0C2MmpJnXNEISoZpVinLOcxx/VfGXtA7rwUk9uD6qNmrzlqAMe7APNQjgFBpwCVfQBAoTuIcHeHTunCfn2XmZr5acT88u/IDz+gEc2Z6N</latexit>

p1
<latexit sha1_base64="4N7Wgeg/xukOKoR4S2YJYTSNh3M=">AAACEXicbZDLSiNBFIZPexljxhlbXbopzAiuQvfgdRdw4zKCUSEJobpyYgrr0lRVi6HJU7h1q+/gTtz6BL6CT2F1J4i3Hwp+/v8c6vAlqeDWRdFLMDM7N/9robJY/b305+9yuLJ6anVmGLaYFtqcJ9Si4ApbjjuB56lBKhOBZ8nlYdGfXaGxXKsTN0qxK+mF4gPOqPNRLww7MtHXudJE6iIZ98JaVI9Kke8mnppaowKlmr3wtdPXLJOoHBPU2nYcpa6bU+M4EziudjKLKWWX9ALb3ioq0Xbz8vIx2fRJnwy08U85UqYfN3IqrR3JxE9K6ob2a1eEP3XtzA32uzlXaeZQsclHg0wQp0mBgfS5QebEyBvKDPe3EjakhjLnYVU75aKn4iaYhkj7aMbVEs5BoZ13FN/N6f96vFuPj7drjX8TSlCBddiALYhhDxpwBE1oAYMruIU7uA9ugofgMXiajM4E0501+KTg+Q03gp6b</latexit>

no motion
<latexit sha1_base64="5pg8PDsTOkhDaw+spFKGXXVogCM=">AAACEHicbZDLSgNBEEVrfMb4SNSlm8EouAoz4nMXcONSwaiQhNDTUzGN/Ri6a8Qw5CfcutV/cCdu/QN/wa9wMhHxdaHhcm8VXZwokcJRELx5E5NT0zOzpbny/MLiUqW6vHLuTGo5NrmRxl5GzKEUGpskSOJlYpGpSOJFdH006i9u0Dph9BkNEuwodqVFT3BGedStVtoqMreZiY1CsoNht1oL6kEh/68JP02tUYJCJ93qezs2PFWoiUvmXCsMEupkzJLgEoflduowYfyaXWErt5opdJ2sOHzob+ZJ7PeMzZ8mv0i/b2RMOTdQUT6pGPXd724U/te1UuoddDKhk5RQ8/FHvVT6ZPwRBT8WFjnJQW4YtyK/1ed9ZhmnnFW5XSxm2tCYUh9ZjHZYLuAcjrT7heKvOd+uh3v18HSn1tgYU4ISrME6bEEI+9CAYziBJnBI4R4e4NG78568Z+9lPDrhfe6swg95rx/aJZ5x</latexit>

odometry <latexit sha1_base64="XI+bVvG2FxLrcO8kqclPBNw10v8=">AAACFHicbVDLSsNAFL3xWeur6tJNsAiuSuLbXcGNSwVbC00pk8ltO3QyE2YmQgn9Dbdu9R/ciVv3/oJf4SQtotYDwxzOvYd7OGHCmTae9+HMzS8sLi2XVsqra+sbm5Wt7aaWqaLYoJJL1QqJRs4ENgwzHFuJQhKHHO/C4WU+v7tHpZkUt2aUYCcmfcF6jBJjpW5lJwglj/Qotl8WIOfj7lG3UvVqXgF3lvhTUq2XoMB1t/IZRJKmMQpDOdG67XuJ6WREGUY5jstBqjEhdEj62LZUkBh1JyvCj919q0RuTyr7hHEL9acjI7HO89nNmJiB/jvLxf9m7dT0zjsZE0lqUNDJoV7KXSPdvAk3Ygqp4SNLCFXMZnXpgChCje2rHBTGTEgzaWqAJEI1LhflXOQ4+a5iljQPa/5pzb85rtYPJi1BCXZhDw7AhzOowxVcQwMojOARnuDZeXBenFfnbbI650w9O/ALzvsXgsuf1g==</latexit> <latexit sha1_base64="Z0JecP88ZehweYIK5KKnmatPFyc=">AAACC3icbZDLSsNAFIZP6q3GW9Wlm2AVXJVEvO4KblxWsBdoQplMT9uhk5kwMxFL6CO4davv4E7c+hC+gk9hmhZR6w8DP/9/DnP4wpgzbVz3wyosLC4trxRX7bX1jc2t0vZOQ8tEUaxTyaVqhUQjZwLrhhmOrVghiUKOzXB4Nembd6g0k+LWjGIMItIXrMcoMVnk+1Eo79NYManGnVLZrbi5nHnjzUy5WoRctU7p0+9KmkQoDOVE67bnxiZIiTKMchzbfqIxJnRI+tjOrCAR6iDNbx47h1nSdXpSZU8YJ09/bqQk0noUhdlkRMxA/+0m4X9dOzG9iyBlIk4MCjr9qJdwx0hnAsDpMoXU8FFmCFUsu9WhA6IINRkm288XUyFNDqgzQNJFNbZzOJcTnX6jmDeN44p3VvFuTsrVgyklKMIe7MMReHAOVbiGGtSBQgyP8ATP1oP1Yr1ab9PRgjXb2YVfst6/ANF7nNs=</latexit> <latexit sha1_base64="XI+bVvG2FxLrcO8kqclPBNw10v8=">AAACFHicbVDLSsNAFL3xWeur6tJNsAiuSuLbXcGNSwVbC00pk8ltO3QyE2YmQgn9Dbdu9R/ciVv3/oJf4SQtotYDwxzOvYd7OGHCmTae9+HMzS8sLi2XVsqra+sbm5Wt7aaWqaLYoJJL1QqJRs4ENgwzHFuJQhKHHO/C4WU+v7tHpZkUt2aUYCcmfcF6jBJjpW5lJwglj/Qotl8WIOfj7lG3UvVqXgF3lvhTUq2XoMB1t/IZRJKmMQpDOdG67XuJ6WREGUY5jstBqjEhdEj62LZUkBh1JyvCj919q0RuTyr7hHEL9acjI7HO89nNmJiB/jvLxf9m7dT0zjsZE0lqUNDJoV7KXSPdvAk3Ygqp4SNLCFXMZnXpgChCje2rHBTGTEgzaWqAJEI1LhflXOQ4+a5iljQPa/5pzb85rtYPJi1BCXZhDw7AhzOowxVcQwMojOARnuDZeXBenFfnbbI650w9O/ALzvsXgsuf1g==</latexit>

`3 prior `3
<latexit sha1_base64="D0t0Gbpa4yeBu+6gtb0njrI4NxM=">AAACJXicbVDLThtBEOwFQowTwEmOXEYxkbhg7SLyuhnlkgNIoGBAsi2rd7aNR57HamY2irXyD/AjXHNN/iG3CIkTd76C3TVCvEoaqVTVNT1TcSqF82F4GczNL7xYfFlbqr96vbyy2njz9siZzHLqcCONPYnRkRSaOl54SSepJVSxpON4/K30j3+SdcLoQz9Jqa/wVIuh4OgLadBY76nY/Mo5aqMLUTKJOlFox5vlrQn7sbuzNx00mmErrMCekuiWNNs1qLA/aFz3EsMzRdpzic51ozD1/RytF1zStN7LHKXIx3hK3YJqVOT6efWbKfuQlYuHxhZHe1ap9xM5KucmKi4mFfqRe+yV4nNeN/PDL/1c6DTzpPls0TCTzBtWVsMSYYl7OSkIciuKtzI+QovcFwXWe1Uw18bPqhsRJmSn9aqcryU+3lXxlBxttaJPrehgu9nemLUENViD97ABEXyGNnyHfegAhzP4DX/gb3Ae/Av+Bxez0bngNvMOHiC4ugEipqZf</latexit> <latexit sha1_base64="lECXM6hVGHTQFnb5AYRe6ojWSDs=">AAACHnicbVDNSisxGP3G39rr1apLEYLlQu+mzFz83VXduFSwKrSlZDJfba6ZZEgyYhm68kXcutV3cCdu9RV8CjNTEf8OBA7nfCf5csJEcGN9/9kbG5+YnJouzZR/zf6em68sLB4blWqGTaaE0qchNSi4xKblVuBpopHGocCT8Hwv908uUBuu5JEdJNiJ6ZnkPc6odVK3stKOQ3WZhamMBBIa/U+NjVFaUtvd+TvsVqp+3S9AvpPgjVQbJShw0K28tCPF0vwGJqgxrcBPbCej2nImcFhupwYTys7pGbYclTRG08mKbwzJH6dEpKe0O26DQv2YyGhszCAO3WRMbd989XLxJ6+V2t5WJ+MySS1KNnqolwpiFck7IRHXyKwYOEKZ5m5XwvpUU2Zdc+V2EcyksqPO+kgj1MNyUc52jvX3Kr6T43/1YKMeHK5VG7VRS1CCZViFGgSwCQ3YhwNoAoMruIFbuPOuvXvvwXscjY55b5kl+ATv6RVcNaNb</latexit>

canonical landmark-based SLAM <latexit sha1_base64="+g2/DAFVnlXF6YvmcHomzWFyuvY=">AAACH3icbZDNSiNBFIVvOzoTozPGcTkghUHIbEK3+De7gBuXGTAqJCFUV26bwvppqm4Phia7eZHZznbmHdyJW1/Bp7C7I+LfgYLDOfdSVV+cKukpDO+ChQ+LSx8/1ZbrK6ufv6w11r+eeps5gT1hlXXnMfeopMEeSVJ4njrkOlZ4Fl8elf3ZL3ReWnNC0xSHml8YmUjBqYhGjc2Bju1V3vLkMkGZQ5Y4q5m2Zf19Nmo0w3ZYib010aNpdmpQqTtq3A/GVmQaDQnFve9HYUrDnDuSQuGsPsg8plxc8gvsF9ZwjX6YV/+Yse0iGbPEuuIYYlX6fCPn2vupjotJzWniX3dl+F7Xzyg5HObSpBmhEfOLkkwxsqyEwsbSoSA1LQwXThZvZWLCHRdUoKsPqsXcWJpDmyAfo5vVKzg/Su09oXhrTnfa0X47+rnb7LTmlKAG32ALWhDBAXTgGLrQAwG/4S/8g//Bn+A6uAlu56MLwePOBrxQcPcAKGakZA==</latexit>

bundle adjustment (BA)
(structure from motion)

<latexit sha1_base64="7iyXCYXvtnf7LlcEQO+3L706C+s=">AAACFHicbVDLSsNAFL3xWeur6tJNsAhdlaT43BXcuKxgq9CUMpnctkMnM2FmIpTQ33DrVv/Bnbh17y/4FU5SEbUeGOZw7j3cwwkTzrTxvHdnYXFpeWW1tFZe39jc2q7s7Ha0TBXFNpVcqtuQaORMYNsww/E2UUjikONNOL7I5zd3qDST4tpMEuzFZCjYgFFirNSv7AWh5JGexPbLAuR82m/0K1Wv7hVw54n/RarNEhRo9SsfQSRpGqMwlBOtu76XmF5GlGGU47QcpBoTQsdkiF1LBYlR97Ii/NQ9tErkDqSyTxi3UH86MhLrPJ/djIkZ6b+zXPxv1k3N4KyXMZGkBgWdHRqk3DXSzZtwI6aQGj6xhFDFbFaXjogi1Ni+ykFhzIQ0s6ZGSCJU03JRznmO4+8q5kmnUfdP6v7VUbVZm7UEJdiHA6iBD6fQhEtoQRsoTOABHuHJuXeenRfndba64Hx59uAXnLdPgS+f1Q==</latexit>

<latexit sha1_base64="b5MWbhCtoIcoBuQZk/2t5aCElIc=">AAACFHicbVDLSsNAFL2pr1pfVZdugkXoqiTic1dw47KCbYWmlMnkth06mQkzE6GE/oZbt/oP7sSte3/Br3CSivg6MMzh3Hu4hxMmnGnjeW9OaWFxaXmlvFpZW9/Y3Kpu73S0TBXFNpVcqpuQaORMYNsww/EmUUjikGM3nFzk8+4tKs2kuDbTBPsxGQk2ZJQYKw2qu0EoeaSnsf2yADmfDfxBteY1vALuX+J/klqzDAVag+p7EEmaxigM5UTrnu8lpp8RZRjlOKsEqcaE0AkZYc9SQWLU/awIP3MPrBK5Q6nsE8Yt1O+OjMQ6z2c3Y2LG+vcsF/+b9VIzPOtnTCSpQUHnh4Ypd4108ybciCmkhk8tIVQxm9WlY6IINbavSlAYMyHNvKkxkgjVrFKUc57j+KuKv6Rz2PBPGv7VUa1Zn7cEZdiDfaiDD6fQhEtoQRsoTOEeHuDRuXOenGfnZb5acj49u/ADzusHf5Of1A==</latexit>
`2
`1
p3
<latexit sha1_base64="A2NNUUAtUlazpp7Hib/5Qu5OzT8=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSuLbXcGNSwVbC20pk8ltO3QyE2YmQgn9Crdu9R/ciVu/wF/wK5ykRdR6YJjDufdwDydMONPG9z+cufmFxaXl0kp5dW19Y9Pd2m5omSqKdSq5VM2QaORMYN0ww7GZKCRxyPEuHF7m87t7VJpJcWtGCXZi0hesxygxVuq6bjuUPNKj2H5ZMu4edd2KX/ULeLMkmJJKrQQFrrvuZzuSNI1RGMqJ1q3AT0wnI8owynFcbqcaE0KHpI8tSwWJUXeyIvnY27dK5PWksk8Yr1B/OjIS6zyc3YyJGei/s1z8b9ZKTe+8kzGRpAYFnRzqpdwz0str8CKmkBo+soRQxWxWjw6IItTYssrtwpgJaSY1DZBEqMblopyLHCffVcySxmE1OK0GN8eV2sGkJSjBLuzBAQRwBjW4gmuoA4V7eIQneHYenBfn1XmbrM45U88O/ILz/gUgEZ6P</latexit>

<latexit sha1_base64="ce4uuLkjfKZLQAC8vzdJb3ctrho=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSiI+dwU3LivYB7SlTCa3dnAyE2YmhRL6FW7d6j+4E7d+gb/gVzhJi6j1wDCHc+/hHk6YcKaN7384C4tLyyurpbXy+sbm1ra7s9vUMlUUG1Ryqdoh0ciZwIZhhmM7UUjikGMrvL/K560RKs2kuDXjBHsxuRNswCgxVuq7bjeUPNLj2H5ZMumf9N2KX/ULePMkmJFKrQQF6n33sxtJmsYoDOVE607gJ6aXEWUY5Tgpd1ONCaH35A47lgoSo+5lRfKJd2iVyBtIZZ8wXqH+dGQk1nk4uxkTM9R/Z7n436yTmsFFL2MiSQ0KOj00SLlnpJfX4EVMITV8bAmhitmsHh0SRaixZZW7hTET0kxrGiKJUE3KRTmXOU6/q5gnzeNqcFYNbk4qtaNpS1CCfTiAIwjgHGpwDXVoAIURPMITPDsPzovz6rxNVxecmWcPfsF5/wIhrZ6Q</latexit>

p4
<latexit sha1_base64="PyPWDAiJqaev1QY37Ip8vMe49yM=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSiI+dwU3LivYB7SlTCa3dnAyE2YmhRL6FW7d6j+4E7d+gb/gVzhJi6j1wDCHc+/hHk6YcKaN7384C4tLyyurpbXy+sbm1ra7s9vUMlUUG1Ryqdoh0ciZwIZhhmM7UUjikGMrvL/K560RKs2kuDXjBHsxuRNswCgxVuq7bjeUPNLj2H5ZMukf992KX/ULePMkmJFKrQQF6n33sxtJmsYoDOVE607gJ6aXEWUY5Tgpd1ONCaH35A47lgoSo+5lRfKJd2iVyBtIZZ8wXqH+dGQk1nk4uxkTM9R/Z7n436yTmsFFL2MiSQ0KOj00SLlnpJfX4EVMITV8bAmhitmsHh0SRaixZZW7hTET0kxrGiKJUE3KRTmXOU6/q5gnzeNqcFYNbk4qtaNpS1CCfTiAIwjgHGpwDXVoAIURPMITPDsPzovz6rxNVxecmWcPfsF5/wIedZ6O</latexit>

p2
<latexit sha1_base64="UjHjxjjNnihlDN+YL7je8bmFNVQ=">AAACCnicbVDLSsNAFL2prxpfVZduglVwVRLxuSu4cVnBPqAtZTK9bYdOZsLMRCwhf+DWrf6DO3HrT/gLfoVJWkStBwYO59zDvXP8kDNtXPfDKiwsLi2vFFfttfWNza3S9k5Dy0hRrFPJpWr5RCNnAuuGGY6tUCEJfI5Nf3yV+c07VJpJcWsmIXYDMhRswCgxqdTuBL68j7mUYdIrld2Km8OZJ96MlKtFyFHrlT47fUmjAIWhnGjd9tzQdGOiDKMcE7sTaQwJHZMhtlMqSIC6G+cnJ85hqvSdgVTpE8bJ1Z+JmARaTwI/nQyIGem/Xib+57UjM7joxkyEkUFBp4sGEXeMdLL/O32mkBo+SQmhiqW3OnREFKEmbcnu5MFYSJP30xsh6aNK7Lycywyn31XMk8ZxxTureDcn5erBtCUowh7swxF4cA5VuIYa1IGChEd4gmfrwXqxXq236WjBmmV24Res9y/wH5xf</latexit>

<latexit sha1_base64="tAUSgPttDVlGAhAf5tUO9UjLf+4=">AAACEXicbVDLSsNAFL2pr1pfUZdugkXoqiTic1dw41LBtkJbymRy2w6dzISZSaGEfoVbt/oP7sStX+Av+BVOUhFfB4Y5nHsP93DChDNtfP/NKS0sLi2vlFcra+sbm1vu9k5Ly1RRbFLJpboNiUbOBDYNMxxvE4UkDjm2w/FFPm9PUGkmxY2ZJtiLyVCwAaPEWKnvut1Q8khPY/tlyawf9N2qX/cLeH9J8EmqjTIUuOq7791I0jRGYSgnWncCPzG9jCjDKMdZpZtqTAgdkyF2LBUkRt3LiuQz78AqkTeQyj5hvEL97shIrPNwdjMmZqR/z3Lxv1knNYOzXsZEkhoUdH5okHLPSC+vwYuYQmr41BJCFbNZPToiilBjy6p0C2MmpJnXNEISoZpVinLOcxx/VfGXtA7rwUk9uD6qNmrzlqAMe7APNQjgFBpwCVfQBAoTuIcHeHTunCfn2XmZr5acT88u/IDz+gEc2Z6N</latexit>

x3 p1
<latexit sha1_base64="tPqvzQuur0DYkQKtz7xTE46gnjY=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSuLbXcGNSwVbC20pk8ltO3QyE2YmxRL6FW7d6j+4E7d+gb/gVzhJRXwdGOZw7j3cwwkTzrTx/TdnZnZufmGxtFReXlldW3c3NhtapopinUouVTMkGjkTWDfMcGwmCkkccrwJh+f5/GaESjMprs04wU5M+oL1GCXGSl3XbYeSR3oc2y+7nXQPum7Fr/oFvL8k+CSVWgkKXHbd93YkaRqjMJQTrVuBn5hORpRhlOOk3E41JoQOSR9blgoSo+5kRfKJt2uVyOtJZZ8wXqF+d2Qk1nk4uxkTM9C/Z7n436yVmt5pJ2MiSQ0KOj3US7lnpJfX4EVMITV8bAmhitmsHh0QRaixZZXbhTET0kxrGiCJUE3KRTlnOY6+qvhLGvvV4LgaXB1WanvTlqAE27ADexDACdTgAi6hDhRGcA8P8OjcOU/Os/MyXZ1xPj1b8APO6wctCZ6X</latexit>

<latexit sha1_base64="acd9JfZNbDkl7lvH5hNr4W4k70I=">AAACEXicbVDLSsNAFL2pr1pfUZdugkVwVRKpr13BjUsFW4W2lMnkth06mQkzE7GEfoVbt/oP7sStX+Av+BVO0iK+DgxzOPce7uGECWfa+P67U5qbX1hcKi9XVlbX1jfcza2Wlqmi2KSSS3UTEo2cCWwaZjjeJApJHHK8Dkdn+fz6FpVmUlyZcYLdmAwE6zNKjJV6rtsJJY/0OLZfdjfp1Xtu1a/5Bby/JJiRaqMMBS567kcnkjSNURjKidbtwE9MNyPKMMpxUumkGhNCR2SAbUsFiVF3syL5xNuzSuT1pbJPGK9QvzsyEus8nN2MiRnq37Nc/G/WTk3/pJsxkaQGBZ0e6qfcM9LLa/AippAaPraEUMVsVo8OiSLU2LIqncKYCWmmNQ2RRKgmlaKc0xyHX1X8Ja2DWnBUCy7r1cb+tCUoww7swj4EcAwNOIcLaAKFW3iAR3hy7p1n58V5na6WnJlnG37AefsELqWemA==</latexit>

loop
x4
<latexit sha1_base64="RAy4UoAOA9HkVwW+H5wjVP/K+JE=">AAACDXicbZDLSsNAFIZPvNZ4q7p0E6yCq5KI113BjcsK9gJtLJPpaTt0MhNmJmIJeQa3bvUd3Ilbn8FX8ClM0yJq/WHg5//PYQ5fEHGmjet+WHPzC4tLy4UVe3VtfWOzuLVd1zJWFGtUcqmaAdHImcCaYYZjM1JIwoBjIxhejvvGHSrNpLgxowj9kPQF6zFKTBbdtsNA3ieUSx0rTDvFklt2czmzxpuaUqUAuaqd4me7K2kcojCUE61bnhsZPyHKMMoxtduxxojQIeljK7OChKj9JL86dQ6ypOv0pMqeME6e/txISKj1KAyyyZCYgf7bjcP/ulZseud+wkQUGxR08lEv5o6RzhiB02UKqeGjzBCqWHarQwdEEWoyUHY7X0yENDmizgBJF1Vq53Auxjr5RjFr6kdl77TsXR+XKvsTSlCAXdiDQ/DgDCpwBVWoAQUFj/AEz9aD9WK9Wm+T0TlrurMDv2S9fwF2jp3A</latexit>

closure
p5
<latexit sha1_base64="X5TBhKNf0Y3tsPfq47nHaCz9y0M=">AAACEXicbVDLSsNAFL3xWesr6tJNsAhdlUSsj13BjUsF+4C2lMnk1g5OZsLMRCihX+HWrf6DO3HrF/gLfoWTtIhaDwxzOPce7uGECWfa+P6Hs7C4tLyyWlorr29sbm27O7stLVNFsUkll6oTEo2cCWwaZjh2EoUkDjm2w7uLfN6+R6WZFDdmnGA/JreCDRklxkoD1+2Fkkd6HNsvSyaD+sCt+DW/gDdPghmpNEpQ4GrgfvYiSdMYhaGcaN0N/MT0M6IMoxwn5V6qMSH0jtxi11JBYtT9rEg+8Q6tEnlDqewTxivUn46MxDoPZzdjYkb67ywX/5t1UzM862dMJKlBQaeHhin3jPTyGryIKaSGjy0hVDGb1aMjogg1tqxyrzBmQpppTSMkEapJuSjnPEf9u4p50jqqBSe14Pq40qhOW4IS7MMBVCGAU2jAJVxBEyjcwyM8wbPz4Lw4r87bdHXBmXn24Bec9y8jSZ6R</latexit>

<latexit sha1_base64="Yr4EdV941G5d4F9Y7HI7OEu2xuQ=">AAACEXicbVDLSsNAFL2pr1pfUZdugkVwVZLic1dw41LBVqEtZTK5bYdOZsLMRCyhX+HWrf6DO3HrF/gLfoWTtIivA8Mczr2Hezhhwpk2vv/ulObmFxaXysuVldW19Q13c6ulZaooNqnkUt2ERCNnApuGGY43iUIShxyvw9FZPr++RaWZFFdmnGA3JgPB+owSY6We63ZCySM9ju2X3U169Z5b9Wt+Ae8vCWak2ihDgYue+9GJJE1jFIZyonU78BPTzYgyjHKcVDqpxoTQERlg21JBYtTdrEg+8fasEnl9qewTxivU746MxDoPZzdjYob69ywX/5u1U9M/6WZMJKlBQaeH+in3jPTyGryIKaSGjy0hVDGb1aNDogg1tqxKpzBmQpppTUMkEapJpSjnNMfhVxV/SateC45qweVBtbE/bQnKsAO7sA8BHEMDzuECmkDhFh7gEZ6ce+fZeXFep6slZ+bZhh9w3j4BK22elg==</latexit>

x2
<latexit sha1_base64="VSjXtvk3tP4GqyhJeZJraKYvkis=">AAACEXicbVDLSsNAFL3xWesr6tJNsAhdlUR87gpuXCrYB7SlTCa3dnAyE2YmYgn9Crdu9R/ciVu/wF/wK5ykRdR6YJjDufdwDydMONPG9z+cufmFxaXl0kp5dW19Y9Pd2m5qmSqKDSq5VO2QaORMYMMww7GdKCRxyLEV3p7n89YdKs2kuDajBHsxuRFswCgxVuq7bjeUPNKj2H7Z/bgf9N2KX/MLeLMkmJJKvQQFLvvuZzeSNI1RGMqJ1p3AT0wvI8owynFc7qYaE0JvyQ12LBUkRt3LiuRjb98qkTeQyj5hvEL96chIrPNwdjMmZqj/znLxv1knNYPTXsZEkhoUdHJokHLPSC+vwYuYQmr4yBJCFbNZPTokilBjyyp3C2MmpJnUNEQSoRqXi3LOchx9VzFLmge14LgWXB1W6tVJS1CCXdiDKgRwAnW4gEtoAIU7eIQneHYenBfn1XmbrM45U88O/ILz/gUp0Z6V</latexit>

x1
p7
<latexit sha1_base64="AHK0nrRU1LpOxVTa1IhH+ei1FZY=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSiJqdVdw47KCrUJbymRy2w6dzISZSaGEfoVbt/oP7sStX+Av+BVO0iK+DgxzOPce7uGECWfa+P67s7C4tLyyWlorr29sbm27O7stLVNFsUkll+ouJBo5E9g0zHC8SxSSOOR4G44u8/ntGJVmUtyYSYLdmAwE6zNKjJV6rtsJJY/0JLZflkx7tZ5b8at+Ae8vCeakUi9BgUbP/ehEkqYxCkM50bod+InpZkQZRjlOy51UY0LoiAywbakgMepuViSfeodWiby+VPYJ4xXqd0dGYp2Hs5sxMUP9e5aL/83aqemfdzMmktSgoLND/ZR7Rnp5DV7EFFLDJ5YQqpjN6tEhUYQaW1a5UxgzIc2spiGSCNW0XJRzkeP0q4q/pHVcDc6qwfVJpX40awlKsA8HcAQB1KAOV9CAJlAYwwM8wpNz7zw7L87rbHXBmXv24Aect08mgZ6T</latexit>

p6
<latexit sha1_base64="RtLkGHayYJw83DAC6HZdu/eFnMo=">AAACEXicbVDLSsNAFL3xWesr6tJNsAhdlUS06q7gxqWCfUBbymRyawcnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DWVhcWl5ZLa2V1zc2t7bdnd2Wlqmi2KSSS9UJiUbOBDYNMxw7iUIShxzb4d1FPm/fo9JMihszTrAfk1vBhowSY6WB6/ZCySM9ju2XJZNBfeBW/JpfwJsnwYxUGiUocDVwP3uRpGmMwlBOtO4GfmL6GVGGUY6Tci/VmBB6R26xa6kgMep+ViSfeIdWibyhVPYJ4xXqT0dGYp2Hs5sxMSP9d5aL/826qRme9TMmktSgoNNDw5R7Rnp5DV7EFFLDx5YQqpjN6tERUYQaW1a5VxgzIc20phGSCNWkXJRznuPku4p50jqqBfVacH1caVSnLUEJ9uEAqhDAKTTgEq6gCRTu4RGe4Nl5cF6cV+dturrgzDx78AvO+xck5Z6S</latexit>

<latexit sha1_base64="gboL0Go5q/OALpzyy8xHMtqKhAQ=">AAACQnicbZDLThsxFIbPQIEQbilddmM1QmIVzSCui0qpuumSSoREykSRx3OSWPFlZHsQ0SjP0xfptltQX4EdYssCzwRVUPgly7++c458/CeZ4NaF4d9gafnDyupabb2+sbm1vdP4uHtpdW4YdpgW2vQSalFwhR3HncBeZpDKRGA3mX4v690rNJZrdeFmGQ4kHSs+4ow6j4aNb3GiRWpn0l/F9XzIyVcSJ4kkL3lW8jh+xa4qhokcNpphK6xE3pro2TTbNah0PmzcxalmuUTlmKDW9qMwc4OCGseZwHk9zi1mlE3pGPveKirRDorqq3Oy50lKRtr4oxyp6MuJgkpbbug7JXUT+3+thO/V+rkbnQ4KrrLcoWKLh0a5IE6TMjeScoPMiZk3lBnudyVsQg1lzqdbj6vBQmm3yHWCNEUzr1fhnJU6+hfFW3N50IqOW9HPw2Z7f5ES1OAzfIF9iOAE2vADzqEDDH7BH7iB2+B3cBfcBw+L1qXgeeYTvFLw+AQySLMV</latexit>

 <latexit sha1_base64="EnEwFa/DqrLXjHVf7dwDurBTOqQ=">AAACEXicbVDLSsNAFL3xWesr6tJNsAhdlUR87gpuXCrYB7SlTCa3dnAyE2YmhRL6FW7d6j+4E7d+gb/gVzhJi6j1wDCHc+/hHk6YcKaN7384C4tLyyurpbXy+sbm1ra7s9vUMlUUG1Ryqdoh0ciZwIZhhmM7UUjikGMrvL/M560RKs2kuDXjBHsxuRNswCgxVuq7bjeUPNLj2H5ZMumf992KX/MLePMkmJFKvQQFrvvuZzeSNI1RGMqJ1p3AT0wvI8owynFS7qYaE0LvyR12LBUkRt3LiuQT79AqkTeQyj5hvEL96chIrPNwdjMmZqj/znLxv1knNYPzXsZEkhoUdHpokHLPSC+vwYuYQmr42BJCFbNZPTokilBjyyp3C2MmpJnWNEQSoZqUi3Iucpx8VzFPmke14LQW3BxX6tVpS1CCfTiAKgRwBnW4gmtoAIURPMITPDsPzovz6rxNVxecmWcPfsF5/wIoHZ6U</latexit>

p8
p
xi = i
<latexit sha1_base64="XI+bVvG2FxLrcO8kqclPBNw10v8=">AAACFHicbVDLSsNAFL3xWeur6tJNsAiuSuLbXcGNSwVbC00pk8ltO3QyE2YmQgn9Dbdu9R/ciVv3/oJf4SQtotYDwxzOvYd7OGHCmTae9+HMzS8sLi2XVsqra+sbm5Wt7aaWqaLYoJJL1QqJRs4ENgwzHFuJQhKHHO/C4WU+v7tHpZkUt2aUYCcmfcF6jBJjpW5lJwglj/Qotl8WIOfj7lG3UvVqXgF3lvhTUq2XoMB1t/IZRJKmMQpDOdG67XuJ6WREGUY5jstBqjEhdEj62LZUkBh1JyvCj919q0RuTyr7hHEL9acjI7HO89nNmJiB/jvLxf9m7dT0zjsZE0lqUNDJoV7KXSPdvAk3Ygqp4SNLCFXMZnXpgChCje2rHBTGTEgzaWqAJEI1LhflXOQ4+a5iljQPa/5pzb85rtYPJi1BCXZhDw7AhzOowxVcQwMojOARnuDZeXBenFfnbbI650w9O/ALzvsXgsuf1g==</latexit>

<latexit sha1_base64="ACt9Z2ln8ntfZS9GhpVYaWod1cs=">AAACF3icbZDLSgNBEEVrfMb4igpu3AxGwY1hRnzuAm5cKhgVkhB6OhXT2I+hu0YMYz7ErVv9B3fi1qW/4Fc4mYj4utBwubeKLk4US+EoCN68kdGx8YnJwlRxemZ2br60sHjmTGI51riRxl5EzKEUGmskSOJFbJGpSOJ5dHU46M+v0Tph9Cn1YmwqdqlFR3BGWdQqLTdUZG5SbjQJnZjEbZJQ2G+VykElyOX/NeGnKVcLkOu4VXpvtA1PFGrikjlXD4OYmimzJLjEfrGROIwZv2KXWM+sZgpdM83v7/vrWdL2O8ZmT5Ofp983Uqac66kom1SMuu53Nwj/6+oJdfabqdBxQqj58KNOIn0y/gCG3xYWOcleZhi3IrvV511mGacMWbGRL6ba0BBWF1kbbb+YwzkYaOcLxV9ztlUJdyvhyXa5ujakBAVYgVXYgBD2oApHcAw14HAL9/AAj96d9+Q9ey/D0RHvc2cJfsh7/QCBIqF3</latexit>

vi
continuous-time
<latexit sha1_base64="Gp1DCmbwOtMsi6yoWu3Yyr8O4yE=">AAACFHicbVDLSgNBEOz1GeMrmqOXwSh4Crvi8xbw4lHBGCEJYXbSSYbMY5mZFcOS3/DqVf/Bm3j17i/4Fe5uRNRYMFBT1UU3FUaCW+f7797M7Nz8wmJhqbi8srq2XtrYvLY6NgzrTAttbkJqUXCFdcedwJvIIJWhwEY4PMv8xi0ay7W6cqMI25L2Fe9xRl0qdUrllgz1XSJ19iWR4dqMO6WKX/VzkGkSfJFKrQA5Ljqlj1ZXs1iickxQa5uBH7l2Qo3jTOC42IotRpQNaR+bKVVUom0n+fFjspsqXdLTJn3KkVz9mUiotHYkw3RSUjewf71M/M9rxq530k64imKHik0W9WJBnCZZE6TLDTInRimhzPD0VsIG1FDm0r6KrTyYKO0mTQ2QdtGMi3k5pxkOv6uYJtf71eCoGlweVGo7k5agAFuwDXsQwDHU4BwuoA4MRvAAj/Dk3XvP3ov3Ohmd8b4yZfgF7+0T2K2gCA==</latexit>
`3 <latexit sha1_base64="5pg8PDsTOkhDaw+spFKGXXVogCM=">AAACEHicbZDLSgNBEEVrfMb4SNSlm8EouAoz4nMXcONSwaiQhNDTUzGN/Ri6a8Qw5CfcutV/cCdu/QN/wa9wMhHxdaHhcm8VXZwokcJRELx5E5NT0zOzpbny/MLiUqW6vHLuTGo5NrmRxl5GzKEUGpskSOJlYpGpSOJFdH006i9u0Dph9BkNEuwodqVFT3BGedStVtoqMreZiY1CsoNht1oL6kEh/68JP02tUYJCJ93qezs2PFWoiUvmXCsMEupkzJLgEoflduowYfyaXWErt5opdJ2sOHzob+ZJ7PeMzZ8mv0i/b2RMOTdQUT6pGPXd724U/te1UuoddDKhk5RQ8/FHvVT6ZPwRBT8WFjnJQW4YtyK/1ed9ZhmnnFW5XSxm2tCYUh9ZjHZYLuAcjrT7heKvOd+uh3v18HSn1tgYU4ISrME6bEEI+9CAYziBJnBI4R4e4NG78568Z+9lPDrhfe6swg95rx/aJZ5x</latexit>

motion prior odometry

<latexit sha1_base64="lpGSL93mWhYMnX2/a8hp+K5o5eU=">AAACJXicbZDPSltBFMbP1f6JUWu0SzdDUyFdGO4VW+su4KLumkKjQhLC3MlJMjj/mDm3NF7yAn2Rbt227+BOhK7c+xS9uZFSaz8Y+Pi+c5iZX+qUDBTHv6Kl5SdPnz2vrFRX19ZfbNQ2t06CzbzAjrDK+rOUB1TSYIckKTxzHrlOFZ6m50fz/vQL+iCt+UxTh33Nx0aOpOBURIPa655O7dfc2YC7Y8/dhFlHUsuLsmeN9oePb2aDWj1uxqXYY5Pcm3qrAqXag9pdb2hFptGQUDyEbhI76ufckxQKZ9VeFtBxcc7H2C2s4RpDPy9/M2M7RTJkI+uLY4iV6d8bOdchTHVaTGpOk/BvNw//13UzGr3v59K4jNCIxUWjTDGybI6GDaVHQWpaGC68LN7KxIR7LqgAWO2Vi7mxtEA3QT5EP6uWcA7nevsHxWNzstdM3jWTT/v1VmNBCSqwDa+gAQkcQAuOoQ0dEPANLuEH/Iy+R1fRdXSzGF2K7ndewgNFt78BM1OmaA==</latexit>

<latexit sha1_base64="8tITPc6bXNvcbi3uPX8pOIRZTOs=">AAACKnicbVDLahtBEOyVnVhWHpadYy6DRcC5iN1gJ/FNkIuPMkSyQRJidtSSJprHMtNrIhZ9Q34k11yTf/BN5OpTviKjlTCJ5YKBoqqa6a40U9JTHC+jys7uk6d71f3as+cvXh7UD4+63uZOYEdYZd11yj0qabBDkhReZw65ThVepbNPK//qBp2X1nymeYYDzSdGjqXgFKRh/W1fp/Zr4aXOFXGDNveMHP+CgqybM/QkdRldDOuNuBmXYNsk2ZBGqwol2sP6n/7IilyjIaG4970kzmhQcEdSKFzU+rnHjIsZn2AvUMM1+kFRnrRgb4IyYmPrwjPESvXfiYJr7+c6Dcmw39Q/9FbiY14vp/HHQSFNlhMasf5onCtGlq36YSPpwu1qHggXToZdmZhyxwWFFmv9crAwltb9TZGP0C1qZTnnK5zdV7FNuu+ayftmcnnaaJ2sW4IqvIZjOIEEPkALLqANHRDwDX7AT/gVfY9uo2X0ex2tRJuZV/Aforu/quWp8w==</latexit>

simultaneous trajectory estimation <latexit sha1_base64="bJWI60TfCJ4Tb+2iBjPog2fC+fA=">AAACG3icbZBPSxtBGMbf9V9jtDa1N70MDUJ6Cbtiq95SSsFLQdGokITw7uybZHD+LDOzxbAE+kW8em2/Q2/i1UO/gp/CzUbE1j4w8PA878vM/OJUCufD8E8wN7+wuPSqslxdWX299qb2dv3UmcxyanMjjT2P0ZEUmtpeeEnnqSVUsaSz+OLLtD/7TtYJo0/8OKWewqEWA8HRF1G/ttFVsbnMUSdMYZoKPWSN45Ovn799mPRr9bAZlmIvTfRo6q0KlDrs1+67ieGZIu25ROc6UZj6Xo7WCy5pUu1mjlLkFzikTmE1KnK9vPzDhG0VScIGxhZHe1amzzdyVM6NVVxMKvQj9283Df/XdTI/2OvlQqeZJ81nFw0yybxhUyAsEZa4l+PCILeieCvjI7TIfYGt2i0Xc238DNiIMCE7qZZw9qf6+ITipTndbkafmtHRTr3VmFGCCmzCe2hABLvQggM4hDZw+AHX8BN+BVfB7+AmuJ2NzgWPO+/gLwV3DyPOoZo=</latexit>

pose-graph optimization (PGO) <latexit sha1_base64="cUPWz9ra70fDOsZ4/H++hArTos0=">AAACGXicbZDNSgMxFIXv+FvrX9WN4CZYhLqwzIi/u4obFwqKVoW2lEx62wYzyZBkxDLUF3HrVt/Bnbh15Sv4FE6nRdR6IHA4516SfH4ouLGu++GMjI6NT0xmprLTM7Nz87mFxUujIs2wzJRQ+tqnBgWXWLbcCrwONdLAF3jl3xz2+qtb1IYreWE7IdYC2pK8yRm1SVTPLVcDX93FhVAZ3GhpGrbJ+fHByXq3nsu7RTcVGTbewORLGUh1Ws99VhuKRQFKywQ1puK5oa3FVFvOBHaz1chgSNkNbWElsZIGaGpx+oMuWUuSBmkqnRxpSZr+3IhpYEwn8JPJgNq2+dv1wv+6SmSbe7WYyzCyKFn/omYkiFWkh4M0uEZmRScxlGmevJWwNtWU2QRatpouxlLZPq420gbqbjaFs9/T9jeKYXO5WfR2it7ZVr5U6FOCDKzAKhTAg10owRGcQhkY3MMjPMGz8+C8OK/OW390xBnsLMEvOe9f1Reg8A==</latexit>

and mapping (STEAM) (pose-graph SLAM)

Figure 2.3 A few variants of SLAM problems that can all be viewed through the factor-
graph lens. Canonical landmark-based SLAM has both pose and landmark variables; land-
marks are measured from poses and there is some motion prior between poses typically
based on odometry. BA is the same but without the motion prior. STEAM is similar but
now poses can be replaced by higher-order states and a smooth continuous-time motion
prior is used. PGO does not have landmark variables but enjoys extra loop-closure mea-
surements between poses.

many assignments of a 2D location to the landmark will be equally ‘correct’. This is

the same as saying that we have infinite uncertainty in some subset of the domain
of the unknowns, which is where prior knowledge should come to the rescue.

2.2 From MAP Inference to Least Squares

In SLAM, maximum a posteriori (MAP) inference is the process of determining the
values for the unknowns x that maximally agree with the information present in the
uncertain measurements. In real life we are not given the ground-truth locations for
the landmarks, nor the time-varying pose of the robot, although in many practical
cases we might have a good initial estimate. Below we review how to model both
prior knowledge and measurements using probability densities, how the posterior
 2.2 From MAP Inference to Least Squares 27

p1 p2 p3 p4
<latexit sha1_base64="c2peMPeVPvUSy8RewgFjN9zoU1c=">AAACEXicbVDLSgMxFM3UV62vUUEEN4NFcFVmBF+7ghuXFewD2lIymds2NJMMSaZQhn6FW7f6Ae7c+Vj6Bf6CP6GZaRG1Hgg5nHsP93D8iFGlXffdys3NLywu5ZcLK6tr6xv25lZNiVgSqBLBhGz4WAGjHKqaagaNSAIOfQZ1f3CRzutDkIoKfq1HEbRD3OO0SwnWRurYdssXLFCj0HxJNO54HbvoltwMzizxpqRY3n19qH0+71Q69kcrECQOgWvCsFJNz410O8FSU8JgXGjFCiJMBrgHTUM5DkG1kyz52DkwSuB0hTSPaydTfzoSHKo0nNkMse6rv7NU/G/WjHX3rJ1QHsUaOJkc6sbM0cJJa3ACKoFoNjIEE0lNVof0scREm7IKrcyYcKEnNfUBByDHhayc8xTH31XMktpRyTspeVdeseyiCfJoD+2jQ+ShU1RGl6iCqoigIbpFd+jeurEerSfrZbKas6aebfQL1tsXbLGiQw==</latexit> <latexit sha1_base64="phrtd3Xp7cwooxJyO7tk7KttQOM=">AAACEXicbVDLSgMxFM34rPU1KojgZrAIrspMwdeu4MZlBfuAtpRM5rYNzSRDkimUoV/h1q1+gDt3PpZ+gb/gT2hmWkStB0IO597DPRw/YlRp13235uYXFpeWcyv51bX1jU17a7umRCwJVIlgQjZ8rIBRDlVNNYNGJAGHPoO6P7hI5/UhSEUFv9ajCNoh7nHapQRrI3Vsu+ULFqhRaL4kGndKHbvgFt0MzizxpqRQ3nt9qH0+71Y69kcrECQOgWvCsFJNz410O8FSU8JgnG/FCiJMBrgHTUM5DkG1kyz52Dk0SuB0hTSPaydTfzoSHKo0nNkMse6rv7NU/G/WjHX3rJ1QHsUaOJkc6sbM0cJJa3ACKoFoNjIEE0lNVof0scREm7LyrcyYcKEnNfUBByDH+ayc8xTH31XMklqp6J0UvSuvUHbRBDm0jw7QEfLQKSqjS1RBVUTQEN2iO3Rv3ViP1pP1Mlmds6aeHfQL1tsXbk2iRA==</latexit> <latexit sha1_base64="gh4kf8/VH0qi50vub22NJ+SNqfo=">AAACEXicbVDLSgMxFM34rPU1KojgZrAIrsqM4mtXcOOygn1AW0omc9uGZpIhyRTK0K9w61Y/wJ07H0u/wF/wJzQzLaLWAyGHc+/hHo4fMaq0675bM7Nz8wuLuaX88srq2rq9sVlVIpYEKkQwIes+VsAoh4qmmkE9koBDn0HN71+k89oApKKCX+thBK0QdzntUIK1kdq23fQFC9QwNF8SjdpHbbvgFt0MzjTxJqRQ2nl9qH4+b5fb9kczECQOgWvCsFINz410K8FSU8JglG/GCiJM+rgLDUM5DkG1kiz5yNk3SuB0hDSPaydTfzoSHKo0nNkMse6pv7NU/G/WiHXnrJVQHsUaOBkf6sTM0cJJa3ACKoFoNjQEE0lNVof0sMREm7LyzcyYcKHHNfUAByBH+ayc8xTH31VMk+ph0TspeldeoeSiMXJoF+2hA+ShU1RCl6iMKoigAbpFd+jeurEerSfrZbw6Y008W+gXrLcvb+miRQ==</latexit> <latexit sha1_base64="6gDH420vd7LbG+VvekO2O3wewhw=">AAACEXicbVDLSgMxFM34rPU1KojgZrAIrsqM+NwV3LisYB/QlpLJ3LahmWRIMoUy9CvcutUPcOfOx9Iv8Bf8Cc1Mi6j1QMjh3Hu4h+NHjCrtuu/WzOzc/MJibim/vLK6tm5vbFaViCWBChFMyLqPFTDKoaKpZlCPJODQZ1Dz+xfpvDYAqajg13oYQSvEXU47lGBtpLZtN33BAjUMzZdEo/ZR2y64RTeDM028CSmUdl4fqp/P2+W2/dEMBIlD4JowrFTDcyPdSrDUlDAY5ZuxggiTPu5Cw1COQ1CtJEs+cvaNEjgdIc3j2snUn44EhyoNZzZDrHvq7ywV/5s1Yt05ayWUR7EGTsaHOjFztHDSGpyASiCaDQ3BRFKT1SE9LDHRpqx8MzMmXOhxTT3AAchRPivnPMXxdxXTpHpY9E6K3pVXKLlojBzaRXvoAHnoFJXQJSqjCiJogG7RHbq3bqxH68l6Ga/OWBPPFvoF6+0LcYWiRg==</latexit>

<latexit sha1_base64="ODSV+eXCaEvOBru2vgngDIcQy6A=">AAACFHicbVC7SgNBFJ2Nrxhf0WBlsxgEq7Cr+OoCNpYRzAOyIczO3pghszPLzKwQlmDvB9jaam1rJ7b2dtZa+AvOboL4OjDM4dx7uIfjR4wq7TgvVm5qemZ2Lj9fWFhcWl4prq41lIglgToRTMiWjxUwyqGuqWbQiiTg0GfQ9AfH6bx5AVJRwc/0MIJOiM857VGCtZG6xZLnCxaoYWi+xAPGRt3dbrHsVJwM9l/iTki5uv5++Xr1cV/rFt+8QJA4BK4Jw0q1XSfSnQRLTQmDUcGLFUSYDPA5tA3lOATVSbLwI3vLKIHdE9I8ru1M/e5IcKjSfGYzxLqvfs9S8b9ZO9a9w05CeRRr4GR8qBczWws7bcIOqASi2dAQTCQ1WW3SxxITbfoqeJkx4UKPm+oDDkCOClk5Ryn2vqr4Sxo7FXe/4p665aqDxsijDbSJtpGLDlAVnaAaqiOChugG3aI769p6sB6tp/Fqzpp4SugHrOdP/uikXg==</latexit>

<latexit sha1_base64="clXRegnHg7LXSCriWybj2yujQ28=">AAACFHicbVC7SgNBFJ31GeMrGqxsFoNgFXYFX13AxjKCeUA2hNnZm2TI7MwyMyuEJdj7Aba2Wtvaia29nbUW/oKzmyBqPDDM4dx7uIfjR4wq7Tiv1szs3PzCYm4pv7yyurZe2NisKxFLAjUimJBNHytglENNU82gGUnAoc+g4Q9O03njEqSigl/oYQTtEPc47VKCtZE6haLnCxaoYWi+xAPGRh23Uyg5ZSeDPU3cCSlVtj6u3q4/H6qdwrsXCBKHwDVhWKmW60S6nWCpKWEwynuxggiTAe5By1COQ1DtJAs/sneNEthdIc3j2s7Un44EhyrNZzZDrPvq7ywV/5u1Yt09bieUR7EGTsaHujGztbDTJuyASiCaDQ3BRFKT1SZ9LDHRpq+8lxkTLvS4qT7gAOQon5VzkuLgu4ppUt8vu4dl99wtVRw0Rg5tox20h1x0hCroDFVRDRE0RLfoDt1bN9aj9WQ9j1dnrImniH7BevkC+7CkXA==</latexit> <latexit sha1_base64="w8yJ7CrK0Bsi8YFKYFo4XpqSpX0=">AAACFHicbVC7SgNBFJ31GeMrGqxsFoNgFXYFX13AxjKCiUI2hNnZm2TI7MwyMyuEZbH3A2xttba1E1t7O2st/AVnd0V8HRjmcO493MPxI0aVdpxna2JyanpmtjRXnl9YXFqurKy2lYglgRYRTMgzHytglENLU83gLJKAQ5/BqT86zOan5yAVFfxEjyPohnjAaZ8SrI3Uq1Q9X7BAjUPzJR4wlva2e5WaU3dy2H+J+0lqjbW3i5fL97tmr/LqBYLEIXBNGFaq4zqR7iZYakoYpGUvVhBhMsID6BjKcQiqm+ThU3vTKIHdF9I8ru1c/e5IcKiyfGYzxHqofs8y8b9ZJ9b9/W5CeRRr4KQ41I+ZrYWdNWEHVALRbGwIJpKarDYZYomJNn2VvdyYcKGLpoaAA5BpOS/nIMPOVxV/SXu77u7W3WO31nBQgRJaRxtoC7loDzXQEWqiFiJojK7RDbq1rqx768F6LFYnrE9PFf2A9fQB/UykXQ==</latexit>

`1 `2 `3

Figure 2.4 Factor graph for a larger, simulated SLAM example.

density given measurements is most conveniently represented as a factor graph, and

how given Gaussian priors and Gaussian noise models the corresponding optimiza-
tion problem is nothing but the familiar nonlinear least-squares problem.

2.2.1 Factor Graphs for MAP Inference

We are interested in the unknown state variables x, such as poses and/or landmarks,
given the measurements z. The most-often-used estimator for these unknown state
variables x is the maximum a posteriori (MAP) estimate, so named because it
maximizes the posterior density p(x|z) of the states x given the measurements z:

xMAP = arg max p(x|z) (2.6a)

x
p(z|x)p(x)
= arg max (2.6b)
x p(z)
= arg max p(z|x)p(x) (2.6c)
x

The second equation above is Bayes’ law, and expresses the posterior as the product
of the measurement density p(z|x) and the prior p(x) over the states, appropriately
normalized by the factor p(z). The third equation drops the p(z) since this does
not depend on the x and therefore will not impact the arg max operation.
We use factor graphs to express the unnormalized posterior p(z|x)p(x). Formally
a factor graph is a bipartite graph F = (U, V, E) with two types of nodes: factors
28 Factor Graphs for SLAM

ϕi ∈ U and variables xj ∈ V. Edges eij ∈ E are always between factor nodes and
variables nodes. The set of variable nodes adjacent to a factor ϕi is written as X (ϕi ),
and we write xi for an assignment to this set. With these definitions, a factor graph
F defines the factorization of a global function ϕ(x) as
Y
ϕ(x) = ϕi (xi ). (2.7)
i

In other words, the independence relationships are encoded by the edges eij of the
factor graph, with each factor ϕi a function of only the variables xi in its adjacency
set X (ϕi ).
In the rest of this chapter, we show how to find an optimal assignment, the MAP
estimate, through optimization over the unknown variables in the factor graph.
Indeed, for an arbitrary factor graph, MAP inference comes down to maximizing
the product (2.7) of all factor-graph potentials:

xMAP = arg max ϕ(x) (2.8a)

x
Y
= arg max ϕi (xi ). (2.8b)
x
i

What is left now is to derive the exact form of the factors ϕi (xi ), which depends
very much on how we model the measurement models p(z|x) and the prior densities
p(x). We discuss this in detail next.

2.2.2 Specifying Probability Densities

The exact form of the densities p(z|x) and p(x) above depends very much on
the application and the sensors used. The most often used densities involve the
multivariate Gaussian density, with probability density
 
1 1 2
N (θ; µ, Σ) = p exp − ∥θ − µ∥Σ , (2.9)
|2πΣ| 2
where µ ∈ Rn is the mean, Σ is an n × n covariance matrix, and
2 ∆ ⊤
∥θ − µ∥Σ = (θ − µ) Σ−1 (θ − µ) (2.10)
p
denotes the squared Mahalanobis distance. The normalization constant |2πΣ| =
n/2 1/2
(2π) |Σ| , where |.| denotes the matrix determinant, ensures the multivariate
Gaussian density integrates to 1.0 over its domain.
Priors on unknown quantities are often specified using a Gaussian density, and in
many cases it is both justified and convenient to model measurements as corrupted
by zero-mean Gaussian noise. For example, a bearing measurement2 from a given
2
As a reminder, there are some subtleties associated with rotational state variables that we will
discuss more thoroughly in the next chapter.
 2.2 From MAP Inference to Least Squares 29

pose p to a given landmark ℓ would be modeled as

z = h(p, ℓ) + η, (2.11)
where h(·) is a measurement prediction function, and the noise η is drawn from
a zero-mean Gaussian density with measurement covariance ΣR . This yields the
following conditional density p(z|p, ℓ) on the measurement z:
 
1 1 2
p(z|p, ℓ) = N (z; h(p, ℓ), ΣR ) = p exp − ∥z − h(p, ℓ)∥ΣR . (2.12)
|2πΣR | 2
The measurement functions h(·) are often nonlinear in practical robotics appli-
cations. Still, while they depend on the sensor used and the SLAM front-end, they
are typically not difficult to reason about or write down. The measurement function
for a 2D bearing measurement is simply
h(p, ℓ) = atan2(ℓy − py , ℓx − px ), (2.13)
where atan2 is the well-known two-argument arctangent variant. Hence, the final
probabilistic measurement model p(z|p, ℓ) is obtained as
 
1 1 2
p(z|p, ℓ) = p exp − ∥z − atan2(ℓy − py , ℓx − px )∥ΣR . (2.14)
|2πΣR | 2
Note that we will not always assume Gaussian measurement noise: to cope with the
occasional data association mistake, for example, many authors have proposed the
use of robust measurement densities, with heavier tails than a Gaussian density;
these are discussed in Chapter 4.
Not all probability densities involved are derived from measurements. For exam-
ple, in the toy SLAM problem the prior p(x) on the trajectory is made up of a prior
p(p1 ) and conditional densities p(pt+1 |pt ), specifying a probabilistic motion model
that the robot is assumed to obey given known control inputs ut . In practice, we
often use a conditional Gaussian assumption,
 
1 1 2
p(pt+1 |pt , ut ) = p exp − ∥pt+1 − g(pt , ut )∥ΣQ , (2.15)
|2πΣQ | 2
where g(·) is a motion model, and ΣQ a covariance matrix of the appropriate
dimensionality, e.g., 3 × 3 in the case of robots operating in the plane.
Often we have no known control inputs ut but instead we measure how the
robot moved, e.g., via an odometry measurement ot . For example, if we assume the
odometry simply measures the difference between poses, subject to Gaussian noise
with covariance ΣS , we obtain
 
1 1 2
p(ot |pt+1 , pt ) = p exp − ∥ot − (pt+1 − pt )∥ΣS . (2.16)
|2πΣS | 2
If we have both known control inputs ut and odometry measurements ot we can
combine (2.15) and (2.16).
30 Factor Graphs for SLAM

Note that for robots operating in three-dimensional space, we will need slightly
more sophisticated machinery to specify densities on nonlinear manifolds such as
SE(3), as discussed in the next chapter.

2.2.3 Nonlinear Least Squares

We now show that MAP inference for SLAM problems with Gaussian noise models
as above is equivalent to solving a nonlinear least-squares problem. If we assume
that all factors are of the form
 
1 2
ϕi (xi ) ∝ exp − ∥zi − hi (xi )∥Σi , (2.17)
2
which include both simple Gaussian priors and likelihood factors derived from mea-
surements corrupted by zero-mean, normally distributed noise. Taking the negative
log of (2.8b) and dropping the factor 12 allows us to instead minimize a sum of
nonlinear least-squares terms:
X 2
xMAP = arg min ∥zi − hi (xi )∥Σi . (2.18)
x
i

Minimizing this objective function performs sensor fusion through the process
of combining several measurement-derived factors, and possibly several priors, to
uniquely determine the MAP solution for the unknowns.
An important and non-obvious observation is that the factors in (2.18) typi-
cally represent rather under-specified densities on the involved unknown variables
xi . Indeed, except for simple prior factors, the measurements zi are typically of
lower dimension than the unknowns xi . In those cases, the factor by itself accords
the same likelihood to an infinite subset of the domain of xi . For example, a 2D
measurement in a camera image is consistent with an entire ray of 3D points that
project to the same image location.
Even though the functions hi are nonlinear, if we have a decent initial guess
available, then the nonlinear optimization methods we discuss in this chapter will
be able to converge to the global minimum of (2.18). We should caution, however,
that as our objective in (2.18) is non-convex, there is no guarantee that we will
not get stuck in a local minimum if our initial guess is poor. This has led to so-
called certifiably optimal solvers, which are the subject of a later chapter. Below,
however, we focus on local methods rather than global solvers. We start off below
by considering the the easier problem of solving a linearized version of the problem.

2.3 Solving Linear Least Squares

Before tackling the more difficult problem of nonlinear least squares, in this section
we first show to linearize the problem, show how this leads to a linear least squares
 2.3 Solving Linear Least Squares 31

problem, and review matrix factorization as computationally efficient way to solve

the corresponding normal equations. A seminal reference for these methods is the
book by Golub and Loan [101].

2.3.1 Linearization
We can linearize all measurement functions hi (·) in the nonlinear least-squares
objective function (2.18) using a simple Taylor expansion,
hi (xi ) = hi (x0i + δi ) ≈ hi (x0i ) + Hi δi , (2.19)
where the measurement Jacobian Hi is defined as the (multivariate) partial deriva-
tive of hi (·) at a given linearization point x0i ,
∆ ∂hi (xi )
Hi = , (2.20)
∂xi x0i

∆
and δi = xi − x0i is the state update vector. Note that we make an assumption that
xi lives in a vector space or, equivalently, can be represented by a vector. This is not
always the case, e.g., when some of the unknown states in x represent 3D rotations
or other more complex manifold types. We will revisit this issue in Chapter 3.
Substituting the Taylor expansion (2.19) into the nonlinear least-squares expres-
sion (2.18) we obtain a linear least-squares problem in the state update vector δ,

X 2
δ ∗ = arg min zi − hi (x0i ) − Hi δi Σi
(2.21a)
δ i
X
2
= arg min zi − hi (x0i ) − Hi δi Σi
, (2.21b)
δ i

where zi −hi (x0i ) is the prediction error at the linearization point, i.e., the difference
between actual and predicted measurement. Above, δ ∗ denotes the solution to the
locally linearized problem.
By a simple change of variables we can drop the covariance matrices Σi from
this point forward: defining Σ1/2 as the matrix square root of Σ, we can rewrite
the square Mahalanobis norm as follows:
 ⊤   2
∆
∥e∥2Σ = e⊤ Σ−1 e = Σ−1/2 e Σ−1/2 e = Σ−1/2 e . (2.22)
2

Hence, we can eliminate the covariances Σi by pre-multiplying the Jacobian Hi

−1/2
and the prediction error in each term in (2.21b) with Σi :
−1/2
Ai = Σi Hi (2.23a)
−1/2

bi = Σi zi − hi (x0i ) . (2.23b)
This process is a form of whitening. For example, in the case of scalar measurements
32 Factor Graphs for SLAM

it simply means dividing each term by the measurement standard deviation σi . Note
that this eliminates the units of the measurements (e.g., length, angles) so that the
different rows can be combined into a single cost function.

2.3.2 SLAM as Least-Squares

After linearization, we finally obtain the following standard least-squares problem:

X 2
δ ∗ = arg min ∥Ai δi − bi ∥2 (2.24a)
δ i
2
= arg min ∥Aδ − b∥2 , (2.24b)
δ

Above A and b are obtained by collecting all whitened Jacobian matrices Ai and
whitened prediction errors bi into one large matrix A and right-hand-side (RHS)
vector b, respectively.
The Jacobian A is a large-but-sparse matrix, with a block structure that mirrors
the structure of the underlying factor graph. We will examine this sparsity structure
in detail below. First, however, we review the the classical linear algebra approach
to solving this least-squares problem.

2.3.3 Matrix Factorization for Least-Squares

For a full-rank m × n matrix A, with m ≥ n, the unique least-squares solution to
(2.24b) can be found by solving the normal equations:


A⊤ A δ ∗ = A⊤ b. (2.25)

This is normally done by factoring the information matrix Λ (also called the Hessian
matrix), defined and factored as follows:
∆
Λ = A⊤ A = R⊤ R. (2.26)

Above, the Cholesky triangle R is an upper-triangular n × n matrix3 and is com-

puted using Cholesky factorization, a variant of lower-upper (LU) factorization for
symmetric positive-definite matrices. After this, δ ∗ can be found by solving first

R ⊤ y = A⊤ b (2.27)

for y and then

R δ∗ = y (2.28)
3
Some treatments, including [101], define the Cholesky triangle as the lower-triangular matrix
L = R⊤ , but the other convention is more convenient here.
 2.3 Solving Linear Least Squares 33

for δ ∗ by forward and backward substitution, respectively. For dense matrices,

Cholesky factorization requires n3 /3 flops, and the entire algorithm, including com-
puting half of the symmetric A⊤ A, requires (m + n/3)n2 flops. One could also use
lower-diagonal-upper (LDU) factorization, a variant of Cholesky decomposition that
avoids the computation of square roots.
An alternative to Cholesky factorization that is more accurate and more numeri-
cally stable is to proceed via QR-factorization, which works without computing the
information matrix Λ. Instead, we compute the QR-factorization of A itself along
with its corresponding RHS:
   
R d
A=Q , = Q⊤ b. (2.29)
0 e
Here Q is an m × m orthogonal matrix, d ∈ Rn , e ∈ Rm−n , and R is the same
upper-triangular Cholesky triangle. The preferred method for factorizing a dense
matrix A is to compute R column by column, proceeding from left to right. For each
column j, all nonzero elements below the diagonal are zeroed out by multiplying
A on the left with a Householder reflection matrix Hj . After n iterations A is
completely factorized:
 
⊤ R
Hn · · · H2 H1 A = Q A = . (2.30)
0
The orthogonal matrix Q is not usually formed: instead, the transformed RHS
Q⊤ b is computed by appending b as an extra column to A. Because the Q factor
is orthogonal, we have
2 2 2 2
∥A δ − b∥2 = Q⊤ Aδ − Q⊤ b 2
= ∥R δ − d∥2 + ∥e∥2 , (2.31)
2
where we made use of the equalities from (2.29). Clearly, ∥e∥2 will be the least-
squares sum of squared residuals, and the least-squares solution δ ∗ can be obtained
by solving the triangular system
R δ∗ = d (2.32)
via back-substitution. Note that the upper-triangular factor R obtained using QR
factorization is the same (up to possible sign changes on the diagonal) as would be
obtained by Cholesky factorization, since
 ⊤  
R R
A⊤ A = Q⊤ Q = R⊤ R, (2.33)
0 0
where we again made use of the fact that Q is orthogonal. The cost of QR is domi-
nated by the cost of the Householder reflections, which is 2(m − n/3)n2 . Comparing
this with Cholesky, we see that both algorithms require O(mn2 ) operations when
m ≫ n, but that QR-factorization is slower by a factor of 2.
In summary, the linearized optimization problem associated with SLAM can be
34 Factor Graphs for SLAM

concisely stated in terms of basic linear algebra. It comes down to factorizing either
the information matrix Λ or the measurement Jacobian A into square-root form.
Because they are based on matrix square roots derived from the SAM √ problem, we
have referred to this family of approaches as square-root SAM, or SAM for short
[65, 68].

2.4 Nonlinear Optimization

In this section, we discuss some classic optimization approaches to the nonlinear
least-squares problem defined in (2.18). As a reminder, in SLAM the nonlinear
least-squares objective function is given by
∆
X 2
J(x) = ∥zi − hi (xi )∥Σi (2.34)
i

and corresponds to a nonlinear factor graph derived from the measurements along
with prior densities on some or all unknowns.
Nonlinear least-squares problems cannot be solved directly in general, but require
an iterative solution starting from a suitable initial estimate. Nonlinear optimiza-
tion methods do so by solving a succession of linear approximations to (2.18) in
order to approach the minimum [72]. A variety of algorithms exist that differ in how
they locally approximate the cost function, and in how they find an improved esti-
mate based on that local approximation. A general in-depth treatment of nonlinear
solvers is provided by [189], while [101] focuses on the linear-algebra perspective.
All of the algorithms share the following basic structure: they start from an initial
estimate x0 . In each iteration, an update step δ is calculated and applied to obtain
the next estimate xt+1 = xt +δ. This process ends when certain convergence criteria
are reached, such as the norm of the change δ falling below a small threshold.

2.4.1 Steepest Descent

Steepest Descent (SD) uses the direction of steepest descent at the current estimate
to calculate the following update step:
δ sd = −α ∇J (x)|x=xt . (2.35)
Here the negative gradient is used to identify the direction of steepest descent. For
the nonlinear least-squares objective function (2.34), we locally approximate the
2
objective function as a quadratic, J(x) ≈ ∥A(x − xt ) − b∥2 , and obtain the exact
gradient ∇J (x)|x=xt = −2A⊤ b at the linearization point xt .
The step size α needs to be carefully chosen to balance between safe updates and
reasonable convergence speed. An explicit line search can be performed to find a
minimum in the given direction. SD is a simple algorithm, but suffers from slow
convergence near the minimum.
 2.4 Nonlinear Optimization 35

2.4.2 Gauss-Newton
Gauss-Newton (GN) provides faster convergence by using a second-order update.
GN exploits the special structure of the nonlinear least-squares problem to approx-
imate the Hessian using the Jacobian as A⊤ A. The GN update step is obtained by
solving the normal equations (2.25),

A⊤ A δ gn = A⊤ b, (2.36)

by any of the methods in Section 2.3.3. For a well-behaved (i.e., nearly quadratic)
objective function and a good initial estimate, Gauss-Newton exhibits nearly quadratic
convergence. If the quadratic fit is poor, a GN step can lead to a new estimate that
is further from the minimum and subsequent divergence.

2.4.3 Levenberg-Marquardt
The Levenberg-Marquardt (LM) algorithm allows for iterating multiple times to
convergence while controlling in which region one is willing to trust the quadratic
approximation made by Gauss-Newton. Hence, such a method is often called a
trust-region method.
To combine the advantages of both the SD and GN methods, Levenberg [149]
proposed to modify the normal equations (2.25) by adding a non-negative constant
λ ∈ R+ ∪ {0} to the diagonal


A⊤ A + λI δ lm = A⊤ b. (2.37)

Note that for λ = 0 we obtain GN, and for large λ we approximately obtain δ ∗ ≈
1 ⊤
λ A b, an update in the negative gradient direction of the cost function J (2.34).
Hence, LM can be seen to blend naturally between the GN and SD methods.
Marquardt [170] later proposed to take into account the scaling of the diagonal
entries to provide faster convergence:


A⊤ A + λ diag(A⊤ A) δ lm = A⊤ b. (2.38)

This modification causes larger steps in the steepest-descent direction if the gra-
dient is small (nearly flat directions of the objective function), because there the
inverse of the diagonal entries will be large. Conversely, in steep directions of the
objective function the algorithm becomes more cautious and takes smaller steps.
Both modifications of the normal equations can be interpreted in Bayesian terms
as adding a zero-mean prior to the system.
A key difference between GN and LM is that the latter rejects updates that
would lead to a higher sum of squared residuals. A rejected update means that the
nonlinear function is locally not well-behaved, and smaller steps are needed. This
is achieved by heuristically increasing the value of λ, for example by multiplying
its current value by a factor of 10, and resolving the modified normal equations.
36 Factor Graphs for SLAM

Gradient descent update

Current
estimate
Dog leg update

Trust region

Gauss-Newton update

Figure 2.5 Powell’s dogleg algorithm combines the separately computed Gauss-Newton
and gradient descent update steps.

On the other hand, if a step leads to a reduction of the sum of squared residuals,
it is accepted, and the state estimate is updated accordingly. In this case, λ is
reduced (e.g., by dividing by a factor of 10), and the algorithm repeats with a new
linearization point, until convergence.

2.4.4 Dogleg Minimization

Powell’s dogleg (PDL) algorithm [212] can be a more efficient alternative to LM
[159]. A major disadvantage of the Levenberg-Marquardt algorithm is that in case
a step gets rejected, the modified information matrix has to be refactored, which is
the most expensive component of the algorithm. Hence, the key idea behind PDL
is to separately compute the GN and SD steps, and then combine appropriately. If
the LM step gets rejected, the directions of the GN and SD steps are still valid, and
they can be combined in a different way until a reduction in the cost is achieved.
Hence, each update of the state estimate only involves one matrix factorization, as
opposed to several.
Figure 2.5 shows how the GN and SD steps are combined. The combined step
starts with the SD update, followed by a sharp bend (hence the term dogleg) to-
wards the GN update, but stopping at the trust region boundary. Unlike LM, PDL
maintains an explicit trust region within which we trust the linear assumption. The
appropriateness of the linear approximation is determined by the gain ratio
J(xt ) − J(xt + δ)
ρ= , (2.39)
L(0) − L(δ)
where L(δ) = A⊤ Aδ − A⊤ b is the linearization of the nonlinear quadratic cost
function J from (2.34) at the current estimate xt . If ρ is small (i.e., ρ < 0.25)
then the cost has not reduced as predicted by the linearization and the trust region
is reduced. On the other hand, if the reduction is as predicted (or better, i.e.,
ρ > 0.75), then the trust region is increased depending on the magnitude of the
update vector, and the step is accepted.
 2.5 Factor Graphs and Sparsity 37

2.5 Factor Graphs and Sparsity

The solvers presented so far assume that the matrices involved may be dense. Dense
methods will not scale to realistic problem sizes in SLAM. For the toy problem in
Figure 2.1 a dense method will work fine. The larger simulation example, with its
factor graph shown in Figure 2.4, is more representative of real-world problems.
However, it is still relatively small as real SLAM problems go, where problems with
thousands or even millions of unknowns are common. Yet, we are able to handle
these without a problem because of sparsity.
The sparsity can be appreciated directly from looking at the factor graph. It is
clear from Figure 2.4 that the graph is sparse (i.e., it is by no means a fully connected
graph). The odometry chain linking the 100 unknown poses is a linear structure
of 100 binary factors, instead of the possible 1002 (binary) factors. In addition,
with 20 landmarks we could have up to 2000 factors linking each landmark to each
pose: the true number is closer to 400. And finally, there are no factors between
landmarks at all. This reflects that we have not been given any information about
their relative position. This structure is typical of most SLAM problems.

2.5.1 The Sparse Jacobian and its Factor Graph

The key to modern SLAM algorithms is exploiting sparsity, and an important prop-
erty of factor graphs in SLAM is that they represent the sparse block structure in
the resulting sparse Jacobian matrix A. To see this, let us revisit the least-squares
problem that is the key computation in the inner loop of the nonlinear SLAM
problem:
X 2
δ ∗ = arg min ∥Ai δi − bi ∥2 . (2.40)
δ i

Each term above is derived from a factor in the original, nonlinear SLAM problem,
linearized around the current linearization point (2.21b). The matrices Ai can be
broken up in blocks corresponding to each variable, and collected in a large, block-
sparse Jacobian whose sparsity structure is given exactly by the factor graph.
Even though these linear problems typically arise as inner iterations in nonlinear
optimization, we drop the δ notation below, as everything holds for general linear
problems regardless of their origin.
Consider the factor graph for the small toy example in Figure 2.1. After lin-
earization, we obtain a sparse system [A|b] with the block structure in Figure 2.6.
Comparing this with the factor graph, it is obvious that every factor corresponds to
a block-row, and every variable corresponds to a block-column of A. In total there
are nine block-rows, one for every factor in the factorization of ϕ(p1 , p2 , p3 , ℓ1 , ℓ2 ).
38 Factor Graphs for SLAM

 δℓ1 δℓ2 δp1 δp2 δp3 b 

ϕ1 A13 b1
ϕ2 
 A23 A24 b2 

ϕ3 A34 A35 b3
 
 
 
ϕ4  A41 b4 
[A|b] =  
ϕ5 
 A52 b5 

ϕ6 
 A63 b6 

ϕ7  A71 A73 b7
 

 
ϕ8  A81 A84 b8 
ϕ9 A92 A95 b9

Figure 2.6 Block structure of the sparse Jacobian A for the toy SLAM example in Figure
⊤ ⊤
2.1 with δ = δℓ⊤ ⊤ ⊤ ⊤
 
1 δℓ2 δp1 δp2 δp3 . Blank entries are zeros.
 
Λ11 Λ13 Λ14

 Λ22 Λ25 

 Λ31 Λ33 Λ34
 

 
 Λ41 Λ43 Λ44 Λ45 
Λ52 Λ54 Λ55

∆
Figure 2.7 The information matrix Λ = A⊤ A for the toy SLAM problem.

2.5.2 The Sparse Information Matrix and its Graph

When using Cholesky factorization for solving the normal equations, as explained
in Section 2.3.3, we first form the Hessian or information matrix Λ = A⊤ A. 4
In general, since the Jacobian A is block-sparse, the Hessian Λ is expected to be
sparse as well. By construction, the Hessian is a symmetric matrix, and if a unique
MAP solution to the problem exists, it is also positive definite.
The information matrix Λ can be associated with another, undirected graphical
model for the SLAM problem, namely a Markov random field (MRF). In contrast
to a factor graph, an MRF is a graphical model that involves only the variables.
The graph G of an MRF is an undirected graph: the edges only indicate that there
is some interaction between the variables involved. At the block level, the sparsity
pattern of Λ = A⊤ A is exactly the adjacency matrix of G.
Figure 2.7 shows the information matrix Λ associated with our running toy ex-
ample. In this case, there are five variables that partition the Hessian as shown.
The zero blocks indicate which variables do not interact (e.g., ℓ1 and ℓ2 have no
direct interaction). Figure 2.8 shows the corresponding MRF.
In what follows, we will frequently refer to the undirected graph G of the MRF
associated with an inference problem. However, we will not use the MRF graphical
4
Note that A⊤ A is not true Hessian, but is often used to approximate Hessian by truncating a
Taylor series of the residual.
 2.5 Factor Graphs and Sparsity 39

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

`l11 `l22

x x xp33
<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

p11 p22

Figure 2.8 The Hessian matrix Λ can be interpreted as the matrix associated with the
Markov random field representation for the problem.

model much beyond that as factor graphs are better suited to our needs. They are
able to express a finer-grained factorization, and are more closely related to the
original problem formulation. For example, if there exist ternary (or higher) factors
in the factor graph, the graph G of the equivalent MRF connects those nodes in
an undirected clique (a group of fully connected variables), but the origin of the
corresponding clique potential is lost. In linear algebra, this reflects the fact that
many matrices A can yield the same Λ = A⊤ A matrix: important information on
the sparsity is lost.

2.5.3 Sparse Factorization

We have seen MAP estimation amounts to solving a linear system of equations as
described in Section 2.3.3. In the case of nonlinear least-squares problems, we solve
such a system repeatedly in an iterative setup. We have seen in the previous two
sections that both A and A⊤ A enjoy sparsity determined by the factor graph and
MRF connectivity, respectively. Without going into detail, this known sparsity pat-
tern can be used to greatly speed up either Cholesky factorization (in the case of
working with A⊤ A) or QR-factorization (in the case of working with A). Efficient
software implementations are available, e.g., CHOLMOD [54] and SuiteSparseQR
[63], which are also used under the hood by several software packages. In prac-
tice, sparse Cholesky or LDU factorization outperform QR factorization on sparse
problems as well, and not just by a constant factor.
The flop count for sparse factorization will be much lower than for a dense ma-
trix. Crucially, the column ordering chosen for the sparse matrices can dramatically
influence the total flop-count. While any order will ultimately produce an identical
MAP estimate, the variable order determines the fill-in of matrix factors (i.e., the
extra nonzero entries beyond the sparsity pattern of the matrix being factored). It
is known that finding the variable ordering that minimizes fill-in during matrix fac-
40 Factor Graphs for SLAM

torization is an NP-hard problem [301], so we must resort to using good heuristics.

This will in turn affect the computational complexity of the factorization algorithm.
We demonstrate this by way of an example. Recall the larger simulation example,
with its factor graph shown in Figure 2.4. The sparsity patterns for the correspond-
ing sparse Jacobian matrix A is shown in Figure 2.9. Also shown is the pattern for
the information matrix Λ ≜ A⊤ A, in the top-right corner. On the right of Figure
2.9, we show the resulting upper triangular Choleksy factor R for two different
orderings. Both of them are sparse, and both of them satisfy R⊤ R = A⊤ A (up
to a permutation of the variables), but they differ in the amount of sparsity they
exhibit. It is exactly this that will determine how expensive it is to factorize A.
The first version of the ordering comes naturally: the poses come first and then
the landmarks, leading to a sparse R factor with 9399 nonzero entries. In contrast,
the sparse factor R in the bottom right was obtained by reordering the variables
according to the Column approximate minimum degree permutation (COLAMD)
heuristic [12, 64] and only has 4168 nonzero entries. Yet back-substitution gives
exactly the same solution for both versions.
It is worth mentioning that other tools, like pre-conditioned conjugate gradient,
can solve the normal equations iteratively. In visual SLAM, which has a very specific
sparsity pattern, power iterations have also been used successfully [282]. However,
sparse factorization is still the method of choice for most SLAM problems and has
a nice graphical model interpretation, which we discuss next.

2.6 Elimination
We have so far restricted ourselves to a linear-algebra explanation of performing
inference for SLAM. In this section, we expand our worldview by thinking about
inference more abstractly using graphical models directly. This will ultimately lead
us to current state-of-the-art SLAM solvers based on a concept called the Bayes
tree for incremental smoothing and mapping in the next section.

2.6.1 Variable Elimination Algorithm

There exists a general algorithm that can, given any (preferably sparse) factor
graph, compute the corresponding posterior density p(x|z) on the unknown vari-
ables x in a form that allows easy recovery of the MAP solution to the problem.
As we saw, a factor graph represents the unnormalized posterior ϕ(x) ∝ p(x|z)
as a product of factors, and in SLAM problems this graph is typically generated
directly from the measurements. The variable elimination algorithm is a recipe for
converting a factor graph into another graphical model called a Bayes net, which
depends only on the unknown variables x. This then allows for easy MAP inference
(as well as other operations such as sampling and/or marginalization).
 2.6 Elimination 41

Figure 2.9 On the left, the measurement Jacobian A associated with the problem in Figure
2.4, which has 3 × 95 + 2 × 24 = 333 unknowns. The number of rows, 1126, is equal to the
number of (scalar) measurements. Also given is the number of nonzero entries “nnz”. On
the right: (top) the information matrix Λ ≜ A⊤ A; (middle) its upper triangular Cholesky
triangle R; (bottom) an alternative factor amdR obtained with a better variable ordering
(COLAMD).
42 Factor Graphs for SLAM

In particular, the variable elimination algorithm is a way to factorize any factor

graph of the form
ϕ(x) = ϕ(x1 , . . . , xn ) (2.41)

into a factored Bayes net probability density of the form

Y
p(x) = p(x1 |s1 )p(x2 |s2 ) . . . p(xn ) = p(xj |sj ), (2.42)
j

where sj denotes an assignment to the separator s(xj ) associated with variable xj

under the chosen variable ordering x1 , . . . , xn . The separator is defined as the set
of variables on which xj is conditioned, after elimination. While this factorization
is akin to the chain rule, eliminating a sparse factor graph will typically lead to
small separators.
The elimination algorithm proceeds by eliminating one variable xj at a time,
starting with the complete factor graph ϕ1:n . As we eliminate each variable xj ,
we generate a single conditional p(xj |sj ), as well as a reduced factor graph ϕj+1:n
on the remaining variables. After all variables have been eliminated, the algorithm
returns the resulting Bayes net with the desired factorization.
To eliminate a single variable xj given a partially eliminated factor graph ϕj:n ,
we first remove all factors ϕi (xi ) that are adjacent to xj and multiply them into the
product factor ψ(xj , sj ). We then factorize ψ(xj , sj ) into a conditional distribution
p(xj |sj ) on the eliminated variable xj , and a new factor τ (sj ) on the separator sj :

ψ(xj , sj ) = p(xj |sj )τ (sj ). (2.43)

Hence, the entire factorization from ϕ(x) to p(x) is seen to be a succession of n

local factorization steps. When eliminating the last variable xn the separator sn
will be empty, and the conditional produced will simply be a prior p(xn ) on xn .
One possible elimination sequence for the toy example is shown in Figure 2.10, for
the ordering ℓ1 , ℓ2 , p1 , p2 , p3 . In each step, the variable being eliminated is shaded
gray, and the new factor τ (sj ) on the separator sj is shown in red. Taken as a whole,
the variable elimination algorithm factorizes the factor graph ϕ(ℓ1 , ℓ2 , p1 , p2 , p3 )
into the Bayes net in Figure 2.10 (bottom right), corresponding to the factorization

p(ℓ1 , ℓ2 , p1 , p2 , p3 ) = p(ℓ1 |p1 , p2 ) p(ℓ2 |p3 ) p(p1 |p2 ) p(p2 |p3 ) p(p3 ). (2.44)

2.6.2 Linear-Gaussian Elimination

In the case of linear measurement functions and additive normally distributed noise,
the elimination algorithm is equivalent to sparse matrix factorization. Both sparse
Cholesky and QR factorization are a special case of the general algorithm.
As explained before, the elimination algorithm proceeds one variable at a time.
For every variable xj we remove all factors ϕi (xi ) adjacent to xj and form the
 2.6 Elimination 43

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

l`11 `l22 l`11 l`22

<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit> <latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

p11
x p22
x p33
x xp11 xp22 xp33

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit> <latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

l`11 `l22 l`11 `l22

<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit>

xp11 xp22 xp33 xp22

<latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit> <latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

xp11 p
x3

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

`l11 `l22

<latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

xp33
<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit>

x
p1 x
p2

Figure 2.10 Variable elimination for the toy SLAM example, transforming the factor graph
from Figure 2.2 into a Bayes net (bottom right), using the ordering ℓ1 , ℓ2 , p1 , p2 , p3 .

intermediate product factor ψ(xj , sj ). This can be done by accumulating all the
matrices Ai into a new, larger block-matrix Āj , as we can write
Y
ψ(xj , sj ) ← ϕi (xi ) (2.45a)
i∈Nj
!
1X 2
= exp − ∥Ai xi − bi ∥2 (2.45b)
2 i
 
1 2
= exp − Āj [xj ; sj ] − b̄j 2 , (2.45c)
2

where the new RHS vector b̄j stacks all bi and ‘;’ also denotes vertical stacking.
Consider eliminating the variable ℓ1 in the toy example. The adjacent factors are
ϕ4 , ϕ7 , and ϕ8 , in turn inducing the separator s1 = [p1 ; p2 ]. The product factor is
 44 Factor Graphs for SLAM

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit>

x3 x2 x1
l`11 `l22 l`11 `l22

l2 <latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit>

l1
xp11
<latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit>

x
<latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

x
<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

p2 p3 p11
x p22
x p33
x

 
A13
R1 T13 T14
 
 A23 A24 
A13
   
A34 A35
   
 
   A23 A24 
 A41   
A34 A35
   
A52
   
 
   A52 
 A63   
   A63 
 A71 A73
   
A92 A95
  
 
 A81 A84 
Ã13 Ã14
A92 A95

Figure 2.11 Eliminating the variable ℓ1 as a partial sparse factorization step.

then equal to
 
1 2
ψ (ℓ1 , p1 , p2 ) = exp − Ā1 [ℓ1 ; p1 ; p2 ] − b̄1 2
, (2.46)
2
with
   
A41 b4
∆ ∆

Ā1 = A71 A73 , b̄1 =  b7  . (2.47)
A81 A84 b8

Looking at the sparse Jacobian in Figure 2.6, this simply boils down to taking out
the block-rows with nonzero blocks in the first column, corresponding to the three
factors adjacent to ℓ1 .
Next, factorizing the product ψ(xj , sj ) can be done in several different ways.
We discuss the QR variant, as it more directly connects to the linearized factors.
In particular, the augmented matrix [Āj |b̄j ] corresponding to the product factor
ψ(xj , sj ) can be rewritten using partial QR-factorization [101] as follows:
 
Rj Tj dj
[Āj |b̄j ] = Q , (2.48)
Ãτ b̃τ

where Rj is an upper-triangular matrix. This allows us to factor ψ(xj , sj ) as follows:

 2.6 Elimination 45

 
1 2
ψ(xj , sj ) = exp − Āj [xj ; sj ] − b̄j 2 (2.49a)
2
   
1 1 2
2
= exp − ∥Rj xj + Tj sj − dj ∥2 exp − Ãτ sj − b̃τ
2 2 2

= p(xj |sj )τ (sj ), (2.49b)

where we used the fact that the rotation matrix Q does not alter the value of the
norms involved.
In the toy example, Figure 2.11 shows the result of eliminating the first variable
in the example, the landmark ℓ1 with separator [p1 ; p2 ]. We show the operation
on the factor graph and the corresponding effect on the sparse Jacobian from Fig-
ure 2.6, omitting the RHS. The partition above the line corresponds to a sparse,
upper-triangular matrix R that is being formed. New contributions to the matrix
are shown: blue for the contributions to R, and red for newly created factors. For
completeness, we show the four remaining variable elimination steps in Figure 2.12,
showing an end-to-end example of how QR factorization proceeds on a small exam-
ple. The final step shows the equivalence between the resulting Bayes net and the
sparse upper-triangular factor R.
The entire elimination algorithm, using partial QR to eliminate a single vari-
able, is equivalent to sparse QR factorization. As the treatment above considers
multi-dimensional variables xj ∈ Rnj , this is in fact an instance of multi-frontal
QR factorization [77], as we eliminate several scalar variables at a time, which
is beneficial for processor utilization. While in our case the scalar variables are
grouped because of their semantic meaning in the inference problem, sparse linear
algebra codes typically analyze the problem to group for maximum computational
efficiency. In many cases these two strategies are closely aligned.

2.6.3 Sparse Cholesky Factor as a Bayes Net

The equivalence between variable elimination and sparse matrix factorization re-
veals that the graphical model associated with an upper triangular matrix is a Bayes
net! Just like a factor graph is the graphical embodiment of a sparse Jacobian, and
an MRF can be associated with the Hessian, a Bayes net reveals the sparsity struc-
ture of a Cholesky factor. In hindsight, this perhaps is not too surprising: a Bayes
net is a directed acyclic graph (DAG), and that is exactly the ‘upper-triangular’
property for matrices.
What’s more, the Cholesky factor corresponds to a Gaussian Bayes net, which
we define as one made up of linear-Gaussian conditionals. The variable elimination
algorithm holds for general densities, but in case the factor graph only contains
linear measurement functions and Gaussian additive noise, the resulting Bayes net
46 Factor Graphs for SLAM
<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit> <latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

l`11 l`22 l`11 `l22

<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit> <latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit>

xp11 xp22 xp33 xp11 x2 xp33

<latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

R1 T13 T14
 
 

 R2 T25 
 R1 T13 T14

 A13 


 R2 T25 

 A23 A24



 R3 T34 

   
 A34 A35   A34 A35 
   

 A63 

 Ã25 
 Ã13 Ã14  Ã34
Ã25

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

`l11 `l22 l`11 `l22

<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

p1
x p2
x p
x3 xp11 xp22 xp33
<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit>

 
R1 T13 T14  
R1 T13 T14
 R2 T25 
   R2 T25 
 R3 T34   
R3 T34
   
R4 T45
   
   
   R4 T45 
 Ã25 
R5
Ã45

Figure 2.12 The remaining elimination steps for the toy example, completing a full QR
factorization. The last step in the bottom right shows the equivalence between the resulting
Bayes net and the sparse Cholesky factor R.

has a very specific form. We discuss the details below, as well as how to solve for
the MAP estimate in the linear case.
As we discussed, the Gaussian factor graph corresponding to the linearized non-
linear problem is transformed by elimination into the density p(x) given by the
 2.7 Incremental SLAM 47

now-familiar Bayes-net factorization:

Y
p(x) = p(xj |sj ). (2.50)
j

In both QR and Cholesky variants, the conditional densities p(xj |sj ) are given by
 
1 2
p(xj |sj ) = k exp − ∥Rj xj + Tj sj − dj ∥2 , (2.51)
2
which is a linear-Gaussian density on the eliminated variable xj . Indeed, we have
2 ⊤ ∆
∥Rj xj + Tj sj − dj ∥2 = (xj − µj ) Rj⊤ Rj (xj − µj ) = ∥xj − µj ∥2Σj , (2.52)

where the mean µj = Rj−1 (dj −Tj sj ) depends linearly on the separator sj , and the
covariance matrix is given by Σj = (Rj⊤ Rj )−1 . Hence, the normalization constant
−1
k = |2πΣj | 2 .
After the elimination step is complete, back-substitution is used to obtain the
MAP estimate of each variable. As seen in Figure 2.12, the last variable eliminated
does not depend on any other variables. Thus, the MAP estimate of the last variable
can be directly extracted from the Bayes net. By proceeding in reverse elimination
order, the values of all the separator variables for each conditional will always
be available from the previous steps, allowing the estimate for the current frontal
variable to be computed.
At every step, the MAP estimate for the variable xj is the conditional mean,

x∗j = Rj−1 (dj − Tj s∗j ), (2.53)

since by construction the MAP estimate for the separator s∗j is fully known by this
point.

2.7 Incremental SLAM

In an incremental SLAM setting, we want to compute the optimal trajectory and
map whenever we receive new measurements while traversing the environment, or
at least at regular intervals. One way to do so is to update the most recent matrix
factorization with the new measurements, to reuse the computation that already
incorporated all previous measurements. In the linear case, this is possible through
incremental factorization methods, the dense versions of which are also discussed
at length in Golub and Loan [101]. However, matrix factorization operates on lin-
ear systems, but most SLAM problems of practical interest are nonlinear. Using
incremental matrix factorization, it is far from obvious how re-linearization can be
performed incrementally without refactoring the complete matrix. To overcome this
problem we once again resort to graphical models, and introduce a new graphical
model, the Bayes tree. We then show how to incrementally update the Bayes tree as
48 Factor Graphs for SLAM

new measurements and states are added to the system, leading to the incremental
smoothing and mapping (iSAM) algorithm.

2.7.1 The Bayes Tree

It is well known that inference in a tree-structured graph is efficient. In contrast,
the factor graphs associated with typical robotics problems contain many loops.
Still, we can construct a tree-structured graphical model in a two-step process:
first, perform variable elimination on the factor graph to obtain a Bayes net with a
special property. Second, exploit that special property to find a tree structure over
cliques in this Bayes net.
In particular, a Bayes net obtained by running the elimination algorithm on a
factor graph satisfies a special property: it is chordal, meaning that any undirected
cycle of length greater than three has a chord, i.e., an edge connecting two non-
consecutive vertices on the cycle. In AI and machine learning, a chordal graph is
more commonly said to be triangulated. Because it is still a Bayes net, the cor-
responding joint density p(x) is given by factorizing over the individual variables
xj ,
Y
p(x) = p(xj |πj ), (2.54)
j

where πj are the parent nodes of xj . However, although the Bayes net is chordal, at
this variable level it is still a non-trivial graph: neither chain-like nor tree-structured.
The chordal Bayes net for our running toy SLAM example is shown in the last step
of Figure 2.10, and it is clear that there is an undirected cycle p1 −p2 −ℓ1 , implying
it does not have a tree-structured form.
By identifying cliques in this chordal graph, the Bayes net may be rewritten as
a Bayes tree. We introduce this new, tree-structured graphical model to capture
the clique structure of the Bayes net. It is not obvious that cliques in the Bayes
net should form a tree. They do so because of the chordal property, although we
will not attempt to prove that here. Listing all these cliques in an undirected tree
yields a clique tree, also known as a junction tree in AI and machine learning. The
Bayes tree is just a directed version of this that preserves information about the
elimination order.
More formally, a Bayes tree is a directed tree where the nodes represent cliques ck
of the underlying chordal Bayes net. In particular, we define one conditional density
p(fk |sk ) per node, with the separator sk as the intersection ck ∩ ϖk of the clique
ck and its parent clique ϖk . The frontal variables fk are the remaining variables,
∆
i.e., fk = ck \ sk . Notationally, we write ck = fk : sk for a clique. The following
expression gives the joint density p(x) on the variables x defined by a Bayes tree:
Y
p(x) = p(fk |sk ). (2.55)
k
 2.7 Incremental SLAM 49
<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

`l11 `l2

<latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

x x x
p3
<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit>

p11 p22
<latexit sha1_base64="ry59MVPTyO/Z6zeX6db+jus3Dnw=">AAACCXicbVDLSsNAFL2pr1pfVZdugkWom5KIz13BjcsK9oFNKJPpbTt0MhNmJmIJ/QK3bvUf3Ilbv8Jf8CtM0yJqPTBwOOce7p0TRJxp4zgfVm5hcWl5Jb9aWFvf2Nwqbu80tIwVxTqVXKpWQDRyJrBumOHYihSSMODYDIaXE795h0ozKW7MKEI/JH3BeowSk0q3XhjI+6RMDsedYsmpOBnseeLOSKmahwy1TvHT60oahygM5UTrtutExk+IMoxyHBe8WGNE6JD0sZ1SQULUfpJdPLYPUqVr96RKnzB2pv5MJCTUehQG6WRIzED/9Sbif147Nr1zP2Eiig0KOl3Ui7ltpD35vt1lCqnho5QQqlh6q00HRBFq0pIKXhZMhDRZPZ0Bki6qcSEr52KCk+8q5knjqOKeVtzr41K1PG0J8rAH+1AGF86gCldQgzpQEPAIT/BsPVgv1qv1Nh3NWbPMLvyC9f4FHeGbUQ==</latexit>

(a)
<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit> <latexit sha1_base64="SQ47TFOWkZBwNsNyfERGuUiTcQY=">AAACFHicbVDLSsNAFL3xWeur2qWbYBVclUR87gpuXFawtdCUMpnc2sHJTJiZCCX0N9y61X9wJ27d+wt+hZO0iFoPDHM49x7u4YQJZ9p43oczN7+wuLRcWimvrq1vbFa2tttapopii0ouVSckGjkT2DLMcOwkCkkccrwJ7y7y+c09Ks2kuDajBHsxuRVswCgxVupXqkEoeaRHsf2yADkf9w/7lZpX9wq4s8SfklqjBAWa/cpnEEmaxigM5UTrru8lppcRZRjlOC4HqcaE0Dtyi11LBYlR97Ii/Njdt0rkDqSyTxi3UH86MhLrPJ/djIkZ6r+zXPxv1k3N4KyXMZGkBgWdHBqk3DXSzZtwI6aQGj6yhFDFbFaXDoki1Ni+ykFhzIQ0k6aGSCJU43JRznmO4+8qZkn7sO6f1P2ro1pjb9ISlGAHduEAfDiFBlxCE1pAYQSP8ATPzoPz4rw6b5PVOWfqqcIvOO9ff/uf0Q==</latexit>

<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit> <latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

`1 `2 p 1 p2 p3
p2 , p3
<latexit sha1_base64="GsIUnO1JWznWHmco02Iwp2yAuQU=">AAACI3icbVDLSgMxFL3js9ZX1aWb0CK4kDLj213BjUsFq4W2lEzm1gYzyZBkhDJ074+4dav/4E7cuPAH/AozUxFfB0IO59xDbk6YCG6s7796E5NT0zOzpbny/MLi0nJlZfXCqFQzbDIllG6F1KDgEpuWW4GtRCONQ4GX4fVx7l/eoDZcyXM7TLAb0yvJ+5xR66RepdoJlYjMMHZXlox621vkl7LTq9T8ul+A/CXBJ6k1SlDgtFd570SKpTFKywQ1ph34ie1mVFvOBI7KndRgQtk1vcK2o5LGaLpZ8ZcR2XBKRPpKuyMtKdTviYzGJl/OTcbUDsxvLxf/89qp7R92My6T1KJk44f6qSBWkbwYEnGNzIqhI5Rp7nYlbEA1ZdbVV+4UwUwqOy5ugDRCPSoX5Rzl2Puq4i+52K4H+/XgbLfW2By3BCVYhypsQgAH0IATOIUmMLiFe3iAR+/Oe/KevZfx6IT3mVmDH/DePgBwWqYb</latexit>

X X X
<latexit sha1_base64="EeFO+nWrzOhju+/zckPqZS2ESkA=">AAACDXicbVDLSgMxFL3js9ZX1aWbwSK4KjPic1dw47KKrYW2lkzmtg3NJEOSEcrQb3DrVv/Bnbj1G/wFv8LMtIhaD4Qczr2HezhBzJk2nvfhzM0vLC4tF1aKq2vrG5ulre2GlomiWKeSS9UMiEbOBNYNMxybsUISBRxvg+FFNr+9R6WZFDdmFGMnIn3BeowSY6W7diB5qEeR/dLrcbdU9ipeDneW+FNSrhYgR61b+myHkiYRCkM50brle7HppEQZRjmOi+1EY0zokPSxZakgEepOmqceu/tWCd2eVPYJ4+bqT0dKIp1Fs5sRMQP9d5aJ/81aiemddVIm4sSgoJNDvYS7RrpZBW7IFFLDR5YQqpjN6tIBUYQaW1SxnRtTIU1eUXeAJEQ1LublnGc4/q5iljQOK/5Jxb86KlcPJi1BAXZhDw7Ah1OowiXUoA4UFDzCEzw7D86L8+q8TVbnnKlnB37Bef8CM1admg==</latexit>
X X
R X X
<latexit sha1_base64="5pOvkpgneeiBKR9i0RzzpX5NGsw=">AAACOXicbZDLSgMxFIbPeK31VnXpJlgEF1JmxEt1VXDjUsGq0Cklkzm1wUwyJBmhDH0VX8StW927dCdufQEzUxFvP4T8fOcccvJHqeDG+v6zNzE5NT0zW5mrzi8sLi3XVlYvjMo0wzZTQumriBoUXGLbcivwKtVIk0jgZXRzXNQvb1EbruS5HabYTei15H3OqHWoV2uGkRKxGSbuykMUYtQLtsl3mDpCjn6jnV6t7jf8UuSvCT5NvVWBUqe92lsYK5YlKC0T1JhO4Ke2m1NtORM4qoaZwZSyG3qNHWclTdB08/KHI7LpSEz6SrsjLSnp94mcJqZYznUm1A7M71oB/6t1MttvdnMu08yiZOOH+pkgVpEiLhJzjcyKoTOUae52JWxANWXWhVoNy8FcKjuOc4A0Rj2qluEcFtr7iuKvudhpBPuN4Gy33toapwQVWIcN2IIADqAFJ3AKbWBwBw/wCE/evffivXpv49YJ73NmDX7Ie/8ASA2vIw==</latexit> <latexit sha1_base64="HRY9zc3q4l4mezF5myv/mgWD46g=">AAACJ3icbVDLahsxFL2TpK3jvpx0mY2oafHKzDh9JStDN10mEDsGjxk0mmtbWCMNkqZghvmD/Ei22Sb/kF1ol93mK6IZm5LUPSB0OOcedHXiTHBjff+3t7W98+z5i8Zu8+Wr12/etvb2h0blmuGAKaH0KKYGBZc4sNwKHGUaaRoLPI8X3yv//Cdqw5U8s8sMJymdST7ljFonRa2PYaxEYpapu4oQhSijHjkmj9WsjA6jVtvv+jXIJgnWpN1vQI2TqHUfJorlKUrLBDVmHPiZnRRUW84Els0wN5hRtqAzHDsqaYpmUtT/KckHpyRkqrQ70pJafZwoaGqq5dxkSu3c/OtV4v+8cW6n3yYFl1luUbLVQ9NcEKtIVQ5JuEZmxdIRyjR3uxI2p5oy6ypshnWwkMquypsjTVCXzbqcowqf/1axSYa9bvClG5x+avc7q5agAQfwHjoQwFfoww84gQEwuIAruIYb79K79e68X6vRLW+deQdP4P15AGK/p5o=</latexit>
X X
`1 , p 1 : p 2 `2 : p 3 X
<latexit sha1_base64="KLbl9Kh0A6hkeMXj2W/hyqMU6OU=">AAACCXicbVDLSsNAFL2pr1pfVZdugkWom5KIz13BjcsK9oFNKJPpbTt0MhNmJmIJ/QK3bvUf3Ilbv8Jf8CtM0yJqPTBwOOce7p0TRJxp4zgfVm5hcWl5Jb9aWFvf2Nwqbu80tIwVxTqVXKpWQDRyJrBumOHYihSSMODYDIaXE795h0ozKW7MKEI/JH3BeowSk0q3XhjI+6QcHI47xZJTcTLY88SdkVI1DxlqneKn15U0DlEYyonWbdeJjJ8QZRjlOC54scaI0CHpYzulgoSo/SS7eGwfpErX7kmVPmHsTP2ZSEio9SgM0smQmIH+603E/7x2bHrnfsJEFBsUdLqoF3PbSHvyfbvLFFLDRykhVLH0VpsOiCLUpCUVvCyYCGmyejoDJF1U40JWzsUEJ99VzJPGUcU9rbjXx6VqedoS5GEP9qEMLpxBFa6gBnWgIOARnuDZerBerFfrbTqas2aZXfgF6/0LH3+bUg==</latexit> <latexit sha1_base64="cjuMfHo7e7VEIuLK4/uxtW98yAM=">AAACCXicbVDLSsNAFL2pr1pfVZdugkWom5KIz13BjcsK9oFNKJPpbTt0MhNmJmIJ/QK3bvUf3Ilbv8Jf8CtM0yJqPTBwOOce7p0TRJxp4zgfVm5hcWl5Jb9aWFvf2Nwqbu80tIwVxTqVXKpWQDRyJrBumOHYihSSMODYDIaXE795h0ozKW7MKEI/JH3BeowSk0q3XhjI+6RMD8edYsmpOBnseeLOSKmahwy1TvHT60oahygM5UTrtutExk+IMoxyHBe8WGNE6JD0sZ1SQULUfpJdPLYPUqVr96RKnzB2pv5MJCTUehQG6WRIzED/9Sbif147Nr1zP2Eiig0KOl3Ui7ltpD35vt1lCqnho5QQqlh6q00HRBFq0pIKXhZMhDRZPZ0Bki6qcSEr52KCk+8q5knjqOKeVtzr41K1PG0J8rAH+1AGF86gCldQgzpQEPAIT/BsPVgv1qv1Nh3NWbPMLvyC9f4FIR2bUw==</latexit>

(b) (c)

Figure 2.13 The Bayes tree (b) and the associated square root information matrix R (c)
describing the clique structure in the chordal Bayes net (a) based on our canonical example
from Figure 2.2. A Bayes tree is similar to a clique tree, but is better at capturing the
formal equivalence between sparse linear algebra and inference in graphical models. The
association of cliques with rows in the R factor is indicated by color.

For the root fr , the separator is empty, i.e., it is a simple prior p(fr ) on the root
variables. The way Bayes trees are defined, the separator sk for a clique ck is always
a subset of the parent clique ϖk , and hence the directed edges in the graph have
the same semantic meaning as in a Bayes net: conditioning.
The Bayes tree associated with our canonical toy SLAM problem (Figure 2.2)
is shown in Figure 2.13. The root clique c1 = p2 , p3 (shown in blue) comprises p2
and p3 , which intersects with two other cliques, c2 = ℓ1 , p1 : p2 shown in green,
and c3 = ℓ2 : p3 shown in red. The colors also indicate how the rows of square-
root information matrix R map to the different cliques, and how the Bayes tree
captures independence relationships between them. For example, the green and red
rows only intersect in variables that belong to the root clique, as predicted.

2.7.2 Updating the Bayes Tree

Incremental inference corresponds to a simple editing of the Bayes tree. This view
provides a better explanation and understanding of the otherwise abstract incre-
mental matrix factorization process. It also allows us to store and compute the
square-root information matrix in the form of a Bayes tree, a deeply meaningful
sparse storage scheme.
50 Factor Graphs for SLAM

x2 xp33
<latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

x p
p22, x
<latexit sha1_base64="CLFt0tlk4Wbsf86y1M06QVm0mWE=">AAACI3icbVDLSgMxFL3js9ZX1aWbYBVcSJmp713BjUsFq0JbSiZza4OZZEgyQhm690fcutV/cCduXPgDfoWZqYivAyGHc+4hNydMBDfW91+9sfGJyanp0kx5dm5+YbGytHxuVKoZNpkSSl+G1KDgEpuWW4GXiUYahwIvwuuj3L+4QW24kmd2kGAnpleS9zij1kndylo7VCIyg9hdWTLs1rfIL2W7W6n6Nb8A+UuCT1JtlKDASbfy3o4US2OUlglqTCvwE9vJqLacCRyW26nBhLJreoUtRyWN0XSy4i9DsuGUiPSUdkdaUqjfExmNTb6cm4yp7ZvfXi7+57VS2zvoZFwmqUXJRg/1UkGsInkxJOIamRUDRyjT3O1KWJ9qyqyrr9wugplUdlRcH2mEelguyjnMsftVxV9yXq8Fe7XgdKfaWB+1BCVYhTXYhAD2oQHHcAJNYHAL9/AAj96d9+Q9ey+j0THvM7MCP+C9fQBvJqYX</latexit>

p33

<latexit sha1_base64="aCwqU1X5t9v9Iu9WIjR4g88Wr3I=">AAACOHicbZDNahsxFIXvOGnjun+TdtmNiFvoopiZ0NZJV4ZuunSh/gGPMRrNtS2skQZJEzCDHyUvkm22yQNk111pl32CasamOHYOCB2+ey+6OnEmuLFBcOfVDg4fPT6qP2k8ffb8xUv/+FXfqFwz7DEllB7G1KDgEnuWW4HDTCNNY4GDePG1rA8uUBuu5A+7zHCc0pnkU86odWjit6NYicQsU3cVEQqxmoQftlnmAPlCdtDpxG8GraAS2TfhxjQ7dajUnfh/okSxPEVpmaDGjMIgs+OCasuZwFUjyg1mlC3oDEfOSpqiGRfVB1fknSMJmSrtjrSkotsTBU1NuZzrTKmdm91aCR+qjXI7PRsXXGa5RcnWD01zQawiZVok4RqZFUtnKNPc7UrYnGrKrMu0EVWDhVR2neYcaYJ61ajCOS/16X8U+6Z/2go/t8LvH5udt+uUoA5v4ATeQwht6MA36EIPGFzCNdzArXfl/fR+eb/XrTVvM/Ma7sn7+w/hg671</latexit> <latexit sha1_base64="V9AneALJl66wvKwGEg85HlDLfOk=">AAACJ3icbVDLThsxFL3DM4RHU1iysRpasYpmQqG0q0hsugSJAFImGnk8N8SKxx7ZHqRoNH/Aj3TLlv4DOwTLbvsV9UxQxetIlo/OuUe+PnEmuLG+/+jNzS8sLi03Vpqra+sbH1ofN8+MyjXDPlNC6YuYGhRcYt9yK/Ai00jTWOB5PDmq/PMr1IYreWqnGQ5Tein5iDNqnRS1voSxEomZpu4qQhSijLrkB3muZmW0F7XafsevQd6S4Im0ew2ocRy1/oaJYnmK0jJBjRkEfmaHBdWWM4FlM8wNZpRN6CUOHJU0RTMs6v+U5LNTEjJS2h1pSa0+TxQ0NdVybjKldmxee5X4njfI7ehwWHCZ5RYlmz00ygWxilTlkIRrZFZMHaFMc7crYWOqKbOuwmZYBwup7Ky8MdIEddmsy/leYf9/FW/JWbcTHHSCk6/t3s6sJWjANnyCXQjgG/TgJxxDHxhcww3cwm/vl3fn3XsPs9E57ymzBS/g/fkHYYunlg==</latexit>

l`11,,xp11 ::xp22 `l22 :: xp33

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit>

xp11
<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit>

`l11

x x
<latexit sha1_base64="i1LTsi0pXgXYdlkAuqdoH9DYknI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSiI+dwU3LivYWmhLmUxu7eBkJsxMCiX0K9y61X9wJ279An/Br3CSFlHrgWEO597DPZww4Uwb3/9w5uYXFpeWSyvl1bX1jU13a7upZaooNqjkUrVCopEzgQ3DDMdWopDEIcfb8P4yn98OUWkmxY0ZJdiNyZ1gfUaJsVLPdTuh5JEexfbLknHvqOdW/KpfwJslwZRUaiUoUO+5n51I0jRGYSgnWrcDPzHdjCjDKMdxuZNqTAi9J3fYtlSQGHU3K5KPvQOrRF5fKvuE8Qr1pyMjsc7D2c2YmIH+O8vF/2bt1PTPuxkTSWpQ0Mmhfso9I728Bi9iCqnhI0sIVcxm9eiAKEKNLavcKYyZkGZS0wBJhGpcLsq5yHHyXcUsaR5Vg9NqcH1cqe1PWoIS7MIeHEIAZ1CDK6hDAygM4RGe4Nl5cF6cV+dtsjrnTD078AvO+xcdQZ6K</latexit> <latexit sha1_base64="xa47ISr24vnFk6T52/jxFM/EsFA=">AAACEXicbVDLSsNAFL3xWesr6tJNsAquSuLbXcGNSwVbhbaUyeS2HTqZCTOTQgn9Crdu9R/ciVu/wF/wK5ykImo9MMzh3Hu4hxMmnGnj++/OzOzc/MJiaam8vLK6tu5ubDa0TBXFOpVcqruQaORMYN0ww/EuUUjikONtOLjI57dDVJpJcWNGCbZj0hOsyygxVuq4biuUPNKj2H5ZMu4cdtyKX/ULeNMk+CKVWgkKXHXcj1YkaRqjMJQTrZuBn5h2RpRhlOO43Eo1JoQOSA+blgoSo25nRfKxt2eVyOtKZZ8wXqH+dGQk1nk4uxkT09d/Z7n436yZmu5ZO2MiSQ0KOjnUTblnpJfX4EVMITV8ZAmhitmsHu0TRaixZZVbhTET0kxq6iOJUI3LRTnnOY6/q5gmjYNqcFINro8qtd1JS1CCbdiBfQjgFGpwCVdQBwpDeIBHeHLunWfnxXmdrM44X54t+AXn7RMe3Z6L</latexit>

p22 p33
x
p11,, xp22,,xp33
<latexit sha1_base64="/FH+AB/Sda4hg7dTSq71nIA7ll4=">AAACNHicbVDLSgMxFL3js9ZX1aWbYBVcSJnx7a7gxqWCVaEtJZO5tcFMMiQZoQz9EH/ErVv9BMGduHHhN5iZimj1QMjhnHvIzQkTwY31/WdvbHxicmq6NFOenZtfWKwsLV8YlWqGDaaE0lchNSi4xIblVuBVopHGocDL8OY49y9vURuu5LntJ9iO6bXkXc6odVKnstMKlYhMP3ZXlgw6wdaIsL1FRpSdTqXq1/wC5C8Jvki1XoICp53KeytSLI1RWiaoMc3AT2w7o9pyJnBQbqUGE8pu6DU2HZU0RtPOis8NyIZTItJV2h1pSaH+TGQ0NvlybjKmtmdGvVz8z2umtnvYzrhMUouSDR/qpoJYRfKmSMQ1Miv6jlCmuduVsB7VlFnXZ7lVBDOp7LDJHtII9aBclHOUY++7ir/kYrsW7NeCs91qfX3YEpRgFdZgEwI4gDqcwCk0gMEdPMAjPHn33ov36r0NR8e8r8wK/IL38QnXca14</latexit>

<latexit sha1_base64="kqDd8lhccdOlhjv6GQGtdYkQRGo=">AAACOXicbZDLSgMxFIbPeK31VnXpJlgFF1JmxPuq4MalglWhU0omc2qDmWRIMkIZ+iq+iFu3unfpTtz6AmamIt5+CPn5zjnk5I9SwY31/WdvbHxicmq6MlOdnZtfWKwtLV8YlWmGLaaE0lcRNSi4xJblVuBVqpEmkcDL6Oa4qF/eojZcyXM7SLGT0GvJe5xR61C3dhBGSsRmkLgrD1GIYTcgR+Q7TR3a+k22u7W63/BLkb8m+DT1ZgVKnXZrb2GsWJagtExQY9qBn9pOTrXlTOCwGmYGU8pu6DW2nZU0QdPJyx8OyYYjMekp7Y60pKTfJ3KamGI515lQ2ze/awX8r9bObO+gk3OZZhYlGz3UywSxihRxkZhrZFYMnKFMc7crYX2qKbMu1GpYDuZS2VGcfaQx6mG1DOew0O5XFH/NxXYj2GsEZzv15vooJajAKqzBJgSwD004gVNoAYM7eIBHePLuvRfv1XsbtY55nzMr8EPe+wdDPq8f</latexit> <latexit sha1_base64="V9AneALJl66wvKwGEg85HlDLfOk=">AAACJ3icbVDLThsxFL3DM4RHU1iysRpasYpmQqG0q0hsugSJAFImGnk8N8SKxx7ZHqRoNH/Aj3TLlv4DOwTLbvsV9UxQxetIlo/OuUe+PnEmuLG+/+jNzS8sLi03Vpqra+sbH1ofN8+MyjXDPlNC6YuYGhRcYt9yK/Ai00jTWOB5PDmq/PMr1IYreWqnGQ5Tein5iDNqnRS1voSxEomZpu4qQhSijLrkB3muZmW0F7XafsevQd6S4Im0ew2ocRy1/oaJYnmK0jJBjRkEfmaHBdWWM4FlM8wNZpRN6CUOHJU0RTMs6v+U5LNTEjJS2h1pSa0+TxQ0NdVybjKldmxee5X4njfI7ehwWHCZ5RYlmz00ygWxilTlkIRrZFZMHaFMc7crYWOqKbOuwmZYBwup7Ky8MdIEddmsy/leYf9/FW/JWbcTHHSCk6/t3s6sJWjANnyCXQjgG/TgJxxDHxhcww3cwm/vl3fn3XsPs9E57ymzBS/g/fkHYYunlg==</latexit>

`l11 :: xp11,,xp22 l`22 ::xp33

<latexit sha1_base64="Tbii+PtKXnyJ7OyYW5J5YpVAs/I=">AAACFHicbVDLSsNAFL2pr1pfVZduglVwVRLxuSu4calgW6EpZTK5tUMnM2FmIpTQ33DrVv/Bnbh17y/4FU7SImo9MMzh3Hu4hxMmnGnjeR9OaW5+YXGpvFxZWV1b36hubrW0TBXFJpVcqtuQaORMYNMww/E2UUjikGM7HF7k8/Y9Ks2kuDGjBLsxuROszygxVupVt4NQ8kiPYvtlAXI+7vm9as2rewXcWeJPSa1RhgJXvepnEEmaxigM5UTrju8lppsRZRjlOK4EqcaE0CG5w46lgsSou1kRfuzuWyVy+1LZJ4xbqD8dGYl1ns9uxsQM9N9ZLv4366Smf9bNmEhSg4JODvVT7hrp5k24EVNIDR9ZQqhiNqtLB0QRamxflaAwZkKaSVMDJBGqcaUo5zzH8XcVs6R1WPdP6v71Ua2xN2kJyrADu3AAPpxCAy7hCppAYQSP8ATPzoPz4rw6b5PVkjP1bMMvOO9ffl+f0A==</latexit>

x
<latexit sha1_base64="kXx44wnjqLe/qAbLMONIjGgtM5U=">AAACEXicbVDLSsNAFL2pr1pfUZduglVwVRLxuSu4calgW6EtZTK5tUMnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DKc3NLywulZcrK6tr6xvu5lZTy1RRbFDJpboNiUbOBDYMMxxvE4UkDjm2wuFFPm/do9JMihszSrAbkzvB+owSY6We63ZCySM9iu2XJeNe0HOrfs0v4M2SYEqq9TIUuOq5n51I0jRGYSgnWrcDPzHdjCjDKMdxpZNqTAgdkjtsWypIjLqbFcnH3r5VIq8vlX3CeIX605GRWOfh7GZMzED/neXif7N2avpn3YyJJDUo6ORQP+WekV5egxcxhdTwkSWEKmazenRAFKHGllXpFMZMSDOpaYAkQjWuFOWc5zj+rmKWNA9rwUktuD6q1vcmLUEZdmAXDiCAU6jDJVxBAyjcwyM8wbPz4Lw4r87bZLXkTD3b8AvO+xcbpZ6J</latexit>

l`11 p11

Figure 2.14 Updating a Bayes tree with a new factor, based on the example in Figure
2.13. The affected part of the Bayes tree is highlighted for the case of adding a new factor
between p1 and p3 . Note that the right branch (green) is not affected by the change. (top
right) The factor graph generated from the affected part of the tree with the new factor
(dashed blue) inserted. (bottom right) The chordal Bayes net resulting from eliminating
the factor graph. (bottom left) The Bayes tree created from the chordal Bayes net, with
the unmodified right ‘orphan’ sub-tree from the original Bayes tree added back in.

To incrementally update the Bayes tree, we selectively convert part of the Bayes
tree back into factor-graph form. When a new measurement is added this corre-
sponds to adding a factor, e.g., a measurement involving two variables will induce a
new binary factor ϕ(xj , xj ′ ). In this case, only the paths in the Bayes tree between
the cliques containing xj and xj ′ and the root will be affected. The sub-trees below
these cliques are unaffected, as are any other sub-trees not containing xj or xj ′ .
Hence, to update the Bayes tree, the affected parts of the tree are converted back
into a factor graph, and the new factor associated with the new measurement is
added to it. By re-eliminating this temporary factor graph, using whatever elimina-
tion ordering is convenient, a new Bayes tree is formed and the unaffected sub-trees
can be reattached.
In order to understand why only the top part of the tree is affected, we look at
two important properties of the Bayes tree. These directly arise from the fact that
it encodes the information flow during elimination. The Bayes tree is formed from
the chordal Bayes net following the inverse elimination order. In this way, variables
in each clique collect information from their child cliques via the elimination of
 2.7 Incremental SLAM 51

these children. Thus, information in any clique propagates only upwards to the
root. Second, the information from a factor enters elimination only when the first
variable connected to that factor is eliminated. Combining these two properties, we
see that a new factor cannot influence any other variables that are not successors
of the factor’s variables. However, a factor involving variables having different (i.e.,
independent) paths to the root means that these paths must now be re-eliminated
to express the new dependency between them.
Figure 2.14 shows how these incremental factorization/inference steps are applied
to our canonical SLAM example. In this example, we add a new factor between p1
and p3 , affecting only the left branch of the tree, marked by the red dashed line
in to top-left figure. We then create the factor graph shown in the top-right figure
by creating a factor for each of the clique densities, p(p2 , p3 ) and p(ℓ1 , p1 |p2 ),
and add the new factor f (p1 , p3 ). The bottom-right figure shows the eliminated
graph using the ordering ℓ1 , p1 , p2 , p3 . And finally, in the bottom-left figure, the
reassembled Bayes tree is shown consisting of two parts: the Bayes tree derived
from the eliminated graph, and the unaffected clique from the original Bayes tree
(shown in green).
Figure 2.15 shows an example of the Bayes tree for a small SLAM sequence.
Shown is the tree for step 400 of the well-known Manhattan world simulated se-
quence by Olson et al. [195]. As a robot explores the environment, new measure-
ments only affect parts of the tree, and only those parts are re-calculated.

2.7.3 Incremental Smoothing and Mapping

Putting all of the above together and addressing some practical considerations about
re-linearization yields a state-of-the-art incremental, nonlinear approach to MAP
estimation in robotics, iSAM. The first version, iSAM1[125], used the incremental
matrix factorization methods from Golub and Loan [101]. However, linearization in
iSAM1 was handled in a sub-optimal way: it was done for the full factor graph at
periodic instances and/or when matrix fill-in became unwieldy. The second version
of the approach, iSAM2, uses a Bayes tree representation for the posterior density
[127]. It then employs Bayes tree incremental updating as each new measurement
comes in, as described above.
What variable ordering should we use in re-eliminating the affected cliques? Only
the variables in the affected part of the Bayes tree are updated. One strategy then
is to apply COLAMD locally to the affected variables. However, we can do better:
we force recently accessed variables to the end of the ordering, i.e., into the root
clique. For this incremental variable ordering strategy one can use the constrained
COLAMD algorithm [64]. This both forces the most recently accessed variables to
the end and still provides a good overall ordering. Generally, subsequent updates
will then only affect a small part of the tree, and can therefore be expected to be
efficient in most cases, except for large loop closures.
52 Factor Graphs for SLAM
x398,x399,x400

x396,x397

x395 x394

x159 x387,x388,x389,x390,x391,x392,x393 x234

x386 x275,x276 x142 x358 x232

x384,x385 x274 x365 x356 x252 x352 x318 x231

x383 x288 x273 x233 x364 x355 x133 x139 x160 x138

x382 x289 x286 x290 x272 x270 x266 x363 x354 x141 x353

x381 x292 x281 x285 x271 x135 x269 x349,x350 x362 x357 x267 x301

x379,x380 x308 x291 x279 x280 x278 x284 x248,x249,x251,x257,x258,x259,x260,x261,x262,x265 x268 x348 x361 x360 x253 x250

x378 x303 x277 x245,x246,x247 x98 x128 x114 x104 x347 x344

x377 x309 x302 x296 x300 x244 x230 x97 x168 x146 x101 x113 x79 x345 x264 x343

x376 x311 x307 x304 x297 x295 x241,x242,x243 x206 x94 x167 x148 x171 x145 x99 x82 x112 x320 x263 x342

x374,x375 x316 x310 x306 x294 x238,x239,x240 x227,x228,x229 x205 x137 x89 x117 x80 x127 x77 x129 x106 x84 x111 x85 x341

x373 x338 x298 x315 x314 x305 x293 x237 x122 x208 x224 x218 x131 x91 x184 x81 x116 x78 x76 x105 x83 x110 x340 x321

x372 x346 x317 x299 x312 x313 x287 x236 x140 x121 x223 x209 x132 x217 x213 x130 x202 x96 x183 x115 x75 x103 x109 x339

x371 x283 x254 x235 x226 x222 x170 x136 x216 x172 x212 x126 x201 x95 x182 x74 x102 x108 x337 x322

x369 x282 x123 x225 x207 x221 x214 x100 x124,x199,x200 x92 x73 x107 x335,x336 x323

x368 x359 x256 x220 x161 x195,x196,x197 x120 x119 x90 x72 x88 x334

x367 x351 x255 x370 x219 x162 x194 x166 x215 x193 x118 x71 x87 x333

x366 x319 x210 x192 x165 x93 x70 x86 x332

x204 x157,x158 x191 x134 x69 x331

x203 x156 x150 x144 x190 x125 x67,x68 x330 x324

x164 x155 x151 x149 x189 x211 x65,x66 x329

x163 x154 x147 x143 x188 x173 x198 x52,x63,x64 x16 x15 x328 x325

x153 x187 x169 x51 x27 x61,x62 x327

x152 x186 x31 x22 x26 x60 x59 x55 x1 x326

x185 x28 x21 x17 x11 x58 x54 x25

x181 x30 x20 x10 x57 x50 x53 x24

x180 x29 x19 x9 x4 x56 x49 x48 x14 x23

x179 x18 x8 x3 x41 x47 x42 x13

x178 x0 x7 x2 x40 x46 x12

x177 x6 x39 x45

x176 x5 x38 x44

x175 x37 x43

x174 x36

x35

x34

x33

x32

Figure 2.15 An example of the Bayes tree data structure for a small SLAM sequence.
The incremental nonlinear least-squares estimation algorithm iSAM2 [127] is based on
viewing incremental factorization as editing the graphical model corresponding to the
posterior probability of the solution, the Bayes tree. As a robot explores the environment,
new measurements often only affect small parts of the tree, and only those parts are re-
calculated (shown in red).

After updating the tree we also need to update the solution. Back-substitution
in the Bayes tree proceeds from the root (which does not depend on any other
variables) and proceeds to the leaves. However, it is typically not necessary to
recompute a solution for all variables: local updates to the tree often do not affect
variables in remote parts of the tree. Instead, at each clique we can check the
difference in variable estimates that is propagated downwards and stop when this
difference falls below a small threshold.
Our motivation for introducing the Bayes tree was to incrementally solve nonlin-
ear optimization problems. For this we selectively re-linearize factors that contain
variables whose deviation from the linearization point exceeds a small threshold. In
contrast to the tree modification above, we now have to redo all cliques that contain
 2.7 Incremental SLAM 53

the affected variables, not just as frontal variables, but also as separator variables.
This affects larger parts of the tree, but in most cases is still significantly cheaper
than recomputing the complete tree. We also have to go back to the original factors,
instead of directly turning the cliques into a factor graph. And that requires caching
certain quantities during elimination. The overall incremental nonlinear algorithm,
iSAM2, is described in much more detail in [127].
iSAM1 and iSAM2 have been applied successfully to many different robotics
estimation problems with non-trivial constraints between variables that number
into the millions, as will be discussed subsequent chapters. Both are implemented in
the GTSAM library, which can be found at https://github.com/borglab/gtsam.
 3
Advanced State Variable Representations
Timothy Barfoot, Frank Dellaert, Michael Kaess

The previous chapter detailed how to set up and solve a SLAM problem using the
factor-graph paradigm. We deliberately avoided discussing some subtleties of the
state variables we were estimating. In this chapter, we revisit the nature of our
state variables and introduce two important topics that are prevalent in modern
SLAM formulations. First and foremost, we need some better tools for handling
state variables that have certain constraints associated with them; these constraints
define a manifold for our variables, which subsequently then require special care
during optimization. There are many examples of manifolds that appear in SLAM,
the most common being those associated with the rotational aspects of a robot
(especially in three dimensions, but even in the plane). A second aspect of state
variables stems from the nature of time itself. In the previous chapter, we implicitly
assumed that our robot moved in discrete-time steps through the world. In this
chapter we introduce smooth, continuous-time representations of trajectories and
discuss how these are fully compatible with our factor-graph formulation. We use
Barfoot [17] as the primary reference with some streamlined notation from Sola
et al. [240].

3.1 Optimization on Manifolds

While in some robotics problems we can get away with vector-valued unknowns,
in most practical situations we have to deal with three-dimensional rotations and
other non-vector manifolds. Loosely speaking, a manifold is collection of points
forming a topologically closed surface (e.g., the perimeter of a circle, or the surface
of a sphere); importantly, a manifold resembles Euclidean space locally near each
point. Manifolds require a more sophisticated machinery that takes into account
their special structure. In this section, we discuss how to perform optimization on
manifolds, which will build upon the optimization framework for vector spaces from
the previous chapter. As an example, Figure 3.1 visualizes a spherical manifold,
M, and its tangent space, Tχ M, which can be used as a local coordinate system
at χ ∈ M for optimization.
 3.1 Optimization on Manifolds 55

tangent space, Tχ M χ

manifold, M

Figure 3.1 For the sphere manifold, M, the local tangent plane, Tχ M, with a local basis
provides the notion of local coordinates.

3.1.1 Rotations and Poses

While there are several manifolds that can be discussed in the context of SLAM,
the two most common are those used to represent rotations and poses. Rotations
are typically either in two (planar) or three dimensions and we therefore refer to
the manifold of rotations as the special orthogonal group SO(d), where d = 2 or 3,
accordingly. A planar rotation matrix, Rab ∈ SO(2), has the form
 
cos θ − sin θ
Rab = , (3.1)
sin θ cos θ

where θ ∈ R, the angle of rotation, is the single degree of freedom in this case. More-
over, Rab allows us to rotate a two-dimensional vector (i.e., landmark) expressed in
reference frame F a to F b : ℓb = Rab ℓa .
A rotation matrix in three dimensions, Rab ∈ SO(3), again rotates vectors (this
time in three dimensions) from one frame to another. Three-dimensional rotation
matrices have nine entries but only three degrees of freedom (e.g., roll, pitch, yaw).
Both two- and three-dimensional rotation matrices must satisfy the constraints
Rab⊤ Rab = I and det(Rab ) = 1 to limit their degrees of freedom appropriately.
The pose of a robot comprises both rotational, Rab ∈ SO(d), and translational,
ta ∈ Rd , variables with 3(d − 1) degrees of freedom in all. Sometimes we keep
b

track of these quantities separately and then can use {Rab , tba } ∈ SO(d) × Rd as the
representation. Alternatively, these quantities can be assembled into a (d+1)×(d+1)
transformation matrix,
 b 
b Ra tba
Ta = . (3.2)
0 1

The manifold of all such transformation matrices is called the special Euclidean
group, SE(d), where again d = 2 (planar motion) or 3 (three-dimensional motion).
The benefit of using SE(d) is that we can easily translate and rotate landmarks
56 Advanced State Variable Representations

using a single matrix multiplication:

 b  b  
ℓ Ra tba ℓa
= . (3.3)
1 0 1 1
|{z} | {z } | {z }
ℓ̃b Tab ℓ̃a

We refer to ℓ̃ as the homogeneous representation of the landmark ℓ.

Due to the constraints imposed on the forms of rotation and transformation
matrices, they are unfortunately not vectors. For example, we cannot simply add
two rotation matrices together and arrive at another valid rotation matrix. However,
it turns out that SO(d) and SE(d) are examples of manifolds that possess some extra
useful properties called matrix Lie groups. Thankfully, we can exploit the structure
of these manifolds to continue to perform unconstrained MAP optimization for
factor-graph SLAM (see, for example, Dellaert et al. [69] or Boumal [36] or Barfoot
[17]).

3.1.2 Matrix Lie Groups

The key to performing optimization on SO(d) and SE(d) is to exploit their group
structure. For example, one nice property is that matrix Lie groups enjoy closure
so that if we multiply two members, e.g., Rbc , Rab ∈ SO(d), the result is also in the
group: Rac = Rbc Rab ∈ SO(d).
Another nice property of matrix Lie groups is that they come along with a very
useful companion structure called a Lie algebra, which is also the tangent space for
the Lie group. For our purposes, the most important aspects of the Lie algebra are
(i) that it comprises a vector space with dimension equal to the number of degrees
of freedom of its Lie group, and (ii) there is a well-established mapping (the matrix
exponential) from the Lie algebra to the Lie group. This allows us to construct
elements of the Lie group with relative ease from elements of the Lie algebra. For
example, for SO(2) we can build a rotation matrix (dropping super/subscripts for
now) according to

X∞  
∧ 1 ∧ n ∧ 0 −θ
R = Exp(θ) = exp(θ ) = (θ ) ∈ SO(2), θ = , θ ∈ R. (3.4)
n! θ 0
n=0

The quantity, θ∧ , is a member of the Lie algebra, so(2), and it is mapped through
the matrix exponential, exp(·), to a member of the Lie group, R. We can go the
∨
other way with the matrix logarithm: θ = Log(R) = (log(R)) .
∧
Each matrix Lie group has its own linear (·) operator used to construct a Lie al-
gebra member from the standard vector space of appropriate dimension. For SO(3),
 3.1 Optimization on Manifolds 57

it is the skew-symmetric operator:

∞
X 1 ∧n
R = Exp(θ) = exp(θ ∧ ) = θ ∈ SO(3), (3.5a)
n=0
n!
   
0 −θ3 θ2 θ1
θ ∧ =  θ3 0 −θ1  ∈ so(3), θ = θ2  ∈ R3 . (3.5b)
−θ2 θ1 0 θ3
For SE(d), we can use

T = Exp(ξ) = exp(ξ ∧ ) ∈ SE(d), (3.6a)

 ∧   
θ ρ ρ
ξ∧ = ∈ se(d), ξ = ∈ R3(d−1) , θ ∈ R2d−3 , ρ ∈ Rd , (3.6b)
0 0 θ
where d = 2 (planar) or 3 (three-dimensional). Note, the version of the (·)∧ operator
can be determined by the size of the input vector.
For each of the matrix Lie groups discussed here, there are also well-known closed-
form expressions for the mappings between the Lie algebra and the Lie group that
can be used rather than the infinite series form of the matrix exponential [17].

3.1.3 Lie Group Optimization

Now that we have these matrix Lie groups established, we can use them to help
‘linearize’ our nonlinear least-squares terms in order to carry out MAP inference.
Looking back to the discussion in Section 2.3.1, we still seek to linearize our mea-
surement functions, hi (·), only now the input to these may involve a member of a
Lie group.
For example, suppose hi (·) represents a camera model that takes as its input a
homogeneous landmark expressed in the camera frame, ℓ̃ci , and returns the pixel
coordinates of the landmark in an image, zi ∈ R2 : zi = hi (ℓ̃ci ). We can write the
generative sensor model therefore as
 
zi = hi Twc ℓ̃w
i + ηi , (3.7)

where Twc ∈ SE(3) is the pose of the camera with respect to a world frame, ℓ̃wi is
the homogeneous landmark expressed in the world frame, and ηi is the usual sensor
noise. We then might like to solve the optimization problem
∗ X   2
Twc = arg min = zi − hi Twc ℓ̃w
i , (3.8)
c
Tw Σi
i

which is known as the perspective-n-point (PNP) problem. For this example, we

assume that the positions of the landmarks in the world frame are known but of
course in SLAM we might like to estimate these as well.
58 Advanced State Variable Representations

To linearize our sensor model, we use the fact that we can produce a perturbed
version of our pose through its Lie algebra according to1
0
Twc = Twc Exp (ξw
c
). (3.9)
c
Here, ξw ∈ R6 is used to produce a ‘small’ pose change that perturbs an initial
c0
guess, Tw ∈ SE(3). This perturbation is also sometimes written succinctly using
the ⊕ operator so that
0
Twc = Twc ⊕ ξw
c
(3.10)

implies (3.9). Owing to the closure property discussed earlier, the product of these
two quantities is guaranteed to be in SE(3). By using the Lie algebra to define our
pose perturbation, we restrict its dimension to be equal to the actual number of
degrees of freedom in a three-dimensional pose, which will mean that we can avoid
introducing constraints during optimization.
We can also approximate the perturbed pose according to
0
 
c∧
Twc ≈ Twc I + ξw , (3.11)
c
where we have kept just the terms up to linear in ξw from the series form of the
matrix exponential. Then, inserting (3.11) into our measurement function (3.7), we
have
 0  
c∧
zi ≈ hi Twc I + ξw ℓ̃w
i + ηi . (3.12)

This can also be rewritten as

 0 
c0 w ⊙ c
zi ≈ hi Twc ℓ̃w
i + Tw iℓ̃ ξ w + ηi , (3.13)

where ⊙ is a (linear) operator for homogeneous points [17]:

 ⊙  
⊙ ℓ I −ℓ∧
ℓ̃ = = . (3.14)
1 0 0

We have essentially ‘linearized’ the pose perturbation in (3.13) and now need to
linearize the camera function hi (·) as well. We can use a standard first-order Taylor
series approximation to write
 0  ∂h 0 ⊙
i
zi ≈ hi Twc ℓ̃wi + c
T c ℓ̃w ξw + ηi , (3.15)
∂ℓ T c0 ℓ̃w w i
w
| {z i
}
Hi (chain rule)
where the chaining of two pieces into the overall Jacobian, Hi , is now clear. Looking
1
It is also possible to perturb on the left side rather than the right. A more subtle question is whether
the perturbation is happening on the ‘sensor’ side or the ‘world’ side, which depends on whether the
c
unknown transform is Tw or Tcw and whether the perturbation is applied to the left or the right.
 3.1 Optimization on Manifolds 59

back to (2.21b), we can write the linearized least-squares term (i.e., negative-log
factor) for this measurement as
  0  2
zi − hi Twc ℓ̃w
i
c
− Hi ξw , (3.16)
Σi

c
where the only unknown is our pose perturbation, ξw . After combining this with
other factors and then solving for the optimal updates to our state variables, in-
c∗ 0
cluding ξw , we need to update our initial guess, Twc . For this, we must return to
the perturbation scheme we chose in (3.9) and update according to
0 0 ∗
Twc ← Twc ⊕ ξw
c
, (3.17)
0
to ensure our solution, Twc , remains in SE(3). As usual, optimization proceeds itera-
c
tively until the change in all the state variable updates (including ξw ) is sufficiently
small.
To recap, we have shown how to carry out unconstrained optimization for a state
variable that is a member of a Lie group. Although our example was specific to
a three-dimensional pose variable, other Lie groups can be optimized in a similar
manner. The key is to arrive at a situation as in (3.15) where the measurement
function has been linearized with respect to a perturbation in the Lie algebra. Most
times, as in our example, this can be done analytically. However, it is also straight-
forward to compute the required Jacobian, Hi , numerically or through automatic
differentiation (by exploiting the chain rule and some primitives for Lie groups).
In (3.9), we perturbed our pose variable on the left side, but this was a choice and
in some cases perturbing on the right may be preferable.
Stepping back a bit, this approach to optimizing a function of a Lie group mem-
ber is an example of Riemannian optimization [36]. By exploiting the Lie algebra,
which is also the tangent space of a manifold, we constrain the optimization to be
tangent to the manifold of poses (or rotations). By carrying out the update accord-
ing to (3.17), we are retracting our update back onto the manifold. Riemannian
optimization is a very general concept that can be applied to quantities that live
on manifolds that are not matrix Lie groups as well. Retractions other than the
matrix exponential are also possible within the manifold-optimization framework
(e.g., see Dellaert et al. [69] or Barfoot et al. [18]).

3.1.4 Lie Group Extras

There is a lot more that we could say about Lie groups [243, 36] but have so far
restrained ourselves in the interest of keeping things simple. We use this section
to collect a few more useful facts that come up later in this and other chapters.
Further details of carrying out derivatives of functions of Lie group elements will
be provided in Section 5.3.
60 Advanced State Variable Representations

3.1.4.1 The ⊕ and ⊖ Operators

We have already seen the use of the ⊕ operator to compose a Lie algebra vector
with a Lie group member. For SE(d) we have

T = T 0 ⊕ ξ = T 0 Exp(ξ) = T 0 exp(ξ ∧ ) ∈ SE(d). (3.18)

We often have occasion to consider the ‘difference’ of two Lie group elements and
for this we can also define the ⊖ operator. Again for SE(d) we have


∨
ξ = T 0 ⊖ T = Log T 0 T −1 = log T 0 T −1 ∈ se(d). (3.19)

These operators are a nice way to abstract away the details of these operations.

3.1.4.2 Inverses
Sometimes when we are carrying out perturbations, we have need to perturb the
inverse of a rotation or transformation matrix. In the SE(d) case, we simply have
that

−1 −1 −1
T 0 Exp(ξ) = Exp(ξ)−1 T 0 = Exp(−ξ)T 0 , (3.20)

where we see the perturbation moves from the right to the left with a negative sign.

3.1.4.3 Adjoints
The adjoint of a Lie group is a way of describing the elements of that group as linear
transformations of its Lie algebra, which we recall is a vector space. For SO(d), the
adjoint representation is the same as the group itself, so we omit the details. For
SE(d), the adjoint differs from the group’s primary representation and so we use
this section to provide some details. The adjoint will prove to be an essential tool
when setting up state estimation problems, particularly for SE(d).
The adjoint map of SE(d) transforms a Lie algebra element ξ ∧ ∈ se(d) to an-
other element of se(d) according to a map known as the inner automorphism or
conjugation:
AdT ξ ∧ = T ξ∧ T −1 . (3.21)

We can equivalently express the output of this map as

∧
AdT ξ ∧ = (Ad(T )ξ) , (3.22)

where Ad(T ) linearly transforms ξ ∈ R6 to R6 . We will refer to Ad(T ) as the

adjoint representation of SE(d).
The (2d) × (2d) transformation matrix, Ad(T ), can be constructed directly from
the components of the (d + 1) × (d + 1) transformation matrix:
   
R t R t∧ R
Ad(T ) = Ad = . (3.23)
0 1 0 R
 3.2 Continuous-Time Trajectories 61

One situation in which adjoints are useful in our estimation problems is to manip-
ulate perturbations from one side of a known transformation to another as in

T Exp(ξ) = Exp (Ad(T )ξ) T , (3.24)

which we emphasize does not require approximation.

3.1.4.4 Jacobians
Every Lie group also has a Jacobian associated with it, which allows us to relate
changes in an element of the group to elements of its algebra. For the case of SO(d),
for example, the common kinematic equation (i.e., Poisson’s equation) relating a
rotation matrix, R ∈ SO(d), to angular velocity, ω ∈ R3(d−1) , is

Ṙ = ω ∧ R. (3.25)

If we parameterize R = Exp(θ), then we can equivalently write

θ̇ = J −1 (θ) ω, (3.26)

where J (θ) is the (left) Jacobian of SO(d). A place where this Jacobian is quite
useful is when combining expressions involving products of matrix exponentials.
For example, we have that


Exp(θ1 )Exp(θ2 ) ≈ Exp θ2 + J (θ2 )−1 θ1 , (3.27)

where θ1 is assumed to be ‘small’. The series expression for J (θ) is

∞
X 1 n
J (θ) = (θ ∧ ) , (3.28)
n=0
(n + 1)!

and a closed-form expression can be found in Barfoot [17]. We will overload and
write J (ξ) for the (left) Jacobian of SE(d) where the context should inform which
is meant.

3.2 Continuous-Time Trajectories

Continuous-time trajectories offer a way to represent smooth robot motions. In our
development so far, we have assumed a discrete sequence of poses along a trajec-
tory is to be estimated. However, robots typically move fairly smoothly through
the world, which motivates the use of a smoother representation of trajectory.
Continuous-time trajectories come primarily in two varieties: parametric methods
combine known temporal basis functions into a smooth trajectory. Typically, these
temporal basis functions are chosen to have local support (e.g., piecewise poly-
nomials / splines), which ensures the factor graph remains sparse, as we will see.
Nonparametric methods have higher representational power by making use of kernel
functions. Specifically, a one-dimensional Gaussian process (GP) with time as the
62 Advanced State Variable Representations

independent variable can be used to represent a trajectory. When an appropriate

physically motivated kernel is chosen, we will see that the factor graph associated
with a GP also remains very sparse.
In addition to trajectory smoothness, the use of a continuous-time trajectory can
be particularly useful when working with high-rate and/or asynchronous sensors.
In the factor-graph examples that we have considered so far, we added robot poses
to the factor graph for each newly collected measurement (e.g., to model that the
current pose is taking a landmark measurement). This quickly leads to unwieldy
factor graphs when using high-rate sensors or when different sensors collect measure-
ments at different time instants. Below, we will see that we can easily represent the
trajectory with a number of variables that is much smaller than the number of mea-
surements, to keep things tractable. This is particularly useful for motion-distorted
sensors such as spinning lidars and radars and even rolling-shutter cameras; using
continuous-time trajectories we can account for the exact time stamp of each point
or pixel and relate them to the trajectory at that instant.
Finally, after MAP inference, continuous-time trajectories allow us to efficiently
query the trajectory at any time of interest, not just at the measurement times.
We can both interpolate and extrapolate (with caution), which can be useful for
consumers of our SLAM outputs. Separating the roles of measurements times, es-
timation variables, and query times, is a major advantage of both parametric and
nonparametric continuous-time methods.

3.2.1 Splines
The idea with parametric continuous-time trajectory methods is to write the pose
as a weighted sum of K known temporal basis functions, Ψk (t):
K
X
p(t) = Ψk (t)ck , (3.29)
k=1

where the ck are the unknown coefficients. For now, we return to a vector-space
explanation and discuss implementation on Lie groups in a later section. The basis
functions are typically chosen to be splines, which are piecewise polynomials (e.g.,
B-splines, cubic Hermite polynomials); splines are advantageous because they have
local support meaning outside of their local region of influence they go to zero. The
setup is depicted in Figure 3.2. In this example, at each instant of time only four
basis functions are nonzero, which we see results in a sparse factor graph.
The main difference, as compared to our earlier discrete-time development, is that
we have coefficient variables instead of pose variables, but this is completely com-
patible with the general factor-graph approach. Now, when we observe a landmark,
ℓ, at a particular time, ti , the sensor model is
zi = hi (p(ti ), ℓ) + ηi . (3.30)
 3.2 Continuous-Time Trajectories 63
<latexit sha1_base64="XTJsUXJ6OIogftiDvr1VNx0f0OY=">AAACIHicbVBNTxsxFHwLpYRA2xSOHGo1QuIU7VZtgVskLj1SqSGRkijyOm+JhT9W9ltEtMqxf4QrV/gP3CqO8BP6K+rdoKotHcnSaObNsz1prqSnOH6IVlZfrL1cb2w0N7devX7Tert96m3hBPaEVdYNUu5RSYM9kqRwkDvkOlXYT8+PK79/gc5La77RPMex5mdGZlJwCtKk9W6kU3tZEurcOq5Y2CU9ywojKt8vJq123IlrsOckeSLtbgNqnExaP0dTKwqNhoTi3g+TOKdxyR1JoXDRHBUecy7O+RkOAzVcox+X9UcWbC8oU5ZZF44hVqt/JkquvZ/rNExqTjP/r1eJ//OGBWWH41KavCA0YnlRVihGllWtsKl0KEjNA+HCyfBWJmbccUGhu+aoDpbG0rK1GfIpukWzLueowqffVTwnpx86yedO8vVju7u/bAkasAvvYR8SOIAufIET6IGA73ANN3AbXUV30Y/ofjm6Ej1lduAvRI+/AJXXpTE=</latexit>

temporal basis functions

<latexit sha1_base64="wx6YX6DcsfJx0QiKQUQeaqpzzcU=">AAACF3icbVDLattAFL1y09ZxX04L2WQjagruxkilj2Rn6KZLB+LYYBkzGl3bg0czYuaqYBR9SLbZtv/QXcg2y/xCviIjOYQ27oFhDufewz2cOJPCUhDceI0nO0+fPW/utl68fPX6TXvv7anVueE45FpqM46ZRSkUDkmQxHFmkKWxxFG8+l7NRz/RWKHVCa0znKZsocRccEZOmrX3o1jLxK5T9xXRwIpyFnbp46zdCXpBDX+bhPek029CjcGsfRslmucpKuKSWTsJg4ymBTMkuMSyFeUWM8ZXbIETRxVL0U6LOn/pf3BK4s+1cU+RX6t/OwqW2iqi20wZLe3jWSX+bzbJaX44LYTKckLFN4fmufRJ+1UZfiIMcpJrRxg3wmX1+ZIZxslV1opqY6E0bcpaIkvQlK26nKMKXx6q2Cann3rh1154/LnT725agiYcwHvoQgjfoA8/YABD4HAGF/ALfnvn3h/v0rvarDa8e887+Afe9R0oe6Cm</latexit> <latexit sha1_base64="pZPfDcjdevpQkfzGYh4oXzbFnsY=">AAACF3icbVDBattAFHxKm9Rx0tRtoJdeREzAvRjJpE17M+SSowtxYrCMWa2e7cWrXbH7FDCqPqTXXtt/yK302mN/IV+RlWxKW2dg2WHeG94wcSaFpSD47e08ebq796yx3zw4fH70ovXy1bXVueE45FpqM4qZRSkUDkmQxFFmkKWxxJt4eVHNb27RWKHVFa0ynKRsrsRMcEZOmrZeR7GWiV2l7iuigRXltNeht9NWO+gGNfxtEm5Iu9+AGoNp6z5KNM9TVMQls3YcBhlNCmZIcIllM8otZowv2RzHjiqWop0Udf7SP3VK4s+0cU+RX6t/OwqW2iqi20wZLez/s0p8bDbOafZhUgiV5YSKrw/NcumT9qsy/EQY5CRXjjBuhMvq8wUzjJOrrBnVxkJpWpe1QJagKZt1OR8rvPtTxTa57nXD993w01m731m3BA14AyfQgRDOoQ+XMIAhcPgMX+EbfPe+eHfeD+/nenXH23iO4R94vx4AKhqgpw==</latexit> <latexit sha1_base64="wq9JxWakw0cA9rTku+a/3aH4hhY=">AAACF3icbVDLattAFL1KH3Hcl9NCN9mImoK7MVKaR7szdNOlA/EDLGNGo2t78GhGzFwFjKIP6bbb9h+6K9lm2V/oV2Qkh9DEPTDM4dx7uIcTZ1JYCoI/3s6jx0+e7jb2ms+ev3j5qrX/emh1bjgOuJbajGNmUQqFAxIkcZwZZGkscRSvvlTz0QUaK7Q6p3WG05QtlJgLzshJs9bbKNYysevUfUXUt6KcfezQh1mrHXSDGv42CW9Ju9eAGv1Z62+UaJ6nqIhLZu0kDDKaFsyQ4BLLZpRbzBhfsQVOHFUsRTst6vyl/94piT/Xxj1Ffq3+6yhYaquIbjNltLQPZ5X4v9kkp/mnaSFUlhMqvjk0z6VP2q/K8BNhkJNcO8K4ES6rz5fMME6usmZUGwulaVPWElmCpmzW5XyucHxXxTYZHnbDk254dtTudTYtQQMO4B10IIRT6MFX6MMAOFzCd/gBP71v3i/vt3e1Wd3xbj1v4B686xsruaCo</latexit> <latexit sha1_base64="vSjkAfiHeKxO1J+wMnHlDBt9l2o=">AAACF3icbVDBattAFHxKk9R10tRtIZdcREzAvRgpuE17M/TSowN1YrCMWa2e7cWrXbH7FDCqPqTXXtt/6C3k2mN/oV/RlWxCG2dg2WHeG94wcSaFpSD47e082d3bf9p41jw4fH70ovXy1ZXVueE45FpqM4qZRSkUDkmQxFFmkKWxxOt4+bGaX9+gsUKrz7TKcJKyuRIzwRk5ado6jmItE7tK3VdEAyvKaa9Db6atdtANavjbJNyQdr8BNQbT1p8o0TxPURGXzNpxGGQ0KZghwSWWzSi3mDG+ZHMcO6pYinZS1PlL/8wpiT/Txj1Ffq3+6yhYaquIbjNltLAPZ5X42Gyc0+z9pBAqywkVXx+a5dIn7Vdl+IkwyEmuHGHcCJfV5wtmGCdXWTOqjYXStC5rgSxBUzbrcj5UeHtfxTa5Ou+G77rhZa/d76xbggacwCl0IIQL6MMnGMAQOHyBb/AdfnhfvZ/erXe3Xt3xNp7X8B+8X38BLVigqQ==</latexit> <latexit sha1_base64="OfRZ0QGgO6tIIA6xblkEugt9zRc=">AAACF3icbVDBattAFHxKm9Rx0tRtoJdeREzAvRgp1E17M+SSowtxYrCMWa2e7cWrXbH7FDCqPqTXXtt/yK302mN/IV+RlWxKW2dg2WHeG94wcSaFpSD47e08ebq796yx3zw4fH70ovXy1bXVueE45FpqM4qZRSkUDkmQxFFmkKWxxJt4eVHNb27RWKHVFa0ynKRsrsRMcEZOmrZeR7GWiV2l7iuigRXltNeht9NWO+gGNfxtEm5Iu9+AGoNp6z5KNM9TVMQls3YcBhlNCmZIcIllM8otZowv2RzHjiqWop0Udf7SP3VK4s+0cU+RX6t/OwqW2iqi20wZLez/s0p8bDbOafZhUgiV5YSKrw/NcumT9qsy/EQY5CRXjjBuhMvq8wUzjJOrrBnVxkJpWpe1QJagKZt1OR8r9P5UsU2uz7rh+2746V2731m3BA14AyfQgRDOoQ+XMIAhcPgMX+EbfPe+eHfeD+/nenXH23iO4R94vx4ALvegqg==</latexit> <latexit sha1_base64="515z/8wJe+FsNwNk2SQEI8FOiag=">AAACF3icbVDBattAFHxKm9Rx0lRtoJdeREzAvRgptE57M+SSowtxYrCMWa2e7cWrXbH7FDCqP6TXXtt/yK302mN/IV+RlWxKW2dg2WHeG94wSS6FpTD87e08ebq796yx3zw4fH70wn/56trqwnAccC21GSbMohQKByRI4jA3yLJE4k2yuKjmN7dorNDqipY5jjM2U2IqOCMnTfzXcaJlapeZ+8q4b8Vq0m3T24nfCjthjWCbRBvS6jWgRn/i38ep5kWGirhk1o6iMKdxyQwJLnHVjAuLOeMLNsORo4plaMdlnX8VnDolDabauKcoqNW/HSXLbBXRbWaM5vb/WSU+NhsVNP0wLoXKC0LF14emhQxIB1UZQSoMcpJLRxg3wmUN+JwZxslV1oxrY6k0rcuaI0vRrJp1OR8rvP9TxTa5PutE3U706V2r1163BA14AyfQhgjOoQeX0IcBcPgMX+EbfPe+eHfeD+/nenXH23iO4R94vx4AMJagqw==</latexit> <latexit sha1_base64="pDqXccyKUN3WHWF/d08WTa8GAV4=">AAACF3icbVDBattAFHxKm8Rx0tRtoJdeREzAvRgptHV7M+SSowtxYrCMWa2e7cWrXbH7FDCqP6TXXtt/yK302mN/IV+RlWRKW2dg2WHeG94wcSaFpSD47e08ebq7t984aB4ePTt+3nrx8trq3HAcci21GcXMohQKhyRI4igzyNJY4k28vCjnN7dorNDqilYZTlI2V2ImOCMnTVuvoljLxK5S9xXRwIr1tNehN9NWO+gGFfxtEm5Iu9+ACoNp6z5KNM9TVMQls3YcBhlNCmZIcInrZpRbzBhfsjmOHVUsRTspqvxr/8wpiT/Txj1FfqX+7ShYasuIbjNltLD/z0rxsdk4p9mHSSFUlhMqXh+a5dIn7Zdl+IkwyEmuHGHcCJfV5wtmGCdXWTOqjIXSVJe1QJagWTercj6WePenim1yfd4N33fDT2/b/U7dEjTgNZxCB0LoQR8uYQBD4PAZvsI3+O598e68H97PenXH23hO4B94vx4AMjWgrA==</latexit>

1 (t) 2 (t) 3 (t) 4 (t) 5 (t) 6 (t) 7 (t)

<latexit sha1_base64="jsXKZYtIc+Vmx4FxXXOMdbKIcaU=">AAACFHicbZDLSgMxFIbPeK31Vu3SzWARuioz4nVXcOOygr1AW0omPdOGZpIhyYhl6Gu4davv4E7cuvcVfArTaRG1/hD4+c/5yeELYs608bwPZ2l5ZXVtPbeR39za3tkt7O03tEwUxTqVXKpWQDRyJrBumOHYihWSKODYDEZX03nzDpVmUtyacYzdiAwECxklxka9QrETBfI+pRJDGzIURk96hZJX8TK5i8afm1I1B5lqvcJnpy9pEtk25UTrtu/FppsSZRjlOMl3Eo0xoSMywLa1gkSou2l2/MQ9sknfDaWyTxg3S382UhJpPY4CuxkRM9R/Z9Pwv1k7MeFFN2UiTgwKOvsoTLhrpDsl4faZQmr42BpCFbO3unRIFKHG8sp3smIqpJmRGiLpo5rkMziXU51+o1g0jeOKf1bxb05K1fKMEuTgAA6hDD6cQxWuoQZ1oDCGR3iCZ+fBeXFenbfZ6pIz7xThl5z3L/NioBw=</latexit>

coefficients
c3 c6
<latexit sha1_base64="tuU/1EGVihSvBpdusa7FGcACNxc=">AAACEXicbVDLSsNAFL2pr1pfUZdugkXoqiTic1dw41LBtkJbymRy2w6dzISZSaGEfoVbt/oP7sStX+Av+BVOUhFfB4Y5nHsP93DChDNtfP/NKS0sLi2vlFcra+sbm1vu9k5Ly1RRbFLJpboNiUbOBDYNMxxvE4UkDjm2w/FFPm9PUGkmxY2ZJtiLyVCwAaPEWKnvut1Q8khPY/tldNYP+m7Vr/sFvL8k+CTVRhkKXPXd924kaRqjMJQTrTuBn5heRpRhlOOs0k01JoSOyRA7lgoSo+5lRfKZd2CVyBtIZZ8wXqF+d2Qk1nk4uxkTM9K/Z7n436yTmsFZL2MiSQ0KOj80SLlnpJfX4EVMITV8agmhitmsHh0RRaixZVW6hTET0sxrGiGJUM0qRTnnOY6/qvhLWof14KQeXB9VG7V5S1CGPdiHGgRwCg24hCtoAoUJ3MMDPDp3zpPz7LzMV0vOp2cXfsB5/QAHxp6A</latexit> <latexit sha1_base64="NpdHZvDEltGvU5fK1Dvc5QSXj7w=">AAACEXicbVDLSsNAFL3xWesr6tJNsAhdlaT43BXcuKxgVWhLmUxu26GTmTAzKZTQr3DrVv/Bnbj1C/wFv8JJWsTXgWEO597DPZww4Uwb3393FhaXlldWS2vl9Y3NrW13Z/dGy1RRbFHJpboLiUbOBLYMMxzvEoUkDjnehqOLfH47RqWZFNdmkmA3JgPB+owSY6We63ZCySM9ie2X0Wmv3nMrfs0v4P0lwZxUGiUo0Oy5H51I0jRGYSgnWrcDPzHdjCjDKMdpuZNqTAgdkQG2LRUkRt3NiuRT79AqkdeXyj5hvEL97shIrPNwdjMmZqh/z3Lxv1k7Nf2zbsZEkhoUdHaon3LPSC+vwYuYQmr4xBJCFbNZPTokilBjyyp3CmMmpJnVNEQSoZqWi3LOcxx/VfGX3NRrwUktuDqqNKqzlqAE+3AAVQjgFBpwCU1oAYUxPMAjPDn3zrPz4rzOVhecuWcPfsB5+wQJYp6B</latexit> <latexit sha1_base64="ioXcenwf52WQczvpo6TMOPHKxkE=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSuLbXcGNSwVbC20pk8ltO3QyE2YmQgn9Crdu9R/ciVu/wF/wK5ykRdR6YJjDufdwDydMONPG9z+cufmFxaXl0kp5dW19Y9Pd2m5omSqKdSq5VM2QaORMYN0ww7GZKCRxyPEuHF7m87t7VJpJcWtGCXZi0hesxygxVuq6bjuUPNKj2H4ZHXePum7Fr/oFvFkSTEmlVoIC1133sx1JmsYoDOVE61bgJ6aTEWUY5Tgut1ONCaFD0seWpYLEqDtZkXzs7Vsl8npS2SeMV6g/HRmJdR7ObsbEDPTfWS7+N2ulpnfeyZhIUoOCTg71Uu4Z6eU1eBFTSA0fWUKoYjarRwdEEWpsWeV2YcyENJOaBkgiVONyUc5FjpPvKmZJ47AanFaDm+NK7WDSEpRgF/bgAAI4gxpcwTXUgcI9PMITPDsPzovz6rxNVuecqWcHfsF5/wIK/p6C</latexit> <latexit sha1_base64="+IGtFwFfOOatIEKmBPtDFR/HQh8=">AAACEXicbVDLSsNAFL2pr1pfUZdugkXoqiTic1dw47KCVaEtZTK5bYdOZsLMpFBCv8KtW/0Hd+LWL/AX/AonaRFfB4Y5nHsP93DChDNtfP/dKS0sLi2vlFcra+sbm1vu9s6Nlqmi2KKSS3UXEo2cCWwZZjjeJQpJHHK8DUcX+fx2jEozKa7NJMFuTAaC9Rklxko91+2Ekkd6Etsvo9PeUc+t+nW/gPeXBHNSbZShQLPnfnQiSdMYhaGcaN0O/MR0M6IMoxynlU6qMSF0RAbYtlSQGHU3K5JPvQOrRF5fKvuE8Qr1uyMjsc7D2c2YmKH+PcvF/2bt1PTPuhkTSWpQ0Nmhfso9I728Bi9iCqnhE0sIVcxm9eiQKEKNLavSKYyZkGZW0xBJhGpaKco5z3H8VcVfcnNYD07qwdVRtVGbtQRl2IN9qEEAp9CAS2hCCyiM4QEe4cm5d56dF+d1tlpy5p5d+AHn7RMMmp6D</latexit> <latexit sha1_base64="Do1v9y9uiRwBh5XBke3erL0vjjI=">AAACEXicbVDLSsNAFL3xWesr6tJNsAhdlUS06q7gxqWCfUBbymRyawcnM2FmIpTQr3DrVv/Bnbj1C/wFv8JJWkStB4Y5nHsP93DChDNtfP/DWVhcWl5ZLa2V1zc2t7bdnd2Wlqmi2KSSS9UJiUbOBDYNMxw7iUIShxzb4d1FPm/fo9JMihszTrAfk1vBhowSY6WB6/ZCySM9ju2X0cmgPnArfs0v4M2TYEYqjRIUuBq4n71I0jRGYSgnWncDPzH9jCjDKMdJuZdqTAi9I7fYtVSQGHU/K5JPvEOrRN5QKvuE8Qr1pyMjsc7D2c2YmJH+O8vF/2bd1AzP+hkTSWpQ0OmhYco9I728Bi9iCqnhY0sIVcxm9eiIKEKNLavcK4yZkGZa0whJhGpSLso5z3HyXcU8aR3VgnotuD6uNKrTlqAE+3AAVQjgFBpwCVfQBAr38AhP8Ow8OC/Oq/M2XV1wZp49+AXn/QsP0p6F</latexit>

c1 c2 c4 c5 c7
<latexit sha1_base64="SdrZV5z20U8l5E8YYFoSC6A1+Jo=">AAACEXicbVDLSsNAFL3xWesr6tJNsAhdlUSsj13BjUsF+4C2lMnk1g5OZsLMRCihX+HWrf6DO3HrF/gLfoWTtIhaDwxzOPce7uGECWfa+P6Hs7C4tLyyWlorr29sbm27O7stLVNFsUkll6oTEo2cCWwaZjh2EoUkDjm2w7uLfN6+R6WZFDdmnGA/JreCDRklxkoD1+2Fkkd6HNsvo5NBfeBW/JpfwJsnwYxUGiUocDVwP3uRpGmMwlBOtO4GfmL6GVGGUY6Tci/VmBB6R26xa6kgMep+ViSfeIdWibyhVPYJ4xXqT0dGYp2Hs5sxMSP9d5aL/826qRme9TMmktSgoNNDw5R7Rnp5DV7EFFLDx5YQqpjN6tERUYQaW1a5VxgzIc20phGSCNWkXJRznqP+XcU8aR3VgpNacH1caVSnLUEJ9uEAqhDAKTTgEq6gCRTu4RGe4Nl5cF6cV+dturrgzDx78AvO+xcONp6E</latexit> <latexit sha1_base64="/aq88fN9fvxSRb/ETVIf9Ot3uBM=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSiJqdVdw47KCrUJbymRy2w6dzISZSaGEfoVbt/oP7sStX+Av+BVO0iK+DgxzOPce7uGECWfa+P67s7C4tLyyWlorr29sbm27O7stLVNFsUkll+ouJBo5E9g0zHC8SxSSOOR4G44u8/ntGJVmUtyYSYLdmAwE6zNKjJV6rtsJJY/0JLZfRqe9Ws+t+FW/gPeXBHNSqZegQKPnfnQiSdMYhaGcaN0O/MR0M6IMoxyn5U6qMSF0RAbYtlSQGHU3K5JPvUOrRF5fKvuE8Qr1uyMjsc7D2c2YmKH+PcvF/2bt1PTPuxkTSWpQ0Nmhfso9I728Bi9iCqnhE0sIVcxm9eiQKEKNLavcKYyZkGZW0xBJhGpaLsq5yHH6VcVf0jquBmfV4PqkUj+atQQl2IcDOIIAalCHK2hAEyiM4QEe4cm5d56dF+d1trrgzD178APO2ycRbp6G</latexit>

<latexit sha1_base64="B1c/MS3DPBeOl7HyBEK6cDUhha8=">AAACEnicbVDLattAFL1y08ZVmlZul92ImICzMVLoc2fopksX6gfYxoxG1/bg0YyYuWowwn/RbbftP3RXss0P9Bf6FR3JJqR1DgxzOPce7uEkuRSWoui313hw9PDRcfOxf/Lk9OmzoPV8aHVhOA64ltqME2ZRCoUDEiRxnBtkWSJxlKw/VPPRFzRWaPWZNjnOMrZUYiE4IyfNg9Y00TK1m8x9Zb7t0MU8aEfdqEZ4SOI9afeaUKM/D/5MU82LDBVxyaydxFFOs5IZElzi1p8WFnPG12yJE0cVy9DOyjr6Njx3ShoutHFPUVirdx0ly2yVzm1mjFb2/1kl3jebFLR4NyuFygtCxXeHFoUMSYdVD2EqDHKSG0cYN8JlDfmKGcbJteVPa2OpNO16WiFL0Wz9upz3FV7fVnFIhpfd+E03/vSq3evsWoImvIQz6EAMb6EHH6EPA+BwBd/gO/zwvno/vV/e9W614e09L+AfeDd/AZ2+nsw=</latexit>

p(t)
<latexit sha1_base64="eDvHW79tKIOBLnaUNkn48Lwr170=">AAACAnicbVDLSsNAFL2pr1pfVZdugkXoqiS+3RXcuKxobaEtZTK9bYdOJmHmRiihO7du9R/ciVt/xF/wK0zSImo9MHA45x7uneOFUhhynA8rt7C4tLySXy2srW9sbhW3d+5MEGmOdR7IQDc9ZlAKhXUSJLEZamS+J7HhjS5Tv3GP2ohA3dI4xI7PBkr0BWeUSDfUPeoWS07FyWDPE3dGStU8ZKh1i5/tXsAjHxVxyYxpuU5InZhpElzipNCODIaMj9gAWwlVzEfTibNTJ/ZBovTsfqCTp8jO1J+JmPnGjH0vmfQZDc1fLxX/81oR9c87sVBhRKj4dFE/kjYFdvpvuyc0cpLjhDCuRXKrzYdMM05JO4V2FoxVQFkv3SGyHupJISvnIsXJdxXz5O6w4p5W3OvjUrU8bQnysAf7UAYXzqAKV1CDOnAYwCM8wbP1YL1Yr9bbdDRnzTK78AvW+xetKZhV</latexit>

<latexit sha1_base64="ptXvpYnN0Ty2KZ9a3kRZwTwPS1w=">AAACAnicbVDLSsNAFL3xWeur6tJNsAhdlaT43BXcuKxoH9CWMpnetkMnkzBzI5TQnVu3+g/uxK0/4i/4FSZpEbUeGDiccw/3zvFCKQw5zoe1tLyyurae28hvbm3v7Bb29hsmiDTHOg9koFseMyiFwjoJktgKNTLfk9j0xlep37xHbUSg7mgSYtdnQyUGgjNKpFvqVXqFolN2MtiLxJ2TYjUHGWq9wmenH/DIR0VcMmParhNSN2aaBJc4zXcigyHjYzbEdkIV89F04+zUqX2cKH17EOjkKbIz9WciZr4xE99LJn1GI/PXS8X/vHZEg4tuLFQYESo+WzSIpE2Bnf7b7guNnOQkIYxrkdxq8xHTjFPSTr6TBWMVUNZLb4Ssj3qaz8q5THH6XcUiaVTK7lnZvTkpVkuzliAHh3AEJXDhHKpwDTWoA4chPMITPFsP1ov1ar3NRpeseeYAfsF6/wKrjZhU</latexit>

t2 t3 <latexit sha1_base64="qcCMnSQ0gRbcWYMIUDoCBDr7orw=">AAACF3icbZDLSgNBEEVrfMb4igpu3AwGwY1hRnzuAm5cRjAqJCH0dCqmsR9Dd40YxnyIW7f6D+7ErUt/wa9wMhHxdaHhcm8VXZwolsJRELx5Y+MTk1PThZni7Nz8wmJpafnMmcRyrHMjjb2ImEMpNNZJkMSL2CJTkcTz6Opo2J9fo3XC6FPqx9hS7FKLruCMsqhdWm2qyNyk3GgSOjGJ2yKhcNAulYNKkMv/a8JPU64WIFetXXpvdgxPFGrikjnXCIOYWimzJLjEQbGZOIwZv2KX2MisZgpdK83vH/gbWdLxu8ZmT5Ofp983Uqac66som1SMeu53Nwz/6xoJdQ9aqdBxQqj56KNuIn0y/hCG3xEWOcl+Zhi3IrvV5z1mGacMWbGZL6ba0AhWD1kH7aCYwzkcavcLxV9ztl0J9yrhyU65ujmiBAVYg3XYhBD2oQrHUIM6cLiFe3iAR+/Oe/KevZfR6Jj3ubMCP+S9fgCCVqF7</latexit>

continuous-time
<latexit sha1_base64="JyXPdnG0UmSVpJY0R/zuXgyTVnY=">AAACAnicbVDLSsNAFL2pr1pfVZdugkXoqiTic1dw47KifUBbymR62w6dTMLMjVBCd27d6j+4E7f+iL/gV5ikRdR6YOBwzj3cO8cLpTDkOB9Wbml5ZXUtv17Y2Nza3inu7jVMEGmOdR7IQLc8ZlAKhXUSJLEVamS+J7Hpja9Sv3mP2ohA3dEkxK7PhkoMBGeUSLfUc3vFklNxMtiLxJ2TUjUPGWq94menH/DIR0VcMmParhNSN2aaBJc4LXQigyHjYzbEdkIV89F04+zUqX2UKH17EOjkKbIz9WciZr4xE99LJn1GI/PXS8X/vHZEg4tuLFQYESo+WzSIpE2Bnf7b7guNnOQkIYxrkdxq8xHTjFPSTqGTBWMVUNZLb4Ssj3payMq5THH6XcUiaRxX3LOKe3NSqpZnLUEeDuAQyuDCOVThGmpQBw5DeIQneLYerBfr1XqbjeaseWYffsF6/wKp8ZhT</latexit>

t1 <latexit sha1_base64="d9h9JQAyI59RvOuLkXad5c7n8DQ=">AAACEnicbVDLSgNBEOyNrxhfUY9eFoPgKeyKz1vAi0cFo0ISwuykY8bMziwzvWpY8hdeveo/eBOv/oC/4Fc42Yj4KhgoqrronooSKSwFwZtXmJicmp4pzpbm5hcWl8rLK2dWp4ZjnWupzUXELEqhsE6CJF4kBlkcSTyP+ocj//wajRVandIgwVbMLpXoCs7ISe3ycjOO9G1Ghl0hJ20Gw3a5ElSDHP5fEn6SSq0IOY7b5fdmR/M0RkVcMmsbYZBQK2OGBJc4LDVTiwnjfXaJDUcVi9G2svz0ob/hlI7f1cY9RX6ufk9kLLZ2EEduMmbUs7+9kfif10ipu9/KhEpSQsXHi7qp9En7ox78jjDuw3LgCONGuFt93mOGcXJtlZp5MFOaxj31kHXQDEt5OQcj7HxV8ZecbVXD3Wp4sl2pbY5bgiKswTpsQgh7UIMjOIY6cLiBe3iAR+/Oe/KevZfxaMH7zKzCD3ivH4g0n10=</latexit>

trajectory
<latexit sha1_base64="b5MWbhCtoIcoBuQZk/2t5aCElIc=">AAACFHicbVDLSsNAFL2pr1pfVZdugkXoqiTic1dw47KCbYWmlMnkth06mQkzE6GE/oZbt/oP7sSte3/Br3CSivg6MMzh3Hu4hxMmnGnjeW9OaWFxaXmlvFpZW9/Y3Kpu73S0TBXFNpVcqpuQaORMYNsww/EmUUjikGM3nFzk8+4tKs2kuDbTBPsxGQk2ZJQYKw2qu0EoeaSnsf2yADmfDfxBteY1vALuX+J/klqzDAVag+p7EEmaxigM5UTrnu8lpp8RZRjlOKsEqcaE0AkZYc9SQWLU/awIP3MPrBK5Q6nsE8Yt1O+OjMQ6z2c3Y2LG+vcsF/+b9VIzPOtnTCSpQUHnh4Ypd4108ybciCmkhk8tIVQxm9WlY6IINbavSlAYMyHNvKkxkgjVrFKUc57j+KuKv6Rz2PBPGv7VUa1Zn7cEZdiDfaiDD6fQhEtoQRsoTOEeHuDRuXOenGfnZb5acj49u/ADzusHf5Of1A==</latexit> <latexit sha1_base64="7iyXCYXvtnf7LlcEQO+3L706C+s=">AAACFHicbVDLSsNAFL3xWeur6tJNsAhdlaT43BXcuKxgq9CUMpnctkMnM2FmIpTQ33DrVv/Bnbh17y/4FU5SEbUeGOZw7j3cwwkTzrTxvHdnYXFpeWW1tFZe39jc2q7s7Ha0TBXFNpVcqtuQaORMYNsww/E2UUjikONNOL7I5zd3qDST4tpMEuzFZCjYgFFirNSv7AWh5JGexPbLAuR82m/0K1Wv7hVw54n/RarNEhRo9SsfQSRpGqMwlBOtu76XmF5GlGGU47QcpBoTQsdkiF1LBYlR97Ii/NQ9tErkDqSyTxi3UH86MhLrPJ/djIkZ6b+zXPxv1k3N4KyXMZGkBgWdHRqk3DXSzZtwI6aQGj6xhFDFbFaXjogi1Ni+ykFhzIQ0s6ZGSCJU03JRznmO4+8q5kmnUfdP6v7VUbVZm7UEJdiHA6iBD6fQhEtoQRsoTOABHuHJuXeenRfndba64Hx59uAXnLdPgS+f1Q==</latexit>

`1 `2

Figure 3.2 A parametric spline can be used to represent a continuous-time trajectory.

In this example, the pose at a given time p(t) is assembled as a weighted sum of known
temporal basis functions Ψk (t) with local support; at most four basis functions are nonzero
at a given time. This results in each landmark measurement being represented by a quinary
(five-way) factor between four coefficient variables and one landmark variable. The overall
factor graph is still very sparse.

Inserting (3.29) we have

K
!
X
zi = hi Ψk (ti )ck , ℓ + ηi . (3.31)
k=1

As mentioned above, if our basis functions are chosen to have local support, then
 T
only a small subset of the coefficients will be active at ti . If we let xi = cT
i ℓT
represent the active coefficient variables at ti as well as the landmark variable, then
we are back to being able to write the measurement function as

zi = hi (xi ) + ηi , (3.32)

whereupon we can use our general approach to construct the nonlinear least-squares
problem and optimize.
Moreover, if our basis functions are sufficiently differentiable, we can easily take
the derivative of our pose trajectory,
K
X
ṗ(t) = Ψ̇k (t)ck (3.33)
k=1

so that we can handle sensor outputs that are functions of, say, velocity or even
higher derivatives while still optimizing the same coefficient variables. We simply
need to compute the derivatives of our basis functions, Ψ̇k (t).
64 Advanced State Variable Representations

Finally, once we have solved for the optimal coefficients through MAP inference,
we can then query the trajectory (or its derivatives) at any time of interest us-
ing (3.29) or (3.33). If we compute the covariance of the estimated coefficients dur-
ing inference (e.g., by inverting the information matrix), this can also be mapped
through to covariance of a queried pose (or derivative) quite easily since (3.29)
or (3.33) are linear relationships; and, local support in the basis functions implies
only the appropriate marginal covariance is needed from the coefficients.

3.2.2 From Parametric to Nonparametric

The main challenge with basic parametric continuous-time methods is that we must
decide what type and how many basis functions to use. If we have too many basis
functions, it becomes very easy to overfit to the measurement data. If we have too
few basis functions, we may not have sufficient capacity to represent the true shape
of the trajectory, resulting in an overly smooth solution. This challenge is partly
addressed by moving to a nonparametric method.
To simplify the explanation slightly, in this section we will assume for now that
there are no landmark variables only pose variables. Using the parametric approach
introduced in the previous section, our linearized least-squares term (negative-log
factor) will have the form


2
zi − hi x0i − Hi Ψi δc,i Σ , (3.34)
i

where x0i is the current solution (active coefficients), δc,i is the update (to the active
coefficients), Ψi is the stacking of all basis functions active (and evaluted) at ti ,
and the Jacobian, Hi , is given by
∂hi
Hi = . (3.35)
∂x x0i

Gathering quantities into larger matrices as before, we can write our least-squares
problem as
 
2 2
δc∗ = arg min ∥b − AΨδc ∥ + ∥δc ∥ , (3.36)
δc

2
where we now include a regularizer term, ∥δc ∥ , that seeks to keep the description
length of our solution reasonable (i.e., we prefer spline coefficients to be closer to
zero). The regularizer term helps to avoid the over-fitting problem mentioned in
the last section. The optimal solution will be given by


ΨT AT AΨ + I δc∗ = ΨT AT b, (3.37)
which would allow us to compute the optimal update for the coefficients, δc∗ . How-
ever, what we typically care about is to produce an estimate for the pose, not
the spline coefficients (they are a means to an end). The optimal update to the
 3.2 Continuous-Time Trajectories 65

pose variables at the measurement times is actually δ ∗ = Ψδc∗ . With a little bit of
algebra, we can show that


AT A + K −1 δ ∗ = AT b, (3.38)

which is a modified version of the normal equations, first introduced in (2.25).

The kernel matrix, K = ΨT Ψ, serves a regularization or smoothing function. The
careful reader will notice that (3.38) represents a larger linear system of equations
than (3.37) because there are more poses than basis function coefficients. However,
in the end we will be able to reduce the size of the linear system we need to solve in
our nonparametric approach by using built-in interpolation capabilities. For now,
we will work with (3.38) and come back to this issue towards the end of the section.
To move away from explicit basis functions, we can employ the so-called kernel
trick, which replaces the explicit inner product of basis functions with evaluations
of a chosen kernel function, K(t, t′ ) (e.g., squared-exponential). We can see that
in (3.38) it is only the inner product of the basis functions
 that
 is required to build
the kernel matrix. The kernel matrix is then K = K(ti , tj ) ij , which is to say we
populate it with evaluations of the kernel function at every pairing of measurement
times. We can now refer to this as a nonparametric method since we are no longer
estimating the coefficients (i.e., parameters) of a spline. We do, however, have to
tune the hyperparameters of our chosen kernel function (e.g., length scale for squared
exponential) to achieve the desired trajectory smoothness.
Since we need the inverse kernel matrix right away in (3.38), it would seem to
be expensive to formulate things this way. However, the next section shows how
we can choose a kernel function that guarantees that we have a very sparse inverse
kernel matrix and therefore a sparse factor graph.

3.2.3 Gaussian Processes

We will construct a family of kernel functions that by design results in a sparse
inverse kernel matrix and corresponding factor graph. We saw in the last section
that we could swap out our basis functions for a kernel function, creating a non-
parametric continuous-time method. However, if done naively, this could result in a
dense inverse kernel matrix, which is undesirable. In this section, we come at things
from a slightly different direction. As a teaser, Figure 3.3 shows an example of a
factor graph resulting from the ideas in this section, which we see remains sparse
yet results in smooth trajectories.
We start by choosing a linear, time-invariant, stochastic differential equation
(SDE) driven by white noise:2

ẋ(t) = Ax(t) + Lw(t), (3.39)

2
It is also possible to include control inputs in this equation but we omit them in the interest of
simplicity.
66 Advanced State Variable Representations
<latexit sha1_base64="XI+bVvG2FxLrcO8kqclPBNw10v8=">AAACFHicbVDLSsNAFL3xWeur6tJNsAiuSuLbXcGNSwVbC00pk8ltO3QyE2YmQgn9Dbdu9R/ciVv3/oJf4SQtotYDwxzOvYd7OGHCmTae9+HMzS8sLi2XVsqra+sbm5Wt7aaWqaLYoJJL1QqJRs4ENgwzHFuJQhKHHO/C4WU+v7tHpZkUt2aUYCcmfcF6jBJjpW5lJwglj/Qotl8WIOfj7lG3UvVqXgF3lvhTUq2XoMB1t/IZRJKmMQpDOdG67XuJ6WREGUY5jstBqjEhdEj62LZUkBh1JyvCj919q0RuTyr7hHEL9acjI7HO89nNmJiB/jvLxf9m7dT0zjsZE0lqUNDJoV7KXSPdvAk3Ygqp4SNLCFXMZnXpgChCje2rHBTGTEgzaWqAJEI1LhflXOQ4+a5iljQPa/5pzb85rtYPJi1BCXZhDw7AhzOowxVcQwMojOARnuDZeXBenFfnbbI650w9O/ALzvsXgsuf1g==</latexit>

`3
<latexit sha1_base64="7iyXCYXvtnf7LlcEQO+3L706C+s=">AAACFHicbVDLSsNAFL3xWeur6tJNsAhdlaT43BXcuKxgq9CUMpnctkMnM2FmIpTQ33DrVv/Bnbh17y/4FU5SEbUeGOZw7j3cwwkTzrTxvHdnYXFpeWW1tFZe39jc2q7s7Ha0TBXFNpVcqtuQaORMYNsww/E2UUjikONNOL7I5zd3qDST4tpMEuzFZCjYgFFirNSv7AWh5JGexPbLAuR82m/0K1Wv7hVw54n/RarNEhRo9SsfQSRpGqMwlBOtu76XmF5GlGGU47QcpBoTQsdkiF1LBYlR97Ii/NQ9tErkDqSyTxi3UH86MhLrPJ/djIkZ6b+zXPxv1k3N4KyXMZGkBgWdHRqk3DXSzZtwI6aQGj6xhFDFbFaXjogi1Ni+ykFhzIQ0s6ZGSCJU03JRznmO4+8q5kmnUfdP6v7VUbVZm7UEJdiHA6iBD6fQhEtoQRsoTOABHuHJuXeenRfndba64Hx59uAXnLdPgS+f1Q==</latexit>

`2
<latexit sha1_base64="b5MWbhCtoIcoBuQZk/2t5aCElIc=">AAACFHicbVDLSsNAFL2pr1pfVZdugkXoqiTic1dw47KCbYWmlMnkth06mQkzE6GE/oZbt/oP7sSte3/Br3CSivg6MMzh3Hu4hxMmnGnjeW9OaWFxaXmlvFpZW9/Y3Kpu73S0TBXFNpVcqpuQaORMYNsww/EmUUjikGM3nFzk8+4tKs2kuDbTBPsxGQk2ZJQYKw2qu0EoeaSnsf2yADmfDfxBteY1vALuX+J/klqzDAVag+p7EEmaxigM5UTrnu8lpp8RZRjlOKsEqcaE0AkZYc9SQWLU/awIP3MPrBK5Q6nsE8Yt1O+OjMQ6z2c3Y2LG+vcsF/+b9VIzPOtnTCSpQUHnh4Ypd4108ybciCmkhk8tIVQxm9WlY6IINbavSlAYMyHNvKkxkgjVrFKUc57j+KuKv6Rz2PBPGv7VUa1Zn7cEZdiDfaiDD6fQhEtoQRsoTOEeHuDRuXOenGfnZb5acj49u/ADzusHf5Of1A==</latexit>

`1
<latexit sha1_base64="YYxVZb/ytCpQGjA2z/xlrLyTDVM=">AAACJ3icbVBNTxsxFHxLoYRAS1qOXCyiovQS7SJa2kOlSFx6pBIBpCRaeb0vxMJrr+y3iGiVf9A/0muv5T9wQ/TIlV9R7wYhPjqS5dHMG/l5klxJR2H4N1h4tbj0ermx0lxde/N2vfXu/ZEzhRXYF0YZe5Jwh0pq7JMkhSe5RZ4lCo+Ts/3KPz5H66TRhzTNcZTxUy3HUnDyUtzaHiZGpW6a+au8mMWSfWNPpQ7F8mPcaofdsAZ7SaJ70u41oMZB3LobpkYUGWoSijs3iMKcRiW3JIXCWXNYOMy5OOOnOPBU8wzdqKz/M2MfvJKysbH+aGK1+jhR8sxV+/nJjNPEPfcq8X/eoKDxl1EpdV4QajF/aFwoRoZV5bBUWhSkpp5wYaXflYkJt1yQr7A5rIOlNjQvb4I8RTtr1uV8rfDpoYqX5GinG33uRj92273OvCVowCZsQQci2IMefIcD6IOAn/Ab/sBl8Cu4Cq6Dm/noQnCf2YAnCG7/AZSKp7Y=</latexit>

xi = x(ti )
x3
<latexit sha1_base64="tPqvzQuur0DYkQKtz7xTE46gnjY=">AAACEXicbVDLSsNAFL3xWesr6tJNsAiuSuLbXcGNSwVbC20pk8ltO3QyE2YmxRL6FW7d6j+4E7d+gb/gVzhJRXwdGOZw7j3cwwkTzrTx/TdnZnZufmGxtFReXlldW3c3NhtapopinUouVTMkGjkTWDfMcGwmCkkccrwJh+f5/GaESjMprs04wU5M+oL1GCXGSl3XbYeSR3oc2y+7nXQPum7Fr/oFvL8k+CSVWgkKXHbd93YkaRqjMJQTrVuBn5hORpRhlOOk3E41JoQOSR9blgoSo+5kRfKJt2uVyOtJZZ8wXqF+d2Qk1nk4uxkTM9C/Z7n436yVmt5pJ2MiSQ0KOj3US7lnpJfX4EVMITV8bAmhitmsHh0QRaixZZXbhTET0kxrGiCJUE3KRTlnOY6+qvhLGvvV4LgaXB1WanvTlqAE27ADexDACdTgAi6hDhRGcA8P8OjcOU/Os/MyXZ1xPj1b8APO6wctCZ6X</latexit>

<latexit sha1_base64="acd9JfZNbDkl7lvH5hNr4W4k70I=">AAACEXicbVDLSsNAFL2pr1pfUZdugkVwVRKpr13BjUsFW4W2lMnkth06mQkzE7GEfoVbt/oP7sStX+Av+BVO0iK+DgxzOPce7uGECWfa+P67U5qbX1hcKi9XVlbX1jfcza2Wlqmi2KSSS3UTEo2cCWwaZjjeJApJHHK8Dkdn+fz6FpVmUlyZcYLdmAwE6zNKjJV6rtsJJY/0OLZfdjfp1Xtu1a/5Bby/JJiRaqMMBS567kcnkjSNURjKidbtwE9MNyPKMMpxUumkGhNCR2SAbUsFiVF3syL5xNuzSuT1pbJPGK9QvzsyEus8nN2MiRnq37Nc/G/WTk3/pJsxkaQGBZ0e6qfcM9LLa/AippAaPraEUMVsVo8OiSLU2LIqncKYCWmmNQ2RRKgmlaKc0xyHX1X8Ja2DWnBUCy7r1cb+tCUoww7swj4EcAwNOIcLaAKFW3iAR3hy7p1n58V5na6WnJlnG37AefsELqWemA==</latexit>

x4
<latexit sha1_base64="ACt9Z2ln8ntfZS9GhpVYaWod1cs=">AAACF3icbZDLSgNBEEVrfMb4igpu3AxGwY1hRnzuAm5cKhgVkhB6OhXT2I+hu0YMYz7ErVv9B3fi1qW/4Fc4mYj4utBwubeKLk4US+EoCN68kdGx8YnJwlRxemZ2br60sHjmTGI51riRxl5EzKEUGmskSOJFbJGpSOJ5dHU46M+v0Tph9Cn1YmwqdqlFR3BGWdQqLTdUZG5SbjQJnZjEbZJQ2G+VykElyOX/NeGnKVcLkOu4VXpvtA1PFGrikjlXD4OYmimzJLjEfrGROIwZv2KXWM+sZgpdM83v7/vrWdL2O8ZmT5Ofp983Uqac66kom1SMuu53Nwj/6+oJdfabqdBxQqj58KNOIn0y/gCG3xYWOcleZhi3IrvV511mGacMWbGRL6ba0BBWF1kbbb+YwzkYaOcLxV9ztlUJdyvhyXa5ujakBAVYgVXYgBD2oApHcAw14HAL9/AAj96d9+Q9ey/D0RHvc2cJfsh7/QCBIqF3</latexit>

continuous-time
<latexit sha1_base64="u0E0kaobO4dcLg5hr+J6hBA4a5A=">AAACanicbVHPb9MwGHWzAaX8KuOEdrEoSEWCKtnGxm6VuOzYSes2qSmR43xprDl2ZH9BVFn+Rs78C9OuuwJOOk37wZMsP73nZ396jgspLPr+7463tv7o8ZPu096z5y9evuq/3ji2ujQcplxLbU5jZkEKBVMUKOG0MMDyWMJJfPat8U9+gLFCqyNcFjDP2UKJVHCGTor6IpSQYnhOw1jLxC5zt1U/66jaqennO2I4yUQ9xMb5RDHa/ng/sk1DIxaZu+z7VlTdNg+dWUf9gT/yW9CHJLgmg3GXtJhE/Ysw0bzMQSGXzNpZ4Bc4r5hBwSXUvbC0UDB+xhYwc1SxHOy8aiup6QenJDTVxi2FtFVvJyqW22Y6dzJnmNn7XiP+z5uVmH6dV0IVJYLiq4fSUlLUtOmXJsIAR7l0hHEj3KyUZ8wwju4XemEbrJTGVf8ZsARM3WvL2W/w5aaKh+R4axTsjoLDncF4uGqJdMkmeUeGJCB7ZEwOyIRMCSe/yBX5Q/52Lr0N7623uTrqda4zb8gdeO//Ab7Xv6A=</latexit>

2
<latexit sha1_base64="Yr4EdV941G5d4F9Y7HI7OEu2xuQ=">AAACEXicbVDLSsNAFL2pr1pfUZdugkVwVZLic1dw41LBVqEtZTK5bYdOZsLMRCyhX+HWrf6DO3HrF/gLfoWTtIivA8Mczr2Hezhhwpk2vv/ulObmFxaXysuVldW19Q13c6ulZaooNqnkUt2ERCNnApuGGY43iUIShxyvw9FZPr++RaWZFFdmnGA3JgPB+owSY6We63ZCySM9ju2X3U169Z5b9Wt+Ae8vCWak2ihDgYue+9GJJE1jFIZyonU78BPTzYgyjHKcVDqpxoTQERlg21JBYtTdrEg+8fasEnl9qewTxivU746MxDoPZzdjYob69ywX/5u1U9M/6WZMJKlBQaeH+in3jPTyGryIKaSGjy0hVDGb1aNDogg1tqxKpzBmQpppTUMkEapJpSjnNMfhVxV/SateC45qweVBtbE/bQnKsAO7sA8BHEMDzuECmkDhFh7gEZ6ce+fZeXFep6slZ+bZhh9w3j4BK22elg==</latexit>

x2
<latexit sha1_base64="VSjXtvk3tP4GqyhJeZJraKYvkis=">AAACEXicbVDLSsNAFL3xWesr6tJNsAhdlUR87gpuXCrYB7SlTCa3dnAyE2YmYgn9Crdu9R/ciVu/wF/wK5ykRdR6YJjDufdwDydMONPG9z+cufmFxaXl0kp5dW19Y9Pd2m5qmSqKDSq5VO2QaORMYMMww7GdKCRxyLEV3p7n89YdKs2kuDajBHsxuRFswCgxVuq7bjeUPNKj2H7Z/bgf9N2KX/MLeLMkmJJKvQQFLvvuZzeSNI1RGMqJ1p3AT0wvI8owynFc7qYaE0JvyQ12LBUkRt3LiuRjb98qkTeQyj5hvEL96chIrPNwdjMmZqj/znLxv1knNYPTXsZEkhoUdHJokHLPSC+vwYuYQmr4yBJCFbNZPTokilBjyyp3C2MmpJnUNEQSoRqXi3LOchx9VzFLmge14LgWXB1W6tVJS1CCXdiDKgRwAnW4gEtoAIU7eIQneHYenBfn1XmbrM45U88O/ILz/gUp0Z6V</latexit>

x1 kx4 (t4 , t3 )x3 kQ3

<latexit sha1_base64="Gp1DCmbwOtMsi6yoWu3Yyr8O4yE=">AAACFHicbVDLSgNBEOz1GeMrmqOXwSh4Crvi8xbw4lHBGCEJYXbSSYbMY5mZFcOS3/DqVf/Bm3j17i/4Fe5uRNRYMFBT1UU3FUaCW+f7797M7Nz8wmJhqbi8srq2XtrYvLY6NgzrTAttbkJqUXCFdcedwJvIIJWhwEY4PMv8xi0ay7W6cqMI25L2Fe9xRl0qdUrllgz1XSJ19iWR4dqMO6WKX/VzkGkSfJFKrQA5Ljqlj1ZXs1iickxQa5uBH7l2Qo3jTOC42IotRpQNaR+bKVVUom0n+fFjspsqXdLTJn3KkVz9mUiotHYkw3RSUjewf71M/M9rxq530k64imKHik0W9WJBnCZZE6TLDTInRimhzPD0VsIG1FDm0r6KrTyYKO0mTQ2QdtGMi3k5pxkOv6uYJtf71eCoGlweVGo7k5agAFuwDXsQwDHU4BwuoA4MRvAAj/Dk3XvP3ov3Ohmd8b4yZfgF7+0T2K2gCA==</latexit>

motion prior
<latexit sha1_base64="TTmtJJJgYmBn7n4RbC66UYQXrNA=">AAACanicbVFba9swGFW8W5bdsvZp9EUsG2SwBTtt1/UtsJc9prC0hTgzsvw5FpUlI30eDa5/4577F0pf+7pNdsroZQeEDufoSB9HcSGFRd8/73gPHj56/KT7tPfs+YuXr/qvNw6tLg2HGddSm+OYWZBCwQwFSjguDLA8lnAUn3xt/KOfYKzQ6juuCljkbKlEKjhDJ0V9EUpIMTyjYaxlYle526rTOqq2a/rplhhOM1EPsXE+UozGH+5GxjQ0Ypm5y36Mo+qmeeDMOuoP/JHfgt4nwTUZTLqkxTTqX4SJ5mUOCrlk1s4Dv8BFxQwKLqHuhaWFgvETtoS5o4rlYBdVW0lN3zsloak2bimkrXozUbHcNtO5kznDzN71GvF/3rzE9MuiEqooERRfP5SWkqKmTb80EQY4ypUjjBvhZqU8Y4ZxdL/QC9tgpTSu+8+AJWDqXlvOfoPdf1XcJ4fjUfB5FBzsDCbDdUukS7bIWzIkAdkjE/KNTMmMcPKLXJHf5E/n0tvw3nhb66Ne5zqzSW7Be/cXtfa/mw==</latexit>

<latexit sha1_base64="2oG3EZWO2xoOOXOSedwYPCwninU=">AAACSnicbVBNb9NAEF2HQtPwFeDIZdUIKUgosqtS4BaplyIurdS0leJgrdfjZNX1rrU7RkSLfxN/pNceuMCdY2+IS9dOVbWUJ4326b0ZzexLSykshuGPoHNv7f6D9e5G7+Gjx0+e9p89P7K6MhwmXEttTlJmQQoFExQo4aQ0wIpUwnF6utv4x1/AWKHVIS5LmBVsrkQuOEMvJf2PsYQc4280TrXM7LLwj/taJy6qaWzEfOG9z1uJu2nHBcMFZ9J9qushJtEbX6/rpD8IR2ELepdEV2Qw7pIW+0n/d5xpXhWgkEtm7TQKS5w5ZlBwCXUvriyUjJ+yOUw9VawAO3Ptl2v6yisZzbXxpZC26s0JxwrbnOs7m2vtv14j/s+bVpi/nzmhygpB8dWivJIUNW3yo5kwwFEuPWHcCH8r5QtmGEefci9uB53SuMp3ASwDU/facD40eHsdxV1ytDWKdkbRwfZgPFylRLrkJdkkQxKRd2RM9sg+mRBOvpNz8pP8Cs6Ci+BP8HfV2gmuZl6QW+isXQKN8rWp</latexit>

2
2 kx3 (t3 , t2 )x2 kQ2
kx1 kK(t1 ,t1 ) <latexit sha1_base64="LCLkPYigzkh4aS+EVQHQSHiAREM=">AAACanicbVFba9swGFXcXbLs0qx9Gn0RzQYZbMEOu3Rvgb7sMYWlLcSZkeXPsagsGelzaXD9G/u8v1D2utduslNGbweEDufoSB9HcSGFRd//1fE2Hj1+8rT7rPf8xctXm/3XW4dWl4bDjGupzXHMLEihYIYCJRwXBlgeSziKT/Yb/+gUjBVa/cBVAYucLZVIBWfopKgvQgkphuc0jLVM7Cp3W3VWR9W4ph9vieE0E/UQG+cDxSh4fzcS0NCIZeYu+zmOqpvmgTPrqD/wR34Lep8E12Qw6ZIW06h/GSaalzko5JJZOw/8AhcVMyi4hLoXlhYKxk/YEuaOKpaDXVRtJTV955SEptq4pZC26s1ExXLbTOdO5gwze9drxIe8eYnp3qISqigRFF8/lJaSoqZNvzQRBjjKlSOMG+FmpTxjhnF0v9AL22ClNK77z4AlYOpeW863Bp//V3GfHI5HwZdRcPBpMBmuWyJdskN2yZAE5CuZkO9kSmaEkwvyh1yRv53f3pb3xttZH/U615ltcgve23+tFb+W</latexit>

2
kx2 (t2 , t1 )x1 kQ1

Figure 3.3 Example of Gaussian process (GP) continuous-time factor graph. The motion
prior is based on a kernel function derived from a stochastic differential equation (SDE)
for Markovian state x(t). This results in a very sparse set of factors: a single unary factor
at the initial state and then binary factors linking consecutive states.

where w(t) = GP(0, Qδ(t − t′ )) is a zero-mean white noise Gaussian process, Q

is a power-spectral density matrix, and δ(·) is the Dirac delta function. The idea
is that this will serve as a motion prior. We can integrate this SDE once in closed
form:
Z t
x(t) = Φ(t, t1 )x(t1 ) + Φ(t, s)Lw(s) ds, (3.40)
t1

where Φ(t, s) = exp (A(t − s)) is known as the transition function and t1 is the
time stamp of the first measurement. The function, x(t), is also a Gaussian pro-
cess. To keep the explanation simple, if we assume the mean of the initial state is
zero, E[x(t1 )] = 0, then the mean will remain zero for all subsequent times. The
covariance function of the state (i.e., the kernel function), K(t, t′ ), can be calculated
as

Z min(t,t′ )
′ ′
K(t, t ) = Φ(t, t1 )K(t1 , t1 )Φ(t , t1 ) + T
Φ(t, s)LQLT Φ(t′ , s)T ds, (3.41)
t1

which looks daunting. However, we can evaluate this kernel function at all pairs of
measurement times (i.e., build the kernel matrix) using the tidy relation

K = ΦQΦT , (3.42)
 3.2 Continuous-Time Trajectories 67
R ti
where Q = diag(K(t1 , t1 ), Q1 , . . . , QM ), Qi = ti−1 Φ(ti , s)LQLT Φ(ti , s)T ds, and
 
I
 Φ(t , t ) I 
 2 1 
 
 Φ(t3 , t1 ) Φ(t3 , t2 ) I 
Φ=  .. .. ..  , (3.43)
.. 
 . . . . 
 
Φ(tM −1 , t1 ) Φ(tM −1 , t2 ) Φ(tM −1 , t3 ) · · · I 
Φ(tM , t1 ) Φ(tM , t2 ) Φ(tM , t3 ) · · · Φ(tM , tM −1 ) I
with M the last measurement time index. However, since it is the inverse kernel
matrix that we want in (3.38), K −1 = Φ−⊤ Q−1 Φ−1 , we can compute this directly.
The middle matrix, Q, is block-diagonal and so its inverse can be computed one
diagonal block at a time. Importantly, when we compute the inverse of Φ, we find
 
I
−Φ(t , t ) I 
 2 1 
 
 −Φ(t3 , t2 ) I 
−1  
Φ = ..  , (3.44)
 −Φ(t4 , t3 ) . 
 
 .. 
 . I 
−Φ(tM , tM −1 ) I
which is all zeros except for the main block-diagonal and one block-diagonal below.
Thus, when we construct the inverse kernel matrix, K −1 , it will be block-tridiagonal,
for any length of trajectory. Based on our earlier discussions about factor graphs,
we know that the sparsity of the left-hand side in (3.38) is closely tied to the
factor-graph structure. In this case, K −1 serves as a motion prior over the entire
trajectory, but it is easily described using a very sparse factor graph. Figure 3.3
shows how the block-tridiagonal structure of K −1 turns into a factor graph.
The reason K −1 has such a sparse factor graph is that we started from an
SDE whose state, x(t), is Markovian. Practically speaking, what this means is that
depending on the motion prior that we want to express using (3.39), we may need
to use a higher-order state, i.e., not simply the pose but also some of its derivatives.
For example, if we want to use the so-called ‘constant-velocity’ prior, our SDE can
be chosen to be
      
ṗ(t) 0 I p(t) 0
= + w(t), (3.45)
v̇(t) 0 0 v(t) I
| {z } | {z } | {z } |{z}
ẋ(t) A x(t) L

where the state now comprises pose and its derivative, v(t) = ṗ(t). Due to the
use of this augmented state, this is sometimes this is referred to as simultaneous
trajectory estimation and mapping (STEAM), a variation of SLAM.
This formulation of continuous-time trajectory estimation is really an example of
Gaussian process regression [217]. By making this connection, once we have solved
68 Advanced State Variable Representations

at the measurement times, we can easily query the trajectory at other times of
interest using GP interpolation (for both mean and covariance); with our sparse
kernel approach, the cost of each query is constant time with respect to the number
of measurements, M , as it only involves the estimated states at the two times
bracketing the query.
Importantly, we can also use the resulting GP interpolation scheme to reduce the
number of control points needed (i.e., we do not need one at every measurement
time), which is similar to the idea of GP inducing points. For example, we might put
one control point per lidar scan but still make use of all the individual time stamps
of each point gathered during a sweep. This last point is quite important because in
contrast to discrete-time estimation, the measurement times, the estimation times,
and the query times can now all be different in this continuous formulation. More-
over, in the GP approach we do not need to worry about overfitting by including
too many estimation times as the kernel provides proper regularization. However,
we still need enough estimation times to capture the detail of the trajectory.

3.2.4 Spline and GPs on Lie Groups

It is also possible to use both splines and GP continuous-time methods when the
state lives on a manifold. In the case that the manifolds are Lie groups, both
methods make use of the Lie algebra to accomplish this, but in different ways. We
begin with splines and then move to Gaussian processes.

3.2.4.1 Splines on Lie Groups

The key to making splines work on Lie groups is to use a cumulative formulation.
For a vector space, we can simplify (3.29) by assuming we are using the same basis
functions for all degrees of freedom so that we can write
K
X
p(t) = ψk (t) pk , (3.46)
k=1

where the pk are now control points of our spline (replacing the earlier coefficients)
and the basis functions, ψk (t), are now scalar. Then, we can rewrite this in cumu-
lative form as
K
X
p(t) = ψ1c (t) p1 + ψkc (t) (pk − pk−1 ) , (3.47)
k=2

where
K
X
ψkc (t) = ψℓ (t) (3.48)
ℓ=k

are the cumulative basis functions.

 3.2 Continuous-Time Trajectories 69
<latexit sha1_base64="Kpn/TFRetcR5G4vSZSOfZHafXfE=">AAACJXicbVDLSgMxFL3js9ZX1aWbYCu4KjPic1dw41LBqtCWkknv2GAmGZKMWIb+gD/i1q3+gzsRXLn3K8xMi/g6EDicc89NcsJEcGN9/82bmJyanpktzZXnFxaXlisrq+dGpZphkymh9GVIDQousWm5FXiZaKRxKPAivD7K/Ysb1IYreWYHCXZieiV5xBm1TupWau04VLdZHqeamCQnxC3khkSpZPmQGXYrVb/uFyB/STAm1UYJCpx0Kx/tnmJpjNIyQY1pBX5iOxnVljOBw3I7NZhQdk2vsOWopDGaTlb8Zkg2ndIjkdLuSEsK9Xsio7Exgzh0kzG1ffPby8X/vFZqo4NOxmWSWpRsdFGUCmIVyashPa6RWTFwhDLN3VsJ61NNmXUFlttFMJPKjqrrI+2hHpaLcg5z7H5V8Zecb9eDvXpwulNt1EYtQQnWYQO2IIB9aMAxnEATGNzBAzzCk3fvPXsv3utodMIbZ9bgB7z3T2FZpyE=</latexit>

linear spline basis functions

<latexit sha1_base64="vjKjmZzSAZy4dyZ7uorGTFottZU=">AAACCnicbVDLSgNBEOz1GeMr6tHLYhT0EnYlvm4BLx4jGBNIQpiddJIhszPLTK8QlvyBV6/6D97Eqz/hL/gVbjZBfBUMFFVddE8FkRSWPO/dmZtfWFxazq3kV9fWNzYLW9u3VseGY41rqU0jYBalUFgjQRIbkUEWBhLrwfBy4tfv0Fih1Q2NImyHrK9ET3BGqdRsRVZ0kvL4kI46haJX8jK4f4k/I8VKDjJUO4WPVlfzOERFXDJrm74XUTthhgSXOM63YosR40PWx2ZKFQvRtpPs5LF7kCpdt6dN+hS5mfo9kbDQ2lEYpJMho4H97U3E/7xmTL3zdiJUFBMqPl3Ui6VL2p383+0Kg5zkKCWMG5He6vIBM4xT2lK+lQUTpSnrpzNA1kUzzmflXExw8lXFX3J7XPJPS/51uVjZn7YEOdiFPTgEH86gAldQhRpw0PAAj/Dk3DvPzovzOh2dc2aZHfgB5+0TpmObkw==</latexit> <latexit sha1_base64="4hZel9xVX31OTCmVQWejoStYyRg=">AAACCnicbVDLSgNBEOz1GeMr6tHLYhT0EnbF+LgFvHiMYEwgCWF20kmGzM4sM71CWPIHXr3qP3gTr/6Ev+BXuNkE8VUwUFR10T0VRFJY8rx3Z25+YXFpObeSX11b39gsbG3fWh0bjjWupTaNgFmUQmGNBElsRAZZGEisB8PLiV+/Q2OFVjc0irAdsr4SPcEZpVKzFVnRScrjQzrqFIpeycvg/iX+jBQrOchQ7RQ+Wl3N4xAVccmsbfpeRO2EGRJc4jjfii1GjA9ZH5spVSxE206yk8fuQap03Z426VPkZur3RMJCa0dhkE6GjAb2tzcR//OaMfXO24lQUUyo+HRRL5YuaXfyf7crDHKSo5QwbkR6q8sHzDBOaUv5VhZMlKasn84AWRfNOJ+VczFB+auKv+T2uOSflvzrk2Jlf9oS5GAX9uAQfDiDClxBFWrAQcMDPMKTc+88Oy/O63R0zpllduAHnLdPqAOblA==</latexit> <latexit sha1_base64="QJK7MWG9ZDtKnSOSrw1rdvY+w8E=">AAACDHicbZDNSsNAFIVv/K31r+rSTbAKurAk4u+u4EZwU8HWQhvKZHrbDk4mceZGKKGv4NatvoM7ces7+Ao+hWlaRK0HBg7n3MtcPj+SwpDjfFhT0zOzc/O5hfzi0vLKamFtvWbCWHOs8lCGuu4zg1IorJIgifVIIwt8iTf+7fmwv7lHbUSorqkfoRewrhIdwRmlkdeMjGgll/sHg13aaxWKTsnJZE8ad2yK5RxkqrQKn812yOMAFXHJjGm4TkRewjQJLnGQb8YGI8ZvWRcbqVUsQOMl2dEDeydN2nYn1OlTZGfpz42EBcb0Az+dDBj1zN9uGP7XNWLqnHqJUFFMqPjoo04sbQrtIQG7LTRykv3UMK5FeqvNe0wzTimnfDNbTFRIGaFWD1kb9SCfwTkb6ugbxaSpHZTc45J7dVgsb48oQQ42YQt2wYUTKMMFVKAKHO7gEZ7g2XqwXqxX6200OmWNdzbgl6z3L7YbnB0=</latexit> <latexit sha1_base64="Nw1EqjcqPC8zjb4nQ+g4bgZB9AY=">AAACDHicbZDNSsNAFIVv/K31r+rSTbAKurAk4u+u4EZwU8HaQhvKZHprh04mceZGKKGv4NatvoM7ces7+Ao+hWlaRK0HBg7n3MtcPj+SwpDjfFhT0zOzc/O5hfzi0vLKamFt/caEseZY5aEMdd1nBqVQWCVBEuuRRhb4Emt+73zY1+5RGxGqa+pH6AXsVomO4IzSyGtGRrSSy313sEt7rULRKTmZ7Enjjk2xnINMlVbhs9kOeRygIi6ZMQ3XichLmCbBJQ7yzdhgxHiP3WIjtYoFaLwkO3pg76RJ2+6EOn2K7Cz9uZGwwJh+4KeTAaOu+dsNw/+6RkydUy8RKooJFR991ImlTaE9JGC3hUZOsp8axrVIb7V5l2nGKeWUb2aLiQopI9TqImujHuQzOGdDHX2jmDQ3ByX3uOReHRbL2yNKkINN2IJdcOEEynABFagChzt4hCd4th6sF+vVehuNTlnjnQ34Jev9C7R7nBw=</latexit> <latexit sha1_base64="PHSAoNAcutCQrsit/YygzULifWQ=">AAACCHicbVDLSgNBEOz1GeMr6tHLYhTiJeyKz1vAi+AlgnlIEsLspJMMmZ1dZnqFsOQHvHrVf/AmXv0Lf8GvcLMJosaCgaKqi+4pL5TCkON8WHPzC4tLy5mV7Ora+sZmbmu7aoJIc6zwQAa67jGDUiiskCCJ9VAj8z2JNW9wOfZr96iNCNQtDUNs+aynRFdwRol01wyNaF8X6LCdyztFJ4U9S9wpyZcykKLczn02OwGPfFTEJTOm4TohtWKmSXCJo2wzMhgyPmA9bCRUMR9NK04PHtkHidKxu4FOniI7VX8mYuYbM/S9ZNJn1Dd/vbH4n9eIqHveioUKI0LFJ4u6kbQpsMe/tztCIyc5TAjjWiS32rzPNOOUdJRtpsFYBZS20+4j66AeZdNyLsY4+a5illSPiu5p0b05zpf2Jy1BBnZhDwrgwhmU4ArKUAEOPjzCEzxbD9aL9Wq9TUbnrGlmB37Bev8C6jSang==</latexit>

<latexit sha1_base64="T6rhl8tVBm2qgaRgAzygvtTLM5Q=">AAACCnicbVDLSgNBEOz1GeMr6tHLYhTiJeyKz1vAi8cIxgSSEGYnnWRwdmaZ6RXCkj/w6lX/wZt49Sf8Bb/CzSaIGgsGiqouuqeCSApLnvfhzM0vLC4t51byq2vrG5uFre1bq2PDsca11KYRMItSKKyRIImNyCALA4n14O5y7Nfv0Vih1Q0NI2yHrK9ET3BGqdRsRVZ0En9UosNOoeiVvQzuLPGnpFjJQYZqp/DZ6moeh6iIS2Zt0/ciaifMkOASR/lWbDFi/I71sZlSxUK07SQ7eeQepErX7WmTPkVupv5MJCy0dhgG6WTIaGD/emPxP68ZU++8nQgVxYSKTxb1YumSdsf/d7vCICc5TAnjRqS3unzADOOUtpRvZcFEacr66QyQddGM8lk5F2OcfFcxS26Pyv5p2b8+Llb2Jy1BDnZhD0rgwxlU4AqqUAMOGh7hCZ6dB+fFeXXeJqNzzjSzA7/gvH8BoYObkA==</latexit> <latexit sha1_base64="sGqMoaX/M4uKbxvfd2slmPN2tI0=">AAACCnicbVDLSgNBEOz1GeMr6tHLYhT0EnaDz1vAi8cIxgSSEGYnnWTI7Mwy0yuEJX/g1av+gzfx6k/4C36Fm00QXwUDRVUX3VNBJIUlz3t35uYXFpeWcyv51bX1jc3C1vat1bHhWONaatMImEUpFNZIkMRGZJCFgcR6MLyc+PU7NFZodUOjCNsh6yvRE5xRKjVbkRWdpDw+pKNOoeiVvAzuX+LPSLGSgwzVTuGj1dU8DlERl8zapu9F1E6YIcEljvOt2GLE+JD1sZlSxUK07SQ7eewepErX7WmTPkVupn5PJCy0dhQG6WTIaGB/exPxP68ZU++8nQgVxYSKTxf1YumSdif/d7vCICc5SgnjRqS3unzADOOUtpRvZcFEacr66QyQddGM81k5FxOcfFXxl9yWS/5pyb8+Llb2py1BDnZhDw7BhzOowBVUoQYcNDzAIzw5986z8+K8TkfnnFlmB37AefsEoyObkQ==</latexit> <latexit sha1_base64="xkAowyhmfXQ7/ToYPKU0LixR29g=">AAACCnicbVDLSgNBEOyNrxhfUY9eFqMQL2HXt7eAF48KJgaSEGYnnWTI7Mwy0yuEJX/g1av+gzfx6k/4C36Fm42Ir4KBoqqL7qkgksKS5705uZnZufmF/GJhaXllda24vlG3OjYca1xLbRoBsyiFwhoJktiIDLIwkHgTDM8n/s0tGiu0uqZRhO2Q9ZXoCc4olZqtyIpOcjAu016nWPIqXgb3L/E/SamahwyXneJ7q6t5HKIiLpm1Td+LqJ0wQ4JLHBdascWI8SHrYzOlioVo20l28tjdTZWu29MmfYrcTP2eSFho7SgM0smQ0cD+9ibif14zpt5pOxEqigkVny7qxdIl7U7+73aFQU5ylBLGjUhvdfmAGcYpbanQyoKJ0pT10xkg66IZF7JyziY4+qriL6nvV/zjin91WKruTFuCPGzBNpTBhxOowgVcQg04aLiHB3h07pwn59l5mY7mnM/MJvyA8/oBpMObkg==</latexit>

<latexit sha1_base64="IRydpEp7JrY20NYZvqwHW7ohptw=">AAACAHicbVDLSsNAFL2pr1pfVZduBovgqiTic1dw47IFWwttKJPJTTt0MgkzE6GEbty61X9wJ279E3/BrzBJi6j1wMDhnHu4d44XC66NbX9YpaXlldW18nplY3Nre6e6u9fRUaIYtlkkItX1qEbBJbYNNwK7sUIaegLvvPF17t/do9I8krdmEqMb0qHkAWfUZFLLGVRrdt0uQBaJMye1RhkKNAfVz74fsSREaZigWvccOzZuSpXhTOC00k80xpSN6RB7GZU0RO2mxaFTcpQpPgkilT1pSKH+TKQ01HoSetlkSM1I//Vy8T+vl5jg0k25jBODks0WBYkgJiL5r4nPFTIjJhmhTPHsVsJGVFFmsm4q/SKYysgUrQxGSH1U00pRzlWOs+8qFknnpO6c153Waa1BZi1BGQ7gEI7BgQtowA00oQ0MEB7hCZ6tB+vFerXeZqMla57Zh1+w3r8ACKWXZA==</latexit>

1 (t) 2 (t) 3 (t) 4 (t) 5 (t) K 2 (t) K 1 (t) K (t)

1

<latexit sha1_base64="tf0tQCEYLEqgtHW/mU29DnziMd4=">AAACAHicbVDLSsNAFL2pr1pfVZduBovgqiTic1dw47IFWwttKJPJTTt0MgkzE6GEbty61X9wJ279E3/BrzBJi6j1wMDhnHu4d44XC66NbX9YpaXlldW18nplY3Nre6e6u9fRUaIYtlkkItX1qEbBJbYNNwK7sUIaegLvvPF17t/do9I8krdmEqMb0qHkAWfUZFLLHlRrdt0uQBaJMye1RhkKNAfVz74fsSREaZigWvccOzZuSpXhTOC00k80xpSN6RB7GZU0RO2mxaFTcpQpPgkilT1pSKH+TKQ01HoSetlkSM1I//Vy8T+vl5jg0k25jBODks0WBYkgJiL5r4nPFTIjJhmhTPHsVsJGVFFmsm4q/SKYysgUrQxGSH1U00pRzlWOs+8qFknnpO6c153Waa1BZi1BGQ7gEI7BgQtowA00oQ0MEB7hCZ6tB+vFerXeZqMla57Zh1+w3r8ABwmXYw==</latexit>
<latexit sha1_base64="nvdq8F622th/BF8IyaHma8eSVbY=">AAACBXicbVDLSgNBEOyNrxhfUY9eFoPgKexKfN0CXjxGMFFIQpid7SRjZmeWmV4hLDl79ar/4E28+h3+gl/hZiPiq2CgqOqieyqIpbDkeW9OYW5+YXGpuFxaWV1b3yhvbrWsTgzHJtdSm+uAWZRCYZMESbyODbIokHgVjM6m/tUtGiu0uqRxjN2IDZToC84ok1odHmqyvXLFq3o53L/E/ySVehFyNHrl906oeRKhIi6ZtW3fi6mbMkOCS5yUOonFmPERG2A7o4pFaLtpfu3E3cuU0O1rkz1Fbq5+T6QssnYcBdlkxGhof3tT8T+vnVD/pJsKFSeEis8W9RPpknanX3dDYZCTHGeEcSOyW10+ZIZxygoqdfJgqjTl1fSGyEI0k1JezukUh19V/CWtg6p/VPUvapV6bdYSFGEHdmEffDiGOpxDA5rA4Qbu4QEenTvnyXl2XmajBeczsw0/4Lx+AHmbmfI=</latexit>

···
0 <latexit sha1_base64="tf0tQCEYLEqgtHW/mU29DnziMd4=">AAACAHicbVDLSsNAFL2pr1pfVZduBovgqiTic1dw47IFWwttKJPJTTt0MgkzE6GEbty61X9wJ279E3/BrzBJi6j1wMDhnHu4d44XC66NbX9YpaXlldW18nplY3Nre6e6u9fRUaIYtlkkItX1qEbBJbYNNwK7sUIaegLvvPF17t/do9I8krdmEqMb0qHkAWfUZFLLHlRrdt0uQBaJMye1RhkKNAfVz74fsSREaZigWvccOzZuSpXhTOC00k80xpSN6RB7GZU0RO2mxaFTcpQpPgkilT1pSKH+TKQ01HoSetlkSM1I//Vy8T+vl5jg0k25jBODks0WBYkgJiL5r4nPFTIjJhmhTPHsVsJGVFFmsm4q/SKYysgUrQxGSH1U00pRzlWOs+8qFknnpO6c153Waa1BZi1BGQ7gEI7BgQtowA00oQ0MEB7hCZ6tB+vFerXeZqMla57Zh1+w3r8ABwmXYw==</latexit> <latexit sha1_base64="IV49BjYvrOBP6CAFMC5K2/ImtzA=">AAACAHicbVDLSsNAFL2pr1pfVZduBovgqiTic1dw47KFvqANZTK5bYdOJmFmIpTQjVu3+g/uxK1/4i/4FSZpEbUeGDiccw/3zvEiwbWx7Q+rsLK6tr5R3Cxtbe/s7pX3D9o6jBXDFgtFqLoe1Si4xJbhRmA3UkgDT2DHm9xmfuceleahbJpphG5AR5IPOaMmlRrNQbliV+0cZJk4C1KpFSFHfVD+7PshiwOUhgmqdc+xI+MmVBnOBM5K/VhjRNmEjrCXUkkD1G6SHzojJ6nik2Go0icNydWfiYQGWk8DL50MqBnrv14m/uf1YjO8dhMuo9igZPNFw1gQE5Ls18TnCpkR05RQpnh6K2FjqigzaTelfh5MZGjyVgZjpD6qWSkv5ybDxXcVy6R9VnUuq07jvFIj85agCEdwDKfgwBXU4A7q0AIGCI/wBM/Wg/VivVpv89GCtcgcwi9Y719A+ZeH</latexit> <latexit sha1_base64="mD29PdzRIsapMVqtHxDgsfJC4zo=">AAACAXicbVDLSsNAFL3xWeur6tLNYBFclaT43BXcuKzSF7ShTKY37dDJJMxMhBK6cutW/8GduPVL/AW/wiQtotYDA4dz7uHeOV4kuDa2/WEtLa+srq0XNoqbW9s7u6W9/ZYOY8WwyUIRqo5HNQousWm4EdiJFNLAE9j2xteZ375HpXkoG2YSoRvQoeQ+Z9Sk0l210S+V7YqdgywSZ07KtQLkqPdLn71ByOIApWGCat117Mi4CVWGM4HTYi/WGFE2pkPsplTSALWb5JdOyXGqDIgfqvRJQ3L1ZyKhgdaTwEsnA2pG+q+Xif953dj4l27CZRQblGy2yI8FMSHJvk0GXCEzYpISyhRPbyVsRBVlJi2n2MuDiQxNXkt/hHSAalrMy7nKcPZdxSJpVSvOecW5PS3XyKwlKMAhHMEJOHABNbiBOjSBgQ+P8ATP1oP1Yr1ab7PRJWueOYBfsN6/ALafl8M=</latexit> <latexit sha1_base64="29S4cSqDnXVJkjwuwmz0FZQ8xRk=">AAACAXicbVDLSsNAFL2prxpfVZduBovgqiS+3RXcuKzSF7SlTKY37dDJJMxMhBK6cutW/8GduPVL/AW/wiQt4uvAwOGce7h3jhcJro3jvFuFhcWl5ZXiqr22vrG5VdreaeowVgwbLBShantUo+ASG4Ybge1IIQ08gS1vfJX5rTtUmoeybiYR9gI6lNznjJpUuj2u90tlp+LkIH+JOyflahFy1Pqlj+4gZHGA0jBBte64TmR6CVWGM4FTuxtrjCgb0yF2UippgLqX5JdOyUGqDIgfqvRJQ3L1eyKhgdaTwEsnA2pG+reXif95ndj4F72Eyyg2KNlskR8LYkKSfZsMuEJmxCQllCme3krYiCrKTFqO3c2DiQxNXkt/hHSAamrn5VxmOP2q4i9pHlXcs4p7c1KukllLUIQ92IdDcOEcqnANNWgAAx8e4BGerHvr2XqxXmejBWue2YUfsN4+Abg8l8Q=</latexit> <latexit sha1_base64="Foup/KXIecj5oiAVgzbPX1x9xWQ=">AAACAXicbVDLSsNAFL2pr1pfVZduBovgqiTic1dw47JKX9CGMpnetEMnkzAzEUroyq1b/Qd34tYv8Rf8CpO0iFoPDBzOuYd753iR4NrY9odVWFpeWV0rrpc2Nre2d8q7ey0dxophk4UiVB2PahRcYtNwI7ATKaSBJ7Dtja8zv32PSvNQNswkQjegQ8l9zqhJpbvTRr9csat2DrJInDmp1IqQo94vf/YGIYsDlIYJqnXXsSPjJlQZzgROS71YY0TZmA6xm1JJA9Rukl86JUepMiB+qNInDcnVn4mEBlpPAi+dDKgZ6b9eJv7ndWPjX7oJl1FsULLZIj8WxIQk+zYZcIXMiElKKFM8vZWwEVWUmbScUi8PJjI0eS39EdIBqmkpL+cqw9l3FYukdVJ1zqvO7WmlRmYtQREO4BCOwYELqMEN1KEJDHx4hCd4th6sF+vVepuNFqx5Zh9+wXr/ArnZl8U=</latexit> <latexit sha1_base64="lrKCIVIOoINO+iR9jVlEp1FXE7I=">AAACAXicbVDLSsNAFL3xWeur6tLNYBFclUSsj13BjcsqfUEbymR60w6dTMLMRCihK7du9R/ciVu/xF/wK0zSImo9MHA45x7uneNFgmtj2x/W0vLK6tp6YaO4ubW9s1va22/pMFYMmywUoep4VKPgEpuGG4GdSCENPIFtb3yd+e17VJqHsmEmEboBHUruc0ZNKt1VG/1S2a7YOcgiceakXCtAjnq/9NkbhCwOUBomqNZdx46Mm1BlOBM4LfZijRFlYzrEbkolDVC7SX7plBynyoD4oUqfNCRXfyYSGmg9Cbx0MqBmpP96mfif142Nf+kmXEaxQclmi/xYEBOS7NtkwBUyIyYpoUzx9FbCRlRRZtJyir08mMjQ5LX0R0gHqKbFvJyrDNXvKhZJ67TinFec27NyjcxaggIcwhGcgAMXUIMbqEMTGPjwCE/wbD1YL9ar9TYbXbLmmQP4Bev9C7t2l8Y=</latexit>
<latexit sha1_base64="EKOMK3uPp1vs3zaWXWKl/HHIMws=">AAACBXicbVDLSsNAFL2pr1pfVZduBotQF5ak+NwV3AhuKvQFbSiT6W07djIJMxOhhK7dutV/cCdu/Q5/wa8wTYv4OjBwOOce7p3jhYJrY9vvVmZhcWl5JbuaW1vf2NzKb+80dBAphnUWiEC1PKpRcIl1w43AVqiQ+p7Apje6nPrNO1SaB7JmxiG6Ph1I3ueMmkRqFK+Pyoe1br5gl+wU5C9x5qRQyUKKajf/0ekFLPJRGiao1m3HDo0bU2U4EzjJdSKNIWUjOsB2QiX1Ubtxeu2EHCRKj/QDlTxpSKp+T8TU13rse8mkT81Q//am4n9eOzL9czfmMowMSjZb1I8EMQGZfp30uEJmxDghlCme3ErYkCrKTFJQrpMGYxmYtJruEGkP1SSXlnMxxclXFX9Jo1xyTkvOzXGhQmYtQRb2YB+K4MAZVOAKqlAHBrfwAI/wZN1bz9aL9TobzVjzzC78gPX2CZETmLQ=</latexit> <latexit sha1_base64="xmWGJ/ZUcmwbbXYdg+MpfxXwm/g=">AAACBXicbVDLSsNAFL2pr1pfVZduBotQF5ZEfO4KbgQ3FfqCtpTJ9LYdO5mEmYlQQtdu3eo/uBO3foe/4FeYpEXUemDgcM493DvHDQTXxrY/rMzC4tLySnY1t7a+sbmV396paz9UDGvMF75qulSj4BJrhhuBzUAh9VyBDXd0lfiNe1Sa+7JqxgF2PDqQvM8ZNbFUL94cOYfVbr5gl+wUZJ44M1IoZyFFpZv/bPd8FnooDRNU65ZjB6YTUWU4EzjJtUONAWUjOsBWTCX1UHei9NoJOYiVHun7Kn7SkFT9mYiop/XYc+NJj5qh/usl4n9eKzT9i07EZRAalGy6qB8KYnySfJ30uEJmxDgmlCke30rYkCrKTFxQrp0GI+mbtJruEGkP1SSXlnOZ4PS7inlSPy45ZyXn9qRQJtOWIAt7sA9FcOAcynANFagBgzt4hCd4th6sF+vVepuOZqxZZhd+wXr/Ao91mLM=</latexit>

<latexit sha1_base64="EAImJcwfwD6OvGQJQ6ogTOIKVSU=">AAACAXicbVDLSsNAFL2prxpfVZduBovgqiTic1dwI7ip0he0pUymN+3QySTMTIQSunLrVv/Bnbj1S/wFv8IkLeLrwMDhnHu4d44XCa6N47xbhYXFpeWV4qq9tr6xuVXa3mnqMFYMGywUoWp7VKPgEhuGG4HtSCENPIEtb3yZ+a07VJqHsm4mEfYCOpTc54yaVLq9rvdLZafi5CB/iTsn5WoRctT6pY/uIGRxgNIwQbXuuE5keglVhjOBU7sba4woG9MhdlIqaYC6l+SXTslBqgyIH6r0SUNy9XsioYHWk8BLJwNqRvq3l4n/eZ3Y+Oe9hMsoNijZbJEfC2JCkn2bDLhCZsQkJZQpnt5K2Igqykxajt3Ng4kMTV5Lf4R0gGpq5+VcZDj5quIvaR5V3NOKe3NcrpJZS1CEPdiHQ3DhDKpwBTVoAAMfHuARnqx769l6sV5nowVrntmFH7DePgHe9Jfc</latexit>

<latexit sha1_base64="IRydpEp7JrY20NYZvqwHW7ohptw=">AAACAHicbVDLSsNAFL2pr1pfVZduBovgqiTic1dw47IFWwttKJPJTTt0MgkzE6GEbty61X9wJ279E3/BrzBJi6j1wMDhnHu4d44XC66NbX9YpaXlldW18nplY3Nre6e6u9fRUaIYtlkkItX1qEbBJbYNNwK7sUIaegLvvPF17t/do9I8krdmEqMb0qHkAWfUZFLLGVRrdt0uQBaJMye1RhkKNAfVz74fsSREaZigWvccOzZuSpXhTOC00k80xpSN6RB7GZU0RO2mxaFTcpQpPgkilT1pSKH+TKQ01HoSetlkSM1I//Vy8T+vl5jg0k25jBODks0WBYkgJiL5r4nPFTIjJhmhTPHsVsJGVFFmsm4q/SKYysgUrQxGSH1U00pRzlWOs+8qFknnpO6c153Waa1BZi1BGQ7gEI7BgQtowA00oQ0MEB7hCZ6tB+vFerXeZqMla57Zh1+w3r8ACKWXZA==</latexit>
0 T 2T 3T 4T 5T (K 2)T (K 1)T KT
1
<latexit sha1_base64="sb63SUsb1V0UasSTB8546dUnstQ=">AAACCnicbVDLSgNBEOyNrxhfUY9eFqOgl7ArPm8BLx4VTCIkMcxOOsmQ2ZllplcIS/7Aq1f9B2/i1Z/wF/wKNxsRNRYMFFVddE8FkRSWPO/dyc3Mzs0v5BcLS8srq2vF9Y2a1bHhWOVaanMTMItSKKySIIk3kUEWBhLrweB87Nfv0Fih1TUNI2yFrKdEV3BGqdRoRla0/Vu+R/vtYskrexncaeJ/kVIlDxku28WPZkfzOERFXDJrG74XUSthhgSXOCo0Y4sR4wPWw0ZKFQvRtpLs5JG7myodt6tN+hS5mfozkbDQ2mEYpJMho779643F/7xGTN3TViJUFBMqPlnUjaVL2h3/3+0Ig5zkMCWMG5He6vI+M4xT2lKhmQUTpSnrp91H1kEzKmTlnI1x9F3FNKkdlP3jsn91WKrsTFqCPGzBNuyBDydQgQu4hCpw0PAAj/Dk3DvPzovzOhnNOV+ZTfgF5+0TR/ObWQ==</latexit>

c
1 (t)

<latexit sha1_base64="tf0tQCEYLEqgtHW/mU29DnziMd4=">AAACAHicbVDLSsNAFL2pr1pfVZduBovgqiTic1dw47IFWwttKJPJTTt0MgkzE6GEbty61X9wJ279E3/BrzBJi6j1wMDhnHu4d44XC66NbX9YpaXlldW18nplY3Nre6e6u9fRUaIYtlkkItX1qEbBJbYNNwK7sUIaegLvvPF17t/do9I8krdmEqMb0qHkAWfUZFLLHlRrdt0uQBaJMye1RhkKNAfVz74fsSREaZigWvccOzZuSpXhTOC00k80xpSN6RB7GZU0RO2mxaFTcpQpPgkilT1pSKH+TKQ01HoSetlkSM1I//Vy8T+vl5jg0k25jBODks0WBYkgJiL5r4nPFTIjJhmhTPHsVsJGVFFmsm4q/SKYysgUrQxGSH1U00pRzlWOs+8qFknnpO6c153Waa1BZi1BGQ7gEI7BgQtowA00oQ0MEB7hCZ6tB+vFerXeZqMla57Zh1+w3r8ABwmXYw==</latexit>
<latexit sha1_base64="nvdq8F622th/BF8IyaHma8eSVbY=">AAACBXicbVDLSgNBEOyNrxhfUY9eFoPgKexKfN0CXjxGMFFIQpid7SRjZmeWmV4hLDl79ar/4E28+h3+gl/hZiPiq2CgqOqieyqIpbDkeW9OYW5+YXGpuFxaWV1b3yhvbrWsTgzHJtdSm+uAWZRCYZMESbyODbIokHgVjM6m/tUtGiu0uqRxjN2IDZToC84ok1odHmqyvXLFq3o53L/E/ySVehFyNHrl906oeRKhIi6ZtW3fi6mbMkOCS5yUOonFmPERG2A7o4pFaLtpfu3E3cuU0O1rkz1Fbq5+T6QssnYcBdlkxGhof3tT8T+vnVD/pJsKFSeEis8W9RPpknanX3dDYZCTHGeEcSOyW10+ZIZxygoqdfJgqjTl1fSGyEI0k1JezukUh19V/CWtg6p/VPUvapV6bdYSFGEHdmEffDiGOpxDA5rA4Qbu4QEenTvnyXl2XmajBeczsw0/4Lx+AHmbmfI=</latexit>

···
0
<latexit sha1_base64="A/rnaKBv4IIwCzAIh8cKeKXSu+4=">AAACCnicbVDLSgNBEOz1GeMr6tHLYhTiJeyKz1vAi0cFYwJJDLOTTjI4O7PM9AphyR949ar/4E28+hP+gl/hZhNEjQUDRVUX3VNBJIUlz/twZmbn5hcWc0v55ZXVtfXCxuaN1bHhWOVaalMPmEUpFFZJkMR6ZJCFgcRacHc+8mv3aKzQ6poGEbZC1lOiKzijVGo0IyvaB7e8RPvtQtErexncaeJPSLGSgwyX7cJns6N5HKIiLpm1Dd+LqJUwQ4JLHOabscWI8TvWw0ZKFQvRtpLs5KG7lyodt6tN+hS5mfozkbDQ2kEYpJMho779643E/7xGTN3TViJUFBMqPl7UjaVL2h393+0Ig5zkICWMG5He6vI+M4xT2lK+mQUTpSnrp91H1kEzzGflnI1w9F3FNLk5KPvHZf/qsFjZHbcEOdiGHSiBDydQgQu4hCpw0PAIT/DsPDgvzqvzNh6dcSaZLfgF5/0LSZSbWg==</latexit> <latexit sha1_base64="+f2VlbW7kUyOe5pRm1+t4b3J5qY=">AAACCnicbVDLSgNBEOz1GeMr6tHLYhTiJez69hbw4lHBGCGJYXbSSYbMziwzvUJY8gdeveo/eBOv/oS/4Fe42YiosWCgqOqieyqIpLDkee/O1PTM7Nx8biG/uLS8slpYW7+2OjYcq1xLbW4CZlEKhVUSJPEmMsjCQGIt6J+N/NodGiu0uqJBhM2QdZXoCM4oleqNyIrW/i0v0W6rUPTKXgZ3kvhfpFjJQYaLVuGj0dY8DlERl8zauu9F1EyYIcElDvON2GLEeJ91sZ5SxUK0zSQ7eejupErb7WiTPkVupv5MJCy0dhAG6WTIqGf/eiPxP68eU+ekmQgVxYSKjxd1YumSdkf/d9vCICc5SAnjRqS3urzHDOOUtpRvZMFEacr6afWQtdEM81k5pyMcflcxSa73yv5R2b88KFa2xy1BDjZhC0rgwzFU4BwuoAocNDzAIzw5986z8+K8jkennK/MBvyC8/YJSzWbWw==</latexit> <latexit sha1_base64="macRzjk86TnjlLqPUa76UmWMbxg=">AAACCnicbVDLSgNBEOz1GeMr6tHLYhTiJeyKz1vAi0cFYwJJDLOTTjI4O7PM9AphyR949ar/4E28+hP+gl/hZhNEjQUDRVUX3VNBJIUlz/twZmbn5hcWc0v55ZXVtfXCxuaN1bHhWOVaalMPmEUpFFZJkMR6ZJCFgcRacHc+8mv3aKzQ6poGEbZC1lOiKzijVGo0Iyvah7e8RPvtQtErexncaeJPSLGSgwyX7cJns6N5HKIiLpm1Dd+LqJUwQ4JLHOabscWI8TvWw0ZKFQvRtpLs5KG7lyodt6tN+hS5mfozkbDQ2kEYpJMho779643E/7xGTN3TViJUFBMqPl7UjaVL2h393+0Ig5zkICWMG5He6vI+M4xT2lK+mQUTpSnrp91H1kEzzGflnI1w9F3FNLk5KPvHZf/qsFjZHbcEOdiGHSiBDydQgQu4hCpw0PAIT/DsPDgvzqvzNh6dcSaZLfgF5/0LTNabXA==</latexit>
<latexit sha1_base64="P3YKFE7ogKs4GIRJAtBEaahZdd4=">AAACCnicbVDLSgNBEOz1GeMr6tHLYhTiJeyKz1vAi0cFYwJJDLOTTjI4O7PM9AphyR949ar/4E28+hP+gl/hZhNEjQUDRVUX3VNBJIUlz/twZmbn5hcWc0v55ZXVtfXCxuaN1bHhWOVaalMPmEUpFFZJkMR6ZJCFgcRacHc+8mv3aKzQ6poGEbZC1lOiKzijVGo0IyvaR7e8RPvtQtErexncaeJPSLGSgwyX7cJns6N5HKIiLpm1Dd+LqJUwQ4JLHOabscWI8TvWw0ZKFQvRtpLs5KG7lyodt6tN+hS5mfozkbDQ2kEYpJMho779643E/7xGTN3TViJUFBMqPl7UjaVL2h393+0Ig5zkICWMG5He6vI+M4xT2lK+mQUTpSnrp91H1kEzzGflnI1w9F3FNLk5KPvHZf/qsFjZHbcEOdiGHSiBDydQgQu4hCpw0PAIT/DsPDgvzqvzNh6dcSaZLfgF5/0LTnebXQ==</latexit> <latexit sha1_base64="D7l3U4xoGvUkBoLsB2p4xiYO+Qk=">AAACEHicbZDLSsNAFIZPvNZ6q7p0E6yCLixJ8boruBHcKNhWaGqZTE/t0MkkzJwIJfQl3LrVd3Anbn0DX8GnME1EvP0w8PP/5zCHz4+kMOQ4b9bE5NT0zGxhrji/sLi0XFpZbZgw1hzrPJShvvKZQSkU1kmQxKtIIwt8iU1/cDLum7eojQjVJQ0jbAfsRome4IzSqFNa9iIjOsnZbnV0zbdpp1MqOxUnk/3XuJ+mXCtApvNO6d3rhjwOUBGXzJiW60TUTpgmwSWOil5sMGJ8wG6wlVrFAjTtJDt8ZG+lSdfuhTp9iuws/b6RsMCYYeCnkwGjvvndjcP/ulZMvaN2IlQUEyqef9SLpU2hPaZgd4VGTnKYGsa1SG+1eZ9pxillVfSyxUSFlFPqI+uiHhUzOMdj7X+h+Gsa1Yp7UHEv9sq1zZwSFGAdNmAbXDiEGpzCOdSBQwz38ACP1p31ZD1bL/nohPW5swY/ZL1+ALuPnSM=</latexit> <latexit sha1_base64="nMDhS5B1D1sjJBwy60z03JcSI3U=">AAACEHicbZDNSsNAFIVv/K31p1WXboJV0IUlEX93BTeCmwq2FtpaJtPbduhkEmZuhBL6Em7d6ju4E7e+ga/gU5imRdR6YOBwzr3M5fNCKQw5zoc1Mzs3v7CYWcour6yu5fLrG1UTRJpjhQcy0DWPGZRCYYUESayFGpnvSbz1+hej/vYetRGBuqFBiE2fdZXoCM4oiVr5XCM0ohVfHbjDO75H+618wSk6qexp405MoZSBVOVW/rPRDnjkoyIumTF11wmpGTNNgkscZhuRwZDxPutiPbGK+WiacXr40N5NkrbdCXTyFNlp+nMjZr4xA99LJn1GPfO3G4X/dfWIOmfNWKgwIlR8/FEnkjYF9oiC3RYaOclBYhjXIrnV5j2mGaeEVbaRLsYqoDGlHrI26mE2hXM+0vE3imlTPSy6J0X3+qhQ2hlTggxswTbsgQunUIJLKEMFOETwCE/wbD1YL9ar9TYenbEmO5vwS9b7F7ntnSI=</latexit>

<latexit sha1_base64="5dbsJI9Gf2Rzwu69/S83j+Nl4tk=">AAACDHicbZDNSsNAFIVv6l+tf1WXboJV0E1JxN9dwY3gRsG2QhvLZHrbDk4mceZGKKGv4NatvoM7ces7+Ao+hWlaRK0HBg7n3MtcPj+SwpDjfFi5qemZ2bn8fGFhcWl5pbi6VjNhrDlWeShDfe0zg1IorJIgideRRhb4Euv+7emwr9+jNiJUV9SP0AtYV4mO4IzSyGtGRrSS88EN36HdVrHklJ1M9qRxx6ZUyUOmi1bxs9kOeRygIi6ZMQ3XichLmCbBJQ4KzdhgxPgt62IjtYoFaLwkO3pgb6dJ2+6EOn2K7Cz9uZGwwJh+4KeTAaOe+dsNw/+6RkydYy8RKooJFR991ImlTaE9JGC3hUZOsp8axrVIb7V5j2nGKeVUaGaLiQopI9TqIWujHhQyOCdDHXyjmDS1vbJ7WHYv90uVrRElyMMGbMIOuHAEFTiDC6gChzt4hCd4th6sF+vVehuN5qzxzjr8kvX+BVXUnH8=</latexit>

c c c c c c c
2 (t) 3 (t) 4 (t) 5 (t) K 2 (t) K 1 (t) K (t)
<latexit sha1_base64="BGIeEnjLpw4AGJOhKe3hnmObZ1M=">AAACMHicbVDLSgMxFL3js9ZX1aWbYBVc1RnxuSu4calgVWhLyaR3bDCTDElGLEM/wx9x61b/QVfitl9hZlrE14HA4dxzbpITJoIb6/tv3sTk1PTMbGmuPL+wuLRcWVm9NCrVDBtMCaWvQ2pQcIkNy63A60QjjUOBV+HtST6/ukNtuJIXtp9gO6Y3kkecUeukTmWnFYfqPsvjVBOT5ISwNE6FM9whcbu5IVEqWe43g06l6tf8AuQvCcakWi9BgbNOZdjqKrcQpWWCGtMM/MS2M6otZwIH5VZqMKHslt5g01FJYzTtrPjYgGw5pUsipd2RlhTq90RGY2P6ceicMbU983uWi//NmqmNjtoZl0lqUbLRRVEqiFUkb4l0uUZmRd8RyjR3byWsRzVl1nVZbhXBTCo7arGHtIt6UC7KOc6x/1XFX3K5WwsOasH5XrW+OWoJSrAOG7ANARxCHU7hDBrA4AGe4BlevEfv1Xv3PkbWCW+cWYMf8Iafub2r7g==</latexit>

linear spline cumulative basis functions

Figure 3.4 Example of linear spline basis functions both in (top) normal and (bottom)
cumulative form.

For example, if we want to have linear interpolation with uniform temporal spac-
ing, T , the basis functions are
 
1 − α1 (t) 0 ≤ t < T αK (t) (K − 1)T ≤ t < KT
ψ1 (t) = , ψK (t) = ,
0 otherwise 0 otherwise
(3.49)

 αk−1 (t) (k − 2)T ≤ t < (k − 1)T
k = 2 . . . K − 1 : ψk (t) = 1 − αk (t) (k − 1)T ≤ t < kT , (3.50)

0 otherwise
t−(k−1)T
where αk (t) = T . The corresponding cumulative basis functions are

0 ≤ t < (K − 1)T
ψ1c (t) = 1, c
ψK (t) = , (3.51)
αk (t) (k − 1)T ≤ t

 0 t < (k − 1)T
k = 2...K − 1 : ψkc (t) = αk (t) (k − 1)T ≤ t < kT . (3.52)

1 kT ≤ t

Figure 3.4 shows what these basis functions look like.

The key advantage of the cumulative basis functions is that at a given time stamp,
most of the basis functions are inactive. In the case of our linear spline example,
we can write
p(t) = pk−1 + ψkc (t) (pk − pk−1 ) (3.53)

when (k − 1)T ≤ t < kT . We see that only a single basis function needs to be
70 Advanced State Variable Representations

evaluated. With higher-order splines, we will still have only a small active set at a
particular time stamp.
To apply splines on a Lie group, the idea is to then use the cumulative formulation
with the Lie group operator (matrix multiplication) replacing the summation. For
example, in the case of a linear spline, an element of SE(d) can be written as
−1



T (t) = Exp ψkc (t) Log Tk Tk−1 · Tk−1 , (3.54)
when (k − 1)T ≤ t < kT . We can now insert T (ti ) into any measurement ex-
pression at some time stamp ti , linearize it with respect to the Tk control points
(our estimation variables), and then use it within our MAP framework. Again, with
compact-support basis functions, only a few are active at a given measurement time
(one in the example of linear splines).
In a bit more detail for our linear spline example, we can rewrite (3.54) as


−1 αk (t)
T (t) = Tk Tk−1 Tk−1 . (3.55)
When linearizing expressions involving T (t), we can make use of the optimization
approach introduced in Section 3.1.3. We perturb each of the poses3 so that
 αk (t)
0−1
Exp(ξ(t))T 0 (t) = Exp(ξk )Tk0 Tk−1 Exp(−ξk−1 ) 0
Exp(ξk−1 )Tk−1 . (3.56)

Our goal is to relate the perturbation of the interpolated pose, ξ(t), to those of
the control points, ξk and ξk−1 . As shown by Barfoot [17], this relationship can be
approximated (to first order in the perturbations) as
ξ(t) ≈ (I − A(αk (t))) ξk−1 + A(αk (t)) ξk , (3.57)
where
   −1
0−1 0−1
A(αk (t)) = αk (t)J αk (t)Tk0 Tk−1 J Tk0 Tk−1 (3.58)

and J (·) is the left Jacobian of SE(d). We can then use (3.57) to relate changes in
our pose at a measurement time to the two bracketing control-point poses in order
to form linearized error terms for use in MAP estimation. For example, consider
the linearized measurement model in (3.15) again, where we rearrange it as an error
with slightly simpler notation for the pose and its perturbation as a function of t:
 
ei (t) ≈ zi − hi T 0 (t)ℓ̃i − Hi ξ(t). (3.59)

It is now a simple matter of substituting (3.57) in for ξ(t) to produce a linearized

error in terms of the bracketing control points:
 
ei (t) ≈ zi − hi T (t)0 ℓ̃i − Hi (I − A(αk (t))) ξk−1 − Hi A(αk (t)) ξk . (3.60)
3
In this case, we are perturbing on the left side instead of the right as shown in Section 3.1.3. The
s
reason is that if the unknown poses represent Tw (t) (‘sensor’ with respect to ‘world’), we typically
apply splines in the ‘sensor’ frame and so choose the perturbations to occur there as well.
 3.2 Continuous-Time Trajectories 71
<latexit sha1_base64="ACt9Z2ln8ntfZS9GhpVYaWod1cs=">AAACF3icbZDLSgNBEEVrfMb4igpu3AxGwY1hRnzuAm5cKhgVkhB6OhXT2I+hu0YMYz7ErVv9B3fi1qW/4Fc4mYj4utBwubeKLk4US+EoCN68kdGx8YnJwlRxemZ2br60sHjmTGI51riRxl5EzKEUGmskSOJFbJGpSOJ5dHU46M+v0Tph9Cn1YmwqdqlFR3BGWdQqLTdUZG5SbjQJnZjEbZJQ2G+VykElyOX/NeGnKVcLkOu4VXpvtA1PFGrikjlXD4OYmimzJLjEfrGROIwZv2KXWM+sZgpdM83v7/vrWdL2O8ZmT5Ofp983Uqac66kom1SMuu53Nwj/6+oJdfabqdBxQqj58KNOIn0y/gCG3xYWOcleZhi3IrvV511mGacMWbGRL6ba0BBWF1kbbb+YwzkYaOcLxV9ztlUJdyvhyXa5ujakBAVYgVXYgBD2oApHcAw14HAL9/AAj96d9+Q9ey/D0RHvc2cJfsh7/QCBIqF3</latexit>

continuous-time <latexit sha1_base64="t+FmYV3J/PPTVV+EWCO22F3f6PU=">AAACCHicbVDLSsNAFL2prxpfVZdugkXoqiS+3RXcuKzQl7SlTKa37dCZSZiZCCXkB9y61X9wJ279C3/BrzBNi6j1wMDhnHu4d44fcqaN635YuaXlldW1/Lq9sbm1vVPY3WvoIFIU6zTggWr5RCNnEuuGGY6tUCERPsemP76e+s17VJoFsmYmIXYFGUo2YJSYVLrrCF/HtaR30isU3bKbwVkk3pwUK3nIUO0VPjv9gEYCpaGcaN323NB0Y6IMoxwTuxNpDAkdkyG2UyqJQN2Ns4MT5yhV+s4gUOmTxsnUn4mYCK0nwk8nBTEj/debiv957cgMLrsxk2FkUNLZokHEHRM40987faaQGj5JCaGKpbc6dEQUoSbtyO5kwVgGJmunN0LSR5XYWTlXU5x9V7FIGsdl77zs3Z4WK6VZS5CHAziEEnhwARW4gSrUgYKAR3iCZ+vBerFerbfZaM6aZ/bhF6z3L47amwc=</latexit>

<latexit sha1_base64="Gp1DCmbwOtMsi6yoWu3Yyr8O4yE=">AAACFHicbVDLSgNBEOz1GeMrmqOXwSh4Crvi8xbw4lHBGCEJYXbSSYbMY5mZFcOS3/DqVf/Bm3j17i/4Fe5uRNRYMFBT1UU3FUaCW+f7797M7Nz8wmJhqbi8srq2XtrYvLY6NgzrTAttbkJqUXCFdcedwJvIIJWhwEY4PMv8xi0ay7W6cqMI25L2Fe9xRl0qdUrllgz1XSJ19iWR4dqMO6WKX/VzkGkSfJFKrQA5Ljqlj1ZXs1iickxQa5uBH7l2Qo3jTOC42IotRpQNaR+bKVVUom0n+fFjspsqXdLTJn3KkVz9mUiotHYkw3RSUjewf71M/M9rxq530k64imKHik0W9WJBnCZZE6TLDTInRimhzPD0VsIG1FDm0r6KrTyYKO0mTQ2QdtGMi3k5pxkOv6uYJtf71eCoGlweVGo7k5agAFuwDXsQwDHU4BwuoA4MRvAAj/Dk3XvP3ov3Ohmd8b4yZfgF7+0T2K2gCA==</latexit>

motion prior T3 <latexit sha1_base64="pynJLpF2fLKsnj23VpB2J1ecRr0=">AAACCHicbVDLSsNAFL2pr1pfVZdugkXoqiRSX7uCG5cV+pK2hMnkth06MwkzE6GE/oBbt/oP7sStf+Ev+BWmaRG1Hhg4nHMP987xI860cZwPK7eyura+kd8sbG3v7O4V9w9aOowVxSYNeag6PtHImcSmYYZjJ1JIhM+x7Y+vZ377HpVmoWyYSYR9QYaSDRglJpXuesLXSWPqVb1iyak4Gexl4i5IqZaHDHWv+NkLQhoLlIZyonXXdSLTT4gyjHKcFnqxxojQMRliN6WSCNT9JDt4ap+kSmAPQpU+aexM/ZlIiNB6Ivx0UhAz0n+9mfif143N4LKfMBnFBiWdLxrE3DahPfu9HTCF1PBJSghVLL3VpiOiCDVpR4VeFkxkaLJ2vBGSANW0kJVzNcPZdxXLpHVacc8r7m21VCvPW4I8HMExlMGFC6jBDdShCRQEPMITPFsP1ov1ar3NR3PWInMIv2C9fwGQdpsI</latexit>

T4
<latexit sha1_base64="FQn1ZxFiffFO9CGFx9kP8LpSv5M=">AAACCHicbVDLSsNAFL2pr1pfVZdugkXoqiTF567gxmWFvqQtYTK5bYfOTMLMRCihP+DWrf6DO3HrX/gLfoVpWkStBwYO59zDvXP8iDNtHOfDyq2srq1v5DcLW9s7u3vF/YOWDmNFsUlDHqqOTzRyJrFpmOHYiRQS4XNs++Prmd++R6VZKBtmEmFfkKFkA0aJSaW7nvB10ph6Va9YcipOBnuZuAtSquUhQ90rfvaCkMYCpaGcaN11ncj0E6IMoxynhV6sMSJ0TIbYTakkAnU/yQ6e2iepEtiDUKVPGjtTfyYSIrSeCD+dFMSM9F9vJv7ndWMzuOwnTEaxQUnniwYxt01oz35vB0whNXySEkIVS2+16YgoQk3aUaGXBRMZmqwdb4QkQDUtZOVczXD2XcUyaVUr7nnFvT0t1crzliAPR3AMZXDhAmpwA3VoAgUBj/AEz9aD9WK9Wm/z0Zy1yBzCL1jvX40+mwY=</latexit>

<latexit sha1_base64="L/p+Vr8kdesOV/vRaTJLy1/9Ww8=">AAACCHicbVDLSsNAFL3xWeur6tJNsAhdlUR87gpuXFboS5pQJtPbdujMJMxMhBL6A27d6j+4E7f+hb/gV5ikRdR6YOBwzj3cOyeIONPGcT6speWV1bX1wkZxc2t7Z7e0t9/SYawoNmnIQ9UJiEbOJDYNMxw7kUIiAo7tYHyd+e17VJqFsmEmEfqCDCUbMEpMKt15ItBJY9pze6WyU3Vy2IvEnZNyrQA56r3Sp9cPaSxQGsqJ1l3XiYyfEGUY5TgterHGiNAxGWI3pZII1H6SHzy1j1Olbw9ClT5p7Fz9mUiI0HoignRSEDPSf71M/M/rxmZw6SdMRrFBSWeLBjG3TWhnv7f7TCE1fJISQhVLb7XpiChCTdpR0cuDiQxN3k5vhKSPalrMy7nKcPZdxSJpnVTd86p7e1quVWYtQQEO4Qgq4MIF1OAG6tAECgIe4QmerQfrxXq13majS9Y8cwC/YL1/AYuimwU=</latexit> <latexit sha1_base64="lGgnh+c9Kxz8btEHUNL8lInH9GU=">AAACQnicbZBNbxMxEIZnS4E0fAU49mI1IJUD0W4pX7egXjj00EpJWymbrrzOJLHqtVe2FxE5/j38Ea5cQf0LvaFeOeDdFASUV7L06J0ZzfjNS8GNjePzaO3G+s1bt1sb7Tt3791/0Hn46MioSjMcMiWUPsmpQcElDi23Ak9KjbTIBR7nZ3t1/fgDasOVHNhFieOCziSfckZtsLLOu+WSpEWuPrp9NfPbAY0b+GyX/KIXp+554p+R5fJ0J3ODxk4LaueMCnfovc863bgXNyLXIbmCbr8FjQ6yzkU6UawqUFomqDGjJC7t2FFtORPo22llsKTsjM5wFFDSAs3YNV/15GlwJmSqdHjSksb9c8LRwphFkYfO+kjzb602/1cbVXb6Zuy4LCuLkq0WTStBrCJ1bmTCNTIrFgEo0zzcSticaspsSLedNoNOKrvKdY50gtq3m3De1nr5O4rrcLTTS171ksPdbv/JKiVowSZswTYk8Br68B4OYAgMPsEX+Arfos/RRfQ9uly1rkVXM4/hL0U/fgKAX7Ky</latexit>

T1 T2 ||Log(T 4 T 3 1 )||2T Q
<latexit sha1_base64="IKJ3OpNoYBgG/yBlJEphW1gdWMY=">AAACQnicbZBNbxMxEIZny0fT8NEARy4WAakciHYD5eOWigsHDq2UtJWy6crrTBKrXntlexGR49/DH+HaK6h/oTfElQPeTUFAeSVLj96Z0YzfvBTc2Dg+jzauXb9xc7O11b51+87d7c69+4dGVZrhiCmh9HFODQoucWS5FXhcaqRFLvAoP31b148+oDZcyaFdljgp6FzyGWfUBivr7K1WJC1y9dG9V3O/E9C4oc+ek1/UP3HPEv+UrFYn/cwNGzstqF0wKtyB9z7rdONe3IhcheQSuoMWNNrPOhfpVLGqQGmZoMaMk7i0E0e15Uygb6eVwZKyUzrHcUBJCzQT13zVkyfBmZKZ0uFJSxr3zwlHC2OWRR466yPNv7Xa/F9tXNnZ64njsqwsSrZeNKsEsYrUuZEp18isWAagTPNwK2ELqimzId122gw6qew61wXSKWrfbsJ5U2v3dxRX4bDfS172koMX3cHjdUrQgofwCHYggVcwgHewDyNg8AnO4At8jT5HF9G36Pu6dSO6nHkAfyn68RN83bKw</latexit>

||Log(T 3 T 2 1 )||2T Q
<latexit sha1_base64="+Xgy1SB/OhkBpRaaevm+0gysdfE=">AAACUXicbVBNbxMxEJ1s+UhTPtJy5GIRkFIJot0KCtwicUGCQ5CatlI2XXmdSWLVa69sLyJy/Lf4I5yQuNID/4Ab3k2EgPKkkd68mSePX14Kbmwcf21FOzdu3rrd3u3s3bl77353/+DUqEozHDMllD7PqUHBJY4ttwLPS420yAWe5Zdv6vnZR9SGK3liVyVOC7qQfM4ZtUHKuqP1mqRFrj6592rh+4Ead+KzhKSl5m7bhv7CPUv8IVmvL44yRxo9LahdMircO+/7Nkuehjr0WbcXD+IG5DpJtqQ3bEODUdb9kc4UqwqUlglqzCSJSzt1VFvOBPpOWhksKbukC5wEKmmBZuqan3vyJCgzMlc6lLSkUf90OFoYsyrysFlfa/6d1eL/ZpPKzl9NHZdlZVGyzUPzShCrSB0jmXGNzIpVIJRpHm4lbEk1ZTaE3Ukbo5PKbmJeIp2h9p0mnNc1XvyO4jo5PRokx4Pkw/Pe8PEmJWjDQ3gEfUjgJQzhLYxgDAw+wzf4DletL62fEUTRZjVqbT0P4C9Ee78A8cC1MA==</latexit>

1
||Log(T 1 Ť 1 )||2K(t1 ,t1 )
<latexit sha1_base64="vQ4eA6yNTXLbCkA8TgdJcqQTap4=">AAACQnicbZBNbxMxEIZnW6Bp+Apw5GIRkMqBaDeifNxSceHAoZWStlI2XXmdSWLVa69sLyJy/Hv6R3rlCuIv9FZx5YB3UxBQXsnSo3dmNOM3LwU3No6/RRubN27e2mptt2/fuXvvfufBw0OjKs1wxJRQ+jinBgWXOLLcCjwuNdIiF3iUn76r60cfURuu5NAuS5wUdC75jDNqg5V19lYrkha5+uQ+qLnfCWjc0Gd98ouSE/ci8c/JanXSz9ywsdOC2gWjwh1477NON+7Fjch1SK6gO2hBo/2sc5FOFasKlJYJasw4iUs7cVRbzgT6dloZLCk7pXMcB5S0QDNxzVc9eRacKZkpHZ60pHH/nHC0MGZZ5KGzPtL8W6vN/9XGlZ29mTguy8qiZOtFs0oQq0idG5lyjcyKZQDKNA+3EragmjIb0m2nzaCTyq5zXSCdovbtJpy3tXZ/R3EdDvu95FUvOXjZHTxdpwQteAxPYAcSeA0DeA/7MAIGZ/AZvsDX6Dy6iC6j7+vWjehq5hH8pejHT3lbsq4=</latexit>

||Log(T 2 T 1 1 )||2T Q

Figure 3.5 Example GP motion prior factors when using a ‘random walk’ model.

 αk (t)
0−1
Note, we also need to substitute T 0 (t) = Tk0 Tk−1 0
Tk−1 for the nominal pose
at t, both within hi and Hi . We have essentially chained the derivative through
our linear spline. The same process can be followed for higher-order splines as well.

3.2.4.2 Gaussian Processes on Lie Groups

To use Gaussian processes on a Lie group, we will again exploit its Lie algebra to do
so. Figure 3.5 provides a visual teaser of the GP motion-prior factors resulting from
the ideas in this section. Note, as in the vector-space case, depending on the chosen
motion prior, the control-point state may comprise additional trajectory derivatives
as well.
To apply GPs on a Lie group, we will employ a local GP between a set of control-
point states [17], similar to splines. Figure 3.6 provides a depiction of these local
variables for SE(d). This means the SDE used to derive our kernel function operates
on these local variables. For example, in the case of a ‘random-walk’ prior for SE(d),

<latexit sha1_base64="Ci/UgnlvYDYDR644KxdugvsffM0=">AAACDHicbZDLSsNAFIZP6q3WW9Wlm2ARCkJJxOuu4MZlhd6gDWUyOWmHTiZxZiKUkFdw61bfwZ249R18BZ/CNC2i1h8Gfv7/HObwuRFnSlvWh1FYWl5ZXSuulzY2t7Z3yrt7bRXGkmKLhjyUXZco5ExgSzPNsRtJJIHLseOOr6d95x6lYqFo6kmETkCGgvmMEp1FTj9wVdJMB8n42E4H5YpVs3KZi8aem0q9CLkag/Jn3wtpHKDQlBOlerYVaSchUjPKMS31Y4URoWMyxF5mBQlQOUl+dGoeZYln+qHMntBmnv7cSEig1CRws8mA6JH6203D/7perP1LJ2EiijUKOvvIj7mpQ3NKwPSYRKr5JDOESpbdatIRkYTqjFOpny8mItQ5ocEIiYcyLeVwrqY6+0axaNonNfu8Zt+eVurVGSUowgEcQhVsuIA63EADWkDhDh7hCZ6NB+PFeDXeZqMFY76zD79kvH8BsWucuw==</latexit>

T k+1 <latexit sha1_base64="Hus2T3JWsAD8kA093tenlJbut0M=">AAACTXicbVBNaxsxFNS6H3HdL7c99iJqCg6lZrck/TgEDL30kEMKcRLwuotWfmuL1UqL9LbEiP1T+SO9tsfmmHtvpVS7G0LbdEAwb94MkiYtpbAYht+C3o2bt25v9e8M7t67/+Dh8NHjI6srw2HGtdTmJGUWpFAwQ4ESTkoDrEglHKf5+2Z//BmMFVod4qaERcFWSmSCM/RSMtyPi9S6+FTUST7GxOUvonqb7lEv61O3r1d1LCHDMW19h3XnuJryT+5lMxqxWuN2MhyFk7AFvU6iSzKa9kmLg2R4ES81rwpQyCWzdh6FJS4cMyi4hHoQVxZKxnO2grmnihVgF679dU2fe2VJM238UUhb9c+EY4W1myL1zoLh2v67a8T/7eYVZm8XTqiyQlC8uyirJEVNmwrpUhjgKDeeMG6Efyvla2YYR1/0IG6DTmnsKl4DW4KpB2057xrsXlVxnRy9mkSvJ9HHndF03LVE+uQpeUbGJCJvyJR8IAdkRjg5I1/Jd3IefAl+BD+DX521F1xmnpC/0Nv6DQHVtSY=</latexit>

<latexit sha1_base64="23jlmdD8pRRNmeGNdh/q6fXln0M=">AAACGHicbZDNSiNBFIVv+xvjz8Rx4cJNYRBchW6RUXeB2cxSwaiQhHC7cpMUVlc1VdViaPpF3LqdeYfZyWxn5yv4FFY6wf8DBYdz76GqvjiVwrowfAzm5hcWl5YrK9XVtfWNb7XN7xdWZ4ZTi2upzVWMlqRQ1HLCSbpKDWESS7qMr39O5pc3ZKzQ6tyNU+omOFRiIDg6H/Vq250k1rf5UOoYJbtBI9BXbdGr1cNGWIp9NtHM1JsVKHXaqz11+ppnCSnHJVrbjsLUdXM0TnBJRbWTWUqRX+OQ2t4qTMh28/IDBdvzSZ8NtPFHOVambxs5JtaOk9hvJuhG9uNsEn41a2ducNzNhUozR4pPLxpkkjnNJjRYXxjiTo69QW6EfyvjIzTInWdW7ZTFXGk3pTUi7JMpqh5O9MrkxOvocGZOohc4FweN6EcjOjusN/enlKACO7AL+xDBETThF5xCCzgUcA+/4U9wF/wNHoJ/09W5YNbZgncK/j8D4IGhxg==</latexit>

⇠ k (tk+1 ) = Log T k+1 T k 1

global variables <latexit sha1_base64="VU+hOrSHh0+eRUAHRxQrmPyyW68=">AAACCXicbVDLSgNBEOyNrxhfUY9eFoMQL2FXfN4CXjxGyAuTEGYnnWTI7Owy0yuEJV/g1av+gzfx6lf4C36Fm00QNRYMFFVddE95oRSGHOfDyiwtr6yuZddzG5tb2zv53b26CSLNscYDGeimxwxKobBGgiQ2Q43M9yQ2vNH11G/cozYiUFUah9jx2UCJvuCMEumu7Xsmrk6KdNzNF5ySk8JeJO6cFMpZSFHp5j/bvYBHPirikhnTcp2QOjHTJLjESa4dGQwZH7EBthKqmI+mE6cXT+yjROnZ/UAnT5Gdqj8TMfONGfteMukzGpq/3lT8z2tF1L/sxEKFEaHis0X9SNoU2NPv2z2hkZMcJ4RxLZJbbT5kmnFKSsq102CsAkrr6Q6R9VBPcmk5V1OcfVexSOonJfe85N6eFsrFWUuQhQM4hCK4cAFluIEK1ICDgkd4gmfrwXqxXq232WjGmmf24Res9y8JWJtE</latexit>

T (t) <latexit sha1_base64="odrlfACFwFBMs9D8g80hYOLj1Bo=">AAACRHicbVBNaxNBGH63Vo3xK9Wjl6FBSA+GXfGjHoRALx56qNC0hWxcZifvJkNmZ5aZd6Vh2R/kH/Haa/sXxFvxKs5ugqj1gYGH54OZedJCSUdheBVs3dq+fedu5173/oOHjx73dp6cOFNagWNhlLFnKXeopMYxSVJ4VljkearwNF0eNP7pZ7ROGn1MqwKnOZ9rmUnByUtJ7yDOU1fF57JOlgPaY++ZF8x5dWjmdawwowFrE8d1425osvxUvYhqFls5X9Be0uuHw7AFu0miDemPOtDiKOl9i2dGlDlqEoo7N4nCgqYVtySFwroblw4LLpZ8jhNPNc/RTav2szV77pUZy4z1RxNr1T8bFc+dW+WpT+acFu5frxH/501KyvanldRFSajF+qKsVIwMa5ZjM2lRkFp5woWV/q1MLLjlgvy+3bgtVtrQetkF8hnautuO867B699T3CQnL4fRm2H08VV/NFivBB14BrswgAjewgg+wBGMQcAXuIBLuAq+Bt+D6+DHOroVbDpP4S8EP38BLM+yVA==</latexit>

<latexit sha1_base64="A2/4dkhVt1+la+AuHanbipMnE/4=">AAACCHicbVDLSsNAFL3xWeur6tJNsAhdlUR87gpuXFboS5pQJtPbdujMJMxMhBL6A27d6j+4E7f+hb/gV5ikRdR6YOBwzj3cOyeIONPGcT6speWV1bX1wkZxc2t7Z7e0t9/SYawoNmnIQ9UJiEbOJDYNMxw7kUIiAo7tYHyd+e17VJqFsmEmEfqCDCUbMEpMKt15ItBJY9ob90plp+rksBeJOyflWgFy1HulT68f0ligNJQTrbuuExk/IcowynFa9GKNEaFjMsRuSiURqP0kP3hqH6dK3x6EKn3S2Ln6M5EQofVEBOmkIGak/3qZ+J/Xjc3g0k+YjGKDks4WDWJum9DOfm/3mUJq+CQlhCqW3mrTEVGEmrSjopcHExmavJ3eCEkf1bSYl3OV4ey7ikXSOqm651X39rRcq8xaggIcwhFUwIULqMEN1KEJFAQ8whM8Ww/Wi/Vqvc1Gl6x55gB+wXr/Auj6mz8=</latexit>
⇠ k (t) = Log T (t)T k 1
Tk
<latexit sha1_base64="bB4ELLFHG+Cl8yLxmTP7YU7gtio=">AAACFnicbZDNSgMxFIXv1L9a/6riyk2wCK7KjBSru4IblxVsFdpS7qSpDc0kQ5IRy9D3cOtW38GduHXrK/gUptPi/4HA4dyc5PKFseDG+v6bl5ubX1hcyi8XVlbX1jeKm1tNoxJNWYMqofRViIYJLlnDcivYVawZRqFgl+HwdDK/vGHacCUv7ChmnQivJe9zitZF3eJOOwrVbSoURUFuUHN0zXG3WPLLfiby1wQzU6rlIVO9W3xv9xRNIiYtFWhMK/Bj20lRW07de4V2YliMdIjXrOWsxIiZTpqtPyb7LumRvtLuSEuy9HsjxciYURS6mxHagfk9m4T/zVqJ7R93Ui7jxDJJpx/1E0GsIhMWpMc1o1aMnEGquduV0AFqpNYRK7SzYiqVnbIaMOwxPS44OMEXkxOnamVmToJPOM3DcnBUDs4rpdrBlBLkYRf24AACqEINzqAODaCQwj08wKN35z15z97L9GrOm3W24Ye81w8nTaDZ</latexit>

local variable

<latexit sha1_base64="tlItKr9j0nEeAr3N4ipsykVah1A=">AAACFXicbZDNSiNBFIVv+zMTozNmHHezKQxCZhO6JYzjYiDgxqWCUSEdmurKjSlSXd1U3RYzTZ5jtrPVd3Ans3XtK/gUVjph/D1QcDjnXuryxZmSlnz/3ltYXFr+8LGyUl1d+/R5vfZl48SmuRHYEalKzVnMLSqpsUOSFJ5lBnkSKzyNR/vT/vQCjZWpPqZxhr2En2s5kIKTi6LaZpjEtggv5SQaNSgafWe/mB/V6n7TL8XemmBu6u0KlDqMag9hPxV5gpqE4tZ2Az+jXsENSaFwUg1zixkXI36OXWc1T9D2ivL6Cdt2SZ8NUuOeJlamzzcKnlg7TmI3mXAa2tfdNHyv6+Y0+NkrpM5yQi1mHw1yxShlUxSsLw0KUmNnuDDS3crEkBsuyAGrhuVioVOaoRoi76OZVB2c4InJntNua272gv9wTnaawY9mcNSqtxszSlCBb7AFDQhgF9pwAIfQAQG/4S9cwbX3x7vxbr1/s9EFb77zFV7Iu3sEVuOfLQ==</latexit>

⇠ k (tk ) = 0

Figure 3.6 When using a GP for continuous-time estimation on Lie groups (e.g., SE(d)),
a local variable, ξk (t), is defined between control-point states.
72 Advanced State Variable Representations

we could choose the SDE to be

ξ̇k (t) = w(t), w(t) = GP(0, Qδ(t − t′ )), (3.61)

where we note that we have defined it using the local variable (between control
points Tk and Tk+1 ). The transition function for this SDE is simply Φ(t, s) = I
and so stochastically integrating we have
Z t
ξk (t) = ξk (tk ) + w(s) ds (3.62)
| {z } tk
0

and then after taking the mean and covariance we can say that the motion prior is

ξk (t) ∼ GP(0, min(t, t′ )Q). (3.63)

If we place our control-point poses uniformly spaced every T seconds then our
inverse kernel matrix will be simply K−1 = Φ−⊤ Q−1 Φ−1 with
 
I
−I I 
 
Φ−1 =  .. ..  , Q = diag (K(t1 , t1 ), T Q, . . . , T Q) . (3.64)
 . . 
−I I

The individual errors in terms of the local variables will be



Log T1 Ť1−1 k=1
ek = , (3.65)
ξk−1 (tk ) − ξk−1 (tk−1 ) k > 1

where Ť1 is some prior initial pose value. In terms of the global variables, these
same errors are


Log T1 Ť1−1
k = 1
ek = −1 . (3.66)
Log Tk Tk−1 k>1

Figure 3.5 shows what the ‘random walk’ GP motion prior looks like as a factor
graph. Similarly to the previous section discussing linear splines, if we want to query
the trajectory at other times of interest, we can do this using GP interpolation. For
the ‘random walk’ prior, this results again in linear interpolation [17]:
−1

αk (t)
T (t) = Tk Tk−1 Tk−1 , (3.67)

where αk (t) = t−(k−1)T

T and (k − 1)T ≤ t < kT . In contrast to the spline method,
this linear interpolation results indirectly from our choice of SDE at the beginning
rather than an explicit choice. Choosing higher-order SDEs at the start will result
in higher-order splines for interpolation.
The last part we need to understand is how to linearize our error terms for use
in MAP estimation. To do this, we again make use of the Lie group perturbation
 3.2 Continuous-Time Trajectories 73

approach detailed earlier. For example, looking at the second case in (3.66) we can
write
 
0−1
ek = Log Exp(ξk )Tk0 Tk−1 Exp(−ξk−1 )
   
0−1 0−1
≈ Log Tk0 Tk−1 + ξk − Ad Tk0 Tk−1 ξk−1 , (3.68)

where Tk0 and Tk−10

are current guesses, ξk and ξk−1 are the to-be-solved-for per-
turbations, and Ad(·) is the adjoint for SE(d). This linearized form for ek can be
inserted in our standard MAP estimation framework at each iteration.
Additionally, if we want to use (3.67) to reduce the number of control points in
this ‘random walk’ example, we can make use of the same approach developed for
linear splines detailed in (3.60), since both methods boil down to linear interpolation
between SE(d) control points. Ultimately, then, the big difference between the spline
and GP approaches is that the GP approach employs motion-prior terms (see Figure
3.5) to regularize the problem, while the spline approach does not.4

4
Johnson et al. [124] provide a detailed comparison between spline and GP approaches and shows
that motion-prior terms can also be introduced to regularize spline methods.
 4
Robustness to Incorrect Data
Association and Outliers
Heng Yang, Josh Mangelson, Yun Chang, Jingnan Shi, and Luca Carlone

In Chapter 2, we have seen that factor graphs are a powerful representation to model
and visualize SLAM problems, and that maximum a posteriori (MAP) estimation
provides a grounded and general framework to infer variables of interest (e.g., robot
poses and landmark positions) given a set of measurements (e.g., odometry and
landmark measurements). For instance, we observed that when the measurements
zi are affected by additive and zero-mean Gaussian noise with covariance Σi , MAP
estimation leads to a nonlinear least-squares optimization:
X 2
xMAP = arg min ∥zi − hi (xi )∥Σi , (4.1)
x
i

where xi denotes the subset of the states involved in measurement i.1 In this chap-
ter we notice that in practice many measurements zi —possibly due to incorrect
data association— may have large errors, which are far from following a zero-mean
Gaussian (Section 4.1); these measurements typically induce large perturbations in
the estimate xMAP from eq. (4.1). Therefore, we discuss how to reject gross outliers
in the SLAM front-end (Section 4.2) and then focus on how to increase robustness
to remaining outliers in the SLAM back-end (Section 4.3). We close the chapter
with a short review of recent trends and extra pointers to related work (Section
4.4).

4.1 What Causes Outliers and Why Are They a Problem?

This section argues that outliers are inevitable in most SLAM applications and that
not handling them appropriately leads to grossly incorrect estimates.

4.1.1 Data Association and Outliers

To understand the cause of outlier measurements, let us consider two examples.
First, consider a landmark-based SLAM problem, where we have to reconstruct
1
While for simplicity eq. (4.1) assumes that measurements belong to a vector space, the algorithms in
this chapter apply to arbitrary SLAM problems where variables belong to manifolds, see Chapter 3.
 4.1 What Causes Outliers and Why Are They a Problem? 75

the trajectory of the robot and the position of external landmarks from odometry
measurements and relative observations of landmarks from certain robot poses. As-
suming (as we did in Chapter 2) that the landmark measurements have zero-mean
Gaussian noise leads to terms in the optimization in the form ∥zij − h(pi , ℓj )∥2Σ .
These terms model the fact that a given measurement zij is an observation of land-
mark ℓj from pose pi up to Gaussian noise, where h(·) is the function describing the
type of relative measurement (e.g., range, bearing, etc.). In practice, the measure-
ments zij are obtained by pre-processing raw sensor data in the SLAM front-end.
For instance, if the robot has an onboard camera and zij is a visual observation of
the bearing to a landmark ℓj , the measurement zij might be extracted by perform-
ing object (or more generally, feature) detection and matching in the image, and
then computing the bearing corresponding to the detected pixels. Now, the issue
is that the detections are imperfect and a landmark detected as ℓj in the image,
might be actually a different landmark in reality. This causes zij to largely deviate
from the assumed model. The problem of associating a measurement to a certain
landmark is typically referred to as the data association problem and is common to
many other estimation problems (e.g., target tracking). Therefore, incorrect data
association creates outliers in the estimation problem.

As a second example, consider a pose-graph optimization problem, where we

are primarily interested in estimating the trajectory of the robot (represented as
a set of poses), and the measurements are either odometry measurements (which
relate consecutive poses along the trajectory) or loop closures (which relate non-
consecutive and possibly temporally distant poses). In practice, the loop closures
are detected using (vision-based or lidar-based) place recognition methods, which
are in charge of detecting if a pair of poses pi and pj have observed the same portion
of the environment. Unfortunately current place recognition methods are prone to
making mistakes and detecting loop closures between poses that are not observing
the same scene. This is partially due to limitations of current methods, but it is
often due to perceptual aliasing, that is the situation where two similarly looking
locations actually correspond to different locations (think of two classrooms in a
university building, or similarly looking cubicles in an office environment). This can
be again understood as a failure of data association, where we mistakenly associate
the loop closure measurement to two incorrectly chosen robot poses.

Note that outliers are not only caused by incorrect data associations, but can
also be caused by violations of the assumptions made in the SLAM approach. For
instance, the majority of SLAM approaches assume landmarks to be static, hence
detections of a moving object —even when correctly associated to that object—
may lead to outlier measurements with large residuals. Similarly, sensor failure
and degradation, e.g., a faulty wheel encoder or dust on the camera lens, might
contribute to creating outliers in the measurements.
76 Robustness to Incorrect Data Association and Outliers

4.1.2 Least-Squares in the Presence of Outliers

In the presence of outliers, the estimate resulting from the least-squares formula-
tion (4.1) can be grossly incorrect. From the theoretical standpoint, the Gaussian
noise we assumed for the measurement is “light-tailed”, in that it essentially rules
out the possibility of measurements with very large error. From a more practical
perspective, the outliers lead to terms in the objective function where the residual
error ri (x) := ∥zij − h(pi , ℓj )∥Σ is very large, when evaluated near the ground
truth. Since the residuals are squared in the objective of the optimization, i.e., the
P
objective is i ri (x)2 , these residuals have a disproportionately large impact on
the cost, and the optimization focuses on minimizing the large terms induced by
the outliers rather than making good use of the remaining (inlier) measurements.

M3500 SubT Victoria Park

Ground Truth

(a) (b) (c)

Least Squares

(d) (e) (f)

Figure 4.1 SLAM problems with outliers: (a)-(c) Ground truth trajectories for the M3500,
SubT, and Victoria Park datasets. (d)-(f) Trajectory estimates obtained with the least-
squares formulation in the presence of outliers. Inlier measurements are visualized as gray
edges, while outliers are visualized as red edges. In the SubT dataset, we also visualize a
dense map built from the SLAM pose estimate.

To illustrate this point, Figure 4.1 shows results for three SLAM problems with
outliers. The first column is a simulated pose-graph optimization benchmark, known
as M3500, with poses arranged in a grid-like configuration; the dataset includes 3500
2D poses and 8953 measurements. The second column is a real-world pose-graph
dataset, denoted as SubT, collected in a tunnel during the DARPA Subterranean
Challenge [82]; the dataset includes 682 3D poses and 3278 measurements. The third
column is a real-world landmark-SLAM dataset, known as Victoria Park [187]; the
dataset includes 7120 2D poses and landmarks, and 17728 measurements. Figure
 4.2 Detecting and Rejecting Outliers in the SLAM Front-end 77

Figure 4.2 Feature tracking across three frames (collected at time k − 1, k, and k + 1) in a
visual SLAM problem. Inliers are visualized in green, while outliers are visualized in red.

4.1(a)-(c) show the ground truth trajectories for the three problems. Figure 4.1 (d)-
(f) show the estimate produced by the least-squares formulation in the presence of
outliers. In particular, for M3500 and Victoria Park we add 15% random outliers
to the (loop closures or landmark) measurements, while the SubT dataset already
includes outliers. In the figure, we visualize outlier measurements in red. We observe
that the presence of outliers leads to completely incorrect trajectories and map esti-
mates. Moreover, the outliers often expose perceptual aliasing in the environment:
for instance, the two similarly looking vertical corridors in the middle of the SubT
dataset induce many spurious loop closures, which mislead the back-end to create
a map with a single vertical corridor.

4.2 Detecting and Rejecting Outliers in the SLAM Front-end

The main role of the SLAM front-end is to extract intermediate representations
or (pseudo-)measurements —which will be converted into factors for the back-
end— from the raw sensor data. Typical SLAM front-ends accomplish this by first
computing an initial set of measurements (possibly corrupted by many outliers)
and then post-processing the initial set to remove outliers. This section discusses
two approaches to reject outliers in the SLAM front-end.

4.2.1 RANdom SAmple Consensus (RANSAC)

RANSAC is a well-established tool for outlier rejection [89] and is a key component
of many landmark-based SLAM systems. In order to understand what RANSAC is
and its role in SLAM, consider a landmark-based visual SLAM approach.

Example 4.1 (Outliers in landmark-based visual SLAM). A landmark-based (or

78 Robustness to Incorrect Data Association and Outliers

feature-based) visual-SLAM approach extracts 2D feature points in each image and

then associates them across consecutive frames using either optical-flow-based fea-
ture tracking or descriptor-based feature matching (Figure 4.2). In particular, at
time k, the approach detects 2D feature points and matches them with correspond-
ing points observed in the previous frame (say, at time k − 1); the matching pixels
are typically referred to as 2D-2D correspondences. Due to inaccuracies of optical
flow or descriptor-based matching, this initial set of correspondences might contain
outliers. Therefore, it is important to filter out gross outliers before passing them
to the back-end, which estimates the robot poses and landmark positions.

RANSAC is a tool to quickly detect and remove outliers in the correspondences

before passing them to the back-end. Detecting outliers relies on two key insights.
The first insight is that in SLAM problems, inlier correspondences must satisfy
geometric constraints. For instance, in our example, inlier correspondences picture
the observed pixel motion of static 3D points as the camera moves. The resulting
pixel motion cannot be arbitrary, but must follow a precise geometric constraint,
known as the epipolar constraint, which dictates how corresponding pixels in two
frames are related depending on the camera motion. In particular, for calibrated
cameras, the epipolar constraint imposes that corresponding pixels zi (k − 1), zi (k)
—picturing landmark i at time k − 1 and k, respectively— satisfy


zi (k − 1)T [tk−1
k ]× Rkk−1 zi (k) = 0, (4.2)

where tk−1
k and Rkk−1 are the relative position and rotation describing the (un-
known) motion of the camera between time k − 1 and k.2 More generally, if we
denote the i-th correspondence as zi (in the example above, zi = {zi (k −1), zi (k)}),
these geometric constraints are in the form

C(zi , x) ≤ γ, (4.3)

which states that the correspondences have to satisfy some inequality, which is
possibly a function of the unknown state x; in (4.3) the parameter γ on the right-
hand-side is typically tuned to account for the presence of noise. For instance, while
ideally the epipolar constraint in (4.2) is exactly satisfied, in practice it might have
small errors since the pixel detections are noisy, and
hence we would relax the
constraint to only require zi (k − 1)T [tk−1k ]×Rk
k−1
zi (k) ≤ γ, for some small γ.
The second insight is that —assuming we do not have too many outliers— we
can find the inliers as the largest set of correspondences that satisfy the geometric

2
Clearly, different problems will have different geometric constraints, but luckily there is a
well-established literature in robotics and computer vision, that studies geometric constraints
induced by different types of sensor measurements. The example in this section considers 2D-2D
correspondences, and the corresponding constraints have been studied in the context of 2-view
geometry in computer vision, see [111].
 4.2 Detecting and Rejecting Outliers in the SLAM Front-end 79

constraint (4.3) for some x:

S∗CM = argmax |S|

x,S⊂M (4.4)
s.t. C(zi , x) ≤ γ, ∀i ∈ S

where M is the set of initial putative correspondences, and |S| denotes the cardi-
nality (number of elements) in the subset S [169]. In words, the optimization (4.4)
looks for the largest subset S of the set of putative correspondences M, such that
measurements in S satisfy the geometric constraints for the same value of x. Intu-
itively, problem (4.4) captures the intuition that the inliers (estimated by the set S)
must “agree” on the same x (e.g., they must all be consistent with the actual mo-
tion of the robot). Problem (4.4) is known as consensus maximization in computer
vision. Note that (4.4) does not require solving the entire SLAM problem (which
might involve many poses and landmarks), since it only involves a small portion of
the SLAM state; for instance, the epipolar constraint (4.2) only involves the rela-
tive pose between two frames rather than the entire SLAM trajectory. At the same
time, (4.4) is still a hard combinatorial problem, which clashes with the fast run-
time requirements of typical SLAM front-ends. Therefore, rather than looking for
exact solutions to (4.4), it is common to resort to quick heuristics to approximately
solve (4.4).
RANdom SAmple Consensus (RANSAC) is probably the most well-known ap-
proach to find an approximate solution to the consensus maximization problem
in (4.4). RANSAC builds on the key assumption that x in (4.4) is relatively low-
dimensional and can be estimated from a small set of measurements (the so-called
minimal set), using fast estimators (the so called minimal solvers).3 For instance, in
our visual SLAM example, one can estimate the relative motion between two cam-
era frames using only 5 pixel correspondences, using Nister’s 5-point method [188].
Then the key idea behind RANSAC is that, instead of exhaustively checking every
possible subset S ⊂ M, one can sample minimal sets of measurements looking for
inliers. More in detail, RANSAC iterates the following three steps:

1 Sample a subset of n correspondences, where n is the size of the minimal set for
the problem at hand;4
2 Compute an estimate x̂ from the n sampled correspondences using a minimal
solver;5
3 Select the correspondences S ⊂ M that satisfy the geometric constraint C(zi , x̂) ≤
3
The development of minimal solvers can be considered a sub-area of computer vision research,
hence for typical problems it is well-understood what is the size of the minimal set and there are
well-developed (and typically off-the-shelf) minimal solvers one can use.
4
In our example with pixel correspondences between calibrated camera images, the minimal set has
size n = 5, since 5 non-collinear measurements are sufficient to determine the pose between two
cameras up to scale.
5
In our example, this involves computing the relative motion (up to scale) between time k − 1 and k
using the 5-point method.
80 Robustness to Incorrect Data Association and Outliers

γ for the x̂ computed at the previous step. Store the set S if it is larger than the
set computed at the previous iterations.

The set S computed in the last step is called the consensus set and RANSAC
typically stops after computing a sufficiently large consensus set (as specified by
a user parameter) or after a maximum number of iterations. RANSAC essentially
attempts to sample n inliers from the set of measurements, since these are likely to
“agree” with all the other inliers and hence have a large consensus set.
RANSAC is the go-to solution for many outlier-rejection problems. In particu-
lar, it quickly converges to good estimates (i.e., good sets of correspondences) in
problems with small number of outliers and small minimal sets. Assuming that the
probability of sampling an inlier from the set of measurements is ω,6 it is easy
to conclude that the expected number of iterations RANSAC requires for finding
a set of inliers is ω1n . For instance, when n = 5 and ω = 0.7 (i.e., 70% of the
measurements are inliers), the expected number of iterations is less then 10. This,
combined with the fact that non-minimal solvers are extremely fast in practice (al-
lowing even thousand of iterations in a handful of milliseconds), makes RANSAC
extremely appealing. Moreover, RANSAC also provides an estimate x̂ (e.g., the
robot odometry), that can be useful as an initial guess for the back-end.
On the downside, RANSAC may not be the right approach for all problems. In
particular, the expected number of iterations becomes impractically large when the
number of inliers is small or when the minimal set is large; for instance, when n = 10
and ω = 0.1, the expected number of iterations to find a set of inliers becomes 1010 ,
and terminating RANSAC after a smaller number of iterations is likely to return
incorrect solutions (i.e., incorrect x̂ and correspondences). As we discuss in the
next section, in context of many SLAM problems, the assumptions of having many
inliers and small minimal sets are not always valid.

4.2.2 Graph-theoretic Outlier Rejection and

Pairwise Consistency Maximization
As we mentioned, RANSAC is very effective when the number of outliers is rea-
sonable (say, below 70%) and the size of the minimal set is small (say, less than
8). However, environments with severe perceptual aliasing might have very high
number of outliers. Moreover, not all the problems we are interested in have a fast
minimal solver with a small minimal set. For instance, if we consider a pose-graph
SLAM problem with N nodes, the minimal set must include at least N − 1 mea-
surements (forming a spanning tree of the pose-graph), and N is typically in the
thousands.
For these reasons, this section introduces an alternative approach, known as pair-
6
Assuming that samples are drawn uniformly at random, ω can be thought of as the fraction of
measurements that are inliers.
 4.2 Detecting and Rejecting Outliers in the SLAM Front-end 81

(a) (b)

Figure 4.3 (a) 3D-3D correspondences from two RGB-D scans representing two partial
views of a scene. The green lines indicate inlier correspondences and the red lines indicate
outlier correspondences. (b) Pose graph with outliers in the loop closures. The dashed
green lines indicate inlier loop closures while the dotted red line is an outlier loop closure.

wise consistency maximization (PCM), that, rather than sampling minimal sets,
seeks to find the largest set of measurements that are internally “consistent” with
one another, using graph theory. This approach can be used to sort through sets of
measurements with upwards of 90% outliers and prune gross outliers before passing
them to the back-end. The approach was initially proposed in [169] and extended
beyond pairwise consistency in [236, 91].
The key insight behind PCM is that for many problems one can define consistency
functions that capture whether a pair of measurements are consistent with each
other. Let’s elucidate on this point with two examples.

Example 4.2 (Consistency Function in landmark-based visual SLAM with RGB-D

cameras). A landmark-based visual-SLAM approach with RGB-D cameras extracts
3D feature points in each RGB-D frame and then associates them across consecutive
frames (Figure 4.3(a)). In particular, at time k, the approach detects 3D feature
points and matches them with corresponding points observed in the previous frame
(say, at time k − 1); the matching 3D points are typically referred to as 3D-3D
correspondences. We observe that the 3D points collected at time k and k − 1
ideally correspond to the same set of 3D static points observed from two different
viewpoints; therefore, the distance between a pair of corresponding points {zi (k −
1), zj (k − 1)} and {zi (k), zj (k)} has to be constant over time (up to noise):

|∥zi (k − 1) − zj (k − 1)∥ − ∥zi (k) − zj (k)∥| ≤ γ (4.5)

We observe that contrary to the geometric constraints used in RANSAC, the con-
sistency function (4.5) (i) does not depend on the state, hence it can be evaluated
directly without the need for a minimal solver, and (ii) involves a pair of correspon-
dences regardless of the size of the minimal set. While the previous example could
also be solved with RANSAC,7 let us now consider a higher dimensional problem.

7
Motion estimation from 3D-3D correspondences admits a fast 3-point minimal solver, e.g., Horn’s
method [115].
82 Robustness to Incorrect Data Association and Outliers

Example 4.3 (Consistency Function in pose-graph SLAM). Consider a pose-graph

SLAM problem where loop closures might contain outliers due to place recognition
failure and perceptual aliasing; we assume the odometry is reliable and outlier
free. In order to understand if two loop closures are consistent with each other,
we observe that in the noiseless case, pose measurements along cycles in the graph
must compose to the identity (Figure 4.3(b)).8 Therefore, a pair of loop closures
Tab (between poses a and b) and Tcd (between poses c and d) must satisfy:
dist(Tab · T̄bc · Tcd · T̄da , I) ≤ γ (4.6)
where T̄bc and T̄da are the chain of odometry measurements from node b to node
c, and from node d to node a, respectively, and dist is a suitable distance function
that measures how far is Tab · T̄bc · Tcd · T̄da from the identity pose. As usual, γ is
a parameter chosen to account for the noise: measurements along a loop might not
compose to the identity due to noise in the odometry and loop closures.9
More generally, a consistency function is a function relating two measurements
and that have to satisfy a given constraint. For a pair of measurements zi and zj ,
the resulting pairwise consistency constraints are in the form:
F (zi , zj ) ≤ γ, (4.7)
where F is the consistency function, and γ is a user-specified parameter that ac-
counts for measurement noise. We remark that the pairwise consistency constraint
are state independent, hence they can be efficiently checked without resorting to a
minimal solver by just inspecting every pair of measurements.
Using (4.7), we can formulate an alternative approach for outlier rejection, which
selects the largest set of measurements that are pairwise consistent:
S∗PCM = argmax |S|
S⊂M (4.8)
s.t. F (zi , zj ) ≤ γ, ∀i, j ∈ S
Problem (4.8) looks for the largest subset S of measurements such that every pair of
measurements in S are pairwise consistent. We refer to this as the pairwise consis-
tency maximization (PCM) problem. This problem is still combinatorial in nature,
but appears slightly easier than (4.4): the problem does not involve x, and the con-
straints F (zi , zj ) ≤ γ can be pre-computed for every pair (i, j) in M. Furthermore,
the problem admits a graph-theoretic interpretation, which allows solving (4.4) us-
ing well-established tools from graph theory, namely, maximum clique algorithms.
In order to draw a connection between problem (4.8) and graph theory, let us
visualize the outlier-rejection problem as a graph G, where the nodes of the graph
are the putative measurements i ∈ M and an edge exists between two nodes i and j if
8
Intuitively, if we walk back along a loop in the environment we come back to our initial location.
9
In practice, one would select γ to account for the size of the loop: intuitively, longer loops will
accumulate more noise, see [169, 81].
 4.2 Detecting and Rejecting Outliers in the SLAM Front-end 83

(a) (b)

Figure 4.4 (a) Consistency graph of the 3D-3D correspondences example in Figure 4.3(a).
(b) Consistency graph of the loop closures for the pose-graph example in Figure 4.3(b)

F (zi , zj ) ≤ γ. This is typically called the consistency graph. Now problem (4.8) asks
to select the largest subset of nodes S such that every pair of nodes in S is connected
by an edge: this is exactly the definition of maximum clique of a graph. More in
detail, a clique in graph theory is a subset of nodes in which every pair of nodes has
an edge between them, and the maximum clique is the largest such subset of nodes
in the graph. Therefore, the solution to problem (4.8) is the maximum clique of the
consistency graph G. This graph theoretic connection is really useful in practice,
since the problem of finding the maximum clique for a given graph is a well-studied
problem in graph theory and is called the maximum clique problem. The maximum
clique problem is an NP-hard problem [290] and hard to approximate [313, 88],
meaning that finding a solution arbitrarily close to the true solution is also NP-
hard. However, dozens of potential solutions have been proposed, some of which
can handle significantly sized problems depending on the density of the graph. The
majority of proposed methods can be classified as either exact or heuristic-based
methods. All of the exact algorithms are exponential in complexity and are usually
based on branch and bound, while the heuristic algorithms often try to exploit
some type of structure in the problem, making them faster, at the expense of not
necessarily guaranteeing the optimal solution [290]. Relatively recent works, e.g.,
[203], propose maximum clique algorithms that are parallelizable and able to quickly
find maximum cliques in sparse graphs.
In summary, solving the PCM problem using a maximum clique algorithm in-
volves the following steps:

1 Select a Consistency Function F for the problem at hand.

2 Evaluate the Consistency Function for every pair of putative measurements (i, j) ∈
M, and create a consistency graph with edges between i and j when F (zi , zj ) ≤ γ.
3 Solve for the Maximum Clique of the Consistency Graph using exact or approx-
imate maximum clique algorithms.
84 Robustness to Incorrect Data Association and Outliers

4 Return measurements S in the (possibly approximate) maximum clique.

We remark that the choice of consistency function is problem-dependent. More-

over, choosing a good consistency function might largely influence the quality of
the outlier rejection. For instance, one could select a dummy function F (zi , zj ) = 0
which always returns zero regardless of the arguments; such a function would not
allow rejecting any outliers, hence making PCM ineffective. On the other hand, if
we make the function such that only the inliers can pass the test, then we would ex-
actly reject all the outliers. A selection of potential consistency functions for broad
variety of geometric problems is discussed in [236, 91].
Before concluding this section a few remarks are in order. While we observed that
PCM has the ability to handle a large number of outliers compared to RANSAC
and is more suitable for certain problems (e.g., pose-graph SLAM), the trade-offs
between PCM and RANSAC are more nuanced. RANSAC evaluates the consistency
of individual measurements using an estimate computed by a minimal solver; PCM,
on the other hand, evaluates the consistency of a set of measurements to each
other in a pairwise manner. In certain cases, RANSAC’s individual consistency is
insufficient to evaluate the set of measurements as a whole: this is often the case
in pose-graph optimization where individual consistency of a pair of loop-closure
measurements does not necessarily ensure pairwise consistency of the two loop-
closures.10 On the other hand, for certain problems such as 3D-3D pose estimation
(Example 4.2), the pairwise consistency function (4.5) used in PCM might be more
permissive then RANSAC and lead to classifying certain outliers as inliers.
In the context of PCM, it is also important to note that exact maximum clique
solvers tend to be slow in dense consistency graphs (i.e., when many pairs of mea-
surements are consistent), hence heuristic-based maximum-clique solutions may be
a better choice for certain problems. Finally, for certain problems, it might be hard
to design a suitable consistency function; for instance, for 2D-2D correspondences,
there is no easy way to rigorously design a general consistency function due to the
lack of suitable invariances (see discussion in [236]).

4.3 Increasing Robustness to Outliers in the SLAM Back-end

Front-end outlier rejection, including both RANSAC and PCM, might still miss
outliers and pass an outlier-contaminated set of measurements to the back-end.11
As we have seen in Section 4.1.2, a handful of outliers can lead to completely wrong
10
This is especially pronounced in the context of the multi-robot pose-graph optimization — where
the goal is to estimate the trajectory of two (or more) robots jointly within a single pose-graph. In
these contexts in particular, PCM has been shown to dramatically outperform RANSAC [169].
11
Intuitively, both the geometric constraints (4.3) —even when evaluated at the ground truth x—
and the pairwise consistency constraints (4.7) are necessary (but not sufficient) conditions for
measurements to be inliers. Moreover, consensus maximization and PCM are often solved with
approximation algorithms that do not guarantee an optimal selection of the inliers.
 4.3 Increasing Robustness to Outliers in the SLAM Back-end 85

results when using standard least squares estimation. Therefore, it is important to

enhance the back-end to be robust to remaining outliers.
In Section 4.1.2, we observed that the use of squared residuals “amplifies” the
impact of outlying measurements on the cost function. In this section we slightly
modify the objective function in the SLAM optimization to regain robustness to
outliers, following the standard theory of M-Estimation in robust statistics [117].
M-Estimation (“Maximum-likelihood-type Estimation”) is a framework for ro-
bust estimation and suggests replacing the squared loss in eq. (4.1) with a suitably
chosen robust loss function ρ:
X X
xMAP = arg min ri (x)2 =⇒ xMEST = arg min ρ (ri (x)) . (4.9)
x x
i i

The key requirement for the robust loss ρ is to be a non-negative function and grow
less than quadratically for large residuals; in other words, robust loss functions
∂∥ri ∥2
need to have derivative ∂ρ(ri)
∂ri ≪ ∂ri = 2ri as ri becomes large; in many cases,
it is desirable for ∂ρ(ri)
to approach zero as ri becomes large. To elucidate this
∂ri P
requirement, consider the case where we solve minx i ρ (ri (x)) using gradient
. P
descent. Using the chain rule, the gradient of the objective f (x) = i ρ (ri (x))
becomes:
∂f X ∂ρ (ri ) ∂ri (x)
= · (4.10)
∂x i
∂ri ∂x

From (4.10), it is clear that if we start for a good initial guess (i.e., relatively
close to the ground truth), outlier measurements will have large residual and hence
very small ∂ρ(r i)
∂ri , thus having a minor influence on the overall descent direction.
Hence they will have almost no influence in the estimation. Indeed, the function
ψ(ri ) := ∂ρ(ri)
∂ri is typically referred to as the influence function [31].
Rather than a single choice of robust loss function, the robust estimation litera-
ture provides a “menu” of potential choices. Figure 4.5 lists common choices of loss
functions. This list includes common robust losses, such as Huber, Geman-McClure,
Tukey’s biweight and the truncated quadratic loss, and also includes a more radical
choice, named maximum consensus loss. The latter is not typically listed among
the loss functions in the robust estimation literature, but we mention it here, since
it connects back to the consensus maximization problem we discussed in (4.4).12
The choice of robust loss is fairly problem-dependent [133, 253]. For instance, loss
functions with hard cut-offs (e.g., the truncated quadratic loss, where there is a
sudden transition between the quadratic and the “flat” portion of the function) are
preferable when a reasonable threshold for the cut-off (i.e., the maximum error ex-
pected from the inliers) is known. One also has to take into account computational
12
Intuitively, the maximum consensus loss “counts” the outliers in the estimation problem, since it is
constant (typically equal to 1) for large residuals and zero for small residuals. Therefore, minimizing
such loss leads to an estimate that produces the least number of outliers. This is the same as
solving the consensus maximization problem in (4.4).
86 Robustness to Incorrect Data Association and Outliers

(a) Quadratic loss (b) Huber loss (c) Geman-McClure

(d) Tukey’s biweight (e) Truncated Quadratic (f) Maximum Consensus loss

Figure 4.5 Quadratic loss and examples of robust loss functions. The shape of the robust
loss functions is controlled by a parameter that controls the separation between inliers
and outliers.
considerations. For instance, Huber is often used in bundle adjustment problems
since it is a convex function and is better-behaved during the optimization, despite
leaving non-zero influence for the outliers (the influence becomes zero only if the
loss is constant for large residuals). On the other hand, the truncated quadratic
and maximum consensus losses are known to be particularly insensitive to outliers,
but they often require ad-hoc solvers.13
Figure 4.6(g)-(l) show the SLAM trajectories obtained by applying gradient de-
scent to two of the robust losses mentioned above: the Huber loss and the trun-
cated quadratic loss. Here we consider the same datasets used in Figure 4.1. We
implemented the gradient descent solver using GTSAM’s NonlinearConjugateGra-
dientOptimizer [67] with the gradientDescent flag enabled, and using robust noise
models to instantiate the robust loss functions. We set the maximum number of
iterations and the stopping conditions thresholds (relative and absolute tolerance)
to 10000 and 10−7 , respectively. All other parameters were left to the default GT-
SAM values. In the figure, an edge is colored in gray if it is correctly classified
as inlier or outlier by the optimization (i.e., an inlier that falls in the quadratic
region of the Huber or truncated quadratic loss); it is colored in red if it is an
outlier incorrectly classified as an inlier (a “false positive”); it is colored in blue if
it is an inlier incorrectly classified as an outlier (a “false negative”). Compared to
13
For instance, RANSAC (Section 4.2.1) optimizes the maximum consensus loss via sampling (an
option that is only viable for the low-dimensional optimization problems arising in the front-end),
while graduated non-convexity allows optimizing a broad variety of losses including the truncated
quadratic loss (more on this in Section 4.3.4 below).
 4.3 Increasing Robustness to Outliers in the SLAM Back-end 87

Ground Truth M3500 SubT Victoria Park

(a) (b) (c)

Huber + gradient

(g) (h) (i)

TLS + gradient

(j) (k) (l)

Figure 4.6 Solving SLAM problems with outliers using robust loss functions and gradi-
ent descent: (a)-(c) Ground truth trajectories for the M3500, SubT, and Victoria Park
datasets. (d)-(f) Trajectory estimates obtained using gradient descent and Huber loss.
(h)-(l) Trajectory estimates obtained using gradient descent and truncated quadratic loss.
Measurements are visualized as colored edges. In particular, an edge is colored in gray if
it is correctly classified as inlier or outlier by the optimization (i.e., an inlier that falls in
the quadratic region of the Huber or truncated quadratic loss); it is colored in red if it is
an outlier incorrectly classified as an inlier (a “false positive”); it is colored in blue if it is
an inlier incorrectly classified as an outlier (a “false negative”).

(non-robust) least squares optimization (Figure 4.1(d)-(f)), we note that the use of
the robust losses allows us to quickly regain robustness to outliers in the case of
the M3500 and SubT datasets, but a simple gradient descent method may still fail
to correctly optimize heavily non-convex functions such as the truncated quadratic
cost, as in the case of the Victoria Park dataset. We will address this issue with
a better solver, based on graduated non-convexity, below. Moreover, while gradient
descent already improves performance in many of the instances as shown in Figure
4.6, it has slow convergence tails. For instance, in our experiments, it often takes
88 Robustness to Incorrect Data Association and Outliers

thousands of iterations to converge. Therefore, in the rest of this section we discuss

more advanced solvers, that improve both convergence quality and speed.
As a concluding remark before delving into more advanced solvers, we observe
that while it might seem that we gave up on our probabilistic framework when
switching to robust loss functions, it is actually possible to derive several robust
losses by applying MAP estimation to heavy-tailed noise distributions. For instance,
the truncated quadratic loss results from MAP estimation when assuming the noise
follow a max-mixture distribution between a Gaussian density (describing the in-
liers) and a uniform distribution (describing the outliers) [15].

4.3.1 Iteratively Reweighted Least Squares

M-Estimation replaces the least-squares loss by a robust loss ρ in (4.9) — a function
that grows sub-quadratically for large residuals. This comes with two prices. First,
we lose the efficient solutions already developed for least-squares formulations; for
instance, the Gauss-Newton and the Levenberg-Marquardt methods are designed
for least squares problems. Second, due to the typical non-convex landspace of M-
Estimation, iterative solvers (e.g., based on gradient descent) are sensitive to the
quality of initialization and often converge to undesired suboptimal estimates. In
this section, we introduce a popular algorithm for solving M-Estimation called iter-
atively reweighted least squares (IRLS) which, as the name suggests, allows reusing
the efficient least-squares solvers. In the next section, we introduce graduated non-
convexity as a technique to improve the convergence of IRLS.
The basic idea behind IRLS is to optimize (4.9) by solving a weighted least squares
problem at each iteration
X
x(t+1) = arg min wi (x(t) )ri2 (x), (4.11)
x
i

where the weights wi ’s depend on the estimate x(t) from the last iteration. We wish
the iterative solutions x(t) to converge to the optimal solution of M-Estimation (4.9).
This implies that the gradient of the robust loss ρ, shown in (4.10), must match
the gradient of the loss in (4.11). By writing down the gradient of (4.11) as
X ∂ri (x)
2wi (x(t) )ri (x)
i
∂x

and comparing it to (4.10), we obtain the IRLS weight update rule

1 ∂ρ(ri (x(t) )) ψ(ri (x(t) ))

wi (x(t) ) = = , (4.12)
2ri (x(t) ) ∂ri (x(t) ) 2ri (x(t) )

where we recall that ψ(ri ) := ∂ρ(r

∂ri
i)
is the influence function. Therefore, IRLS
alternates computing the weights wi (x(t) ) for each measurement i, with performing
 4.3 Increasing Robustness to Outliers in the SLAM Back-end 89

an optimization step (i.e., a Gauss-Newton or Levengberg-Marquardt iteration) on

the weighted least squares problem (4.11).
Figure 4.7 shows the performance of IRLS on the M3500, SubT, and Victoria
Park datasets. IRLS converges in tens of iterations and is typically much faster
than gradient descent; for instance, gradient descent requires around 5 seconds to
optimize the Huber loss in our M3500 experiments, while IRLS took less than 1.5
seconds. On the other hand, this faster convergence often comes at the cost of a
slightly decreased accuracy, as can be seen by comparing Figure 4.7 and Figure 4.6.
The convergence properties of the update rule (4.12) has been studied in [6, 191].

4.3.2 Black-Rangarajan Duality

The weight update rule (4.12) is widely used in practice, but its derivation was
somewhat heuristic. It also has the issues that (4.12) is not well-defined at the non-
differentiable points of ρ (e.g., the cut-off point of the truncated quadratic loss). We
now introduce a more principled framework, namely the Black-Rangarajan (B-R)
duality [31], to solve M-Estimation using IRLS.
Let us present the intuition of B-R duality using the truncated quadratic loss

ρ(ri (x)) := min{ri2 (x), βi2 }, (4.13)

where βi2 is a bound on the i-th residual such that the i-th measurement is an inlier
if ri2 (x) ≤ βi2 and an outlier otherwise. We observe that the cost in (4.13) can be
equivalently written as a sum of two terms by introducing a new weight variable
wi ∈ [0, 1]
ρ(ri (x)) := min wi ri2 (x) + (1 − wi )βi2 , (4.14)
wi ∈[0,1]

where the first term is exactly the weighted least squares, and the second term
is a function of wi that does not depend on x. With (4.14), the M-Estimation
problem (4.9) with a truncated quadratic loss can be reformulated as
X 
min
x
wi ri2 (x) + (1 − wi )βi2 , (4.15)
wi ∈[0,1],i=1,...,N i

where we have introduced one wi for each measurement residual ri (x). Prob-
lem (4.15) is easy to interpret: wi = 1 implies ri2 (x) ≤ βi2 and the i-th mea-
surement is an inlier; wi = 0 implies ri2 (x) > βi2 and the i-th measurement is an
outlier. Moreover, all the residuals with wi = 0 are effectively discarded from the
optimization (4.15) and hence robustness is ensured.
B-R duality generalizes the derivation above to a family of robust losses.

Theorem 4.4 (Black-Rangarajan Duality [31]). Given a robust loss function ρ(·),
√
define ϕ(z) := ρ( z). If ϕ(z) satisfies limz→0 ϕ′ (z) = 1, limz→∞ ϕ′ (z) = 0, and
90 Robustness to Incorrect Data Association and Outliers

Ground Truth M3500 SubT Victoria Park

(a) (b) (c)

IRLS + Huber

(d) (e) (f)

IRLS + GM

(g) (h) (i)

IRLS + TLS

(j) (k) (l)

Figure 4.7 Solving SLAM problems with outliers using robust loss functions and Itera-
tively Re-weighted Least-Squares (IRLS): (a)-(c) Ground truth trajectories for the M3500,
SubT, and Victoria Park datasets. (d)-(f) Trajectory estimates obtained using the Huber
loss. (g)-(i) Trajectory estimates obtained using the Geman-McClure loss. (j)-(l) Trajec-
tory estimates obtained using the truncated quadratic loss. Measurements are visualized
as colored edges. In particular, an edge is colored in gray if it is correctly classified as
inlier or outlier by the optimization (i.e., an inlier that falls in the quadratic region of
the Huber or truncated quadratic loss); it is colored in red if it is an outlier incorrectly
classified as an inlier (a “false positive”); it is colored in blue if it is an inlier incorrectly
classified as an outlier (a “false negative”).
 4.3 Increasing Robustness to Outliers in the SLAM Back-end 91

ϕ′′ (z) < 0, then the M-Estimation problem (4.9) is equivalent to

N
X  
min
x
wi ri2 (x) + Φρ (wi ) , (4.16)
wi ∈[0,1],i=1,...,N i=1

where wi ∈ [0, 1], i = 1, . . . , N are weight variables associated to each residual ri ,

and the function Φρ (wi ), referred to as an outlier process, defines a penalty on the
weight wi whose form is dependent on the choice of robust loss ρ.
In the case of ρ being the truncated quadratic loss, we easily derived from (4.14)
that Φρ (wi ) = (1−wi )βi2 . When ρ takes other forms, [31] provides a recipe to derive
Φρ (wi ). We give an example for the Geman-McClure (G-M) robust loss.
Example 4.5 (B-R Duality for G-M Loss). Consider the G-M robust loss function
βi2 ri2 (x)
ρ(ri (x)) = , (4.17)
βi2 + ri2 (x)
where βi2 is a noise bound for the i-th residual similar to (4.13). The outlier process
associated to (4.17) is
√
Φρ (wi ) = βi2 ( wi − 1)2 . (4.18)
To verify the correctness of (4.18), consider
min wi ri2 (x) + Φρ (wi ), (4.19)
wi ∈[0,1]

whose optimal solution is (via setting the gradient of (4.19) to zero)

 2
⋆ βi2
wi = . (4.20)
ri2 (x) + βi2
Plugging (4.20) back to the objective of (4.19) recovers the G-M robust loss (4.17).

4.3.3 Alternating Minimization

With the introduction of B-R duality, the IRLS algorithm naturally comes out using
a common optimization strategy called alternating minimization [267, 27]. The idea
is that, although it is difficult to jointly optimize both x and wi ∈ [0, 1], i = 1, . . . , N
in (4.16), optimization of either x or wi ’s when fixing the other is easy. To see this,
observe that when wi ’s are fixed, problem (4.16) becomes a weighted least squares;
analogously, when x is fixed, problem (4.16) becomes
N
X
min Φρ (wi ) + wi ri2 (x),
wi ∈[0,1],i=1,...,N
i=1

which splits into N subproblems, each optimizing a scalar wi

min Φρ (wi ) + wi ri2 (x). (4.21)
wi ∈[0,1]
92 Robustness to Incorrect Data Association and Outliers

Problem (4.21) is easy to solve and often admits a closed-form solution. In fact,
for the G-M robust loss, the solution of (4.21) is just (4.20). For the truncated
quadratic loss, problem (4.21) reads

min (1 − wi )βi2 + wi ri2 (x)

wi ∈[0,1]

and admits a closed-form solution


 if ri2 (x) < βi2
1

⋆
wi = 0 if ri2 (x) > βi2 .


[0, 1] otherwise

In summary, the t-th iteration of IRLS in the context of B-R duality alternates
between two steps

1 Variable update: solve a weighted least squares problem using the current
(t)
weights wi
X (t)
x(t) ∈ arg min wi ri2 (x). (4.22)
x
i

2 Weight update: update the weights using x(t)

(t+1)
wi ∈ arg min Φρ (wi ) + wi ri2 (x(t) ), i = 1, . . . , N. (4.23)
wi ∈[0,1]

The weight update rule obtained via B-R duality actually matches the popular
weight update rule (4.12). Interestingly, instantiating the above IRLS algorithm in
SLAM using the G-M robust loss leads to the dynamic covariance scaling algo-
rithm [7], which has been proposed in the context of outlier-robust SLAM.

4.3.4 Graduated Non-Convexity

The previous section leveraged Black-Rangarajan duality and alternating mini-
mization to derive the IRLS framework that alternates in solving (4.22) and (4.23).
However, due to the non-convexity of common robust losses, the convergence of the
IRLS framework can be highly sensitive to the quality of initialization, i.e., how
(0)
close is x(0) to the optimal solution of (4.9) or how well does wi reflect the inlier-
outlier membership of each measurement. For example, [237] showed that IRLS
with the truncated quadratic loss and the Geman-McClure loss might fail when
there are as little as 10% outliers in the measurements (cf. also with our results in
Figure 4.7).
In this section, we introduce the graduated non-convexity (GNC) scheme that can
make IRLS significantly less sensitive to the quality of initialization. Given a robust
cost function ρ, the basic idea of GNC is to create a smooth version of ρ, denoted
 4.3 Increasing Robustness to Outliers in the SLAM Back-end 93

as ρµ , using a scalar smoothing factor µ. Tuning µ controls the amount of non-

convexity in ρµ : ρµ is convex at one end of the spectrum and recovers the original
ρ at the other end of the spectrum. Let us illustrate this using two examples.

Example 4.6 (GNC Truncated Quadratic Loss). Consider the GNC truncated
quadratic loss function
 h i

 r 2
(x) if r 2
(x) ∈ 0, µ
β 2

 i i
h µ+1 i i
p µ
ρµ (ri (x)) = 2βi |ri (x)| µ(µ + 1) − µ(βi2 + ri2 (x)) if ri2 (x) ∈ µ+1 βi2 , µ+1
µ βi2

 h i
 2
 βi ri2 (x) ∈ µ+1 µ β 2
i , +∞ .
(4.24)
ρµ is convex for µ approaching zero and retrieves the truncated quadratic loss
in (4.13) for µ approaching infinity.

Example 4.7 (GNC Geman-McClure Loss). Consider the GNC Geman-McClure

loss function
µβi2 ri2 (x)
ρµ (ri (x)) = . (4.25)
µβi2 + ri2 (x)

ρµ is convex for µ approaching ∞ and recovers the G-M loss (4.17) when µ = 1.

(a) (b)
Figure 4.8 Graduated Non-Convexity with control parameter µ for (a) Truncated
Quadratic loss and (b) Geman-McClure loss.

Figure 4.8(a) and (b) plot the GNC truncated quadratic loss and the GNC
Geman-McClure loss, respectively. Observe how increasing or decreasing the control
parameter µ adds more non-convexity to the function.
One nice property of the smoothed GNC functions in (4.24) and (4.25) is that
the B-R duality still applies. For the GNC truncated quadratic loss (4.24), applying
94 Robustness to Incorrect Data Association and Outliers

B-R duality leads to the outlier process

µ(1 − wi ) 2
Φρµ (wi ) = β .
µ + wi i
For the GNC Geman-McClure loss (4.25), applying B-R duality leads to the outlier
process
√
Φρµ (wi ) = µβi2 ( wi − 1)2 .

We are now ready to state the GNC algorithm, which at each iteration performs
three steps

1 Variable update: solve a weighted least squares problem using the current
(t)
weights wi
X (t)
x(t) ∈ arg min wi ri2 (x). (4.26)
x
i

2 Weight update: update the weights using x(t)

(t+1)
wi ∈ arg min Φρµ (wi ) + wi ri2 (x(t) ), i = 1, . . . , N. (4.27)
wi ∈[0,1]

3 Control parameter update: Increase or decrease µ to add more nonconvexity

to ρµ .

The GNC algorithm is similar to the IRLS algorithm, except that it starts with a
convex, smoothed function ρµ and then iteratively updates the control parameter µ
to gradually add more non-convexity to ρµ to approach the original loss function ρ.
Depending on the definition of the smoothed loss ρµ , one would recover the original
ρ by either increasing or decreasing µ. For instance, the smoother GNC truncated
quadratic loss recovers the original truncated quadratic loss when µ is large, hence
µ is increased by a constant factor γ > 1 at each GNC iteration (e.g., γ = 1.4
in [296]). Conversely, the smoother Geman-McClure loss recovers the original GM
loss when µ is close to 1, hence µ is divided by γ > 1 at each GNC iteration.
Figure 4.9 showcases the SLAM trajectories obtained by applying GNC on the
same three datasets of Figure 4.6, with two different robust losses: the Geman-
McClure loss and the truncated quadratic loss. We implemented GNC using GT-
SAM’s GNCOptimizer. By comparing the figure with Figure 4.6 and Figure 4.7
we observe that GNC ensures better convergence (i.e., it is less prone to being
stuck in local minima) and recovers fairly accurate trajectories in all the three
datasets. While GNC has been shown to be extremely resilient to outliers (e.g.,
it has shown to tolerate around 80-90% incorrect loop closures in real-world prob-
lems [296, 47]), we remark that the approach does not provide any convergence
guarantees. Moreover, its performance has been empirically seen to be problem-
dependent, and while it leads to superb performance in pose-graph optimization
problems, its performance largely degrades in other perception problems [236].
 4.3 Increasing Robustness to Outliers in the SLAM Back-end 95

Ground Truth M3500 SubT Victoria Park

(a) (b) (c)

GNC + GM

(d) (e) (f)

GNC + TLS

(g) (h) (i)

Figure 4.9 Solving SLAM problems with outliers using robust loss functions and graduated
non-convexity (GNC): (a)-(c) Ground truth trajectories for the M3500, SubT, and Victoria
Park datasets. (d)-(f) Trajectory estimates obtained using the Geman-McClure loss. (g)-
(i) Trajectory estimates obtained using the truncated quadratic loss. Measurements are
visualized as colored edges. In particular, an edge is colored in gray if it is correctly
classified as inlier or outlier by the optimization (i.e., an inlier that falls in the quadratic
region of the Huber or truncated quadratic loss); it is colored in red if it is an outlier
incorrectly classified as an inlier (a “false positive”); it is colored in blue if it is an inlier
incorrectly classified as an outlier (a “false negative”).

Below we showcase a problem when GNC fails and also show that combining
front-end and back-end outlier rejection can be beneficial. Towards this goal, we
are going to consider a slightly more challenging SLAM setup compared to the
ones discussed above. Earlier in this chapter, we considered pose-graph optimiza-
tion problems (e.g., Figure 4.1) where the odometry is reliable but there might be
outliers in the loop closures or in the landmark measurements. This setting essen-
tially assumes the presence of an “odometry backbone” that largely simplifies the
problem by providing a set of trusted measurements while also allowing building
an initial guess for the robot poses. While this assumption is realistic in many
96 Robustness to Incorrect Data Association and Outliers

problems,14 certain SLAM applications might lack an odometry backbone. For in-
stance, certain odometry sensors might produce incorrect measurements, e.g., due
to wheel slippage in wheel odometry, or incorrect lidar alignment in lidar odometry.
Another example arises in multi-robot SLAM, where each robot has an odometry
backbone, but the overall SLAM problem (including the trajectory of all robots)
does not [169].

M3500 SubT Victoria Park

GNC

(a) (b) (c)

PCM

(d) (e) (f)

PCM + GNC

(g) (h) (i)

Figure 4.10 SLAM problems with outliers in both the loop closures and landmark mea-
surements, as well as the odometry: (a)-(c) Trajectory estimates obtained using GNC
with the truncated quadratic loss. (d)-(f) Trajectory estimates obtained using PCM for
front-end outlier rejection followed by least squares optimization. (g)-(i) Trajectory esti-
mates obtained using PCM for front-end outlier rejection followed by GNC with truncated
quadratic loss.

Robustly solving SLAM problems where both odometry and loop closure mea-
surements can be outliers is extremely hard. To showcase the shortcomings of the
14
The fact that odometric measurements are computed between consecutive frames makes data
association relatively simpler, in particular when the rate of the sensor (e.g., camera framerate) is
much faster than the motion of the robot. Intuitively, consecutive frames will provide snapshots of
the scene from similar viewpoints, thus reducing sources of errors for feature matching, including
illumination changes, occlusions, lack of viewpoint invariance, or perceptual aliasing.
 4.4 Further References and New Trends 97

approaches we discussed, in this more complex setting with potentially incorrect

odometry measurements, we first modify the pose-graph problems in Figure 4.1
by corrupting two randomly selected odometry measurements. Then we attempt
to solve the problem with GNC. In particular, in GNC we fix all odometry mea-
surements except the two potentially corrupted measurements as inliers. Figure
4.10 shows the trajectories obtained by solving the problem with GNC. GNC fails
due to the corrupted initialization and converges to a local minima by categorizing
all measurements (including inliers) as outliers (blue edges). The same figure also
shows the result of using PCM to filter out gross outliers before using a least-squares
back-end. PCM is agnostic to the initial guess, hence is able to converge to better
solutions in the case of the SubT and Victoria Park datasets (Figure 4.10 (e) and
(f)). Interestingly, we get best results in the SubT and Victoria Park datasets by
combining front-end outlier rejection (PCM) with GNC. At the same time, all three
methods (GNC, PCM, and PCM+GNC) fail to converge to acceptable solutions in
the M3500 dataset, confirming the hardness of this SLAM setup and the limitations
of existing SLAM algorithms in terms of outlier rejection.

4.4 Further References and New Trends

Consensus Maximization. While in this chapter we discussed the most basic in-
stantiation of a RANSAC algorithm (according to the initial proposal in [89]), it is
worth mentioning that the literature offers many RANSAC variants, including vari-
ants that refine estimates through local optimization [57], use better scores (rather
than the size of the consensus set) in the RANSAC iterations (e.g., MLESAC [209]),
or bias the sampling in the RANSAC iterations (e.g., PROSAC [56]). The recent
literature also includes differentiable variants of RANSAC [284] and variants that
attempt to find the inliers when the parameter γ in (4.4) is unknown (e.g., [16]). A
recent survey and evaluation of RANSAC variants can be found in [265].
Beyond RANSAC, the literature also includes approaches for exact consensus
maximization, typically based on branch-and-bound [23, 112, 309, 150, 241, 55, 120,
38, 299, 298]. Despite its global optimality guarantees, branch-and-bound has expo-
nential runtime in the worst case and does not scale to high-dimensional problems.
Pairwise Consistency Maximization. The PCM approach described in Sec-
tion 4.2.2 was originally proposed in the context of multi-robot SLAM in [169],
where this approach showed particular promise. Graph-theoretic outlier rejection
has also been investigated in computer vision. Segundo and Artieda [226] build
an association graph and find the maximum clique for 2D image feature match-
ing. Perera and Barnes [207] segment objects under rigid body motion with a
clique formulation. Leordeanu and Hebert [148] establish image matches by finding
strongly-connected clusters in the correspondence graph with an approximate spec-
tral method. Enqvist et al. [85] develop an outlier rejection algorithm for 3D-3D and
2D-3D registration based on approximate vertex cover. Yang and Carlone [292, 297]
98 Robustness to Incorrect Data Association and Outliers

and Parra et al. [39] investigate graph-theoretic outlier rejection based on maximum
clique for 3D-3D registration. The idea of checking consistency across a subset of
measurements also arises in Latif et al. [143], which perform loop-closure outlier
rejection by clustering measurements together and checking for consistency using
a Chi-squared-based test. The PCM paper [169], similarly to the discussion in this
chapter, focuses on pairwise consistency. More recently, PCM has been extended to
group-k consistency (i.e., the case where the consistency constraint (4.7) involves
k measurements instead of only 2 measurements) in [236, 237, 91]. These papers
essentially generalize the notion of consistency graphs to consistency hypergraphs,
where each hyper-edge involves k nodes. Related work also considers soft varia-
tions of the maximum clique problem, where the binary condition (4.7) is relaxed
to produce continuous weights on the edges of the consistency graph [164, 163].
These methods have been used in practical applications, including subterranean
exploration [81], lidar point-cloud localization [165], multi-robot metrics-semantic
mapping [261], and global localization in unstructured environments [14].
Alternating Minimization and Graduated Non-Convexity. M-estimation
has been a popular approach for robust estimation in robotics [33] and vision [230,
49]. Tavish et al. [253] investigate the performance of different loss functions. Sev-
eral papers investigate formulations with auxiliary variables as the one in (4.16),
without realizing the connection to M-estimation provided by the Black-Rangarajan
duality (Theorem 4.4). For instance, Sünderhauf and Protzel [249, 250] and Agarwal
et al. [7] augment the problem with latent binary variables responsible for deac-
tivating outliers. Lee et al. [147] use expectation maximization. Olson and Agar-
wal [196] use a max-mixture distribution to approximate multi-modal measurement
noise. Recently, Barron [21] proposes a single parametrized function that general-
izes a family of robust loss functions in M-estimation. Chebrolu et al. [50] design an
expectation-maximization algorithm to simultaneously estimate the unknown quan-
tity x and choose a suitable robust loss function ρ. The graduated non-convexity
algorithm was first introduced in [32, 31] for outlier rejection in early vision applica-
tions; more recently, the algorithm was used for point cloud registration [311, 297],
SLAM [296], and other applications [15]. Recently, Peng et al. [205] has proposed an
algorithm similar to GNC and IRLS, that is based on the idea of smooth majoriza-
tion in optimization and can be applied to a broad set of robust losses. Moreover,
[205] derives global and local convergence guarantees for GNC.
Certifiable Algorithms. The algorithms described so far can be broadly di-
vided into two categories: (i) fast heuristics (e.g., RANSAC or local solvers for
M-estimation), which are efficient but provide little performance guarantees, and
(ii) global solvers (e.g., branch-and-bound), which offer optimality guarantees but
scale poorly with the problem size. Recent years have seen the advent of a new
type of methods, called certifiable algorithms, that try to strike a balance between
tractability and optimality. Certifiable algorithms relax non-convex robust estima-
 4.4 Further References and New Trends 99

tion problems into convex semidefinite programs (SDP),15 whose solutions can be
obtained in polynomial time and provide readily checkable a posteriori global opti-
mality certificates. Certifiable algorithms for robust estimation have been proposed
in the context of rotation estimation [293], 3D-3D registration [297], and pose-graph
optimization [142, 43]. A fairly general approach to derive certifiable algorithms for
problems with outliers is described in [294, 295], while connections with parallel
work in statistics is discussed in [42]. With few notable exceptions, these algo-
rithms, albeit running in polynomial time, are still computationally expensive and
typically much slower than heuristics methods. In some cases, the insights behind
these algorithms can be used to certify optimality of a solution obtained with a fast
heuristic [294], hence getting the best of both worlds.

15
We are going to review the notion of certifiable algorithms in the context of SLAM in Chapter 7.
 5
Differentiable Optimization
Chen Wang, Krishna Murthy Jatavallabhula, and Mustafa Mukadam

5.1 Introduction

As presented in Chapter 2, the design of a contemporary SLAM system generally

adheres to a front-end and back-end architecture. In this structure, the front-end
is typically responsible for pre-processing sensor data and generating an initial
estimate of the robot’s trajectory and the map of the environment, while the back-
end refines these initial estimates to improve overall accuracy. Recent advances in
machine learning have provided new approaches, based on deep neural networks,
that have the potential to enhance some of the functionalities in the SLAM front-
end. For instance, deep learning-based methods can exhibit impressive performance
in feature detection and matching [229, 73, 306] and front-end motion estimation
[281, 256]. These methods train a neural network from a large dataset of examples,
and then make estimations without being explicitly programmed to perform the
task. Meanwhile, geometry-based techniques persist as an essential element for the
SLAM back-end, primarily due to their generality and effectiveness in producing a
globally consistent estimate by solving an optimization problem.
While in principle one could just “plug” a learning-based SLAM front-end in the
SLAM architecture and feed the corresponding outputs to the back-end, the use
of learning-based techniques opens the door for a less unidirectional information
exchange. In particular, the back-end can now provide feedback to the front-end,
enabling it to learn directly from the back-end estimates in a way that the two
modules can more harmoniously cooperate to reduce the estimation errors. Rec-
onciling geometric approaches with deep learning to leverage their complementary
strengths is a common thread in a large body of recent work in SLAM. In particular,
an emerging trend is to differentiate through geometry-based optimization problems
arising in the SLAM back-end. Intuitively, differentiating through an optimization
problem allows understanding how the optimal solution of that problem (e.g., our
SLAM estimate) depends on the parameters of that problem — in our case, the
measurements produced by a learning-based front-end; this in turns allows opti-
mizing the front-end to maximize the SLAM accuracy. One could think about this
as a bilevel optimization problem, i.e., an upper-level optimization process subject
 5.1 Introduction 101

to a lower-level optimization — in particular, a neural network based-optimization

to train the front-end, subject to a geometry-based optimization that computes the
SLAM solution for a given front-end output.
The ability to compute gradients end-to-end through an optimization is the core
of solving a bilevel optimization problem, which allows neural models to take ad-
vantage of geometric priors captured by the optimization. The flexibility of such a
scheme has led to promising state-of-the-art results in a wide range of applications
such as structure from motion [255], motion planning [28, 291], SLAM [121, 256],
bundle adjustment [252, 306], state estimation [302, 51], and image alignment [166].
In this chapter, we illustrate the basics of how to differentiate through nonlinear
least squares problems, such as the ones arising in SLAM. Specifically, Section 5.1.1
restates the non-linear least square (NLS) problem. Section 5.2 describes how to
differentiate through the NLS problem. Section 5.3 shows how to differentiate prob-
lems defined on manifold. Section 5.4 discusses numerical challenges of the above
differentiation and introduces related machine learning libraries. Finally, Section 5.5
provides examples of differentiable optimization in contemporary SLAM systems.

5.1.1 Recap on Nonlinear Least Squares

Non-linear least squares (NLS) estimate the parameters of a model by minimizing
the sum of the squares of the mismatch between observed values and those predicted
by the model. Unlike linear least squares, NLS involves a model that is non-linear
in the parameters. Beyond our factors graphs in Chapter 2, this approach is widely
used in many fields such as statistics, physics, and engineering, where it is useful
for fitting complex models to data when the relationship between variables is not
straightforward, enabling more accurate and robust predictions.
Specifically, NLS aim to find variables x ∈ Rn by solving:
1X
x∗ = arg min L(x) = arg min = || wi ci (xi ) ||2 , (5.1)
x x 2 | {z }
i
ri (xi )

where the objective L(x) is a sum of squared vector-valued residual terms ri , each
a function of xi ⊂ x that are (non-disjoint) subsets of the optimization variables
x = {xi }. While for now we assume xi to be vectors, later in the chapter we
generalize the discussion to the case where the variables belong to a manifold. For
flexibility, here we represent a residual ri (xi ) = wi ci (xi ) as a product of a weight
wi and vector cost ci .
As explained in Chapter 2, a NLS is normally solved by iteratively lineariz-
ing the nonlinear objective around the current variables to get the linear system
P P
( i Ji⊤ Ji )δx = ( i Ji⊤ ri ), then solving the linear system to find the update δx,
and finally updating the variables x ← x + δx, until convergence. We have also
commented in Chapter 3 that the addition in the update step is more generally a
102 Differentiable Optimization

Neural 𝒰 (𝒚, 𝒙∗ ) Nonlinear

Network Least Squares
Upper-level Cost
𝑥" 𝑥#
Attributes 𝑥!
Inputs 𝑓𝒚 𝑔𝒙
Solution 𝑥& 𝑥$
𝑥%
Lower-level Cost

ℒ (𝒚, 𝒙)

Figure 5.1 A modern SLAM system often involves both neural networks and nonlinear
least squares. To eliminate compound errors introduced by optimizing the two modules
separately, we can optimize the system in an end-to-end manner by formulating the entire
system as a bilevel optimization, which involves an upper-level cost and a lower-level cost.

retraction mapping for variables that belong to a manifold. In the linear system,
Ji = [∂ri /∂xi ] are the Jacobians of residuals with respect to the variables. This
iterative method above, called Gauss-Newton (GN), is a nonlinear optimizer that is
P
(approximately) second-order, since i Ji⊤ Ji is an approximation of the Hessian.
To improve robustness and convergence, variations like Levenberg-Marquardt (LM)
dampen the linear system, while others adjust the step size for the update with line
search, e.g., Dogleg introduced in Chapter 2.

5.2 Differentiation Through Nonlinear Least Squares

To seamlessly merge deep learning with nonlinear least squares, differentiable non-
linear least squares (DNLS) are often required to solve the optimization problem
illustrated in Figure 5.1. This necessitates gradients of the solution x∗ with re-
spect to any upper-level neural model parameters y that parameterize the objective
U(x; y) and, in turn, any costs ci (xi ; y) or initialization for variables xinit (y). The
goal is to learn these parameters y end-to-end with a lower-level learning objective
L defined as a function of x. This results in a bilevel optimization (BLO), which
can be written as:
y ∗ = arg min U(y, x∗ ), (5.2a)
y∈Θ

s. t. x∗ = arg min L(y, x), (5.2b)

x∈Ψ

where L : Rm ×Rn → R is a lower-level (LL) cost, U : Rm ×Rn → R is a upper-level

(UL) cost, x ∈ Ψ and y ∈ Θ are the feasible sets.
In practice, the variables x are often parameters with explicit physical meanings
such as camera poses, while y are parameters without physical meanings such as
weights in a neural network. We next present two examples to explain this.
 5.2 Differentiation Through Nonlinear Least Squares 103

Example 5.1. Imagine a SLAM system that leverages a neural network (param-
eterized by y) for feature extraction/matching, while utilizing bundle adjustment
(BA) for pose estimation (parameterized by x), which take the feature matching
as an input. In this example, the UL cost (5.2a) can be feature matching error
for optimizing the network, while the LL cost L (5.2b) can be the reprojection
error for BA. Intuitively, the optimal solution x∗ for the camera poses and land-
mark positions plays the role of a supervisory signal in the neural network training.
Therefore, optimizing the BLO (5.2) allows us to further reduce the matching error
via back-propagating the BA reprojection errors [306].
Example 5.2. Imagine a full SLAM system that uses a neural network for front-end
pose estimation, while leverages pose-graph optimization (PGO) as the back-end to
eliminate odometry drifts. In this example, both UL and LL costs can be the pose-
graph error. The difference is the UL cost optimizes the network parameterized by
y, while the LL cost optimizes the camera poses parameterized by x. As a result,
the front-end network can leverage global geometric knowledge obtained through
pose-graph optimization by back-propagating the pose residuals from the back-end
PGO [94].
BLO is a long-standing and well-researched problem [275, 122, 155]. Solving a
BLO often relies on gradient-descent techniques. Specifically, the UL optimization
performs updates in the form y ← y + δy, where δy is a step in the direction of
the negative gradient. Therefore, we need compute the gradient of U with respect
to the UL variable y, which can be written as
∂U(y, x∗ ) ∂U(y, x∗ ) ∂x∗ (y)
∇y U = + , (5.3)
∂y ∂x∗ ∂y
∗
where the term ∂x∂y(y) involves indirect gradient computation. Since other direct
gradient terms in (5.3) are easy to obtain, the challenge of solving a BLO (5.2)
∗
is to compute the term ∂x∂y(y) . For this purpose, a series of techniques have been
developed from either explicit or implicit perspectives. This involves recurrent dif-
ferentiation through dynamical systems or implicit differentiation theory, which are
often referred to as unrolled differentiation and implicit differentiation, re-
spectively. These algorithms have been summarized in [155, 275] and here we list
a generic framework incorporating both methods in Algorithm 1. We next explain
the unrolled differentiation and implicit differentiation, respectively.

5.2.1 Unrolled Differentiation

Unrolled Differentiation needs automatic differentiation (AutoDiff) through the LL
optimization to solve a BLO problem. Specifically, given an initialization x0 =
Φ0 (y) at step t = 0, the iterative process of unrolled LL optimization is
xt = Φt (xt−1 ; y), t = 1, · · · , T, (5.4)
104 Differentiable Optimization

Algorithm 1 Solving BLO by Unrolled Differentiation or Implicit Differentiation.

1: Initialization: y0 , x0 .
2: while Not Convergent (∥yk+1 − yk ∥ is large enough) do
3: Obtain xT by solving (5.2b) by a generic optimizer O with T steps.
4: Efficient estimation of upper-level gradients in (5.3) via
Unrolled Differentiation: ∇ ˆ y U = ∂U (yk ,xT )) via AutoDiff in (5.7).
k ∂yk
Implicit Differentiation (Algorithm 2): Compute

ˆ y U = ∂U ∂U ∂x∗
∇ + ,
k
∂yk xT ∂x∗ ∂yk xT
∗
where the implicit derivatives ∂x
∂yk can be obtained by solving an equation
derived via lower-level optimality conditions (surveyed in following sections).
5: Compute yk+1 via gradients using ∇ ˆ y U.
k

6: end while

where Φt denotes an updating scheme based on the LL problem at the t-th step
and T is the number of iterations. One updating scheme is the gradient descent:

∂L(xt−1 , y)
Φt (xt−1 ; y) = xt−1 − ηt · , (5.5)
∂xt−1

where ηt is a learning rate and the term ∂L(xt−1 ,y)

∂xt−1 can be computed from AutoDiff.1
Therefore, we can compute the ∇y U(y) by substituting xT approximately for x∗
and the full unrolled system can be defined as

x∗ ≈ xT = Φ(y) = (ΦT ◦ · · · ◦ Φ1 ◦ Φ0 ) (y), (5.6)

where the symbol ◦ denotes the function composition. As a result, we only need to
consider the following problem instead of a bilevel optimization in (5.2):

min U(y, Φ(y)), (5.7)

y∈Θ

which needs to compute ∂Φ(y)

∂y via AutoDiff instead of calculating (5.3). It is worth
noting that there exist two approaches for computing the recurrent gradients, one
of which corresponds to backward propagation in a reverse-mode way [204], and
the other corresponds to the forward-mode way [221]. We omit the details of the
two approaches of AutoDiff and refer the readers to the AutoDiff libraries such as
PyTorch [202] for deep learning and PyPose [274] and Theseus [210] for SLAM. A
review of these approaches can also be found in Liu et al. [155].

1
While here we mention gradient descent, the same ideas can be extended to other iterative
optimization methods, such as Gauss-Newton.
 5.2 Differentiation Through Nonlinear Least Squares 105

5.2.2 Truncated Unrolled Differentiation

The reverse and forward modes are two precise recurrent gradient calculation meth-
ods but are time-consuming with the full iterative propagation. This is due to the
complicated long-term dependencies of the UL problem on xt , where t = 0, 1, · · · , T .
This difficulty is further aggravated when both y and x are high-dimensional vec-
tors. To overcome this challenge, the truncated unrolled differentiation has been
investigated as a way to compute high-quality approximate gradients with signifi-
cantly less computation time and memory. Specifically, by ignoring the long-term
dependencies and approximating the gradient of (5.5) with partial history, i.e.,
storing only the last M iterations (t = T, T − 1, · · · , T − M ), we can significantly
reduce the time and space complexity. It has been proved by Shaban et al. [232]
that using fewer backward steps to compute the gradients could perform compa-
rably to optimization with the exact one, while requiring much less memory and
computation.
In case of more stringent computational and memory constraints, truncated un-
rolled differentiation is still often a bottleneck in modern robotic applications.
Therefore, researchers have also tried to further simplify the truncated differen-
tiation by only performing a one-step iteration in (5.4) to remove the recursive
structure [153], i.e.,

∂U(y, x1 (y)) ∂U(y, x1 (y)) ∂x1 (y)

∇y U = + , (5.8)
∂y ∂x1 ∂y

∂x1 (y)
where the term ∂y is a Hessian that can be calculated from (5.5) as

∂x1 (y) ∂ 2 L(x0 , y)

=− . (5.9)
∂y ∂x0 ∂y

Since calculating a Hessian is time-consuming in some applications, we can resort to

numerical solutions that apply small perturbations to the variables x and calculate
an approximation of the second term in (5.8) as a whole:

∂L(x+
0 ,y) L(x−
0 ,y)
∂U(y, x1 (y)) ∂x1 (y) ∂y − ∂y
≈ , (5.10)
∂x1 ∂y 2ϵ

∂U (y,x1 (y))
where ϵ is a small scalar and x±
0 = x0 ± ϵ ∂x1 is a small perturbation. This
bypasses an explicit calculation of the Jacobian ∂x∂y1 (y)
. Nevertheless, we need to
pay attention to the perturbation model if non-Euclidean variables are involved,
e.g., variables belonging to Lie Groups. Fortunately, the AutoDiff of Lie Group
for Hessian-vector and Jacobian-vector multiplications are supported in modern
libraries, such as PyPose [274], which will be introduced in Section 5.4.
106 Differentiable Optimization

5.2.3 Implicit Differentiation

∗
It is intuitive that the term ∂x∂y(y) in (5.3) is dependent on the LL cost (5.2b), thus
implicit differentiation can be used to derive a solution to the gradient.
Example 5.3. In calculus, implicit differentiation refers to the method makes
use of the chain rule to differentiate implicit function. To differentiate an implicit
function y(x), defined by an equation R(x, y) = 0, it is not generally possible to
solve it explicitly for y and then differentiate. Instead, one can totally differentiate
dy
R(x, y) = 0 with respect to x and then solve the resulting linear equation for dx to
explicitly get the derivative in terms of x and y. For instance, consider an implicit
function x + y + 5 = 0, differentiating it with respect to x on its both sides gives
dy dx d dy dy dy
dx + dx + dx (5) = 0 ⇒ dx + 1 + 0 = 0. Solving for dx gives dx = −1.

Assume the LL cost L is at least twice differentiable w.r.t. both y and x, then
∗
we have ∂L(x (y),y)
∂x∗ (y) = 0 due to the optimality condition where x∗ is a stationary
∂L(x∗ (y),y)
point. Derive the equation ∂x∗ (y) = 0 on both sides w.r.t. y giving us

∂ 2 L(x∗ (y), y) ∂ 2 L(x∗ (y), y) ∂x∗ (y)

+ · = 0. (5.11)
∂x∗ (y)∂y ∂x∗ (y)∂x∗ (y) ∂y
∂x∗ (y)
This leads to the indirect gradient ∂y as
 −1
∂x∗ (y) ∂ 2 L(x∗ (y), y) ∂ 2 L(x∗ (y), y)
=− , (5.12)
∂y ∂x∗ (y)∂x∗ (y) ∂x∗ (y)∂y
The strength of (5.12) is that we convert the indirect gradient among the variables
y and x to direct gradients of L at the cost of an inversion of Hessian matrix.
However, the weakness is that a Hessian is often too large to compute, thus it is
common to solve a linear system leveraging the fast Hessian-vector product.
Example 5.4. Assume both UL and LU costs have a network with merely 1
million (106 ) parameters (32-bit float numbers), thus each network only needs a
space of 106 × 4Byte = 4MB to store, while their Hessian matrix needs a space of
(106 )2 ×4Byte = 4TB to store. This indicates that a Hessian matrix cannot even be
explicitly stored in the memory of a low-power computer, thus directly calculating
its inversion is more impractical.
Recollect that our goal is to compute the gradient in (5.3), substituting (5.12)
into (5.3) gives us:
 −1 2
∂U(y, x∗ ) ∂U(y, x∗ ) ∂ 2 L(x∗ (y), y) ∂ L(x∗ (y), y)
∇y U = − ∗ ∗ ∗ (y)
∂y | ∂x {z } | ∂x (y)∂x ∂x∗ (y)∂y
{z }
vT (H T )−1 . (5.13)
∂U(y, x∗ ) ∂ 2 L(x∗ (y), y)
= − qT ·
∂y ∂x∗ (y)∂y
 5.2 Differentiation Through Nonlinear Least Squares 107

Then we can solve the linear system Hq = v for q T by optimizing

1
q ∗ = min argq Q(q) = min argq q T Hq − q T v, (5.14)
2
using efficient linear solvers such as a simple gradient descent or conjugate gradient
method [113]. For gradient descent, we need to compute the gradient of Q as ∂Q(q)
∂q =
Hq − v, where Hq can be computed using the fast Hessian-vector product, i.e., a
Hessian-vector product is the gradient of a gradient-vector product:


∂2L ∂ ∂L
∂x · q
Hq = ·q = , (5.15)
∂x∂x ∂x
where ∂L∂x · q is a scalar. This means that the Hessian matrix H is not explicitly
computed or stored. We summarize the computation of implicit differentiation with
linear systems in Algorithm 2. The algorithm using a conjugate gradient is similar.

Algorithm 2 Computing Implicit Differentiation via Linear System.

1: Input: The current UL variable y and the optimal LL variable x∗ .
2: Initialization: k = 1, learning rate η.
3: while Not Convergent (∥qk − qk−1 ∥ is large enough) do
4: Perform gradient descent:
qk = qk−1 − η (Hqk−1 − v) , (5.16)
where Hqk−1 is computed via the fast Hessian-vector product.
5: end while
6: Assign q = qk
7: Compute ∇y U in (5.3) as:
 2 T
∂U(y, x∗ ) ∂ L(x∗ (y), y)
∇y U = − · q , (5.17)
∂y ∂y∂x∗ (y)
| {z }
(Hyx ·q)T

where Hyx ·q can also be computed efficiently using the Hessian-vector product.

Approximations. Implicit differentiation is complicated to implement but there

is one approximation, which is to ignore the implicit components and only use the
ˆ y U ≈ ∂U
direct part ∇ . This is equivalent to taking the solution xT from the
∂y
xT
LL optimization as constants in the UL problem. Such an approximation is more
efficient but introduces an error term
∂U ∂x∗
ϵ∼ . (5.18)
∂x∗ ∂y
Nevertheless, it is useful when the implicit gradients contain products of small
second-order derivatives, which depends on the specific NLS problems.
108 Differentiable Optimization

5.3 Differentiation on Manifold

Given that the state of a system within SLAM is bound to evolve on specific
manifolds, optimization on manifolds plays a crucial role in solving back-end SLAM
problems. We next derive the Jacobians required to differentiate with respect to
variables belonging to Lie groups, which is an essential step for differentiation on
the manifold.

5.3.1 Derivatives on the Lie Group

Since we introduced the basic concepts of Lie group, Lie algebra, and their basic
operations (e.g., exponential and logarithmic maps) in Chapter 3, this section will
briefly recap those concepts but mainly focus on the definition of their derivatives,
which is essential for solving a differentiable optimization problem.
Consider a Lie group’s manifold M, each point χ on this smooth manifold pos-
sesses a unique tangent space, denoted by Tχ M, where the fundamental principles
of calculus are valid. The Lie algebra, represented as m, is a vector space that can
be locally defined to the point χ as m = Tχ M. The exponential map exp : m → M
projects elements from the Lie algebra to the Lie group, while the logarithmic map
log : M → m serves as its inverse, establishing a bi-directional relationship:

χ = exp(τ ∧ ) ⇔ τ ∧ = log(χ), (5.19)

where hat ∧ is a linear invertible map, and τ ∧ ∈ m. By representing the coordinates

within the Lie algebra as vectors τ in Rn , we can define mappings between vector
τ and the Lie group χ:

χ = Exp (τ ) ⇔ τ = Log (χ) , (5.20)

where we redefined the exponential and logarithm maps to directly use a vector as
input and output, respectively.
To calculate derivatives on Lie groups, it is crucial to first understand the relative
change between two manifold elements, say χ1 and χ2 . These changes are quantified
by first defining the ⊕ and ⊖ operators, which capture the concept of displacement
on the manifold, as described in (5.21) and (5.22) below:
χ2 = χ1 ⊕ τ ≜ χ1 ◦ Exp (τ ) ,

(5.21)
τ = χ2 ⊖ χ1 ≜ Log χ−1
1 ◦ χ2 .

The placement of τ on the right-hand side in (5.21) signifies that it is expressed in

the local frame at χ1 . Conversely, the left operators in (5.22) reflect a global frame
perspective:
χ2 = ε ⊕ χ1 ≜ Exp (ε) ◦ χ1 ,

(5.22)
ε = χ2 ⊖ χ1 ≜ Log χ2 ◦ χ−1
1 ,
 5.3 Differentiation on Manifold 109

where ε is expressed in the global frame. Both τ and ε can be viewed as incremen-
tal perturbations to the manifold elements. By using corresponding composition
operators ⊕ and ⊖, the variations are expressed as vectors in the tangent space.
With the right ⊕ and ⊖ operators in place, we use the Jacobian matrix J to
describe perturbations on manifolds. The Jacobian captures the essence of infinites-
imal perturbations τ within the tangent space m:
∂f (χ) f (χ ⊕ τ ) ⊖ f (χ)
≜ lim
∂χ τ →0 τ
f (χ ◦ Exp (τ )) ⊖ f (χ)
= lim (5.23)
τ →0 τ

Log f (χ)−1 ◦ f (χ ◦ Exp (τ ))
= lim .
τ →0 τ


Let g(τ ) = Log f (χ)−1 ◦ f (χ ◦ Exp (τ )) , then the right Jacobian J R can be ex-
pressed as the derivative of g(τ ) at τ = 0:
∂f (χ) ∂g(τ )
= JR = . (5.24)
∂χ ∂τ τ =0

In this way, the derivatives of f (χ) with respect to χ in the manifold are represented
by the Jacobian matrix JR ∈ Rm×n , where m and n are the dimensions of the
Lie groups M and N , respectively. The right Jacobian matrix performs a linear
mapping from the tangent space m to the tangent space n = Tf (χ) N .
Similarly, consider an infinitesimal perturbation ε ∈ Tg M applied to the Lie
group element χ, the left Jacobian JL can be defined with the left plus and minus
operators:
∂f (χ) f (ε ⊕ χ) ⊖ f (χ)
≜ lim
∂χ ε→0 ε
f (Exp (ε) ◦ χ) ⊖ f (χ)
= lim
ε→0 ε
(5.25)
Log f (Exp (ε) ◦ χ) ◦ f (χ)−1
= lim
ε→0 ε

∂Log f (Exp (ε) ◦ χ) ◦ f (χ)−1
= .
∂ε ε=0

The resulting left Jacobian JL ∈ Rn×m is also a linear mapping, but in the global
tangent space from Tg M to Tg N .
To delve into the local perturbations around a point χ1 , we consider perturbations
τ as τ = χ ⊖ χ1 , with χ being a perturbed version of χ1 . The covariance matrices
Σχ defined on the tangent space are derived using the expectation operator E,
enabling the representation of uncertainties and their propagation:

Σχ ≜ E[τ τ ⊤ ] = E[(χ ⊖ χ1 )(χ ⊖ χ1 )⊤ ]. (5.26)

110 Differentiable Optimization

These covariance matrices facilitate the establishment of Gaussian distributions

on the manifold, expressed as χ ∼ N (χ1 , Σχ ). It is important to note that the
covariance matrices Σχ are defined on the tangent space Tχ1 M, which allows the
uncertainty in the manifold to be represented by a vector and be propagated in the
form of covariance matrices.

Example 5.5. Consider a robot equipped with an inertial measurement unit (IMU)
and a camera. Given noisy observations RIMU and RCam from both sensors, the
orientation of the robot can be estimated by minimizing the discrepancy between
the measurements, which can be formulated as a nonlinear least squares problem
on the manifold SO(3):

R̂ = arg min f (R, RIMU , RCam ), (5.27)

R∈SO(3)

where f (·) is the cost function that quantifies the differences between the estimated
orientation R and the sensor measurements RIMU and RCam . With the Jacobian
matrices in place, the optimization on the manifold SO(3) for the pose estimation
can be effectively managed. The cost function f (·) can be detailed as:
−1

−1


f (R) = ∥Log RIMU R ∥2 + ∥Log RCam R ∥2 . (5.28)

To minimize f (R), we need to compute its gradient with respect to R on the

manifold SO(3). The gradient can be derived using the right Jacobian JR as:

−1

!⊤
∂Log RIMU R −1


∇f (R) =2 Log RIMU R
∂R

!⊤ (5.29)
−1
∂Log RCam R −1


+2 Log RCam R .
∂R

The gradient ∇f (R) can be used in conjunction with optimization algorithms like
gradient descent which moves along the tangent space and reprojecting back to the
manifold to update the pose R iteratively:

Rk+1 = Rk Exp (−α∇f (R)) , (5.30)

where α is the step size. This iterative process continues until the cost function
f (R) converges to a minimum, providing an optimal pose estimate R̂ that aligns
with the sensor measurements.

5.3.2 Differentiation Operations on Manifold

For typical manifold operations, we can derive closed-form expressions for the Jaco-
bians associated with inversion, composition, and group actions. These expressions
 5.3 Differentiation on Manifold 111

facilitate a comprehensive approach to optimization in SLAM, by enabling the

computation of function derivatives on manifolds with the chain rule:
∂Z ∂Z ∂Y
= , (5.31)
∂χ ∂Y ∂χ
where Z = g(Y), and Y = f (χ).
Jacobians of inversion can be derived through the application of the function
f (χ) = χ−1 with (5.23) for the right Jacobian JR , which leads to:


∂χ−1 Log (χ−1 )−1 (χExp (τ ))−1
≜ lim
∂χ τ →0 τ
 
−1
Log χExp (τ ) χ−1 (5.32)
= lim
τ →0 τ
(χ(−τ )∧ χ−1 )∨
= lim .
τ →0 τ
Jacobians of composition can be derived through the application of the func-
tion f (χ) = χ ◦ χ1 with the Equation (5.23). The derivative of the composition
operator χ ◦ χ1 with respect to χ is:


∂(χ ◦ χ1 ) Log (χχ1 )−1 (χExp (τ ) χ1 )
≜ lim
∂χ τ →0 τ


Log χ−1 1 Exp (τ ) χ1 (5.33)
= lim
τ →0 τ
(χ−1 ∧
1 τ χ1 )
∨
= lim .
τ →0 τ
The derivative of the composition operator χ ◦ χ1 with respect to χ1 is:


∂(χ ◦ χ1 ) Log (χχ1 )−1 (χχ1 Exp (τ ))
≜ lim
∂χ1 τ →0 τ
Log (Exp (τ )) (5.34)
= lim
τ →0 τ
= I.
Jacobians of the manifold M are characterized by the right Jacobian of χ
which is derived from the exponential map of τ ∈ Rm . This is expressed as:
τ
∂Exp (τ )
Jr (τ ) ≜ . (5.35)
∂τ
The right Jacobian conveys minor changes in τ to modifications in the local tangent
space at Exp (τ ). Similarly, the left Jacobian of χ maps changes of τ to variations
within the global tangent space of the manifold. This is expressed as:
ε
∂Exp (τ )
Jl (τ ) ≜ . (5.36)
∂τ
112 Differentiable Optimization

Jacobians of group action depends on the specific group action set v ∈ V.

The group action is defined as:
χ
Dχ · v
Jχχ·v ≜ ,
Dχ
v
(5.37)
Dχ · v
Jvχ·v ≜ ,
Dv
where χ ∈ M and v ∈ V.

Example 5.6. Consider a robotic arm with two joints, R1 and R2 , each repre-
sented by an element in SO(3). The final orientation of the robot’s end-effector is
determined by the composition of the joint rotations:

R = R1 ◦ R2 . (5.38)

To evaluate the impact of small perturbations τ in R1 and R2 on the end-effector

orientation R. It can be quantified using the Jacobians of composition:
∂(R1 ◦ R2 ) (R2−1 τ ∧ R2 )∨
= lim ,
∂R1 τ →0 τ
(5.39)
∂(R1 ◦ R2 )
= I.
∂R2
This example implies that adjustments to the first joint R1 affect the final ori-
entation R through a transformation influenced by the current state of the second
joint R2 . However, changes in the second joint R2 directly impact R without being
influenced by the first joint R1 .

5.4 Modern Libraries

5.4.1 Numerical Challenges of Automatic Differentiation
Automatic Differentiation (AutoDiff) is a cornerstone technique for computing
derivatives accurately and efficiently in various optimization contexts, including
differentiation on manifolds. Differentiation on manifolds poses unique challenges
due to the complex geometrical properties inherent in manifold structures, which
can affect the performance and applicability of AutoDiff. In differential optimiza-
tion, these challenges become pronounced as AutoDiff interacts with the curved
space of manifolds, potentially introducing numerical instability and inaccuracies.
This section delves into the specific numerical issues that arise when using au-
tomatic differentiation for manifold-based optimization tasks. Particular attention
will be paid to the complexities involved in maintaining numerical stability and pre-
cision in the presence of manifold constraints, such as those found in constrained
optimization and in systems defined by differential equations on manifolds. For sim-
plicity, we will take the PyPose library [274] as an example, which defines a general
 5.4 Modern Libraries 113

data structure, LieTensor for Lie Group and Lie Algebra. Specifically, we will show
its numerical challenges and how PyPose tackle this challenge.
Analytical Foundations of Exponential Mapping to Quaternions. The
exponential map is a fundamental concept in the theory of Lie groups and is partic-
ularly critical when transitioning between Lie algebras and Lie groups represented
by quaternions. This mapping enables the translation of angular velocities from the
algebraic structure in R3 to rotational orientations in the group of unit quaternions
S3 . Analytically, the exponential map for quaternions is derived from the Rodrigues’
rotation formula, which relates a vector in R3 to the corresponding rotation. Given
a vector x in R3 , representing the axis of rotation scaled by the rotation angle, the
quaternion representation of the rotation is given by:
    ⊤
∥ν∥ ν ⊤ ∥x∥
Exp(ν) = sin , cos (5.40)
2 ∥ν∥ 2
where ∥ν∥ represents the magnitude of ν, corresponding to the angle of rotation,
ν
and ∥ν∥ is the unit vector in the direction of ν.
One of the challenges of implementing a differentiable LieTensor is that one
often need to calculate numerically problematic terms such as sinν ν in (5.40) for the
Exp and Log mapping [257]. The direct computation of sine and cosine functions
for very small angles can lead to precision issues due to the finite representation of
floating-point numbers in computer systems. To manage these issues and maintain
numerical stability, PyPose takes the Taylor expansion to avoid calculating the
division by zero.
 T

 T ∥ν∥

 ν γ e , cos( ) if ∥ν∥ > eps
2
Exp(ν) =  T (5.41)

 ∥ν∥2 ∥ν∥4
 T
 ν γo , 1 − + otherwise,
8 384
∥ν∥ 2 4
sin( )
where γe = ∥ν∥2 when ∥ν∥ is significant, and γo = 21 − ∥ν∥ ∥ν∥
48 + 3840 for small ∥ν∥,
ensuring precise calculations across all ranges of rotation magnitudes. Here, eps is
the smallest machine number where 1 + eps ̸= 1. This analytical-to-numerical pro-
gression demonstrates the importance of accurate and stable methods for computing
exponential maps in applications that require high fidelity in rotation representa-
tion, such as in 3D graphics, robotics, and aerospace engineering.
LieTensor is different from the existing libraries in several aspects: (1) PyPose
supports auto-diff for any order gradient and is compatible with most popular de-
vices, such as CPU, GPU, TPU, and Apple silicon GPU, while other libraries like
LieTorch [257] implement customized CUDA kernels and only support 1st -order
gradient. (2) LieTensor supports parallel computing of gradient with the vmap
operator, which allows it to compute Jacobian matrices much faster. (3) Libraries
such as LieTorch, JaxLie [303], and Theseus only support Lie groups, while Py-
114 Differentiable Optimization

Pose supports both Lie groups and Lie algebras. As a result, one can directly call
the Exp and Log maps from a LieTensor instance, which is more flexible and
user-friendly. Moreover, the gradient with respect to both types can be automat-
ically calculated and back-propagated. The readers may find a list of supported
LieTensor operations in [2] and the tutorial of PyPose is available in [4]. The
usages of a LieTensor and its automatic differentiation can be found at https:
//github.com/pypose/slambook-snippets/blob/main/lietensor.ipynb.

5.4.2 Implementation of Differentiable Optimization

To enable end-to-end learning with bilevel optimization, one need to integrate gen-
eral optimizers beyond the gradient-based methods such as SGD [222] and Adam
[137] required by neural methods, since many problems in SLAM such as bundle
adjustment and factor graph optimization require other optimizations algorithms
such as constrained or 2nd -order optimization [19]. Moreover, practical problems
have outliers, hence one needs to robustify the loss as described in Chapter 4. Next
we consider an Iteratively Reweighted Least Squares (IRLS) approach to SLAM
as introduced in Section 4.3, and present the intuition behind the optimization-
oriented interfaces of PyPose, including solver, kernel, corrector, and strategy
for using the 2nd -order Levenberg-Marquardt (LM) optimizer.
Let us start by considering a weighted least square problem:
X T
min (hi (xi ) − zi ) Σi (hi (xi ) − zi ) , (5.42)
y
i

where h(·) is a regression model (Module), x ∈ Rn is the parameters to be op-

timized, hi deontes prediction for the i-th input sample, Σi ∈ Rd×d is a square
information matrix. The solution to (5.42) of an LM algorithm is computed by it-
eratively updating an estimate xt via xt ← xt−1 + δt , where the update step δt is
computed as:
X X
(Λi + λ · diag(Λi )) δt = − JiT Σi ri , (5.43)
i i

where ri = hi (xi ) − zi is the i-th residual error, Ji is the Jacobian of h computed

at xt−1 , Λi is an approximated Hessian matrix computed as Λi = JiT Σi Ji , and λ
is a damping factor. To find step δt , one needs a linear solver:

A · δt = β, (5.44)
P P
where A = i (Λi + λ · diag(Λi )), β = − i JiT Σi ri . In practice, the square ma-
trix A is often positive-definite, so we could leverage standard linear solvers such as
Cholesky. If the Jacobian Ji is large and sparse, we may also use sparse solvers such
as sparse Cholesky [46] or preconditioned conjugate gradient (PCG) [113] solver.
In practice, one often introduces robust kernel functions ρ : R 7→ R into (5.42) to
 5.4 Modern Libraries 115

reduce the effect of outliers:

X

min ρ rTi Σi ri , (5.45)
y
i

where ρ is designed to down-weigh measurements with large residuals ri . In this

case, we need to adjust (5.43) to account for the presence of the robust kernel.
A popular way is to use an IRLS method, Triggs’ correction [264], which is also
adopted by the Ceres [9] library. However, it needs 2nd -order derivative of the ker-
nel function ρ, which is always negative. This can lead 2nd -order optimizers includ-
ing LM to be unstable [264]. Alternatively, PyPose introduces an IRLS method,
FastTriggs, which is faster yet more stable than Triggs by only involving the
1st -order derivative:
p p
rρi = ρ′ (ci )ri , Jiρ = ρ′ (ci )Ji , (5.46)

where ci = rTi Σi ri , rρi and Jiρ are the corrected model residual and Jacobian due to
the introduction of kernel functions, respectively. More details about FastTriggs
and its proof can be found in [1], while IRLS was introduced in Section 4.3.
A simple LM optimizer may not converge to the global optimum if the initial
guess is too far from the optimum. For this reason, we often need other strate-
gies such as adaptive damping, dogleg, and trust region methods [158] to re-
strict each step, preventing it from stepping “too far”. To adopt those strategies,
one may simply pass a strategy instance, e.g., TrustRegion, to an optimizer.
In summary, PyPose supports easy extensions for the aforementioned algorithms
by simply passing optimizer arguments to their constructor, including solver,
strategy, kernel, and corrector. A list of available algorithms and examples can
be found in [3]. The usages of a 2nd-order optimizatoin can be found at https:
//github.com/pypose/slambook-snippets/blob/main/optimization.ipynb.

5.4.3 Related Open-source Libraries

Open-source libraries related to differentiable optimization can be divided into three
groups: (1) linear algebra, (2) machine learning libraries, and (3) specialized opti-
mization libraries.
Linear Algebra Libraries are essential to machine learning and robotics re-
search. NumPy [194], a linear algebra library for Python, offers comprehensive
operations on vectors and matrices while enjoying higher running speed due to its
underlying well-optimized C code. Eigen [105], a high performance C++ linear al-
gebra library, has been used in many projects such as TensorFlow [5], Ceres [9],
GTSAM [66], and g2 o [104]. ArrayFire [168], a GPU acceleration library for C,
C++, Fortran, and Python, contains simple APIs and provides GPU-tuned func-
tions.
116 Differentiable Optimization

Machine Learning Libraries focus more on operations on tensors (i.e., high-

dimensional matrices) and automatic differentiation. Early machine learning frame-
works, such as Torch [59], OpenNN [197], and MATLAB [172], provide primitive
tools for researchers to develop neural networks. However, they only support CPU
computation and lack concise APIs, which plague engineers using them in applica-
tions. A few years later, deep learning frameworks such as Chainer [262], Theano
[11], and Caffe [123] arose to handle the increasing size and complexity of neural
networks while supporting multi-GPU training with convenient APIs for users to
build and train their neural networks. Furthermore, the recent frameworks, such
as TensorFlow [5], PyTorch [202], and MXNet [52], provide a comprehensive and
flexible ecosystem (e.g., APIs for multiple programming languages, distributed data
parallel training, and facilitating tools for benchmark and deployment). Gvnn [109]
introduced differentiable transformation layers into Torch-based framework, leading
to end-to-end geometric learning. JAX [37] can automatically differentiate native
Python and NumPy functions and is an extensible system for composable function
transformations. In many ways, the existence of these frameworks facilitated and
promoted the growth of deep learning. Recently, more efforts have been taken to
combine standard optimization tools with deep learning. Recent work like The-
seus [211] and CvxpyLayer [10] showed how to embed differentiable optimization
within deep neural networks. PyPose [274] incorporates 2nd -order optimizers such as
Gaussian-Newton and Levenberg-Marquardt and can compute any order gradients
of Lie groups and Lie algebras, which are essential to robotics.
Other Specialized Optimization Libraries have been developed and lever-
aged in robotics. To mention a few, Ceres [9] is an open-source C++ library for
large-scale nonlinear least squares optimization problems and has been widely used
in SLAM. Pyomo [110] and JuMP [78] are optimization frameworks that have been
widely used due to their flexibility in supporting a diverse set of tools for con-
structing, solving, and analyzing optimization models. CasADi [13] has been used
to solve many real-world control problems in robotics due to its fast and effec-
tive implementations of different numerical methods for optimal control. Pose- and
factor-graph optimization also play an important role in robotics. For example, g2 o
[104] and GTSAM [66] are open-source C++ frameworks for graph-based nonlinear
optimization, which provide concise APIs for constructing new problems and have
been leveraged to solve several optimization problems in SLAM.
Optimization libraries have also been widely used in robotic control problems.
To name a few, IPOPT [273] is an open-source C++ solver for nonlinear program-
ming problems based on interior-point methods and is widely used in robotics and
control. Similarly, OpenOCL [138] supports a large class of optimization problems
such as continuous time, discrete time, constrained, unconstrained, multi-phase,
and trajectory optimization problems for real-time model-predictive control. An-
other library for large-scale optimal control and estimation problems is CT [100],
which provides standard interfaces for different optimal control solvers and can be
 5.5 Final Considerations & Recent Trends 117

extended to a broad class of dynamical systems in robotic applications. Drake [254]

has solvers for common control problems and that can be directly integrated with
its simulation tool boxes. Its system completeness made it favorable to researchers.

5.5 Final Considerations & Recent Trends

Deep learning methods have witnessed significant development in recent years [308].
As data-driven approaches, they are believed to perform better on visual track-
ing than traditional handcrafted features. Most studies on the subject employed
end-to-end structures, including both supervised methods such DeepVO [279] and
TartanVO [281] and unsupervised methods such as UnDeepVO [151] and Unsuper-
vised VIO [283]. It is generally observed that the supervised approaches achieve
higher performance compared to their unsupervised counterparts since they can
learn from a diverse range of ground truths such as pose, flow, and depth. Never-
theless, obtaining such ground truths in the real world is a labor-consuming process
[280].
Recently, hybrid methods have received increasing attention as they integrate
the strengths of both geometry-based and deep-learning approaches. Several stud-
ies have explored the potential of integrating Bundle Adjustment (BA) with deep
learning methods to impose topological consistency between frames, such as at-
taching a BA layer to a learning network such as BA-Net [251] and DROID-SLAM
[256]. Additionally, some works focused on compressing image features into codes
(embedded features) and optimizing the pose-code graph during inference such as
DeepFactors [62]. Furthermore, DiffPoseNet [199] is proposed to predict poses and
normal flows using networks and fine-tune the coarse predictions through a Cheiral-
ity layer. However, in these works, the learning-based methods and geometry-based
optimization are decoupled and separately used in different sub-modules. The lack
of integration between the front-end and back-end may result in sub-optimal per-
formance. Besides, they only back-propagate the pose error “through” bundle ad-
justment, thus the supervision is from the ground truth poses. In this case, BA
is just a special layer of the network. Recently, iSLAM [94] connects the front-
end and back-end bidirectionally and enforces the learning model to learn from
geometric optimization through a bilevel optimization framework, which achieves
performance improvement without external supervision. Some other tasks can also
be formulated as bilevel optimization, e.g., reinforcement learning [242], local plan-
ning [291], global planning [53], feature matching [306], and multi-robot routing
[106].
 6
Dense Map Representations
Victor Reijgwart, Jens Behley, Teresa Vidal-Calleja, Helen Oleynikova,
Lionel Ott, Cyrill Stachniss and Ayoung Kim

We now shift our focus to a different aspect of SLAM, specifically its mapping com-
ponent. The mapping problem is approached with the assumption that the robot’s
pose is known, and the objective is to construct a dense map of its surroundings.
Indeed, typical approaches first solve for the robot trajectory using the SLAM back-
end —as discussed in the previous chapters— and then reconstruct a dense map
given the trajectory. In this chapter, we illustrate the details of the dense map
representation, focusing on the map elements, data structures, and methods.
Early mapping approaches were predominantly based on sparse, landmark-based
solutions as discussed in Chapter 2 that extract only a few salient features from
the environment. However, the increase in compute capabilities paired with the
advent of accurate 3D range sensors, such as mechanical 3D LiDARs or RGB-D
cameras that provide detailed 3D range measurements at high frequencies, led to an
increasing research interest in dense map representations. Dense maps are crucial
for downstream tasks that require a detailed understanding of the environment,
such as planning, navigation, manipulation and precise localization. This chapter
explains how these dense methods leverage the full spectrum of range sensor data
to refine and update comprehensive maps.
The chapter begins by presenting key sensor types that facilitate dense mapping,
primarily focusing on range sensors that produce detailed range measurements.
The chapter continues with an introduction to fundamental representation elements
and data structures in Section 6.2, that we then tailor to specific applications in
Section 6.4. Contrasting with sparse landmark-based mapping, the choice of a dense
map representation hinges on the sensor types used and the intended downstream
applications. Key factors influencing the selection of the representation type are
summarized in Section 6.5.

6.1 Range Sensing Preliminaries

Before we delve into dense map representations, we briefly summarize a key sensing
modality, range sensors, often used for SLAM, providing the necessary context for
the following discussion. Such sensors produce range measurements to the objects
 6.1 Range Sensing Preliminaries 119

Figure 6.1 Single ray and multi-ray types for LiDAR sensors. Sample data from real-world
is visualized to show RGB, depth, and LiDAR range image.

in the environment, including LiDAR sensors, time-of-flight (TOF) cameras, RGB-

D cameras, and stereo cameras. Here, we concentrate on the most commonly used
LiDAR sensors and RGB-D cameras that are predominately used in outdoor and
indoor environments for SLAM and dense mapping.

6.1.1 Sensor Measurement Model

Let us start with a brief summary of the sensing mechanism and associated measure-
ment model. In the case of LiDAR sensors,1 the range measurements are generated
using laser beams that are emitted, reflected by the environment, and then detected
[224]. By measuring the time temit when the laser beam is emitted and the time of
detection tdetect , we can derive the range r using the speed of light c as follows:
c (tdetect − temit )
r= . (6.1)
2
A single ray measurement can be enhanced into a two-dimensional (2D) or three-
dimensional (3D) collection of points by employing an array of rays that move in a
designated pattern, such as a 360-degree rotation or a specific shape. The collection
of points generated by the sensor is referred to as point clouds, which serve as the
fundamental element for creating maps. The LiDAR measurements can also be
represented using a range image R ∈ RB×M , where the range of each of the B
beams is stored for a single complete turn of the sensor, i.e., a complete 360◦
rotation. Thus, we have M measurements in the horizontal field of view of the
sensor for each of the B beams.
Another commonly used range sensor in robotics applications are RGB-D cam-
eras, such as Microsoft’s Kinect and Azure Kinect DK, and Intel’s RealSense. RGB-
D cameras provide besides the RGB image IRGB ∈ R3×H×W of height H and width
W , a depth map ID ∈ RH×W of the same dimension, where each pixel location
contains the depth or range. To generate the depth map ID , early RGB-D cameras
1
We illustrate this chapter using simple 2D and mechanically rotating 3D LiDARs, while other
solid-state and flash LiDARs will be introduced in Chapter 9
120 Dense Map Representations

use a structured infrared (IR) light source with a known pattern that is projected
onto the environment. The distance of individual pixels is then determined from
the distortion of the known pattern. As sunlight usually interferes with this sensing
mechanism, such RGB-D cameras using projected light are mainly used in indoor
environments. Fortunately, newer generations of RGB-D sensors introduce an IR
texture projector or TOF less affected by interferences, supporting outdoor appli-
cations.

6.1.2 Conversion to Point Cloud

Using the intrinsics of a range sensor (e.g., a LiDAR or RGB-D camera), we can
convert a range image R or a depth map ID into a point cloud P = {p1 , . . . , pN },
where points pi ∈ R3 are expressed in the local coordinate frame of the sensor. The
point is the most fundamental unit in the map representation and will be discussed
further in this chapter.
For conversion from LiDAR, the sensors provide an
intrinsic calibration for each beam (ϕi,j , θi,j ), where 1 ≤
i < B and 1 ≤ j < M , consisting of the azimuthal angle Figure 6.2
ϕi,j ∈ [0, 2π] and polar/inclination angle θi,j ∈ [−π, π]
as depicted in Figure 6.2. Using these known angles of
each beam, we can convert a range measurement ri,j
at Ri,j into a three-dimensional point p = (x, y, z) as
follows:

x = ri,j cos(θi,j ) cos(ϕi,j ) (6.2)

y = ri,j cos(θi,j ) sin(ϕi,j ) (6.3)
z = ri,j sin(θi,j ) (6.4)

For an RGB-D camera, we commonly use a pinhole camera model to convert

the ranges ru,v = Ru,v at pixel location (u, v) into a three-dimensional coordinate.
For this, we use the intrinsics of the camera K ∈ ℜ3×4 to convert a homogeneous
coordinate x = (u, v, ru,v ) in the image coordinate into a point p in the camera
coordinate:
p = K −1 x. (6.5)

The resulting point cloud is said to be organized or unorganized depending on how

the points are structured. When converted from a depth map, the point cloud is
organized, and each point location is structured with respect to the pixel location of
the associate depth map. This can be exploited to compute neighboring points by a
simple indexing. On the other hand, the point cloud generated by a LiDAR sensor
is more complicated. For example, the generated point cloud is organized in the
static 3D mechanical LiDAR. However, the organization of the point cloud no longer
 6.2 Foundations of Mapping 121

(a) Points (b) Occupancy (c) Implicit Surface (d) Distance Field (e) Surface

Figure 6.3 Examples of common dense representations.

holds in the case of non-repeated pattern solid-state, flash LiDARs, or mechanically

rotating LiDAR under motion distortion. In many cases, the unorganized points
are distorted due to the movement of the sensor and measurement neighborhood
in the range image is not necessarily correlated to spatial neighborhood. Therefore,
an estimate of the motion of the sensor while completing a sweep, e.g., inertial
measurement unit (IMU) measurements or odometry information, together with
the information of the per-beam time is necessary to undistort an LiDAR point
cloud to account for the motion of the sensor [272, 70, 300]. For more details on
motion distortion and compensation, refer to Chapter 9.

6.2 Foundations of Mapping

A map, generated with sensors’ information and data processing approaches, is a
symbolic structure that models the environment [260, 41]. One thing to be noted
here is that the map representation can be diverse, and many different represen-
tations exist for the same spatial information (as in Figure 6.3). The choice and
accuracy of the scene representation strongly impact the performance of the task
at hand, and thus the representation should be determined by the use case. For
instance, motion estimation and localization in robotics favor sparse representa-
tion, such as 3D points features [182, 214] in order to exploit these features for
consistent robot pose estimation. On the other hand, a key objective of scene
reconstruction is an accurate, dense, and high-resolution map, for example, for
inspection purposes [119, 193, 219]. Similarly, path planning tasks require dense
information such as obstacle occupancy or closest distance to collision for obsta-
cles avoidance [179, 83, 266]. Overall, this chapter examines the following three
questions.
Q1. What quantity do we need to estimate for dense mapping? The
most commonly used quantities to represent the environment are occupancy and
distance. Occupancy is a key property in mapping for distinguishing between free
and occupied space. Distance estimation provides a more robot-centric interpre-
tation of free and occupied space by measuring the range to nearby surfaces or
objects.
122 Dense Map Representations

Explicit Implicit

   



     

Figure 6.4 The representation can be either explicit or implicit. The illustration is simpli-
fied for clarity. In reality, the abstraction is not clearly separable and can often be applied
in a combined manner.

Q2. How should the world be represented? This is the question of what
space abstraction we use for representation. Broadly, representation can be either
explicit or implicit. Explicit space abstractions are further classified based on the
type of geometry they utilize, while implicit representation can be categorized based
on their choice of functions. The list of abstraction types are illustrated in Figure
6.4.
Q3. What data structure and storage should be used? The chosen repre-
sentation should be stored in memory for later use. The data structure and storage
method should be selected based on the specific application and intended usage.
In the literature, a wide variety of approaches exist for generating a dense rep-
resentation of the scene using range sensors, varying in terms of their estimated
quantities, space abstraction, storage structure, continuity, and application areas.
Beginning the discussion on different representations, we first explore the primary
quantities estimated from range measurements. The focus is on understanding the
key quantities predominantly estimated in the mapping phase. We will provide a
concise overview of the basic definitions of each quantity, elaborating their signifi-
cance and the specific contexts in which they play a crucial role.

6.2.1 Occupancy Maps

Since their introduction over three decades ago by Elfes and Moravec [83, 179], oc-
cupancy grids have been widely used. Their simplicity and computational efficiency
have made occupancy grids2 popular when mapping indoor (and even outdoor)
environments. In the simplest scenario, the estimated quantity is the probability
of a cell being occupied. In this case, the occupancy of the cell is modeled as a
probability of that cell containing an obstacle, with occupancy equal to 1 for occu-
2
Occupancy mapping can be conducted without grid-based methods (e.g., GPOM [190]). In this
chapter, we will focus on grid-based mapping for simplicity.
 6.2 Foundations of Mapping 123

pied cells and 0 for cells deemed empty. Essentially occupancy mapping is a binary
classification problem to predict the binary class probability of each cell.

mi

robot

Figure 6.5 A simple ray-casting example in 2D. Given a range measurement at a certain
(computed or estimated) azimuth, the return of the range measurement indicates the
existence of an obstacle.

Given a set of sensor measurements z1:t and a set of sensor poses x1:t , the proba-
bility of being occupied for each cell in the map m is modeled as p(m|z1:t , x1:t ). A
sample map is shown in Figure 6.5. Assuming that each cell mk is independent and
that the measurements are conditionally independent, the update of occupancy can
be formulated efficiently using the well-known log-odds form [179],
l(mk |z1:t ) = l(mk |z1:t − 1) + l(mk |zt ) , (6.6)
where l(·|·) = log(o(·|·)), and o(·|·) is the odds form:
p(mk |z1:t )
o(mk |z1:t ) = . (6.7)
1 − p(mk |z1:t )
The main advantage of occupancy mapping is shown in (6.6), where only the
previous occupancy value and the inverse sensor model l(mk |zt ) are needed to up-
date the probability through a simple addition. Despite these benefits, occupancy
grids rely on very strong assumptions of the environment to be efficient. Notably,
the assumption that the likelihood of occupancy in one cell is independent of other
cells disregard spatial correlations that can be important to infer occupancy in
unobserved nearby regions. Additionally, traditional occupancy grids require the
discretization of the environment be defined a priori which makes the spatial reso-
lution constant throughout the map.

6.2.2 Distance Fields

Another way of representing the geometry surrounding the robot is not through
probabilities of occupancy, but rather by describing the boundary between free and
occupied space. In an ideal world, we could describe the shape and location of this
124 Dense Map Representations

boundary using an analytical function in three variables (x, y and z) which reaches 0
whenever a point p = (x, y, z) lies on the surface. In other words, the function’s zero
crossings correspond to the surface itself. Such a function is known as an implicit
surface. By convention, the function’s sign is negative when p lies inside an object
and positive outside. A simple example in Figure 6.6 illustrates how this implicit
function values are determined.
1.0 1.0

0.5 0.5

0.0 0.0
Y

0.5 0.5

1.0 1.0
1.0 0.5 0.0 0.5 1.0
X

Figure 6.6 A solid 2D disk represented as an explicit surface (black outline) and implicit
surface (colored field). The implicit surface function is negative inside the object (blue)
and positive outside (red). Note how the function’s zero crossings correspond to the surface
itself.

There are many advantages to continuous implicit surface representations. Given

that there is no fixed resolution, they can represent objects of arbitrary shapes at
any level of detail. Furthermore, they make it possible to check if a given point in
space is inside or outside an obstacle by simply evaluating the function’s sign.
Different types of functions can be employed to model implicit surfaces, with
the Euclidean Signed Distance Function (ESDF) being a prevalent option. At any
query point, the ESDF expresses the distance to the nearest surface (indicated by
the magnitude of the ESDF) and whether the point is inside an obstacle (indicated
by the sign of the ESDF). ESDF representations are commonly used by accelerated
geometric algorithms for tasks such as collision checking. Furthermore, high-quality
proximity gradients can be derived from the ESDF for optimization-based motion
planning and shape registration.
The ESDF is computed by finding the (Euclidean) closest surface point for each
point in the map. For a known surface, this can efficiently be done using techniques
such as Fast Marching. However, for surface estimation, the projective signed dis-
tance is more commonly used as it can efficiently be computed from measurements
and is better suited for filtering. Given a measurement ray going through a query
point, the projective distance is defined as the distance from the beam’s endpoint to
the query point along the ray. This eliminates the need to search the closest point
 6.3 Map Representations 125

explicitly. Although the projective distance overestimates the Euclidean distance,

its zero crossings (the estimated surface) remain correct. The standard approach of
estimating implicit surfaces, proposed by Curless and Levoy [61] and popularized
in the field of robotics by KinectFusion [185], combines the projective signed dis-
tances for all measurements using a simple weighted average. To reduce the impact
of overestimates, the projective signed distance function is typically clamped to a
fixed range, named the truncation band, in which case it is called the Truncated
Signed Distance Function (TSDF). Note that ESDFs can efficiently be computed
from TSDFs and occupancy maps, as will be described in Section 6.4.4.

6.2.3 Occupancy Maps or Distance Fields?

Being volumetric methods, a shared aspect of these estimated quantities in occu-
pancy and implicit representations is that they model the geometry by estimating
a quantity of interest everywhere in the observed volume. However, each represen-
tation fundamentally prioritizes different things. Which option is best, therefore,
depends on the application. We will briefly summarize two key differences.
Directness in modeling: Given that measurement rays directly tell us which
parts of space are free, occupied or unobserved, maps based on occupancy proba-
bilities can be updated using fewer heuristics and assumptions. In contrast, implicit
surfaces typically model the distance to the surface. This can be computed exactly
for a known surface, but not from partial measurements. For surface estimation,
they therefore rely on distance proxies such as the previously introduced TSDF.
Smoothness: Implicit surface maps are inherently smoother than occupancy
maps, which model a binary property. The smoothness of implicit surfaces has many
benefits. Most importantly, it makes them differentiable. The resulting proximity
gradients are valuable for many applications. Smoothness also reduces the approx-
imation errors resulting from discretization and makes it possible to obtain good,
sub-pixel resolution estimates through interpolation. However, since discontinuities
cannot be represented smoothly, implicit surfaces tend to miss thin obstacles.

6.3 Map Representations

6.3.1 Explicitness of Target Spatial Structures
As summarized in Figure 6.4, the representation can be classified based on their ex-
plicitness and target space. In 3D mapping, representing volume is straightforward;
however, surfaces are equally important in robotic mapping for enabling down-
stream tasks. We can consider four major categorization: explicit surface, implicit
surface, explicit volume, and implicit volume representations.
For surfaces, we can either explicitly or implicitly represent a surface. Defined
as a 2D manifold, explicit type of representation aims to characterize the space
126 Dense Map Representations

in terms of their boundary of the objects in the scene. The simplest abstraction
that can represent the boundary is directly the point cloud produced by the range
sensors. Another general representation of the surface is the polygon mesh (Section
6.4.3), which comprises vertices, edges, and faces. These meshes have the ability
to encode the directed surfaces of a volume by forming connected closed polygons,
more commonly triangles. Surfels (Section 6.4.2) are also popular abstraction widely
used in mapping. Surface representations are a key for any visualization application,
but also are used for rendering simulated environments, augmented reality or for
computed aided design and 3D printing.
Similar strategies are employed in 3D volume modeling. Naive point-based rep-
resentations are commonly used in LiDAR SLAM. Additionally, occupancy or
distance-based voxels (Section 6.4.4) are popular choices for explicit representation.
When storing volumetric maps, careful consideration of data storage is necessary
to minimize computational costs. Implicit representations for volumetric mapping
are also utilized, typically through functions. GP (Section 6.4.5) and Hilbert maps
(Section 6.4.6) are well-known examples of implicit representations.

6.3.2 Types of Spatial Abstractions

6.3.2.1 Points
Given range measurements, a straight-forward dense map representation is using
clouds. For generation, we accumulate the point clouds Pt recorded at time t in the
local coordinate frame F t using the estimated global pose Tt1 in a global map point
cloud PM . Also common practice is to assume our global coordinate frame of PM
is given in the coordinate frame of the first point cloud F 1 .
Since simply accumulating point clouds P1 , . . . , Pt does not scale to larger envi-
ronments, a common strategy is to discard redundant measurements of the same
spatial location. To this end, most methods [307, 70, 272] use efficient nearest neigh-
bor search, such as voxel grids or hierarchical tree-based representations (see Section
6.3.3), to subsample and store the point clouds, e.g.,, store only a limited number
of points per voxel [70, 272] or only specific points that meet a certain criterion
are stored [307]. Additionally, a representation of only keyframes where only a few
point clouds are explicitly stored is possible, but this requires to determine when a
keyframe or submap needs to be generated.
Being the most elemental representation form, a point cloud map can be con-
verted into other representations, e.g., a mesh via Poisson surface reconstruc-
tion [270] or a Signed Distance Function (SDF) via marching cubes [271]. Un-
fortunately, this is feasible only with additional data, such as the viewpoint of a
point’s measurement. Yet, this information may be lost when merging multiple
measurements into a point cloud map. Therefore, assumptions about the surface’s
direction are often required to discern inside or outside regions.
 6.3 Map Representations 127

(a)

(b)

Figure 6.7 Qualitative comparison of maps generated by accumulating point clouds and
surfels from a sequence of LiDAR scans from the KITTI dataset [96] Sequence 07. (a)
Point cloud map. The brightness of points indicates the remission of the LiDAR measure-
ments. (b) Corresponding surfel map based on circular disks. The complete map with all
accumulated point clouds use 2.95 GB, while the corresponding surfel map by SuMa [24]
uses only 160 MB.

6.3.2.2 Surfels
While point cloud maps directly represent the measurements, the stored points
do not contain surface information or can represent from which direction a point
has been measured. With surfels [208], we can encode such information by adding
directional information to a point. Surfels are commonly represented via circular or
elliptic discs [131, 24, 285, 35, 34], or more generally ellipsoids [246, 247, 74, 314]
modeled with a a Gaussian. So-called splatting [314, 34] allows to integrate texture
information but also blend overlapping surfels into coherent renderings of a specific
viewpoint.
A commonly employed circular surfel representing a circular disk is defined by
a location p ∈ R3 , a normal direction n ∈ R3 , and a radius r ∈ R. As rendering
primitive such surface patches can be efficiently rendered using the capabilities of
modern graphics processing units (GPUs), which can be exploited to efficiently
128 Dense Map Representations

render arbitrary views. This accelerates point-to-surfel associations and leads to

the substantial memory reduction.
Figure 6.7 qualitatively compares a point cloud-based and a surfel-based map
representation. A dense point cloud can accurately represent the environment with
a high level of detail, but at the cost of memory. In contrast, while losing fine details
as multiple measurements get aggregated into a single surfel, significant memory
usage can be reduced while preserving the main structural details of larger surfaces.
Closely related to the explicit geometric representation of surfaces via surfels,
i.e., small circular surface patches, is the representation via a normal distributions
transform (NDT) [29, 245]. Using NDT, the space is subdivided into voxels and the
points inside a voxel are approximated via a normal distribution N (µ, Σ), having
estimated mean µ and covariance from the enclosed points Σ. The eigenvalues
λ1 < λ2 < λ3 and corresponding eigenvectors v1 , v2 , v3 of the covariance can be
used to estimate the surface properties inside a voxel. For planar surfaces (λ1 ≪ λ2 ),
the eigenvector v1 of the smallest eigenvalue λ1 corresponds to the surface normal.
Thus, for planar surfaces the NDT represents a surfel, and can also more accurately
represent point distributions that cannot be approximated via a surfel. In that sense,
the NDT is a hybrid representation that is explicit due to the space division into
a voxel grid, but also implicit due to the representation of voxels via a normal
distribution which continuously represents the space inside a voxel.

6.3.2.3 Meshes
While describing local surface properties, both point clouds and surfel maps are still
relatively sparse as they do not model the surface’s connectivity. One way to get a
more complete understanding is to use meshes, which describe the surface as a set
of points that are connected to form a collection of polygons. This, in turn, makes it
possible to represent watertight surfaces, query and interpolate new surface points,
and efficiently iterate along a connected surface.
In meshing terminology, each polygon is referred to as a face, and each corner
point as a vertex. The most common types are triangle meshes, where each face is
bounded by three vertices, and quad meshes, whose faces are bound by four vertices.
Note that a polygon, or face, can always be broken down into an equivalent set of
triangles; hence, triangle meshes are not only the simplest but also the most general.
A mesh is a very flexible and memory-efficient representation because the number
of faces and vertices can be directly adapted to the surface complexity and required
detail. For example, a plane of any size can be represented with just two triangular
faces and four vertices. Furthermore, meshes are well-suited for parallel processing
and rendering. Meshes are often used in applications that overlap with computer
graphics, such as rendering, surface analysis, manipulation, and deformation, and
more generally in applications involving digital models, simulation, or surface-based
algorithms, such as path planning for ground robots.
 6.3 Map Representations 129

6.3.2.4 Voxels
Point clouds and meshes are well suited to represent properties of the environment
that are defined along surfaces. However, certain estimated quantities, including
occupancy and Signed Distance, are defined throughout the entire volume. One
straightforward way to store and process volumetric properties is to discretize them
over a regular grid. Discretized occupancy and Signed Distance maps are called
occupancy grids and Signed Distance Fields, respectively.
Generalizing the concept of 2D pixels, the cells in a 3D grid are referred to as
voxels. Given a grid’s regular structure, each voxel can easily be assigned a unique
index and stored in a data structure. Note that a voxel is merely a container or, more
formally, a space partition. The significance of its contents varies from one method
to another. In a classic occupancy grid, a voxel’s value represents the likelihood
that any point in the voxel is occupied. Hence, the occupancy at an arbitrary point
in the map is equal to the value of the voxel that contains the point. However, a
voxel’s value does not have to represent a constant little cube. For example, voxels
in a Signed Distance Field estimate the signed distance at each voxel’s center.
To retrieve the signed distance at an arbitrary point, one would therefore query
the voxels that neighbor the point and obtain the point’s value using interpolation.
Finally, some applications even use (sparse) voxel grids to store and efficiently query
non-volumetric properties, such as points or surface colors.

6.3.2.5 Continuous Functions

Functions are a key abstraction for mapping in a continuous manner. The problem of
mapping in this case is reduced to fitting a parametric or non-parametric function,
i.e., solving a regression problem. Most of the above-mentioned space abstractions
require the discretization of the environment to be defined a priori, which usually
makes the spatial resolution constant throughout the map. Continuous functions,
however, parametric or non-parametric, give more flexibility allowing the resolution
to be recomputed and also provide interpolation capabilities to fill up data gaps.
Some parametric functions such as infinite lines in 2D [263] and planes in 3D [128,
97, 263] require making strict assumptions about the environment and limit the
representation of the scene. However, these representations are efficient in terms
of memory consumption and computational complexity. Control points-based func-
tions (e.g., B-splines [223]) or non-parametric (e.g., GP-based) have the ability
to model the environment with fewer assumptions, still in a continuous manner.
From occupancy [190], implicit surface [287, 146], distance fields [288], and surface
itself [268], GP-based representations are a popular choice to represent the environ-
ment —despite their high computational complexity— because of their probabilistic
nature, which enables uncertainty quantification and inference over both observed
and unseen areas [99].
A key advantage of the continuous functions for mapping is that if they are
130 Dense Map Representations

Figure 6.8 The algorithm works by projecting the cubes into the implicit surface, querying
the sign of the values at the corners of the cubes, and looking up which one of the 15
configurations these values map to.

chosen to be at least once differentiable they will be able to provide gradients.

Gradient information can be key for localisation to compute surface normals [289],
loop closure to compute terrain features [98], for data fusion [146] and planning [289]
applications.

6.3.2.6 Conversions
The abstractions listed above are not always used exclusively; they are often con-
verted from one form to another or employed simultaneously in multiple forms.
For instance, explicit geometry, such as points and meshes, can be transformed
into an implicit surface. One flexible method to compute the signed distance at any
point in space is through a closest point lookup, which can be performed against
any explicit geometry and on-demand, only when and where needed. Alternatively,
the signed distance can be computed across all points on a regular grid using
wavefront propagation, which can efficiently be implemented via the fast march-
ing method [231].
Conversely, it is common to convert implicit surfaces into mesh representations.
The original technique for converting distance fields into meshes is known as March-
ing Cubes [157]. The algorithm divides the implicit surface into a grid of fixed-size
cubes, which it processes independently. Each cube generates a set of triangle ele-
ments based on the implicit surface’s values at its eight corners (Figure 6.8). The
positions of their vertices are then refined through linear interpolation. Meshes, in-
cluding those from BIM or CAD models, can, in turn, be sampled to create points
or surfels.
Occasionally, a discrete representation must be converted into a continuous one.
This is typically achieved by solving an optimization problem over the parameters
 6.3 Map Representations 131

(a)

(b)

Figure 6.9 (a) Mapping of logical grid coordinates to an underlying naive array-based
storage through a function f (·) that uniquely maps coordinates to linear indices. (b)
Storage of unordered data directly into an array structure.

of the continuous representation, minimizing the fitting error with respect to the
discrete data.

6.3.3 Data Structures and Storage

Previously introduced abstractions all need to be stored in memory. In this section,
we explore how various abstractions are stored in memory by examining the choice
of data structures along with their advantages and disadvantages.

6.3.3.1 Naive Data Storage

For many representations a simple dynamically resizeable array is a reasonable
starting point. For data with a pre-defined spatial partitioning two things are
needed, the type of data to store and a conversion function from a spatial coordinate
to an index coordinate. This is often used when building maps representing occu-
pancy or signed distance values. For irregular data, only the type of data to store
is needed, for example point clouds or surfels. The naive storage of ordered data
using a mapping function and unordered pointcloud data is illustrated in Figure
6.9.
The benefit of this naive approach is that it is simple and provides fast random
access. The trade-off is, that large amounts of memory can be required for such a
representation. Also while read and modify operations are fast, changing the spatial
dimension of the representation can be very costly as the content of the entire data
structure needs to be copied.

6.3.3.2 Hash Map

A natural extension to address the limitations of the naive storage method described
above is to use a hash table. This approach divides the map into shards, applies
132 Dense Map Representations

a hash function to convert the coordinates of each shard into a single value, and
stores the sharded data in a table indexed by this hash value. The shards are
typically chosen to represent map subregions with well-established coordinates, such
as cubes in a regular grid. These cubes may correspond to individual voxels or fixed-
size groups of voxels, referred to as voxel blocks. Alternatively, they can also store
other elements like points, surfels, or mesh fragments contained in their respective
subregions.
A hash table retains the fast O(1) look-up time of a fixed array while allowing
the map to grow dynamically without reallocation. Three key considerations must
be addressed when using a hash table for dense map storage:

1 Granularity of the sharding: Smaller shards improve sparsity by allocating

data only where necessary. However, the number of shards should not grow too
large, as this reduces the hash table’s insertion performance and memory effi-
ciency. This trade-off is particularly relevant when hash maps are used to store
properties that only exist along the surface.
2 Hash function: An ideal hash function distributes keys evenly across the table,
even when the data is spatially adjacent, as is often the case in mapping scenarios.
3 Collision resolution: The method for handling hash collisions, whether through
linear chaining (where each entry contains a linked list) or open addressing, sig-
nificantly affects the performance of the hash table.

In most cases, hash tables offer a good balance of fast access and efficient insertion
and deletion of data. However, they may require initial tuning to perform well for
a given application.

6.3.3.3 Tree-based Data Structures

Another option to efficiently store spatial data while only occupying memory for
relevant parts of the environment is to use hierarchical, tree-based representations.
Just like hash tables, trees generally enable efficient access and insertions. However,
their unique strength is their hierarchical structure, which can be used to efficiently
store multi-resolution data and speed up spatial operations such as nearest neigh-
bor search. The most prominent tree variants are kD-trees [26], bounding volume
hierarchies (BVH) [58], and octrees [175].
Among them, the octree efficiently searches neighbors with the capability to
integrate novel measurements incrementally. The octree is a tree representing each
node by a so-called octant that refers to a subspace. An octant is defined by a
center c ∈ R3 and an extent e ∈ R, corresponding to an axis-aligned bounding-box.
Each octant has potentially 8 child octants of extent 12 e, as depicted in Figure 6.10.
Common practice is to store points only in the leaf octants (i.e., octants without
children) and determine subsets of points at inner octants of the tree structure by
tree traversal.
 6.3 Map Representations 133

Figure 6.10 Example of an octree and its octants at different levels of the tree hierarchy.
Each level of an octree subdivides the space in more fine-grained octants. Note that deeper
levels of the octree only represent the occupied space.

To construct an octree, we iteratively divide space into octants within an axis-

aligned bounding box encompassing a point cloud P. Each division splits P into
subsets P1 , . . . , P8 , corresponding to 8 child octants of half extent 21 e. Non-empty
subsets Pi form child octants with center c and extent 21 e, stopping at a specific
octant size or a minimal point count. Once constructed, updates and insertions can
efficiently be performed by traversing the tree structure and adding inner nodes as
needed. When new data is inserted that falls outside of the tree’s root octant, the
tree can be extended by creating a new root node and assigning the new data and
the old root node to its children.
In contrast to voxel grids, an octree represents only data-containing subspaces,
enabling efficient storage of occupied space. However, this memory efficiency re-
quires tree traversal to access specific leaf octants, potentially leading to increased
runtime to locate points. Additionally, the tree structure itself must be explicitly
represented, incurring extra memory overhead. Several recent approaches address
these memory overheads [84, 192, 25].

6.3.3.4 Hybrid Data Structures

To balance memory requirements and runtime for data access, several data struc-
tures combine the advantages of different data structures in specific ways, leading
to hybrid representations. In this tradeoff, we accept less efficient memory usage
but enable more efficient memory access.
For example, hashed voxel grids [186] combine the strengths of dense voxel grids
and hash tables by splitting the environment into fixed-sized, dense blocks (e.g.
8×8×8 voxels), which are in turn stored in a hash table. Thanks to the hash table’s
flexibility, blocks only need to be allocated in locations that contain meaningful
information (e.g. near the surface). At the same time, using a plain 3D array to
store the voxels inside each block ensures that operations remain simple, efficient,
and even suitable to GPU acceleration.
Another option is to combine hash tables with trees. In a similar vein to hashed
voxel grids, the VDB 3 data structure [184, 183] splits the space into hashed blocks,
but stores a hierarchical tree inside each block. This data structure provides all the
3
VDB refers to sparse volumetric data and stands for several different thing as Voxel Data Base or
Volumetric Data Blocks. Here we follow the terminology used in [184].
134 Dense Map Representations

Section Space Abstraction Type Representing Map Entities

6.4.1 Points Surface
6.4.2 Surfels Surface
6.4.3 Mesh Surface (connected)
6.4.4 Voxels Occupancy or Implicit surface
6.4.5 - 6.4.6 Continuous function Occupancy or Implicit surface

Table 6.1 Summary of presented mapping methods.

benefits of hierarchical trees, including multi-resolution representation and efficient

nearest neighbor lookups. However, since each block has a fixed size, the maximum
tree height is constant regardless of the size of the environment. Lookups and in-
sertions can therefore be performed in constant time, and significantly faster than
when using pure trees.

6.4 Constructing Maps: Methods and Practices

So far, we have explored the target quantities to estimate and the various space
abstractions available for mapping. In this section, we will examine in detail the
methods used to construct these map elements. The approaches are categorized by
their main space abstraction, as shown in Table 6.1. Note that some of the methods
use additional space abstractions to improve performance, for example, by grouping
points into voxels for more efficient storage and faster queries.

6.4.1 Points
As mentioned in Section 6.3.2.1, naively storing points by accumulating the mea-
sured point clouds will not scale to large-scale environments and will lead to redun-
dantly represented measurements. Therefore, most approaches [307, 70, 272] adopt
a point-based representation in combination with a voxel grid or octree to repre-
sent the dense map. Moreover, the selection of a data structure is driven by the
requirement for efficient nearest neighbor searches, essential for conducting scan reg-
istration through iterative closest point (ICP), where point correspondences must
be iteratively established.
In order to handle large-scale environments, some methods, such as the well-
known LiDAR SLAM LOAM [307], filter the raw point clouds to extract corner
and surface points thereby significantly reducing the point cloud size. A voxel grid
is applied to store only a subset of points in the map representation, pruning redun-
dant measurements. Stemming from the point-based voxelization used in LOAM,
several follow-up approaches [234, 276, 198, 152, 213, 233] refine the extraction of
points [234, 276, 198], improve the optimization pipeline [198, 152], or integrate
information of an IMU [213, 233].
 6.4 Constructing Maps: Methods and Practices 135

Another branch of methods handles the amount of point cloud data differently
to avoid reliance on a capable feature extraction approach. Regularly sampling
the point clouds via a voxel grid [70] significantly reduces the number of points
per LiDAR scan and removes potentially redundant information. The key insight
is here that points in the voxel grid are not aggregated and averaged, but original
measurements are retained. Following these insights, Dellenbach et al. [70] and Vizzo
et al. [272] use this strategy to downsample an input point cloud, only storing a
restricted number of points inside a voxel grid map.
Overall, as also mentioned in Section 6.3.2.1, the (hashed) voxel grid serves dual
purposes: it abstracts space by storing a limited number of point measurements per
voxel, and it facilitates accelerated nearest neighbor search through direct indexing
of neighboring voxels.

6.4.2 Surfels
For surfels, similar strategies can be applied as for point clouds, but notably Stückler
et al. [246] and follow-up work by Dröschel et al. [74] use an octree to represent
surfels at multiple levels in the octree hierarchy for data association. The so-called
multi-resolution surfel maps indirectly represent the surfels via accumulated mean
and covariance statistics, like a NDT.
In contrast, Whelan et al. [285] store surfels as a simple list and exploit efficient
rendering techniques to produce a projection for data association for RGB-D SLAM
in indoor environments. In this case, surfels are explicit geometric primitives and,
therefore, need to be directly handled to update the surfel properties (i.e.,, size
and direction) accordingly [131]. A key contribution of Whelan et al. is leveraging
a map deformation that directly deforms the surfels instead of relying on a pose
graph optimization, which enables the use of the measurements represented by the
surfels to deform the map on a loop closure detection. A similar strategy for map
deformation of surfels was employed by Park et al. [200].
Similarly, Behley et al. [24] target outdoor environments, which makes it neces-
sary to represent the surfels via multiple submaps of 100 m×100 m spatial extent
that can be off-loaded from GPU memory. In contrast to ElasticFusiuon [285],
the approach relies on pose graph optimization but exploits that surfels can be
freely positioned and ties surfels to poses enabling a straight-forward deformation
of the map with pose-graph-optimized poses, which was also adopted by other ap-
proaches [278].

6.4.3 Meshes
As introduced ealier, meshes offer an expressive, flexible way to represent connected
surfaces. Mesh generation methods can be split into two families of approaches.
136 Dense Map Representations

The first family directly converts the measured points into a mesh. In contrast, the
second family splits the problem into two steps: reconstructing an implicit surface,
followed by iso-surface extraction to get the final mesh (see Section 6.3.2.6).

Methods in the first family typically work directly by computing the Delaunay
triangulation of the input point set and identifying the subset of Delaunay trian-
gles that lie on the surface. A detailed overview of such methods is provided in [45].
When building directly from points, the mesh implicitly adapts itself to the sam-
pling density. This can be an advantage, as it provides adaptive resolution, but it
also means these methods are more sensitive to sampling irregularities and holes. In
practice, direct meshing methods are chosen when the entire surface can be sampled
densely with a very accurate depth sensor, for example, using surveying equipment.

The second family of approaches uses an implicit surface as an intermediate step,

to simplify the process of fusing and filtering the data before extracting the final
surface mesh. One intuitive way to generate the implicit surface from data is to
estimate the distance to the surface at each point on a regular grid. As described in
Section 6.2.2, the implicit surface’s sign must also be set according to whether each
point is inside or outside an object. This information is often determined based on
estimated surface normals, which can for example be obtained by applying Principal
Component Analysis (PCA) over a small surrounding area. However, as indicated
in [114], such methods may yield implicit surfaces that are discontinuous. Tackling
this issue, Carr et al. [44] model the implicit surface using a collection of Radial
Basis Functions (RBFs) and fit these to the input points by solving a global opti-
mization problem. The resulting implicit surfaces are smooth by construction and
faithfully fill holes based on the global context. Unfortunately, solving the underly-
ing large, dense optimization problem is computationally expensive. Shen et al. [235]
overcome this limitation by locally approximating the input points using moving
least squares (MLS). Going one step further, Poisson Surface Reconstruction [129]
fits the implicit function to the normals of the measured points by solving a partial
differential equation (PDE), resulting in a sparse, computationally tractable opti-
mization problem that is particularly robust to noise.

In robotics applications, constructing a mesh from a live sensor stream is often de-
sirable. One way to make surface reconstruction efficient enough to run in real-time
is to use incremental updates. TSDF-based surface reconstruction is particularly
popular in practice given its inherently incremental nature and general simplicity.
This method falls under the second family of approaches and estimates the implicit
surface by averaging projective distances. Since the cost of updating the TSDF, or
implicit surface in general, overshadows the cost of the mesh extraction, real-time
methods primarily focus on optimizing the former.
 6.4 Constructing Maps: Methods and Practices 137

6.4.4 Voxels
Voxel-based methods are among the most commonly used volumetric represen-
tations in 3D reconstruction and robotics. Instead of covering a swath of existing
literature chronologically, this section will focus on concepts commonly encountered
in practice and organize them according to three fundamental decision criteria: the
chosen estimated quantity, data structure, and scalability considerations.

6.4.4.1 Methods by their Estimated Quantity

The first choice in a voxel-based mapping framework is which quantity to estimate,
with the most common options being occupancy (see Section 6.2.1) or a distance
metric (see Section 6.2.2). The previous discussion in Section 6.2.3 can be used to
decide between the two.
Since the introduction of the original continuous probabilistic occupancy mea-
surement model for sonar [179], simplified piecewise-constant models have been
developed to reduce computational costs [116]. This shift was influenced by the ad-
vent of LiDAR technology and the growing interest in transitioning from 2D to 3D
maps. More recently, Loop et al. [156] presented a continuous probabilistic model
that, instead of inflating objects, converges to an occupancy probability of 0.5 along
objects’ surfaces. Occupancy estimation, popular for collision avoidance due to its
superior recall, is limited by its discontinuous nature and uninformative gradients
compared to distance-based methods (see Section 6.2.3).
For distance metrics, we must not only estimate the positive part of the distance
field but also extrapolate negative distances behind the surface since the surface
is represented by the signed distance field’s zero-crossings. To limit the accuracy
impact of fusing imperfect positive and negative distances estimates (see Section
6.2.2), the updates are typically clamped to a small truncation band around the
surface boundary. However, distance-based methods remain prone to erasing geom-
etry. For example, when thin objects are observed from opposing sides, averaging
the observed positive and hallucinated negative distances makes the zero-crossings
flip around or disappear. Some works have analyzed the effect of the truncation
band and weight drop-offs on the quality of the final reconstruction [40]. Funda-
mentally, the problem can be reduced but not eliminated. Overall, the surfaces
estimated by TSDFs outperform occupancy methods along smooth surfaces at the
cost of lower recall on thin objects.
The distance information provided by TSDFs is inherently valuable. However,
instead of being conservative, TSDFs strictly overestimate the Euclidean distance.
To address this safety concern, voxblox [193] popularized incrementally building
ESDFs. Voxblox fuses the sensor data into a TSDF and then updates its ESDF
using a brushfire algorithm [144]. Subsequently, FIESTA [108] proposed a hybrid
approach that incrementally updates an ESDF map from an occupancy map in-
stead.
138 Dense Map Representations

6.4.4.2 Methods by Data Structure

The simplest data structure for volumetric mapping is a static 3D array. As shown
by KinectFusion [185], this data structure yields good results for small and fixed-
size scenes. However, many applications require the ability to dynamically expand
the map at runtime, while only allocating voxels where needed to save memory.
To address these concerns, Niessner proposed a voxel-block hashing scheme [186],
which groups the voxels into blocks (e.g., 8 × 8 × 8 voxels) that are stored in a hash-
map. This data structure was quickly adopted for TSDFs, providing constant-time
(O(1)) lookups and dynamic insertions. Of course, it can also be used to store
occupancy probabilities, as shown by FIESTA [108]. Compared to hashing voxels
individually, grouping them in blocks offers an adjustable trade-off between the
hash table’s size and the granularity at which voxels are allocated.
Naturally, voxels can also be stored using tree structures. Octomap [116] first
popularized using an octree to store occupancy probabilities and has been the de
facto standard for volumetric mapping for many years. A significant advantage of
using trees is that they inherently support multi-resolution, while a major limitation
is that encoding the tree’s structure introduces a significant memory overhead, and
that the cell lookup time is proportional to the tree’s height. Most recent approaches
address this limitation by leveraging hybrid data structures. Supereight [269], for
example, proposes to use a standard (dynamic) octree for the first levels and static
octrees for the last few levels. These static octrees can be seen as octrees stored using
a fixed-sized array. This removes the memory overhead of encoding parent-child
relationships with pointers, at the cost of reducing granularity since static octrees
are allocated as a block. The VDB [184] data structure was first introduced for the
visual effects (VFX) industry and subsequently used by several volumetric mapping
frameworks [167, 271]. As discussed in Section 6.3.3.4, it combines block-hashing
with trees to obtain the best of both worlds: good memory efficiency, hash-like
constant time lookups and insertions, and tree-like multi-resolution.
A practical consideration is that downstream tasks often demand storing ad-
ditional information, such as colors, semantics [102, 225] or an ESDF [193, 108]
alongside the occupancy probabilities or TSDF. Although virtually any data struc-
ture can be extended to support additional channels, the required implementation
effort scales with how complicated the underlying data structure is. This further
motivates using simple data structures (e.g. voxel-block hashing) or flexible, third-
party libraries.

6.4.4.3 Methods by Measurement Integration Algorithm

The algorithm used to update the map based on depth measurements is referred
to as the measurement integrator. It updates the estimated quantity for each ob-
served voxel by applying the measurement model. The two main approaches used
to integrate measurements are ray-tracing and projection-based methods.
 6.4 Constructing Maps: Methods and Practices 139

For each measured point, ray tracing integrators cast a ray from the sensor to the
point and update all the voxels intersected by the ray. An advantage of this approach
is that it is very general, and only requires that the position of the sensor’s origin is
known. However, voxels may be hit by multiple rays, especially if they are near the
sensor. This leads to duplicated efforts, and handling the resulting race conditions
in parallel implementations creates implementation and performance overheads.
In contrast, projection-based methods directly iterate over the observed voxels
and look up the ray(s) needed to compute their update by projecting each voxel into
sensor coordinates. Iterating over the map instead of the rays inherently avoids race
conditions. Projection-based methods are, therefore, prevalent in multi-threaded
and GPU-accelerated volumetric mapping frameworks. The predictable access pat-
tern resulting from directly iterating over the map also reduces memory bottlenecks.
Yet, a major disadvantage is the need for explicit knowledge of the sensor’s full pose
and projection model. This method is also harder to use with disorganized point
clouds, including the clouds obtained after applying LiDAR motion-undistortion.

6.4.4.4 Methods by Scalability

Memory and computational costs are two of the main bottlenecks in volumetric
mapping. For fixed-resolution methods, the memory and computational complexi-
ties grow linearly with the map’s total volume and cubically with the chosen resolu-
tion. Reducing these complexities is of significant research interest, as it is necessary
to create detailed maps that scale beyond small, restricted volumes.
Early works in volumetric mapping mainly focused on reducing memory usage.
For example, Octomap [116] proposes to use its octree’s inner nodes to store their
children’s max or average occupancy. By recursively pruning out leaf nodes whose
estimated quantities are close to their parent, constant areas in the map are au-
tomatically represented with fewer, lower resolution nodes. This adaptation to the
environment’s geometry is very effective in practice since environments predomi-
nantly consist of free space. Furthermore, storing min, max, or average values in
the octree’s inner nodes could be valuable for downstream tasks, as it enables map
queries at lower resolutions and the use of hierarchical algorithms for tasks such as
fast collision checking or exploration planning. Yet, a core limitation of Octomap
is that it integrates all measurements at the highest resolution, meaning that the
scaling of its computational complexity remains cubic.
Multi-resolution can also be leveraged to reduce the computational cost of mea-
surement updates. Given that measurement rays are emitted at fixed angles, re-
sulting in fewer rays hitting distant geometry, it seems logical to lower the update
resolution as the distance increases. This can be achieved through multi-resolution
ray-tracing [75] or multi-resolution projective integration [269]. Supereight2 [95] re-
duces the computational complexity further by adjusting the update resolution to
the entropy of the measurement updates. Such methods significantly enhance the
update performance, yet a remaining challenge is that the map’s different resolution
140 Dense Map Representations

levels still have to be synchronized explicitly. One way to eliminate this synchro-
nization requirement is to encode only the differences between each resolution level,
instead of storing absolute values in each octree node. This can formally be done by
applying wavelet decomposition. Wavelet-encoded maps can efficiently be queried
at any resolution at any time. Using this property, wavemap [220] reduces the com-
putational complexity even further by updating the map in a coarse-to-fine manner.
In addition to adjusting the update resolution to the measurement entropy, it also
skips uninformative updates, such as when the occupancy for an area in the map
has converged to being free, and all measurements agree.

6.4.5 GPs
As mentioned in Section 6.3.2.5, formulating the mapping problem as a regression
problem is desired to obtain a continuous representation. Moreover, if the aim is
to limit the number of assumptions about the environment, solving a non-linear
regression problem with non-parametric methods is ideal. GP [217] is a stochastic,
non-parametric, non-linear regression approach. It allows estimating the value of
an unknown function at an arbitrary query point given noisy and sparse measure-
ments at other points. We already learned in Chapter 3.2 how GP can be used for
continuous time trajectory representation. As will be apparent, GP are also an ap-
pealing solution for mapping continuous quantities, and they have been extensively
used in the robotics literature to model continuously spatial phenomena with depth
sensors [268, 190, 98, 134].
The information in GP models is contained in its mean m(x) and kernel functions
K(x, x′ ) and model the estimated continuous quantity as

f (x) ∼ GP (m(x), K(x, x′ )) . (6.8)

Let X = {xj ∈ RD } be a set of locations with measurements y, with yj =

f (xj ) + ϵj of the estimated quantity taken at the locations xj . For J number
of training pair (xj , yj ), we assume the noise ϵj to be i.i.d following Gaussian
ϵj ∼ N (0, σj2 ). Given a set of testing locations X∗ = {x∗n ∈ RD | n = 0, . . . , N },
we can express the joint distribution of the function values and the observed target
values as,
    
y K(X, X) + σj2 I K (X, X∗ )
= N 1, , (6.9)
f∗ K (X∗ , X) K (X∗ , X∗ )
 
where K = K(xi , xj ) ij . Thus the conditional distribution of (f∗ | X, y, X∗ ) ∼


N f ∗ , cov (f∗ ) , with the mean equation is given by,
 −1
f∗ = K (X∗ , X) K(X, X) + σj2 I y, (6.10)
 6.4 Constructing Maps: Methods and Practices 141

and the covariance equation is,

 −1
cov (f∗ ) = K (X∗ , X∗ ) − K (X∗ , X) K(X, X) + σj2 I K (X, X∗ ) . (6.11)

Here, (6.10) and (6.11) are the predictive equations for the estimated quantitative.
GPs have proven particularly powerful to represent spatially correlated data,
hence overcoming the traditional assumption of independence between cells, charac-
teristic of the occupancy grid method for mapping environments. Gaussian Process
Occupancy Map (GPOM)s [190] collects sensor observations and the corresponding
labels (free or occupied) as training data; the map cells comprise testing locations,
which are related to the training data as shown in (6.9). After the regression is
performed using (6.10) and (6.11), the cell’s probability of occupancy is obtained
by “squashing” regression outputs into occupancy probabilities using binary clas-
sification functions.
In its original formulation GPOM is a batch mapping technique with cubic com-
putational complexity (O(J 3 +J 2 N ) ). Approaches that aim to tacking this compu-
tational complexity especially for incremental GP map building have been proposed
following this work, for example [134, 135, 277, 99].
A key advantage of mapping with GP-based functions is that the estimated quan-
tity can be linearly operated [228] and still produce a GP as an output. Given that
derivatives and, therefore gradients, are linear operations, the differentiation out-
put of the estimated quantity is probabilistic. A continuous representation of the
uncertainty in the environment can be used to highlight unexplored regions and
optimize a robot’s search plan [99, 154]. The continuity property of the GP map
can improve the flexibility of a planner by inferring directly on collected sensor data
without being limited by the resolution of a grid/voxel cell.

6.4.5.1 Gaussian Process Implicit Surface

Implicit surfaces can also be represented by a GP. Gaussian process implicit surface
(GPIS) techniques [287, 171, 154, 118] use a GP approach to estimate a probabilistic
and continuous representation of the implicit surface given noisy measurements.
Furthermore, GPIS can be also used to estimate not only the surface but also the
distance field in a continuous manner [136, 244, 146, 288].
In the GPIS formulation, let us consider the distance field d to be estimated from
the distance to the nearest surface di given the the points on the surface and its
corresponding gradient ∇d computed through linear operators [228]. Then d with
∇d can be modelled by the joint GP with zero mean (given that at the surface the
distance is zero):
 
d
∼ GP(0, K(X, X′ )). (6.12)
∇d
GPIS approaches have the ability to estimate a continuous implicit surface and
the normal of the surface through the gradient, both with uncertainty. Some works
142 Dense Map Representations

have considered the use of parametric function priors to capture given shapes more
accurately [171, 118]. Other approaches aimed to estimate not only the implicit
surface but the full distance field. Given the nature of the vanilla version of the
GPIS formulation, the distance is well approximated near the measurements, i.e.,
on the surface, but falls back to the mean, which in this case is zero, faraway from
the surface. To estimate the full distance field in a continuous and probabilistic
formulation further away from the surface, works have considered applying a non-
linear operation to a GPIS-like formulation [288, 289, 145].
All these works have to deal with the computational complexity of the GP-
based formulation, but as an exchange, a continuous, generative, and probabilistic
representation of the environment, given only point clouds can be achieved.

6.4.6 Hilbert Maps

Hilbert Map (HM)s [216] are in many ways similar to GPOMs [190]. Both are con-
tinuous probabilistic models that do not discretize the space, unlike voxel-based
methods, and in contrast to point-based methods are capable of interpolating miss-
ing data. As stated, the major challenge in GPOM is high computational expense.
Thus the design goals of Hilbert maps were the following: (i) process data contin-
uously in an online manner, (ii) model dependence between observations, and (iii)
incorporate measurement uncertainty.
To achieve these goals, training a logistic regressor with stochastic gradient de-
scent in a projected feature space is often leveraged. The classifier and optimizer
combination enables online model updates using large amounts of data while the
feature projection permits representing intricate spatial details with such a simple
classifier.
The feature projection serves the same idea as the kernel in a GP, but instead
of a full covariance we use an approximation. There are many options for this, in-
cluding Nystroem [286], Random Fourier Features [215], and Sparse Kernel [176],
which is what we will be using. To goal of the sparse kernel is to limit the range
at which observations have an influence which improves convergence and compu-
tational efficiency. The outcome is a kernel that drops to exactly 0 at a specific
distance.
This kernel allows us to project points in 2D or 3D space into significantly higher
dimensions by placing inducing kernels at regular intervals over the space to be
mapped. Furthermore, this enables computing high-dimensional feature space rep-
resentations of input data to be trained the logistic regression classifier using mini-
batch stochastic gradient descent. Lastly, training is done by sampling free space
points along the range measurement, while adding the return as an obstacle point.
One challenge faced by HMs is the expressivity of the used kernel. A radial basis
function (RBF), as used in Figure 6.11, is a circle or a sphere and their values need
to be combined to reconstruct intricate details of the environment. Therefore there
 6.4 Constructing Maps: Methods and Practices 143

(a) (b) (c)

Figure 6.11 (a) The observations by a robot using a 2D LiDAR sensor, which is turned
into a dataset (b) Free (green) and occupied (red) points. These are then used to train a
Hilbert Map as seen in (c).

is a tradeoff in the form of number of kernels and their lengthscale affecting the
computational cost, reconstruction detail, and interpolation ability.

6.4.7 Deep Learning in Mapping

With the recent interest of the computer vision and robotics community in novel
view synthesis using neural radiance fields (NeRFs) [177], which provided com-
pelling results for image generation via a simple multi-layer perceptron (MLP),
several approaches investigated the usage of neural representations to estimate a
SDF. Learning to predict a SDF at arbitrary spatial location leads to a continuous
representation that can be turned into meshes at arbitrary resolutions, but also can
lead to more complete representation due to the interpolation capabilities of the
learned function.
Similar to implict representations, the representation is learned from input data
and approximated to provide a continuous function that can be queried at arbitary
locations. While often these neural representations are learned offline with given
poses, there has been recently also an interest in incremental approaches [248, 310]
and approaches that estimate poses on-the-fly using a neural representation [248,
227].
In particular, the approach of Sucar et al. [248] uses a neural network to predict
the SDF value of an arbitrary point in the scene based on RGB-D frames. Follow-
up approaches extended this approach by separating the spatial representation of
the features via voxel grids [206, 312], octrees [310], points [227, 71], etc. from the
neural representation. In these approaches, small but descriptive features are stored
in a spatial representation and used to determine with a small, neural network the
SDF value of an arbitrary point in the scene. This allows to decouple the learned
function from the spatial representation, which makes it possible to rely on small
144 Dense Map Representations

neural decoders to turn features into signed distance values, but also being effective
for large-scale scenes, such as outdoor environments.
The area of deep learning-based mapping, reconstruction, and SLAM is currently
rapidly evolving and integrating ideas from classical representation, such as surfel
splatting [130, 173], to achieve remarkable results in terms of reconstruction qual-
ity, but also capabilities. In particular, the ability to render novel views and gen-
erate new data at arbitary positions could be potentially exploited for robot learn-
ing without relying on simulated environments. For example, NeRF and Gaussian
splatting [314, 34] have gained considerable popularity, demonstrating significant
potential in various SLAM-related works. These will be further detailed in Chapter
16.

6.5 Usage Considerations

All map representations trade off distinctive, often complementary, strengths and
weaknesses. When choosing a map representation for a given application or robotic
system, it is therefore important to carefully consider how the map will be used in
all downstream tasks. Further factors to consider are the operating environment and
available sensors. We will start by discussing environmental factors, which motivate
several clear-cut choices, followed by more nuanced task-dependent considerations.
Finally, we conclude this chapter with a brief discussion on usage considerations
related to the existing methods presented in Section 6.4.

6.5.1 Environmental Aspects

Operating environments can be categorized as either structured or unstructured.
In tightly controlled spaces, such as automated factories, custom map representa-
tions – tailored to the robot’s task and specific objects it will encounter – typically
outperform general dense representations in terms of efficiency and accuracy. In
contrast, the dense representations covered in this chapter can model objects of
arbitrary shapes and work in any environment. When operating in changing or
partially unknown environments, it is often important for robots to be able to
distinguish observed free space from unobserved space. This information allows
path planners to avoid unsafe motions through unobserved space, which could be
occupied, and can also be used for exploration planning. Explicit surface repre-
sentations, including points, surfels, and meshes, generally cannot distinguish be-
tween free and unobserved space, while all occupancy-based methods do. Implicit
surface-based methods can also provide this distinction, though very often for more
reconstruction-focused applications, this information is discarded farther from the
surface to save computational and memory costs.
Another consideration is scalability. Explicit representations tend to be more
memory efficient than implicit representations, as they only describe the surface
 6.5 Usage Considerations 145

itself and their fidelity can easily be adapted to the detail required for each part
of the scene. When free-space information is required, multi-resolution approaches
can offer significant improvements over fixed-resolution voxelized representations in
terms of accuracy, memory, and their ability to capture very thin objects.
One final consideration is whether the environment has a significant amount of
dynamic objects and the degree to which these should be modeled. Most existing
mapping frameworks can be grouped into one of three categories of approaches.
The first set of approaches does not consider dynamics and directly fuses all mea-
surements into one of the representations introduced in this chapter. In practice,
this might already suffice when using implicit representations, since their free-space
updates typically do a good job at erasing leftovers of objects after they moved.
The second category of approaches tries to only integrate the environment’s static
elements into the map, by explicitly detecting and discarding all measurements cor-
responding to dynamic objects. This approach is particularly popular when using
explicit maps, where leftovers are more tedious to remove, and generally makes it
possible to generate clean maps even in highly dynamic spaces. The last set of solu-
tions not only represents the background but also the moving elements in the scene.
Note that this is commonly done using hybrid representations, mixing fundamental
geometric representations introduced in this chapter with bespoke representations
at the object level.

6.5.2 Downstream Task Types

In addition to the environment, it is equally important to consider what map infor-
mation is necessary for the robot’s required tasks. While any given operation can
typically be performed on all representations, the efficiency and implementation
complexity tend to vary greatly. The biggest difference lies in whether the opera-
tion is performed along the surface or in Cartesian space. As shown in Table 6.2,
implicit representations generally allow for simple, efficient filtering of properties
that are expressed in Cartesian coordinates, such as occupancy. In contrast, ex-
plicit representations are well suited to filter properties that are expressed along
the surface, such as visual textures. This explains why explicit representations are
generally more sensitive to the quality of the depth measurements, but can create
very detailed, visually appealing 3D reconstructions. On the other hand, implicit
methods are well suited for fusing noisy depth measurements, such as RGB-D cam-
era data.
In terms of queries, explicit representations make it possible to directly iterate
over the surface. This explains their popularity in rendering and graphics applica-
tions, and for tasks such as coverage path planning. However, they require addi-
tional steps, such as nearest neighbor lookups, to answer queries in Cartesian coor-
dinates. The exact opposite is true for implicit representations, which are therefore
commonly used for collision checking tasks.
146 Dense Map Representations

Efficient in
Operation Explicit representation Implicit representation
Filter measurements Along the surface In Cartesian space
(texture,...) (occupancy,...)
Query and iterate In surface coordinates In Cartesian coordinates
(coverage planning,...) (collision checking,...)
Modify surface Geometry Topology
(deformation,...) (merge, cut, simplify,...)

Table 6.2 Complementary strengths and weaknesses of explicit and implicit

surface representations.

Finally, explicit representations allow for efficient modifications of the surface’s

geometry, including deformations. In practice, maps are often constructed by in-
tegrating depth measurements using pose estimates from an imperfect, drifting
odometry system. Over time, the accumulated pose errors also lead to inconsisten-
cies in the dense map. Just like in SLAM systems, these errors can be eliminated by
deforming the dense map when detecting loop closures. Although both explicit and
implicit surfaces can be deformed, this operation is inherently simpler and more
efficient when using an explicit representation. In contrast, using an implicit repre-
sentation simplifies and improves the efficiency of operations affecting the surface’s
topology, or connectivity. Implicit representations are therefore often used to merge
surface estimates, combine or subtract object shapes, and simplify surfaces.
It is important to remember that different representations can also be used in
tandem to leverage their respective strengths. One good example of a hybrid ap-
proach is TSDF-based meshing (Section 6.4.3), where noisy depth measurements
are first conveniently filtered using an implicit surface representation (TSDF) which
is then converted to an explicit representation (mesh) using Marching Cubes. When
deciding whether the advantages of hybrid representations outweigh the overhead
they introduce, it is worth considering how the conversions can be limited to only
happen locally and infrequently.

6.5.3 Summary of Mapping Methods

We now conclude our discussion by summarizing the key differences between the
existing methods presented in this chapter. Starting with the explicit representa-
tions, using a collection of points to describe the surface is simple and requires the
fewest assumptions, but it is also the least informative. Beyond infinitesimal points,
surfels represent the surface’s properties over small neighborhoods, or patches. Fi-
nally, meshes explicitly represent the surface’s connectivity and allow its properties
to smoothly be interpolated. However, estimating the surface’s connectivity requires
the most assumptions and comes at a significant computational cost.
 6.5 Usage Considerations 147

In terms of implicit representations, a particular advantage of implicit surfaces

over occupancy maps is that they offer fast, high-quality distance information and
gradients which are beneficial for optimization-based planning. However, filtering
occupancy estimates requires less assumptions and, for voxel-based methods, oc-
cupancy maps are better at capturing thin obstacles. In cases with particularly
noisy or sparse depth measurements, non-voxelized implicit representations, based
on GPs and Hilbert Maps, provide particularly good uncertainty estimates. As they
explicitly consider the geometry’s spatial correlations, they are generally also better
at interpolating partially observed surfaces.
One rapidly advancing research area is that of learning-based methods. In terms
of learning-based implicit representations, NeRFs have been shown to enable promis-
ing new capabilities, particularly for semantic modeling and spatial reasoning. More
recently, Gaussian splatting [132] – an explicit learning-based representation bear-
ing similarities to surfels – lead to an increasing interest into approaches using
splatting [130, 173, 305]. Researchers are actively working on improving the com-
putational and memory footprint of these approaches, testing what new skills they
can enable, and exploring how they can be integrated into complete robotic systems.
Looking ahead, we suspect that learning-based methods can increase the generality
and expressiveness of dense representation, while improving their ability to handle
noisy measurements, incomplete observations and dynamic objects through learned
priors.
 7
Certifiably Optimal Solvers and Theoretical
Properties of SLAM
Author I, Author II, and Author III
 PART TWO
USE CASES / APPLICATIONS / STATE OF
PRACTICE
 8
Visual SLAM
Author I, Author II, and Author III

Aenean sem dolor, fermentum nec, gravida hendrerit, mattis eget, felis. Nullam
non diam vitae mi lacinia consectetuer. Fusce non massa eget quam luctus posuere.
Aenean vulputate velit. Quisque et dolor. Donec ipsum tortor, rutrum quis, mollis
eu, mollis a, pede. Donec nulla. Duis molestie. Duis lobortis commodo purus. Pel-
lentesque vel quam. Ut congue congue risus. Sed ligula. Aenean dictum pede vitae
felis. Donec sit amet nibh. Maecenas eu orci. Quisque gravida quam sed massa.
 9
LiDAR SLAM
Author I, Author II, and Author III
 10
Radar SLAM
Author I, Author II, and Author III

Aenean sem dolor, fermentum nec, gravida hendrerit, mattis eget, felis. Nullam
non diam vitae mi lacinia consectetuer. Fusce non massa eget quam luctus posuere.
Aenean vulputate velit. Quisque et dolor. Donec ipsum tortor, rutrum quis, mollis
eu, mollis a, pede. Donec nulla. Duis molestie. Duis lobortis commodo purus. Pel-
lentesque vel quam. Ut congue congue risus. Sed ligula. Aenean dictum pede vitae
felis. Donec sit amet nibh. Maecenas eu orci. Quisque gravida quam sed massa.
 11
Event-based SLAM
Author I, Author II, and Author III
 12
Inertial Odometry for SLAM
Author I, Author II, and Author III
 13
Leg Odometry for SLAM
Marco Camurri and Matı́as Mattamala
 PART THREE
FROM SLAM TO SPATIAL PERCEPTION AI
 14
Spatial AI
Author I, Author II, and Author III

Aenean sem dolor, fermentum nec, gravida hendrerit, mattis eget, felis. Nullam non
diam vitae mi lacinia consectetuer. Fusce non massa eget quam luctus posuere. Ae-
nean vulputate velit. Quisque et dolor. Donec ipsum tortor, rutrum quis, mollis eu,
mollis a, pede. Donec nulla. Duis molestie. Duis lobortis commodo purus. Pellentes-
que vel quam. Ut congue congue risus. Sed ligula. Aenean dictum pede vitae felis.
Donec sit amet nibh. Maecenas eu orci. Quisque gravida quam sed massa.[307]
 15
Boosting SLAM with Deep Learning
Author I, Author II, and Author III

Aenean sem dolor, fermentum nec, gravida hendrerit, mattis eget, felis. Nullam
non diam vitae mi lacinia consectetuer. Fusce non massa eget quam luctus posuere.
Aenean vulputate velit. Quisque et dolor. Donec ipsum tortor, rutrum quis, mollis
eu, mollis a, pede. Donec nulla. Duis molestie. Duis lobortis commodo purus. Pel-
lentesque vel quam. Ut congue congue risus. Sed ligula. Aenean dictum pede vitae
felis. Donec sit amet nibh. Maecenas eu orci. Quisque gravida quam sed massa.
 16
NeRF and GS
Author I, Author II, and Author III

Aenean sem dolor, fermentum nec, gravida hendrerit, mattis eget, felis. Nullam non
diam vitae mi lacinia consectetuer. Fusce non massa eget quam luctus posuere. Ae-
nean vulputate velit. Quisque et dolor. Donec ipsum tortor, rutrum quis, mollis eu,
mollis a, pede. Donec nulla. Duis molestie. Duis lobortis commodo purus. Pellentes-
que vel quam. Ut congue congue risus. Sed ligula. Aenean dictum pede vitae felis.
Donec sit amet nibh. Maecenas eu orci. Quisque gravida quam sed massa.[307]
 17
Dynamic and Deformable SLAM
Author I, Author II, and Author III

Aenean sem dolor, fermentum nec, gravida hendrerit, mattis eget, felis. Nullam
non diam vitae mi lacinia consectetuer. Fusce non massa eget quam luctus posuere.
Aenean vulputate velit. Quisque et dolor. Donec ipsum tortor, rutrum quis, mollis
eu, mollis a, pede. Donec nulla. Duis molestie. Duis lobortis commodo purus. Pel-
lentesque vel quam. Ut congue congue risus. Sed ligula. Aenean dictum pede vitae
felis. Donec sit amet nibh. Maecenas eu orci. Quisque gravida quam sed massa.
 18
Metric-Semantic SLAM
Author I, Author II, and Author III
 19
Foundation Models for SLAM
Author I, Author II, and Author III
Notes
 References

[1] FastTriggs. https://pypose.org/docs/main/generated/pypose.optim.

corrector.FastTriggs/.
[2] LieTensor. https://pypose.org/docs/main/generated/pypose.
LieTensor.
[3] PyPose Documents. https://pypose.org/docs/.
[4] PyPose Tutorial. https://github.com/pypose/tutorials.
[5] Abadi, Martı́n, Barham, Paul, Chen, Jianmin, Chen, Zhifeng, Davis, Andy,
Dean, Jeffrey, Devin, Matthieu, Ghemawat, Sanjay, Irving, Geoffrey, Isard,
Michael, et al. 2016. TensorFlow: a system for Large-Scale machine learning.
Pages 265–283 of: 12th USENIX symposium on operating systems design and
implementation (OSDI 16).
[6] Aftab, Khurrum, and Hartley, Richard. 2015. Convergence of iteratively re-
weighted least squares to robust m-estimators. Pages 480–487 of: 2015 IEEE
Winter Conference on Applications of Computer Vision. IEEE.
[7] Agarwal, P., Grisetti, G., Tipaldi, G. D., Spinello, L., Burgard, W., and Stach-
niss, C. 2014. Experimental Analysis of Dynamic Covariance Scaling for Ro-
bust Map Optimization Under Bad Initial Estimates. In: IEEE Intl. Conf.
on Robotics and Automation (ICRA).
[8] Agarwal, S., Snavely, N., Simon, I., Seitz, S. M., and Szeliski, R. 2009. Build-
ing Rome in a Day. In: Intl. Conf. on Computer Vision (ICCV).
[9] Agarwal, Sameer, Mierle, Keir, and Team, The Ceres Solver. 2022 (3). Ceres
Solver.
[10] Agrawal, A., Amos, B., Barratt, S., Boyd, S., Diamond, S., and Kolter, Z.
2019. Differentiable Convex Optimization Layers. In: Advances in Neural
Information Processing Systems.
[11] Al-Rfou, Rami, Alain, Guillaume, Almahairi, Amjad, Angermueller, Christof,
Bahdanau, Dzmitry, Ballas, Nicolas, Bastien, Frédéric, Bayer, Justin, Belikov,
Anatoly, Belopolsky, Alexander, et al. 2016. Theano: A Python framework for
fast computation of mathematical expressions. arXiv e-prints, arXiv–1605.
[12] Amestoy, P.R., Davis, T., and Duff, I.S. 1996. An approximate minimum de-
gree ordering algorithm. SIAM Journal on Matrix Analysis and Applications,
17(4), 886–905.
[13] Andersson, Joel AE, Gillis, Joris, Horn, Greg, Rawlings, James B, and Diehl,
Moritz. 2019. CasADi: a software framework for nonlinear optimization and
optimal control. Mathematical Programming Computation, 11(1), 1–36.
 References 167

[14] Ankenbauer, Jacqueline, Lusk, Parker C., and How, Jonathan P. 2023 (Mar.).
Global Localization in Unstructured Environments Using Semantic Object
Maps Built from Various Viewpoints.
[15] Antonante, P., Tzoumas, V., Yang, H., and Carlone, L. 2021. Outlier-Robust
Estimation: Hardness, Minimally Tuned Algorithms, and Applications. IEEE
Trans. Robotics, 38(1), 281–301. .
[16] Barath, Daniel, Noskova, Jana, Ivashechkin, Maksym, and Matas, Jiri. 2020.
MAGSAC++, a fast, reliable and accurate robust estimator. Pages 1304–1312
of: IEEE Conf. on Computer Vision and Pattern Recognition (CVPR).
[17] Barfoot, T D. 2024. State Estimation for Robotics. 2nd edn. Cambridge
University Press.
[18] Barfoot, T D, Forbes, J R, and D’Eleuterio, G M T. 2022. Vectorial Param-
eterizations of Pose. Robotica, 40(7), 2409–2427.
[19] Barfoot, Timothy D. 2017. State estimation for robotics. Cambridge Univer-
sity Press.
[20] Barrau, A., and Bonnabel, S. 2017. The Invariant Extended Kalman Filter
as a Stable Observer. IEEE Trans. on Automatic Control, 62(4), 1797–1812.
[21] Barron, Jonathan T. 2019. A general and adaptive robust loss function. Pages
4331–4339 of: Proceedings of the IEEE Conference on Computer Vision and
Pattern Recognition.
[22] Bauernfeind, Carl Max v. 1856. Elemente der Vermessungskunde: ein
Lehrbuch der praktischen Geometrie. Cotta.
[23] Bazin, J.C., Seo, Y., Hartley, R.I., and Pollefeys, M. 2014. Globally optimal
inlier set maximization with unknown rotation and focal length. Pages 803–
817 of: European Conf. on Computer Vision (ECCV).
[24] Behley, J., and Stachniss, C. 2018. Efficient Surfel-Based SLAM using 3D
Laser Range Data in Urban Environments. In: Robotics: Science and Systems
(RSS).
[25] Behley, Jens, Steinhage, Volker, and Cremers, Armin B. 2015. Efficient Radius
Neighbor Search in Three-dimensional Point Clouds. In: IEEE Intl. Conf. on
Robotics and Automation (ICRA).
[26] Bentley, J.L. 1975. Multidimensional Binary Search Trees Used for Associa-
tive Searching. Communications of the ACM, 18(9), 509–517.
[27] Bezdek, James C, and Hathaway, Richard J. 2003. Convergence of alternating
optimization. Neural, Parallel & Scientific Computations, 11(4), 351–368.
[28] Bhardwaj, Mohak, Boots, Byron, and Mukadam, Mustafa. 2020. Differen-
tiable gaussian process motion planning. Pages 10598–10604 of: 2020 IEEE
International Conference on Robotics and Automation (ICRA). IEEE.
[29] Biber, Peter, and Straßer, Wolfgang. 2003. The normal distributions trans-
form: A new approach to laser scan matching. Pages 2743–2748 of: IEEE/RSJ
Intl. Conf. on Intelligent Robots and Systems (IROS), vol. 3. IEEE.
[30] Bierman, G.J. 1977. Factorization methods for discrete sequential estimation.
Mathematics in Science and Engineering, vol. 128. New York: Academic
Press.
[31] Black, Michael J., and Rangarajan, Anand. 1996. On the unification of line
processes, outlier rejection, and robust statistics with applications in early
vision. Intl. J. of Computer Vision, 19(1), 57–91.
[32] Blake, Andrew, and Zisserman, Andrew. 1987. Visual reconstruction. MIT
Press.
168 References

[33] Bosse, M., Agamennoni, G., and Gilitschenski, I. 2016. Robust Estimation
and Applications in Robotics. Foundations and Trends in Robotics, 4(4),
225–269.
[34] Botsch, M., Spernat, M., and Kobbelt, L. 2004. Phong splatting. In: Sympo-
sium on Point-Based Graphics (PBG).
[35] Botsch, Mario, Hornung, Alexander, Zwicker, Matthias, and Kobbelt, Leif.
2005. High-Quality Surface Splatting on Today’s GPUs. In: Symposium on
Point-Based Graphics (PBG).
[36] Boumal, Nicolas. 2022 (Mar). An introduction to optimization on smooth
manifolds. To appear with Cambridge University Press.
[37] Bradbury, James, Frostig, Roy, Hawkins, Peter, Johnson, Matthew James,
Leary, Chris, Maclaurin, Dougal, Necula, George, Paszke, Adam, VanderPlas,
Jake, Wanderman-Milne, Skye, and Zhang, Qiao. 2018. JAX: composable
transformations of Python+NumPy programs.
[38] Bustos, Á. P., and Chin, T. J. 2018. Guaranteed outlier removal for point
cloud registration with correspondences. 40(12), 2868–2882.
[39] Bustos, Alvaro Parra, Chin, Tat-Jun, Neumann, Frank, Friedrich, Tobias, and
Katzmann, Maximilian. 2019. A Practical Maximum Clique Algorithm for
Matching with Pairwise Constraints. arXiv preprint arXiv:1902.01534.
[40] Bylow, Erik, Sturm, Jürgen, Kerl, Christian, Kahl, Fredrik, and Cremers,
Daniel. 2013. Real-time camera tracking and 3D reconstruction using signed
distance functions. Page 2 of: Robotics: Science and Systems (RSS), vol. 2.
[41] Cadena, C., Carlone, L., Carrillo, H., Latif, Y., Scaramuzza, D., Neira, J.,
Reid, I., and Leonard, J. J. 2016. Past, Present, and Future of Simultaneous
Localization and Mapping: Toward the Robust-Perception Age. IEEE Trans.
Robotics, 32(6), 1309–1332.
[42] Carlone, L. 2023. Estimation Contracts for Outlier-Robust Geometric Percep-
tion. Foundations and Trends (FnT) in Robotics, arXiv preprint: 2208.10521.
.
[43] Carlone, L., and Calafiore, G. 2018. Convex Relaxations for Pose Graph
Optimization with Outliers. IEEE Robotics and Automation Letters, 3(2),
1160–1167. arxiv preprint: 1801.02112, .
[44] Carr, J. C., Beatson, R. K., Cherrie, J. B., Mitchell, T. J., Fright, W. R.,
McCallum, B. C., and Evans, T. R. 2001. Reconstruction and representation
of 3D objects with radial basis functions. Pages 67–76 of: Intl. Conf. on
Computer Graphics and Interactive Techniques (SIGGRAPH). Association
for Computing Machinery.
[45] Cazals, Frédéric, and Giesen, Joachim. 2006. Delaunay Triangulation Based
Surface Reconstruction. In: Effective Computational Geometry for Curves
and Surfaces.
[46] Chadwick, Jeffrey N, and Bindel, David S. 2015. An efficient solver for
sparse linear systems based on rank-structured Cholesky factorization. arXiv
preprint arXiv:1507.05593.
[47] Chang, Y., Ebadi, K., Denniston, C., Ginting, M. Fadhil, Rosinol, A., Reinke,
A., Palieri, M., Shi, J., A, Chatterjee, Morrell, B., Agha-mohammadi, A.,
and Carlone, L. 2022. LAMP 2.0: A Robust Multi-Robot SLAM System
for Operation in Challenging Large-Scale Underground Environments. IEEE
Robotics and Automation Letters, 7(4), 9175–9182. .
 References 169

[48] Chatila, R., and Laumond, J.-P. 1985. Position referencing and consistent
world modeling for mobile robots. Pages 138–145 of: IEEE Intl. Conf. on
Robotics and Automation (ICRA).
[49] Chatterjee, A., and Govindu, V. M. 2013. Efficient and Robust Large-Scale
Rotation Averaging. Pages 521–528 of: Intl. Conf. on Computer Vision
(ICCV).
[50] Chebrolu, Nived, Läbe, Thomas, Vysotska, Olga, Behley, Jens, and Stach-
niss, Cyrill. 2020. Adaptive Robust Kernels for Non-Linear Least Squares
Problems. arXiv preprint arXiv:2004.14938.
[51] Chen, Hansheng, Wang, Pichao, Wang, Fan, Tian, Wei, Xiong, Lu, and Li,
Hao. 2022. EPro-PnP: Generalized End-to-End Probabilistic Perspective-n-
Points for Monocular Object Pose Estimation. Pages 2781–2790 of: Proceed-
ings of the IEEE/CVF Conference on Computer Vision and Pattern Recog-
nition.
[52] Chen, Tianqi, Li, Mu, Li, Yutian, Lin, Min, Wang, Naiyan, Wang, Minjie,
Xiao, Tianjun, Xu, Bing, Zhang, Chiyuan, and Zhang, Zheng. 2015. Mxnet:
A flexible and efficient machine learning library for heterogeneous distributed
systems. arXiv preprint arXiv:1512.01274.
[53] Chen, Xiangyu, Yang, Fan, and Wang, Chen. 2024. iA*: Imperative Learning-
based A* Search for Pathfinding. arXiv preprint arXiv:2403.15870.
[54] Chen, Y., Davis, T.A., Hager, W.W., and Rajamanickam, S. 2008. Algorithm
887: CHOLMOD, Supernodal Sparse Cholesky Factorization and Update/-
Downdate. ACM Trans. Math. Softw., 35(3), 22:1–22:14.
[55] Chin, T., Kee, Y. H., Eriksson, A., and Neumann, F. 2016 (June). Guaranteed
Outlier Removal with Mixed Integer Linear Programs. Pages 5858–5866 of:
IEEE Conf. on Computer Vision and Pattern Recognition (CVPR).
[56] Chum, O., and Matas, J. 2005. Matching with PROSAC - Progressive Sample
Consensus. In: IEEE Conf. on Computer Vision and Pattern Recognition
(CVPR).
[57] Chum, Ondřej, Matas, Jiřı́, and Kittler, Josef. 2003. Locally optimized
RANSAC. Pages 236–243 of: Joint Pattern Recognition Symposium. Springer.
[58] Clark, J.H. 1976. Hierarchical geometric models for visible surface algorithms.
Communications of the ACM, 19(1), 547–554.
[59] Collobert, Ronan, Bengio, Samy, and Mariéthoz, Johnny. 2002. Torch: a
modular machine learning software library. Tech. rept. Idiap.
[60] Crowley, J.L. 1989. World modeling and position estimation for a mobile
robot using ultra-sonic ranging. Pages 674–680 of: IEEE Intl. Conf. on
Robotics and Automation (ICRA).
[61] Curless, Brian, and Levoy, Marc. 1996. A volumetric method for building
complex models from range images. Pages 303–312 of: Intl. Conf. on Com-
puter Graphics and Interactive Techniques (SIGGRAPH).
[62] Czarnowski, Jan, Laidlow, Tristan, Clark, Ronald, and Davison, Andrew J.
2020. Deepfactors: Real-time probabilistic dense monocular slam. IEEE
Robotics and Automation Letters, 5(2), 721–728.
[63] Davis, T.A. 2011. Algorithm 915: SuiteSparseQR, A multifrontal multi-
threaded sparse QR factorization package. ACM Trans. Math. Softw., 38(1),
8:1–8:22.
[64] Davis, T.A., Gilbert, J.R., Larimore, S.I., and Ng, E.G. 2004. A column
170 References

approximate minimum degree ordering algorithm. ACM Trans. Math. Softw.,

30(3), 353–376.
[65] Dellaert, F. 2005. Square Root SAM: Simultaneous Location and Mapping
via Square Root Information Smoothing. In: Robotics: Science and Systems
(RSS).
[66] Dellaert, Frank. 2012. Factor graphs and GTSAM: A hands-on introduction.
Tech. rept.
[67] Dellaert, Frank, and Contributors, GTSAM. 2022 (May). borglab/gtsam.
[68] Dellaert, Frank, and Kaess, Michael. 2006. Square Root SAM: Simultane-
ous localization and mapping via square root information smoothing. The
International Journal of Robotics Research, 25(12), 1181–1203.
[69] Dellaert, Frank, Kaess, Michael, et al. 2017. Factor graphs for robot percep-
tion. Foundations and Trends® in Robotics, 6(1-2), 1–139.
[70] Dellenbach, P., Deschaud, J., Jacquet, B., and Goulette, F. 2022. CT-ICP
Real-Time Elastic LiDAR Odometry with Loop Closure. In: IEEE Intl. Conf.
on Robotics and Automation (ICRA).
[71] Deng, Junyuan, Chen, Xieyuanli, Xia, Songpengcheng, Sun, Zhen, Liu, Guo-
qing, Yu, Wenxian, and Pei, Ling. 2023. NeRF-LOAM: Neural Implicit Rep-
resentation for Large-Scale Incremental LiDAR Odometry and Mapping. In:
Intl. Conf. on Computer Vision (ICCV).
[72] Dennis, J.E., and Schnabel, R.B. 1983. Numerical methods for unconstrained
optimization and nonlinear equations. Prentice-Hall.
[73] DeTone, Daniel, Malisiewicz, Tomasz, and Rabinovich, Andrew. 2018.
SuperPoint: Self-Supervised Interest Point Detection and Description.
arXiv:1712.07629 [cs], Apr.
[74] Droeschel, D., and Behnke, S. 2018. Efficient Continuous-Time SLAM for 3D
Lidar-Based Online Mapping. In: IEEE Intl. Conf. on Robotics and Automa-
tion (ICRA).
[75] Duberg, D., and Jensfelt, P. 2020. UFOMap: An Efficient Probabilistic 3D
Mapping Framework That Embraces the Unknown. IEEE Robotics and Au-
tomation Letters, 5(4), 6411–6418.
[76] Duckett, T., Marsland, S., and Shapiro, J. 2002. Fast, On-line Learning of
Globally Consistent Maps. Autonomous Robots, 12(3), 287–300.
[77] Duff, I. S., and Reid, J. K. 1983. The Multifrontal Solution of Indefinite
Sparse Symmetric Linear Systems. ACM Trans. Math. Softw., 9(3), 302–325.
[78] Dunning, Iain, Huchette, Joey, and Lubin, Miles. 2017. JuMP: A modeling
language for mathematical optimization. SIAM review, 59(2), 295–320.
[79] Durrant-Whyte, H.F. 1988. Uncertain geometry in robotics. IEEE Trans.
Robot. Automat., 4(1), 23–31.
[80] Durrant-Whyte, H.F., Rye, D., and Nebot, E. 1996. Localisation of automatic
guided vehicles. Pages 613–625 of: Giralt, G., and Hirzinger, G. (eds), Robotics
Research: The 7th International Symposium (ISRR 95). Springer-Verlag.
[81] Ebadi, K., Chang, Y., Palieri, M., Stephens, A., Hatteland, A., Heiden,
E., Thakur, A., Morrell, B., Carlone, L., and Aghamohammadi, A. 2020.
LAMP: Large-Scale Autonomous Mapping and Positioning for Exploration
of Perceptually-Degraded Subterranean Environments. In: IEEE Intl. Conf.
on Robotics and Automation (ICRA).
 References 171

[82] Ebadi, Kamak, Bernreiter, Lukas, Biggie, Harel, Catt, Gavin, Chang, Yun,
Chatterjee, Arghya, Denniston, Christopher E., Deschênes, Simon-Pierre,
Harlow, Kyle, Khattak, Shehryar, Nogueira, Lucas, Palieri, Matteo, Petráček,
Pavel, Petrlı́k, Matěj, Reinke, Andrzej, Krátký, Vı́t, Zhao, Shibo, Agha-
mohammadi, Ali-akbar, Alexis, Kostas, Heckman, Christoffer, Khosoussi,
Kasra, Kottege, Navinda, Morrell, Benjamin, Hutter, Marco, Pauling, Fred,
Pomerleau, François, Saska, Martin, Scherer, Sebastian, Siegwart, Roland,
Williams, Jason L., and Carlone, Luca. 2024. Present and Future of SLAM in
Extreme Environments: The DARPA SubT Challenge. IEEE Trans. Robotics,
40, 936–959.
[83] Elfes, A. 1989. Using occupancy grids for mobile robot perception and navi-
gation. Computer, 22(6), 46–57.
[84] Elseberg, J., Borrmann, D., and Nüchter, A. 2013. One billion points in
the cloud – an octree for efficient processing of 3D laser scans. ISPRS J. of
Photogrammetry and Remote Sensing (JPRS), 76, 76–88.
[85] Enqvist, O., Josephson, K., and Kahl, F. 2009. Optimal correspondences from
pairwise constraints. Pages 1295–1302 of: Intl. Conf. on Computer Vision
(ICCV).
[86] Eustice, R., Singh, H., and Leonard, J. 2005a (April). Exactly Sparse Delayed-
State Filters. Pages 2417–2424 of: IEEE Intl. Conf. on Robotics and Automa-
tion (ICRA).
[87] Eustice, R., Walter, M., and Leonard, J. 2005b (Aug). Sparse Extended Infor-
mation Filters: Insights into Sparsification. Pages 3281–3288 of: IEEE/RSJ
Intl. Conf. on Intelligent Robots and Systems (IROS).
[88] Feige, Uriel, Goldwasser, Shafi, Lovász, László, Safra, Shmuel, and Szegedy,
Mario. 1991 (Sept.). Approximating clique is almost NP-complete. Pages
2–12 of: Symp. on Foundations of Computer Science.
[89] Fischler, M., and Bolles, R. 1981. Random sample consensus: a paradigm for
model fitting with application to image analysis and automated cartography.
Commun. ACM, 24, 381–395.
[90] Fornasier, A., van Goor, P., Allak, E., Mahony, R., and Weiss, S. 2024.
MSCEqF: A Multi State Constraint Equivariant Filter for Vision-Aided In-
ertial Navigation. IEEE Robotics and Automation Letters, 9(1), 731–738.
[91] Forsgren, Brendon, Kaess, Michael, Vasudevan, Ram, McLain, Timothy W.,
and Mangelson, Joshua G. 2024. Group-k consistent measurement set max-
imization via maximum clique over k-uniform hypergraphs for robust multi-
robot map merging. Intl. J. of Robotics Research.
[92] Frese, U. 2005. Treemap: An O(log n) Algorithm for Simultaneous Localiza-
tion and Mapping. Pages 455–476 of: Spatial Cognition IV. Springer Verlag.
[93] Frese, U., Larsson, P., and Duckett, T. 2005. A Multilevel Relaxation Al-
gorithm for Simultaneous Localisation and Mapping. IEEE Trans. Robotics,
21(2), 196–207.
[94] Fu, Taimeng, Su, Shaoshu, Lu, Yiren, and Wang, Chen. 2024. iSLAM: Im-
perative SLAM. IEEE Robotics and Automation Letters (RA-L).
[95] Funk, Nils, Tarrio, Juan, Papatheodorou, Sotiris, Popović, Marija, Alcantar-
illa, Pablo F., and Leutenegger, Stefan. 2021. Multi-Resolution 3D Mapping
With Explicit Free Space Representation for Fast and Accurate Mobile Robot
Motion Planning. IEEE Robotics and Automation Letters, 6(2), 3553–3560.
172 References

[96] Geiger, A., Lenz, P., and Urtasun, R. 2012 (June). Are we ready for Au-
tonomous Driving? The KITTI Vision Benchmark Suite. Pages 3354–3361
of: IEEE Conf. on Computer Vision and Pattern Recognition (CVPR).
[97] Geneva, Patrick, Eckenhoff, Kevin, Yang, Yulin, and Huang, Guoquan. 2018.
LIPS: LiDAR-Inertial 3D Plane SLAM. Pages 123–130 of: IEEE/RSJ Intl.
Conf. on Intelligent Robots and Systems (IROS).
[98] Gentil, Cédric Le, Vayugundla, Mallikarjuna, Giubilato, Riccardo, Sturzl,
Wolfgang, Vidal-Calleja, Teresa, and Triebel, Rudolph. 2020. Gaussian Pro-
cess Gradient Maps for Loop-Closure Detection in Unstructured Planetary
Environments. Pages 1895–1902 of: IEEE/RSJ Intl. Conf. on Intelligent
Robots and Systems (IROS).
[99] Ghaffari Jadidi, Maani, Valls Miro, Jaime, and Dissanayake, Gamini. 2018.
Gaussian processes autonomous mapping and exploration for range-sensing
mobile robots. Autonomous Robots, 42(2), 273–290.
[100] Giftthaler, Markus, Neunert, Michael, Stäuble, Markus, and Buchli, Jonas.
2018. The control toolbox-an open-source c++ library for robotics, opti-
mal and model predictive control. Pages 123–129 of: 2018 IEEE Inter-
national Conference on Simulation, Modeling, and Programming for Au-
tonomous Robots (SIMPAR). IEEE.
[101] Golub, G.H., and Loan, C.F. Van. 1996. Matrix Computations. Third edn.
Baltimore: Johns Hopkins University Press.
[102] Grinvald, Margarita, Furrer, Fadri, Novkovic, Tonci, Chung, Jen Jen, Cadena,
Cesar, Siegwart, Roland, and Nieto, Juan. 2019. Volumetric instance-aware
semantic mapping and 3D object discovery. IEEE Robotics and Automation
Letters, 4(3), 3037–3044.
[103] Grisetti, G., Stachniss, C., and Burgard, W. 2007. Improved Techniques for
Grid Mapping With Rao-Blackwellized particle filters. IEEE Trans. Robotics,
23(1), 34–46.
[104] Grisetti, Giorgio, Kümmerle, Rainer, Strasdat, Hauke, and Konolige, Kurt.
2011. g2o: A general framework for (hyper) graph optimization. Pages 9–
13 of: Proceedings of the IEEE International Conference on Robotics and
Automation (ICRA).
[105] Guennebaud, Gaël, Jacob, Benoit, et al. 2010. Eigen. URl: http://eigen.
tuxfamily. org, 3.
[106] Guo, Yifan, Ren, Zhongqiang, and Wang, Chen. 2024. iMTSP: Solving
Min-Max Multiple Traveling Salesman Problem with Imperative Learning.
In: IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS).
[107] Gutmann, J.-S., and Konolige, K. 2000 (November). Incremental Mapping of
Large Cyclic Environments. Pages 318–325 of: IEEE Intl. Symp. on Compu-
tational Intelligence in Robotics and Automation (CIRA).
[108] Han, Luxin, Gao, Fei, Zhou, Boyu, and Shen, Shaojie. 2019. Fiesta: Fast in-
cremental euclidean distance fields for online motion planning of aerial robots.
Pages 4423–4430 of: IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems
(IROS).
[109] Handa, Ankur, Bloesch, Michael, Pătrăucean, Viorica, Stent, Simon, McCor-
mac, John, and Davison, Andrew. 2016. gvnn: Neural network library for
geometric computer vision. Pages 67–82 of: Computer Vision–ECCV 2016
 References 173

Workshops: Amsterdam, The Netherlands, October 8-10 and 15-16, 2016,

Proceedings, Part III 14. Springer.
[110] Hart, William E, Laird, Carl D, Watson, Jean-Paul, Woodruff, David L,
Hackebeil, Gabriel A, Nicholson, Bethany L, Siirola, John D, et al. 2017.
Pyomo-optimization modeling in python. Vol. 67. Springer.
[111] Hartley, R., and Zisserman, A. 2000. Multiple View Geometry in Computer
Vision. Cambridge University Press.
[112] Hartley, R.I., and Kahl, F. 2009. Global optimization through rotation space
search. Intl. J. of Computer Vision, 82(1), 64–79.
[113] Hestenes, Magnus R, and Stiefel, Eduard. 1952. Methods of conjugate gra-
dients for solving. Journal of research of the National Bureau of Standards,
49(6), 409.
[114] Hoppe, Hugues, DeRose, Tony, Duchamp, Tom, McDonald, John, and Stuet-
zle, Werner. 1992. Surface reconstruction from unorganized points. Pages
71–78 of: Intl. Conf. on Computer Graphics and Interactive Techniques (SIG-
GRAPH). Association for Computing Machinery.
[115] Horn, Berthold K. P. 1987. Closed-form solution of absolute orientation using
unit quaternions. J. Opt. Soc. Am. A, 4(4), 629–642.
[116] Hornung, Armin, Wurm, Kai M., Bennewitz, Maren, Stachniss, Cyrill, and
Burgard, Wolfram. 2013. OctoMap: an efficient probabilistic 3D mapping
framework based on octrees. Autonomous Robots, 189–206.
[117] Huber, P. 1981. Robust Statistics. John Wiley & Sons, New York, NY.
[118] Ivan, Jean-Paul A, Stoyanov, Todor, and Stork, Johannes A. 2022. Online
Distance Field Priors for Gaussian Process Implicit Surfaces. IEEE Robotics
and Automation Letters, 7(4), 8996–9003.
[119] Izadi, Shahram, Kim, David, Hilliges, Otmar, Molyneaux, David, Newcombe,
Richard, Kohli, Pushmeet, Shotton, Jamie, Hodges, Steve, Freeman, Dustin,
Davison, Andrew, et al. 2011. KinectFusion: real-time 3D reconstruction and
interaction using a moving depth camera. Pages 559–568 of: ACM Symp. on
User interface software and technology.
[120] Izatt, G., Dai, H., and Tedrake, R. 2017. Globally Optimal Object Pose
Estimation in Point Clouds with Mixed-Integer Programming. In: Intl. Symp.
of Robotics Research (ISRR).
[121] Jatavallabhula, Krishna Murthy, Iyer, Ganesh, and Paull, Liam. 2020. ∇
slam: Dense slam meets automatic differentiation. Pages 2130–2137 of: 2020
IEEE International Conference on Robotics and Automation (ICRA). IEEE.
[122] Ji, Kaiyi, Yang, Junjie, and Liang, Yingbin. 2021. Bilevel optimization: Con-
vergence analysis and enhanced design. Pages 4882–4892 of: International
conference on machine learning. PMLR.
[123] Jia, Yangqing, Shelhamer, Evan, Donahue, Jeff, Karayev, Sergey, Long,
Jonathan, Girshick, Ross, Guadarrama, Sergio, and Darrell, Trevor. 2014.
Caffe: Convolutional architecture for fast feature embedding. Pages 675–678
of: 22nd ACM international conference on Multimedia.
[124] Johnson, J, Mangelson, J, Barfoot, T D, and Beard, R. 2024. Continuous-
time Trajectory Estimation: A Comparative Study Between Gaussian Process
and Spline-based Approaches. (arXiv:2402.00399 [cs.RO]).
[125] Kaess, M., Ranganathan, A., and Dellaert, F. 2008. iSAM: Incremental
Smoothing and Mapping. IEEE Trans. Robotics, 24(6), 1365–1378.
174 References

[126] Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J., and Dellaert,
F. 2011 (May). iSAM2: Incremental Smoothing and Mapping with Fluid
Relinearization and Incremental Variable Reordering. In: IEEE Intl. Conf.
on Robotics and Automation (ICRA).
[127] Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J., and Dellaert, F.
2012. iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree.
Intl. J. of Robotics Research, 31(Feb), 217–236.
[128] Kaess, Michael. 2015. Simultaneous localization and mapping with infinite
planes. Pages 4605–4611 of: IEEE Intl. Conf. on Robotics and Automation
(ICRA).
[129] Kazhdan, Michael, Bolitho, Matthew, and Hoppe, Hugues. 2006. Poisson
surface reconstruction. Pages 61–70 of: Proceedings of the fourth Eurographics
symposium on Geometry processing. Eurographics Association.
[130] Keetha, Nikhil, Karhade, Jay, Jatavallabhula, Krishna Murthy, Yang, Geng-
shan, Scherer, Sebastian, Ramanan, Deva, and Luiten, Jonathon. 2023.
SplaTAM: Splat, Track & Map 3D Gaussians for Dense RGB-D SLAM. arXiv
preprint.
[131] Keller, M., Lefloch, D., Lambers, M., and Izadi, S. 2013. Real-time 3D Re-
construction in Dynamic Scenes using Point-based Fusion. In: Intl. Conf. on
3D Vision (3DV).
[132] Kerbl, Bernhard, Kopanas, Georgios, Leimkühler, Thomas, and Drettakis,
George. 2023. 3D Gaussian Splatting for Real-Time Radiance Field Render-
ing. arXiv preprint.
[133] Kerl, Christian, Sturm, Jürgen, and Cremers, Daniel. 2013. Robust odometry
estimation for RGB-D cameras. Pages 3748–3754 of: 2013 IEEE international
conference on robotics and automation. IEEE.
[134] Kim, S., and Kim, J. 2012. Building occupancy maps with a mixture of
Gaussian processes. Pages 4756–4761 of: IEEE Intl. Conf. on Robotics and
Automation (ICRA).
[135] Kim, S., and Kim, J. 2013. Occupancy Mapping and Surface Reconstruction
Using Local Gaussian Processes With Kinect Sensors. Pages 1335–1346 of:
IEEE Trans. on Cybernetics.
[136] Kim, Soohwan, and Kim, Jonghyuk. 2015. GPmap: A Unified Framework for
Robotic Mapping Based on Sparse Gaussian Processes. Springer International
Publishing. Pages 319–332.
[137] Kingma, Diederik P, and Ba, Jimmy. 2014. Adam: A method for stochastic
optimization. arXiv preprint arXiv:1412.6980.
[138] Koenemann, Jonas, Licitra, Giovanni, Alp, Mustafa, and Diehl, Moritz. 2017.
Openocl–open optimal control library.
[139] Koller, D., and Friedman, N. 2009. Probabilistic Graphical Models: Principles
and Techniques. The MIT Press.
[140] Konolige, K. 2004. Large-scale map-making. In: Proc. 21th AAAI National
Conference on AI.
[141] Kukko, Antero, Kaijaluoto, Risto, Kaartinen, Harri, Lehtola, Ville V,
Jaakkola, Anttoni, and Hyyppä, Juha. 2017. Graph SLAM correction for
single scanner MLS forest data under boreal forest canopy. ISPRS Journal
of Photogrammetry and Remote Sensing, 132, 199–209.
[142] Lajoie, P., Hu, S., Beltrame, G., and Carlone, L. 2019. Modeling Perceptual
 References 175

Aliasing in SLAM via Discrete-Continuous Graphical Models. IEEE Robotics

and Automation Letters. extended ArXiv version: , Supplemental Material: .
[143] Latif, Y., Lerma, C. D. C., and Neira, J. 2012. Robust Loop Closing Over
Time. In: Robotics: Science and Systems (RSS).
[144] Lau, Boris, Sprunk, Christoph, and Burgard, Wolfram. 2013. Efficient grid-
based spatial representations for robot navigation in dynamic environments.
J. on Robotics and Autonomous Systems (RAS), 61(10), 1116–1130.
[145] Le Gentil, Cedric, Ouabi, Othmane-Latif, Wu, Lan, Pradalier, Cedric, and
Vidal-Calleja, Teresa. 2023. Accurate Gaussian-Process-based Distance Fields
with Applications to Echolocation and Mapping. IEEE Robotics and Automa-
tion Letters, 1–8.
[146] Lee, Bhoram, Zhang, Clark, Huang, Zonghao, and Lee, Daniel D. 2019. Online
continuous mapping using gaussian process implicit surfaces. In: IEEE Intl.
Conf. on Robotics and Automation (ICRA).
[147] Lee, G. H., Fraundorfer, F., and Pollefeys, M. 2013. Robust pose-graph loop-
closures with expectation-maximization. In: IEEE/RSJ Intl. Conf. on Intel-
ligent Robots and Systems (IROS).
[148] Leordeanu, Marius, and Hebert, Martial. 2005. A spectral technique for cor-
respondence problems using pairwise constraints. Pages 1482–1489 of: Intl.
Conf. on Computer Vision (ICCV), vol. 2. IEEE.
[149] Levenberg, K. 1944. A Method for the Solution of Certain Nonlinear Problems
in Least Squares. Quart. Appl. Math, 2(2), 164–168.
[150] Li, H. 2009. Consensus set maximization with guaranteed global optimality
for robust geometry estimation. Pages 1074–1080 of: Intl. Conf. on Computer
Vision (ICCV).
[151] Li, Ruihao, Wang, Sen, Long, Zhiqiang, and Gu, Dongbing. 2018. Undeepvo:
Monocular visual odometry through unsupervised deep learning. Pages 7286–
7291 of: 2018 IEEE international conference on robotics and automation
(ICRA). IEEE.
[152] Lin, J., and Zhang, F. 2019. Loam livox A Robust LiDAR Odemetry and
Mapping LOAM Package for Livox LiDAR. In: IEEE/RSJ Intl. Conf. on
Intelligent Robots and Systems (IROS).
[153] Liu, Hanxiao, Simonyan, Karen, and Yang, Yiming. 2019. Darts: Differen-
tiable architecture search. In: International Conference on Learning Repre-
sentations (ICLR).
[154] Liu, Liyang, Fryc, Simon, Wu, Lan, Vu, Thanh Long, Paul, Gavin, and Vidal-
Calleja, Teresa. 2021a. Active and interactive mapping with dynamic Gaus-
sian process implicit surfaces for mobile manipulators. IEEE Robotics and
Automation Letters, 6(2), 3679–3686.
[155] Liu, Risheng, Gao, Jiaxin, Zhang, Jin, Meng, Deyu, and Lin, Zhouchen.
2021b. Investigating bi-level optimization for learning and vision from a
unified perspective: A survey and beyond. IEEE Transactions on Pattern
Analysis and Machine Intelligence, 44(12), 10045–10067.
[156] Loop, C., Cai, Q., Orts-Escolano, S., and Chou, P. A. 2016. A Closed-Form
Bayesian Fusion Equation Using Occupancy Probabilities. Pages 380–388 of:
Intl. Conf. on 3D Vision (3DV).
[157] Lorensen, William E, and Cline, Harvey E. 1987. Marching cubes: A high res-
olution 3D surface construction algorithm. Intl. Conf. on Computer Graphics
and Interactive Techniques (SIGGRAPH), 21(4), 163–169.
176 References

[158] Lourakis, Manolis LA, and Argyros, Antonis A. 2005a. Is Levenberg-

Marquardt the most efficient optimization algorithm for implementing bundle
adjustment? Pages 1526–1531 of: Tenth IEEE International Conference on
Computer Vision (ICCV’05) Volume 1, vol. 2. IEEE.
[159] Lourakis, MLA, and Argyros, Antonis A. 2005b. Is Levenberg-Marquardt the
most efficient optimization algorithm for implementing bundle adjustment?
Pages 1526–1531 of: Intl. Conf. on Computer Vision (ICCV), vol. 2. IEEE.
[160] Lu, F., and Milios, E. 1997a. Globally consistent range scan alignment for
environment mapping. Autonomous Robots, Apr, 333–349.
[161] Lu, F., and Milios, E. 1997b. Robot pose estimation in unknown environments
by matching 2D range scans. J. of Intelligent and Robotic Systems, April,
249:275.
[162] Lu, Feng, and Milios, Evangelos. 1997c. Globally consistent range scan align-
ment for environment mapping. Autonomous Robots, 4, 333–349.
[163] Lusk, Parker C., Fathian, Kaveh, and How, Jonathan P. 2021a (May). CLIP-
PER: A Graph-Theoretic Framework for Robust Data Association. Pages
13828–13834 of: IEEE Intl. Conf. on Robotics and Automation (ICRA).
[164] Lusk, Parker C., Roy, Ronak, Fathian, Kaveh, and How, Jonathan P. 2021b
(Nov.). MIXER: A Principled Framework for Multimodal, Multiway Data
Association.
[165] Lusk, Parker C., Parikh, Devarth, and How, Jonathan P. 2023. GraffMatch:
Global Matching of 3D Lines and Planes for Wide Baseline LiDAR Registra-
tion. IEEE Robotics and Automation Letters, 8(2), 632–639.
[166] Lv, Zhaoyang, Dellaert, Frank, Rehg, James M, and Geiger, Andreas. 2019.
Taking a deeper look at the inverse compositional algorithm. Pages 4581–
4590 of: Proceedings of the IEEE/CVF Conference on Computer Vision and
Pattern Recognition.
[167] Macenski, Steve, Tsai, David, and Feinberg, Max. 2020. Spatio-temporal
voxel layer: A view on robot perception for the dynamic world. Intl. J. of
Advanced Robotic Systems, 17(2).
[168] Malcolm, James, Yalamanchili, Pavan, McClanahan, Chris, Venugopalakrish-
nan, Vishwanath, Patel, Krunal, and Melonakos, John. 2012. ArrayFire: a
GPU acceleration platform. Pages 49–56 of: Modeling and simulation for
defense systems and applications VII, vol. 8403. SPIE.
[169] Mangelson, J. G., Dominic, D., Eustice, R. M., and Vasudevan, R. 2018. Pair-
wise Consistent Measurement Set Maximization for Robust Multi-robot Map
Merging. Pages 2916–2923 of: IEEE Intl. Conf. on Robotics and Automation
(ICRA).
[170] Marquardt, D.W. 1963. An Algorithm for Least-Squares Estimation of Non-
linear Parameters. J. Soc. Indust. Appl. Math., 11(2), 431–441.
[171] Martens, W., Poffet, Y., Soria, P. R., Fitch, R., and Sukkarieh, S. 2017. Ge-
ometric Priors for Gaussian Process Implicit Surfaces. IEEE Robotics and
Automation Letters, 373–380.
[172] MATLAB. 2010. version 7.10.0 (R2010a). Natick, Massachusetts: The Math-
Works Inc.
[173] Matsuki, Hidenobu, Murai2, Riku, Kelly, Paul H. J., and Davison, Andrew J.
2023. Gaussian Splatting SLAM.
[174] Maybeck, P. 1979. Stochastic Models, Estimation and Control. Vol. 1. New
York: Academic Press.
 References 177

[175] Meagher, D. 1980. Octree Encoding: A New Technique for the Represen-
tation, Manipulation and Display of Arbitrary 3-D Objects by Computer.
Technical Report, Image Processing Laboratory, Rensselaer Polytech-
nic Institute(IPL-TR-80-111).
[176] Melkumyan, Arman, and Ramos, Fabio Tozeto. 2009. A sparse covariance
function for exact Gaussian process inference in large datasets. In: Intl. Joint
Conf. on AI (IJCAI).
[177] Mildenhall, B., Srinivasan, P.P., Tancik, M., Barron, J.T., Ramamoorthi, R.,
and Ng, R. 2020. NeRF: Representing Scenes as Neural Radiance Fields for
View Synthesis. In: European Conf. on Computer Vision (ECCV).
[178] Montemerlo, M., Thrun, S., Koller, D., and Wegbreit, B. 2002. FastSLAM: A
Factored Solution to the Simultaneous Localization and Mapping Problem.
In: Proc. 19th AAAI National Conference on AI.
[179] Moravec, Hans, and Elfes, Alberto. 1985. High resolution maps from wide
angle sonar. Pages 116–121 of: IEEE Intl. Conf. on Robotics and Automation
(ICRA).
[180] Mourikis, A.I., and Roumeliotis, S.I. 2007 (April). A Multi-State Constraint
Kalman Filter for Vision-aided Inertial Navigation. Pages 3565–3572 of: IEEE
Intl. Conf. on Robotics and Automation (ICRA).
[181] Mullane, J., Vo, B-N., Adams, M., and Vo, B-T. 2011. A Random-Finite-Set
Approach to Bayesian SLAM. IEEE Trans. Robotics, 27(2), 268–282.
[182] Mur-Artal, Raul, Montiel, Jose Maria Martinez, and Tardos, Juan D. 2015.
ORB-SLAM: a versatile and accurate monocular SLAM system. IEEE Trans.
Robotics, 31(5), 1147–1163.
[183] Museth, Ken. 2021. NanoVDB: A GPU-Friendly and Portable VDB Data
Structure For Real-Time Rendering And Simulation. In: Intl. Conf. on Com-
puter Graphics and Interactive Techniques (SIGGRAPH).
[184] Museth, Ken, Lait, Jeff, Johanson, John, Budsberg, Jeff, Henderson, Ron,
Alden, Mihai, Cucka, Peter, Hill, David, and Pearce, Andrew. 2013. Open-
VDB: an open-source data structure and toolkit for high-resolution vol-
umes. In: Intl. Conf. on Computer Graphics and Interactive Techniques (SIG-
GRAPH).
[185] Newcombe, R. A., Izadi, S., Hilliges, O., Molyneaux, D., Kim, D., Davison,
A. J., Kohli, P., Shotton, J., Hodges, S., and Fitzgibbon, A. 2011. KinectFu-
sion: Real-Time Dense Surface Mapping and Tracking. In: IEEE and ACM
Intl. Sym. on Mixed and Augmented Reality (ISMAR).
[186] Nießner, M., Zollhöfer, M., Izadi, S., and Stamminger, M. 2013. Real-time
3D Reconstruction at Scale using Voxel Hashing. In: Intl. Conf. on Computer
Graphics and Interactive Techniques (SIGGRAPH) Asia.
[187] Nieto, J., Guivant, H., Nebot, E., and Thrun, S. 2003. Real Time Data
Association for FastSLAM. In: IEEE Intl. Conf. on Robotics and Automation
(ICRA).
[188] Nistér, D. 2003. An Efficient Solution to the Five-Point Relative Pose Prob-
lem. In: IEEE Conf. on Computer Vision and Pattern Recognition (CVPR).
[189] Nocedal, Jorge, and Wright, Stephen J. 1999. Numerical Optimization.
Springer Series in Operations Research. Springer-Verlag.
[190] O’Callaghan, Simon T, and Ramos, Fabio T. 2012. Gaussian process occu-
pancy maps. Intl. J. of Robotics Research, 31(1), 42–62.
178 References

[191] Ochs, Peter, Dosovitskiy, Alexey, Brox, Thomas, and Pock, Thomas. 2015.
On iteratively reweighted algorithms for nonsmooth nonconvex optimization
in computer vision. SIAM Journal on Imaging Sciences, 8(1), 331–372.
[192] Ogayar-Anguita, C. J., Lopez-Ruiz, A., Rueda-Ruiz, A. J., and Segura-
Sanchez, Rafael J. 2023. Nested spatial data structures for optimal indexing
of LiDAR data. ISPRS J. of Photogrammetry and Remote Sensing (JPRS),
195, 287–297.
[193] Oleynikova, H., Taylor, Z., Fehr, M., Siegwart, R., and Nieto, J. 2017.
Voxblox: Incremental 3D Euclidean Signed Distance Fields for on-board MAV
planning. Pages 1366–1373 of: IEEE/RSJ Intl. Conf. on Intelligent Robots
and Systems (IROS).
[194] Oliphant, Travis E. 2006. A guide to NumPy. Vol. 1. Trelgol Publishing USA.
[195] Olson, E., Leonard, J., and Teller, S. 2006 (May). Fast Iterative Alignment
of Pose Graphs with Poor Initial Estimates. Pages 2262–2269 of: IEEE Intl.
Conf. on Robotics and Automation (ICRA).
[196] Olson, Edwin, and Agarwal, Pratik. 2012 (July). Inference on networks of
mixtures for robust robot mapping. In: Robotics: Science and Systems (RSS).
[197] Open, NN. 2016. An open source neural networks c++ library. URL:
http://opennn.cimne.com (2016).
[198] Pan, Yue, Xiao, Pengchuan, He, Yujie, Shao, Zhenlei, and Li, Zesong. 2021.
MULLS: Versatile LiDAR SLAM via multi-metric linear least square. Pages
11633–11640 of: IEEE Intl. Conf. on Robotics and Automation (ICRA).
IEEE.
[199] Parameshwara, Chethan M, Hari, Gokul, Fermüller, Cornelia, Sanket, Nitin J,
and Aloimonos, Yiannis. 2022. DiffPoseNet: Direct differentiable camera pose
estimation. Pages 6845–6854 of: Proceedings of the IEEE/CVF Conference
on Computer Vision and Pattern Recognition.
[200] Park, C., Moghadam, P., Kim, S., Elfes, A., Fookes, C., and Sridharan, S.
2018. Elastic LiDAR Fusion: Dense Map-Centric Continuous-Time SLAM.
In: IEEE Intl. Conf. on Robotics and Automation (ICRA).
[201] Paskin, M.A. 2003. Thin Junction Tree Filters for Simultaneous Localization
and Mapping. In: Intl. Joint Conf. on AI (IJCAI).
[202] Paszke, Adam, Gross, Sam, Massa, Francisco, Lerer, Adam, Bradbury, James,
Chanan, Gregory, Killeen, Trevor, Lin, Zeming, Gimelshein, Natalia, Antiga,
Luca, et al. 2019. Pytorch: An imperative style, high-performance deep learn-
ing library. Advances in neural information processing systems, 32.
[203] Pattabiraman, Bharath, Patwary, Md. Mostofa Ali, Gebremedhin, Asse-
faw H., keng Liao, Wei, and Choudhary, Alok. 2015. Fast Algorithms for the
Maximum Clique Problem on Massive Graphs with Applications to Overlap-
ping Community Detection. Internet Mathematics, 11(4-5), 421–448.
[204] Pearlmutter, Barak A, and Siskind, Jeffrey Mark. 2008. Reverse-mode AD in
a functional framework: Lambda the ultimate backpropagator. ACM Trans-
actions on Programming Languages and Systems (TOPLAS), 30(2), 1–36.
[205] Peng, Liangzu, Kümmerle, Christian, and Vidal, René. 2023. On the Conver-
gence of IRLS and Its Variants in Outlier-Robust Estimation. Pages 17808–
17818 of: IEEE Conf. on Computer Vision and Pattern Recognition (CVPR).
[206] Peng, Songyou, Niemeyer, Michael, Mescheder, Lars, Pollefeys, Marc, and
Geiger, Andreas. 2020. Convolutional occupancy networks. In: European
Conf. on Computer Vision (ECCV).
 References 179

[207] Perera, Samunda, and Barnes, Nick. 2012. Maximal cliques based rigid body
motion segmentation with a RGB-D camera. Pages 120–133 of: Asian Conf.
on Computer Vision. Springer.
[208] Pfister, H., Zwickler, M., v. Baar, J., and Gross, M. 2000. Surfels: surface
elements as rendering primitives. In: Intl. Conf. on Computer Graphics and
Interactive Techniques (SIGGRAPH).
[209] P.H.S. Torr, A. Zisserman. 2000. MLESAC: A New Robust Estimator with
Application to Estimating Image Geometry. Tech. rept. MSR-TR-99-60. MSR.
[210] Pineda, Luis, Fan, Taosha, Monge, Maurizio, Venkataraman, Shobha, Sodhi,
Paloma, Chen, Ricky T. Q., Ortiz, Joseph, DeTone, Daniel, Wang, Austin S.,
Anderson, Stuart, Dong, Jing, Amos, Brandon, and Mukadam, Mustafa.
2022a (Oct.). Theseus: A Library for Differentiable Nonlinear Optimization.
In: Conf. Neural Information Processing Systems (NIPS).
[211] Pineda, Luis, Fan, Taosha, Monge, Maurizio, Venkataraman, Shobha, Sodhi,
Paloma, Chen, Ricky TQ, Ortiz, Joseph, DeTone, Daniel, Wang, Austin, An-
derson, Stuart, Dong, Jing, Amos, Brandon, and Mukadam, Mustafa. 2022b.
Theseus: A Library for Differentiable Nonlinear Optimization. Advances in
Neural Information Processing Systems.
[212] Powell, M.J.D. 1970. A New Algorithm for Unconstrained Optimization.
Pages 31–65 of: Rosen, J., Mangasarian, O., and Ritter, K. (eds), Nonlinear
Programming. Academic Press.
[213] Qin, Chao, Ye, Haoyang, Pranata, Christian E, Han, Jun, Zhang, Shuyang,
and Liu, Ming. 2020. LINS: A Lidar-Inertial State Estimator for Robust
and Efficient Navigation. In: IEEE Intl. Conf. on Robotics and Automation
(ICRA).
[214] Qin, Tong, Li, Peiliang, and Shen, Shaojie. 2018. VINS-Mono: A Robust and
Versatile Monocular Visual-Inertial State Estimator. IEEE Trans. Robotics,
34(4), 1004–1020.
[215] Rahimi, Ali, and Recht, Benjamin. 2007. Random features for large-scale
kernel machines. In: Advances in Neural Information Processing Systems
(NIPS), vol. 20.
[216] Ramos, Fabio, and Ott, Lionel. 2016. Hilbert maps: Scalable continuous
occupancy mapping with stochastic gradient descent. Intl. J. of Robotics
Research, 35(14), 1717–1730.
[217] Rasmussen, Carl Edward, and Williams, Christopher KI. 2006. Gaussian
Processes for Machine Learning. Cambridge, Mass.: MIT Press.
[218] Ravichandran, Z., Peng, L., Hughes, N., Griffith, J.D., and Carlone, L. 2022.
Hierarchical Representations and Explicit Memory: Learning Effective Navi-
gation Policies on 3D Scene Graphs using Graph Neural Networks. In: IEEE
Intl. Conf. on Robotics and Automation (ICRA). .
[219] Reijgwart, V., Millane, A., Oleynikova, H., Siegwart, R., Cadena, C., and
Nieto, J. 2020. Voxgraph: Globally Consistent, Volumetric Mapping Using
Signed Distance Function Submaps. IEEE Robotics and Automation Letters.
[220] Reijgwart, Victor, Cadena, Cesar, Siegwart, Roland, and Ott, Lionel. 2023-07.
Efficient volumetric mapping of multi-scale environments using wavelet-based
compression.
[221] Revels, Jarrett, Lubin, Miles, and Papamarkou, Theodore. 2016. Forward-
mode automatic differentiation in Julia. arXiv preprint arXiv:1607.07892.
180 References

[222] Robbins, Herbert, and Monro, Sutton. 1951. A stochastic approximation

method. The annals of mathematical statistics, 400–407.
[223] Rodrigues, Rômulo T., Tsiogkas, Nikolaos, Pascoal, António, and Aguiar,
A. Pedro. 2021. Online Range-Based SLAM Using B-Spline Surfaces. IEEE
Robotics and Automation Letters, 6(2), 1958–1965.
[224] Roriz, Ricardo, Cabral, Jorge, and Gomes, Tiago. 2022. Automotive LiDAR
Technology: A Survey. IEEE Trans. on Intelligent Transportation Systems
(TITS), 23(7), 6282–6297.
[225] Rosinol, Antoni, Abate, Marcus, Chang, Yun, and Carlone, Luca. 2020.
Kimera: an open-source library for real-time metric-semantic localization and
mapping. Pages 1689–1696 of: IEEE Intl. Conf. on Robotics and Automation
(ICRA).
[226] San Segundo, Pablo, and Artieda, Jorge. 2015. A novel clique formulation for
the visual feature matching problem. Appl. Intelligence, 43(2), 325–342.
[227] Sandström, Erik, Li, Yue, Van Gool, Luc, and R. Oswald, Martin. 2023. Point-
SLAM: Dense Neural Point Cloud-based SLAM. In: Intl. Conf. on Computer
Vision (ICCV).
[228] Särkkä, Simo”. 2011. Linear Operators and Stochastic Partial Differential
Equations in Gaussian Process Regression. Pages 151–158 of: International
Conference on Artificial Neural Networks and Machine Learning.
[229] Sarlin, Paul-Edouard, DeTone, Daniel, Malisiewicz, Tomasz, and Rabinovich,
Andrew. 2020. SuperGlue: Learning Feature Matching with Graph Neural
Networks. In: IEEE Conf. on Computer Vision and Pattern Recognition
(CVPR).
[230] Schonberger, Johannes L, and Frahm, Jan-Michael. 2016. Structure-from-
motion revisited. Pages 4104–4113 of: IEEE Conf. on Computer Vision and
Pattern Recognition (CVPR).
[231] Sethian, James Albert. 1996. Level set methods: Evolving interfaces in ge-
ometry, fluid mechanics, computer vision, and materials science. Cambridge
monographs on applied and computational mathematics, 3.
[232] Shaban, Amirreza, Cheng, Ching-An, Hatch, Nathan, and Boots, Byron.
2019. Truncated back-propagation for bilevel optimization. Pages 1723–1732
of: The 22nd International Conference on Artificial Intelligence and Statis-
tics. PMLR.
[233] Shan, T., Englot, B., Meyers, D., Wang, W., Ratti, C., and Rus, D. 2020. LIO-
SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping.
In: IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS).
[234] Shan, Tixiao, and Englot, Brendan. 2018. LeGO-LOAM: Lightweight and
Ground-Optimized Lidar Odometry and Mapping on Variable Terrain. In:
IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS).
[235] Shen, Chen, O’Brien, James F., and Shewchuk, Jonathan Richard. 2004. In-
terpolating and Approximating Implicit Surfaces from Polygon Soup. Intl.
Conf. on Computer Graphics and Interactive Techniques (SIGGRAPH), 9.
[236] Shi, J., Yang, H., and Carlone, L. 2021. ROBIN: a Graph-Theoretic Approach
to Reject Outliers in Robust Estimation using Invariants. In: IEEE Intl. Conf.
on Robotics and Automation (ICRA). arXiv preprint: 2011.03659, .
[237] Shi, J., Yang, H., and Carlone, L. 2023. Optimal and Robust Category-level
Perception: Object Pose and Shape Estimation from 2D and 3D Semantic
Keypoints. IEEE Trans. Robotics, 39(5), 4131–4151. .
 References 181

[238] Sim, R., Elinas, P., Griffin, M., Shyr, A., and Little, J.J. 2006 (Jun). De-
sign and Analysis of a Framework for Real-time Vision-based SLAM using
Rao-Blackwellised Particle Filters. In: Proc. of the 3rd Canadian Conf. on
Computer and Robotic Vision (CRV).
[239] Smith, R., and Cheeseman, P. 1987. On the representation and estimation of
spatial uncertainty. Intl. J. of Robotics Research, 5(4), 56–68.
[240] Sola, Joan, Deray, Jeremie, and Atchuthan, Dinesh. 2018. A micro Lie theory
for state estimation in robotics. arXiv preprint arXiv:1812.01537.
[241] Speciale, P., Paudel, D. P., Oswald, M. R., Kroeger, T., Gool, L. V., and
Pollefeys, M. 2017 (July). Consensus Maximization with Linear Matrix In-
equality Constraints. Pages 5048–5056 of: IEEE Conf. on Computer Vision
and Pattern Recognition (CVPR).
[242] Stadie, Bradly, Zhang, Lunjun, and Ba, Jimmy. 2020. Learning intrinsic
rewards as a bi-level optimization problem. Pages 111–120 of: Conference
on Uncertainty in Artificial Intelligence. PMLR.
[243] Stillwell, J. 2008. Naive Lie Theory. Springer.
[244] Stork, Johannes A, and Stoyanov, Todor. 2020. Ensemble of Sparse Gaussian
Process Experts for Implicit Surface Mapping with Streaming Data. IEEE
Intl. Conf. on Robotics and Automation (ICRA).
[245] Stoyanov, T., Saarinen, J.P., Andreasson, H., and Lilienthal, A.J. 2013. Nor-
mal Distributions Transform Occupancy Map Fusion: Simultaneous Mapping
and Tracking in Large Scale Dynamic Environments. In: IEEE/RSJ Intl.
Conf. on Intelligent Robots and Systems (IROS).
[246] Stückler, J., and Behnke, S. 2014. Multi-Resolution Surfel Maps for Efficient
Dense 3D Modeling and Tracking. J. of Visual Communication and Image
Representation, 25(1), 137–147.
[247] Stueckler, J., and Behnke, S. 2014. Efficient Deformable Registration of Multi-
Resolution Surfel Maps for Object Manipulation Skill Transfer. In: IEEE Intl.
Conf. on Robotics and Automation (ICRA).
[248] Sucar, Edgar, Liu, Shikun, Ortiz, Joseph, and Davison, Andrew J. 2021. imap:
Implicit mapping and positioning in real-time. In: Intl. Conf. on Computer
Vision (ICCV).
[249] Sünderhauf, N., and Protzel, P. 2012a. Switchable Constraints for Robust
Pose Graph SLAM. In: IEEE/RSJ Intl. Conf. on Intelligent Robots and
Systems (IROS).
[250] Sünderhauf, N., and Protzel, P. 2012b. Towards a robust back-end for pose
graph SLAM. Pages 1254–1261 of: IEEE Intl. Conf. on Robotics and Au-
tomation (ICRA). IEEE.
[251] Tang, Chengzhou, and Tan, Ping. 2019a. Ba-net: Dense bundle adjustment
network. International Conference on Learning Representations (ICLR).
[252] Tang, Chengzhou, and Tan, Ping. 2019b. BA-Net: Dense bundle adjustment
networks. 7th International Conference on Learning Representations, ICLR
2019.
[253] Tavish, K. Mac, and Barfoot, T. D. 2015. At all costs: A comparison of
robust cost functions for camera correspondence outliers. Pages 62–69 of:
Conf. Computer and Robot Vision. IEEE.
[254] Tedrake, Russ, and the Drake Development Team. 2019. Drake: Model-based
design and verification for robotics.
182 References

[255] Teed, Zachary, and Deng, Jia. 2018. Deepv2d: Video to depth with differen-
tiable structure from motion. arXiv preprint arXiv:1812.04605.
[256] Teed, Zachary, and Deng, Jia. 2021a. Droid-slam: Deep visual slam for monoc-
ular, stereo, and rgb-d cameras. Advances in Neural Information Processing
Systems, 34.
[257] Teed, Zachary, and Deng, Jia. 2021b. Tangent space backpropagation for 3d
transformation groups. Pages 10338–10347 of: Proceedings of the IEEE/CVF
Conference on Computer Vision and Pattern Recognition.
[258] Thrun, S., Liu, Y., Koller, D., Ng, A.Y., Ghahramani, Z., and Durrant-Whyte,
H. 2004. Simultaneous Localization and Mapping With Sparse Extended
Information Filters. Intl. J. of Robotics Research, 23(7-8), 693–716.
[259] Thrun, S., Burgard, W., and Fox, D. 2005. Probabilistic Robotics. The MIT
press, Cambridge, MA.
[260] Thrun, Sebastian, et al. 2002. Robotic mapping: A survey. Exploring artificial
intelligence in the new millennium, 1(1-35), 1.
[261] Tian, Y., Chang, Y., Arias, F. Herrera, Nieto-Granda, C., How, J.P., and
Carlone, L. 2022. Kimera-Multi: Robust, Distributed, Dense Metric-Semantic
SLAM for Multi-Robot Systems. IEEE Trans. Robotics. accepted, arXiv
preprint: 2106.14386, .
[262] Tokui, Seiya, Oono, Kenta, Hido, Shohei, and Clayton, Justin. 2015. Chainer:
a next-generation open source framework for deep learning. Pages 1–6 of:
Proceedings of workshop on machine learning systems (LearningSys) in the
twenty-ninth annual conference on neural information processing systems
(NIPS), vol. 5.
[263] Trevor, Alexander J. B., Rogers, John G., and Christensen, Henrik I. 2012.
Planar surface SLAM with 3D and 2D sensors. Pages 3041–3048 of: IEEE
Intl. Conf. on Robotics and Automation (ICRA).
[264] Triggs, Bill, McLauchlan, Philip F, Hartley, Richard I, and Fitzgibbon, An-
drew W. 1999. Bundle adjustment—a modern synthesis. Pages 298–372 of:
International workshop on vision algorithms. Springer.
[265] Trulls, E., Jin, Y., Yi, K.M., Mishkin, D., and Matas, J. 2022. Image matching
challenge. https://www.kaggle.com/competitions/image-matching-challenge-
2022. Accessed: 2022.
[266] Tsardoulias, Emmanouil G, Iliakopoulou, A, Kargakos, Andreas, and Petrou,
Loukas. 2016. A review of global path planning methods for occupancy grid
maps regardless of obstacle density. J. of Intelligent and Robotic Systems,
84(1), 829–858.
[267] Tseng, Paul. 2001. Convergence of a block coordinate descent method for non-
differentiable minimization. Journal of optimization theory and applications,
109, 475–494.
[268] Vasudevan, Shrihari, Ramos, Fabio, Nettleton, Eric, and Durrant-Whyte,
Hugh. 2009. Gaussian process modeling of large-scale terrain. Pages 812–
840 of: J. of Field Robotics, vol. 26.
[269] Vespa, Emanuele, Funk, Nils, Kelly, Paul HJ, and Leutenegger, Stefan.
2019. Adaptive-resolution octree-based volumetric SLAM. Pages 654–662
of: Intl. Conf. on 3D Vision (3DV).
[270] Vizzo, I., Chen, X., Chebrolu, N., Behley, J., and Stachniss, C. 2021. Poisson
Surface Reconstruction for LiDAR Odometry and Mapping. In: IEEE Intl.
Conf. on Robotics and Automation (ICRA).
 References 183

[271] Vizzo, Ignacio, Guadagnino, Tiziano, Behley, Jens, and Stachniss, Cyrill.
2022. VDBFusion: Flexible and Efficient TSDF Integration of Range Sen-
sor Data. IEEE Sensors, 22(3).
[272] Vizzo, Ignacio, Guadagnino, Tiziano, Mersch, Benedikt, Wiesmann, Louis,
Behley, Jens, and Stachniss, Cyrill. 2023. KISS-ICP: In Defense of Point-to-
Point ICP – Simple, Accurate, and Robust Registration If Done the Right
Way. IEEE Robotics and Automation Letters, 8(2), 1029–1036.
[273] Wächter, Andreas, and Biegler, Lorenz T. 2006. On the implementation of
an interior-point filter line-search algorithm for large-scale nonlinear program-
ming. Mathematical programming, 106(1), 25–57.
[274] Wang, Chen, Gao, Dasong, Xu, Kuan, Geng, Junyi, Hu, Yaoyu, Qiu, Yuheng,
Li, Bowen, Yang, Fan, Moon, Brady, Pandey, Abhinav, Aryan, Xu, Jiahe, Wu,
Tianhao, He, Haonan, Huang, Daning, Ren, Zhongqiang, Zhao, Shibo, Fu,
Taimeng, Reddy, Pranay, Lin, Xiao, Wang, Wenshan, Shi, Jingnan, Talak,
Rajat, Cao, Kun, Du, Yi, Wang, Han, Yu, Huai, Wang, Shanzhao, Chen,
Siyu, Kashyap, Ananth, Bandaru, Rohan, Dantu, Karthik, Wu, Jiajun, Xie,
Lihua, Carlone, Luca, Hutter, Marco, and Scherer, Sebastian. 2023. PyPose: A
Library for Robot Learning with Physics-based Optimization. In: IEEE/CVF
Conference on Computer Vision and Pattern Recognition (CVPR).
[275] Wang, Chen, Ji, Kaiyi, Geng, Junyi, Ren, Zhongqiang, Fu, Taimeng, Yang,
Fan, Guo, Yifan, He, Haonan, Chen, Xiangyu, Zhan, Zitong, Du, Qiwei,
Su, Shaoshu, Li, Bowen, Qiu, Yuheng, Lin, Xiao, Du, Yi, Li, Qihang, and
Zhao, Zhipeng. 2024. Imperative Learning: A Self-supervised Neural-Symbolic
Learning Framework for Robot Autonomy. arXiv preprint.
[276] Wang, H., Wang, C., Chen, C., and Xie, L. 2021a. F-LOAM: Fast LiDAR
Odometry and Mapping. In: IEEE/RSJ Intl. Conf. on Intelligent Robots and
Systems (IROS).
[277] Wang, Jinkun, and Englot, Brendan. 2016. Fast, accurate gaussian process
occupancy maps via test-data octrees and nested Bayesian fusion. Pages
1003–1010 of: IEEE Intl. Conf. on Robotics and Automation (ICRA).
[278] Wang, K., Gao, F., and Shen, S. 2019. Real-Time Scalable Dense Surfel
Mapping. In: IEEE Intl. Conf. on Robotics and Automation (ICRA).
[279] Wang, Sen, Clark, Ronald, Wen, Hongkai, and Trigoni, Niki. 2017. Deepvo:
Towards end-to-end visual odometry with deep recurrent convolutional neural
networks. Pages 2043–2050 of: 2017 IEEE international conference on robotics
and automation (ICRA). IEEE.
[280] Wang, Wenshan, Zhu, Delong, Wang, Xiangwei, Hu, Yaoyu, Qiu, Yuheng,
Wang, Chen, Hu, Yafei Hu, Kapoor, Ashish, and Scherer, Sebastian. 2020.
TartanAir: A Dataset to Push the Limits of Visual SLAM. Pages 4909–4916
of: IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS).
[281] Wang, Wenshan, Hu, Yaoyu, and Scherer, Sebastian. 2021b. Tartanvo: A
generalizable learning-based vo. Pages 1761–1772 of: Conference on Robot
Learning. PMLR.
[282] Weber, Simon, Demmel, Nikolaus, Chan, Tin Chon, and Cremers, Daniel.
2023. Power Bundle Adjustment for Large-Scale 3D Reconstruction. In:
IEEE Conference on Computer Vision and Pattern Recognition (CVPR).
[283] Wei, Peng, Hua, Guoliang, Huang, Weibo, Meng, Fanyang, and Liu, Hong.
184 References

2021. Unsupervised monocular visual-inertial odometry network. Pages 2347–

2354 of: Proceedings of the Twenty-Ninth International Conference on Inter-
national Joint Conferences on Artificial Intelligence.
[284] Wei, T., Patel, Y., Shekhovtsov, A., Matas, J., and Barath, D. 2023. Gener-
alized differentiable RANSAC. In: Intl. Conf. on Computer Vision (ICCV).
[285] Whelan, T., Leutenegger, S., Moreno, R. S., Glocker, B., and Davison, A.
2015. ElasticFusion: Dense SLAM Without A Pose Graph. In: Robotics:
Science and Systems (RSS).
[286] Williams, Christopher, and Seeger, Matthias. 2000. Using the Nyström
method to speed up kernel machines. In: Advances in Neural Information
Processing Systems (NIPS), vol. 13.
[287] Williams, Oliver, and Fitzgibbon, Andrew. 2007. Gaussian Process Implicit
Surfaces.
[288] Wu, Lan, Lee, Ki Myung Brian, Liu, Liyang, and Vidal-Calleja, Teresa. 2021.
Faithful Euclidean distance field from log-Gaussian process implicit surfaces.
IEEE Robotics and Automation Letters, 2461–2468.
[289] Wu, Lan, Lee, Ki Myung Brian, Le Gentil, Cedric, and Vidal-Calleja, Teresa.
2023. Log-GPIS-MOP: A Unified Representation for Mapping, Odometry,
and Planning. IEEE Trans. Robotics, 39(5), 4078–4094.
[290] Wu, Qinghua, and Hao, Jin-Kao. 2015. A review on algorithms for maximum
clique problems. European Journal of Operational Research, 242(3), 693–709.
[291] Yang, Fan, Wang, Chen, Cadena, Cesar, and Hutter, Marco. 2023. iPlanner:
Imperative Path Planning. In: Robotics: Science and Systems (RSS).
[292] Yang, H., and Carlone, L. 2019a. A Polynomial-time Solution for Robust
Registration with Extreme Outlier Rates. In: Robotics: Science and Systems
(RSS). , , , , .
[293] Yang, H., and Carlone, L. 2019b. A Quaternion-based Certifiably Optimal
Solution to the Wahba Problem with Outliers. In: Intl. Conf. on Computer Vi-
sion (ICCV). (Oral Presentation, accept rate: 4%), Arxiv version: 1905.12536,
.
[294] Yang, H., and Carlone, L. 2020. One Ring to Rule Them All: Certifiably
Robust Geometric Perception with Outliers. Pages 18846–18859 of: Advances
in Neural Information Processing Systems (NIPS), vol. 33. .
[295] Yang, H., and Carlone, L. 2022. Certifiably Optimal Outlier-Robust Geomet-
ric Perception: Semidefinite Relaxations and Scalable Global Optimization.
IEEE Trans. Pattern Anal. Machine Intell. .
[296] Yang, H., Antonante, P., Tzoumas, V., and Carlone, L. 2020a. Graduated
Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers
to Global Outlier Rejection. IEEE Robotics and Automation Letters, 5(2),
1127–1134. arXiv preprint:1909.08605 (with supplemental material), .
[297] Yang, H., Shi, J., and Carlone, L. 2020b. TEASER: Fast and Certifiable Point
Cloud Registration. IEEE Trans. Robotics, 37(2), 314–333. extended arXiv
version 2001.07715 .
[298] Yang, J., Li, H., Campbell, D., and Jia, Y. 2016. Go-ICP: A Globally Optimal
Solution to 3D ICP Point-Set Registration. 38(11), 2241–2254.
[299] Yang, Jiaolong, Li, Hongdong, and Jia, Yunde. 2014. Optimal essential matrix
estimation via inlier-set maximization. Pages 111–126 of: European Conf. on
Computer Vision (ECCV). Springer.
 References 185

[300] Yang, Wen, Gong, Zheng, Huang, Baifu, and Hong, Xiaoping. 2022. Lidar
With Velocity: Correcting Moving Objects Point Cloud Distortion From Os-
cillating Scanning Lidars by Fusion With Camera. IEEE Robotics and Au-
tomation Letters, 7(3).
[301] Yannakakis, M. 1981. Computing the minimum fill-in is NP-complete. SIAM
J. Algebraic Discrete Methods, 2.
[302] Yi, Brent, Lee, Michelle A, Kloss, Alina, Martı́n-Martı́n, Roberto, and Bohg,
Jeannette. 2021a. Differentiable factor graph optimization for learning
smoothers. Pages 1339–1345 of: 2021 IEEE/RSJ International Conference
on Intelligent Robots and Systems (IROS). IEEE.
[303] Yi, Brent, Lee, Michelle, Kloss, Alina, Martı́n-Martı́n, Roberto, and Bohg,
Jeannette. 2021b. Differentiable Factor Graph Optimization for Learning
Smoothers. In: IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS).
[304] Yokoyama, N., Ha, S., Batra, D., Wang, J., and Bucher, B. 2024. VLFM:
Vision-Language Frontier Maps for Zero-Shot Semantic Navigation. In: IEEE
Intl. Conf. on Robotics and Automation (ICRA).
[305] Yugay, Vladimir, Li, Yue, Gevers, Theo, and Oswald, Martin R. 2023.
Gaussian-SLAM: Photo-realistic Dense SLAM with Gaussian Splatting.
arXiv preprint.
[306] Zhan, Zitong, Gao, Dasong, Lin, Yun-Jou, Xia, Youjie, and Wang, Chen. 2024.
iMatching: Imperative Correspondence Learning. In: European Conference on
Computer Vision (ECCV).
[307] Zhang, J., and Singh, S. 2014. LOAM: Lidar Odometry and Mapping in
Real-time. In: Robotics: Science and Systems (RSS).
[308] Zhao, Shibo, Wang, Peng, Zhang, Hengrui, Fang, Zheng, and Scherer, Se-
bastian. 2020. Tp-tio: A robust thermal-inertial odometry with deep ther-
malpoint. Pages 4505–4512 of: 2020 IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS). IEEE.
[309] Zheng, Y., Sugimoto, S., and Okutomi, M. 2011. Deterministically maximiz-
ing feasible subsystem for robust model fitting with unit norm constraint.
Pages 1825–1832 of: IEEE Conf. on Computer Vision and Pattern Recogni-
tion (CVPR).
[310] Zhong, Xingguang, Pan, Yue, Behley, Jens, and Stachniss, Cyrill. 2023.
SHINE-Mapping: Large-Scale 3D Mapping Using Sparse Hierarchical Implicit
Neural Representations. In: IEEE Intl. Conf. on Robotics and Automation
(ICRA).
[311] Zhou, Q.Y., Park, J., and Koltun, V. 2016. Fast global registration. Pages
766–782 of: European Conf. on Computer Vision (ECCV). Springer.
[312] Zhu, Zihan, Peng, Songyou, Larsson, Viktor, Xu, Weiwei, Bao, Hujun, Cui,
Zhaopeng, Oswald, Martin R, and Pollefeys, Marc. 2022. NICE-SLAM: Neu-
ral Implicit Scalable Encoding for SLAM. In: IEEE Conf. on Computer Vision
and Pattern Recognition (CVPR).
[313] Zuckerman, David. 2006 (May). Linear degree extractors and the inapprox-
imability of max clique and chromatic number. Pages 681–690 of: ACM Symp.
on Theory of Computing (STOC).
[314] Zwicker, Matthias, Pfister, Hanspeter, van Baar, Jeroen, and Gross, Markus.
2001. Surface Splatting. In: Intl. Conf. on Computer Graphics and Interactive
Techniques (SIGGRAPH).
Author index
Subject index