The document summarizes two incremental smoothing and mapping algorithms, iSAM and iSAM2. iSAM uses matrix factorization to efficiently update the information matrix when new measurements are obtained. However, periodic batch steps are required to avoid "fill-in", reducing efficiency. iSAM2 uses a novel Bayes tree data structure to represent the factor graph, allowing incremental updates to be made by only modifying the relevant portions of the tree. This provides an exact, incremental solution without periodic batch steps, making it more efficient than iSAM. The document provides examples of applying iSAM2 to a range odometry SLAM problem and a structure from motion application.

1. Batch and Incremental Smoothing and
Mapping with a Graphical Approach
by Kaess et al.
Vitaly Shalumov∗
Elya Pardes†
Technion - Israel Institute of Technology, Haifa, 32000,Israel
I. Introduction
The problem of simultaneously navigating a robot and mapping its surroundings (SLAM)
is at the heart of it a probabilistic one. Generally, measurements involve noise and inherent
uncertainty is introduced to our believed position and surrounding environment. Optimizing
the estimation of the robots position and surroundings is the key to allowing a robot to
correctly perceive, navigate, and accomplish its task in an autonomous way. There is a vast
variety of applications, ranging from tracking people for human-robot interaction to search
and rescue missions in unknown territories.
In order to overcome the challenge of cumulative errors in navigation, we choose a prob-
abilistic inference approach, and strive to devise efficient solutions to make online operation
feasible.
Since the development of the more basic SLAM algorithms formerly based on Extended
Kalman Filters and Rao-Blackwellized particle filters, significant progress has been made.
Newer solutions have been offered, working towards efficiency by offering an incremental and
thus more practical approach to real-time robot navigation. Previous work1
introduced the
idea of factorizing the information matrix of the smoothing problem. Instead of marginalizing
the previous states of the robot, they are retained and the solution is simplified due to
the information matrix becoming naturally sparse through this smoothing process. The
algorithm called square root SAM made the process possible and notably more efficient,
∗
The Technion Program for Autonomous Systems and Robotics; vitaly.shalumov@gmail.com
†
Faculty of Aerospace Engineering; elyapardes@gmail.com
1 of 12

2. although unnecessary computations have to be made since upon every measurement, a batch
algorithm first updates the information matrix and then factors it completely.
Ref. 2 presented a novel approach to perform incremental smoothing and mapping (iSAM),
based on matrix factorizations. Ref. 3 presented an entirely new data structure called the
Bayes Tree, and exploits its unique properties to develop a new incremental inference method
called iSAM2. This state-of-the-art algorithm offers a full and exact solution to incremental
inference problems beyond SLAM based on this new graphical approach.
The following sections briefly describe the iSAM and iSAM2 algorithms and the superi-
ority of iSAM2 over iSAM.
II. Problem Formulation
The challenge in solving the SLAM problem efficiently for online performance emanates
from having to obtain an updated estimate every time new measurements are acquired. The
data cannot be processed on the ground after the mission, as the robot is dependent on it
during operation for its own navigation.
Let us present the general problem, and its representation using a factor graph, which is
used to efficiently perform inference.
Consider a robot, moving from state to state in sequence (x0 → x1 → ...). Along its path,
it encounters landmarks l1 and l2 and promptly takes measurements whenever it comes into
contact with them (Fig. 1).
Figure 1: Bayesian belief network representation of the SLAM problem.xi is the state of the
robot at time i, lj the location of landmark j, ui the control input at time i,and zk the k-th
landmark measurement.
We are interested in solving the following problem:
2 of 12

3. X∗
, L∗
= arg max
X,L
P(X, L, U, Z) (1)
given the process model:
xi = fi(xi−1, ui) + wi (2)
and the measurement model:
zk = hk(xik
, ljk
) + vk (3)
Figure 2: Factor graph formulation of the SLAM problem, where variable nodes are shown as
large circles, and factor nodes (measurements) as small solid circles. The factors shown are
odometry measurements u, a prior p, loop-closing constraints c and landmark measurements
m.
In Fig. 2, the robot’s trajectory is illustrated in the form of a factor graph. Factor graphs
constitute a visual way to represent the estimation problem, consisting of variable nodes
(states and landmarks) θj ∈ Θ and factor nodes fi ∈ F, that display the relations between
the given variables. The figure also presents a possible loop closure (c1, c2).
Let us define f(Θ) as:
f(Θ) =
i
fi(Θi) (4)
where Θi is the set of variables involved in the various probabilities fi. Our objective is
to maximize our probabilistic estimation of the variables Θi , hence we aspire to find:
Θ∗
= arg max
Θ
f(Θ) (5)
3 of 12

4. In the Gaussian case, this is equivalent to finding the non-linear least squares optimization:
arg min
Θ
(− log f(Θ)) = arg min
Θ
1
2 i
||hi(Θi) − zi||2
Σi
(6)
which is equivalent to solving the linear least-squares problem iteratively:
arg min
∆
(− log f(∆)) = arg min
∆
||A∆ − b||2
(7)
which can be solved either through QR factorization or Cholesky method.
III. Main contribution
A. Incremental Smoothing and Mapping - Matrix Approach (iSAM)
In iSAM, an incremental approach is introduced by taking the square root information
matrix that was previously calculated and directly updating only the relevant components
when measurements come in.
The solution of QR factorization leads to:
||Aθ − b||2
→ Rθ∗
= d (8)
Adding a new measurement is equivalent to adding a row wT
and RHS γ into the current
factor R, RHS d. This yields a new system that is not yet in the correct factorized form
(there are non-zero entries below the diagonal):

 QT
1



 A
wT

 =

 R
wT

 , new RHS :

 d
γ

 (9)
The correct form is achieved via Givens rotations.
If the mission path involves loop closures, the nice property of local updates is lost and
thus sparsity is weakened. It then becomes necessary to periodically perform a variable
reordering to avoid this occurrence called fill-in and prevent the process from slowing down.
Although iSAM offered a full solution with fast incremental updates, these periodic batch
steps are expensive and take away from the efficiency of the algorithm.
To overcome this limitation, iSAM2 uses an entirely new graphical data structure called
the Bayes Tree, along with the sparse linear algebra insights that have already been acquired.
4 of 12

5. B. Incremental Smoothing and Mapping - Graph Approach (iSAM2)
The objective is to solve the estimation problem by operating directly on the graphical
models, without having to convert the factor graph to a sparse matrix and then applying
spare linear algebra methods.
An important part of this new approach is realizing that inference can be seen as con-
verting the factor graph to a Bayes net. The elimination algorithm is presented in Fig. 3
Figure 3: Algorithm 1: Eliminating a variable θj from the factor graph.
In the Gaussian case, this elimination process is equivalent to sparse QR factorization of
the measurement Jacobian or Cholesky factorization of the information matrix (AT
A). It
is then possible to solve the least squares optimization by building the tree starting from
the leaves up to the root of the tree, and then making one more pass down to achieve the
optimization. The algorithms for Bayes tree creation and update are given in Fig. 4.
Figure 4: Left: Algorithm 2 - Creating a Bayes tree from the chordal Bayes net resulting
from elimination (Algorithm 1); Right: Algorithm 3 - Updating the Bayes tree with new
factors F .
Fig. 5 presents an example factor graph, its appropriate Bayes net and Bayes tree, to-
gether with their appropriate matrix representations.
5 of 12

6. Figure 5: (a) The factor graph and the associated Jacobian matrix A for a small SLAM
example, where a robot located at successive poses x1, x2, and x3 makes observations on
landmarks l1 and l2. In addition there is an absolute measurement on the pose x1.(b)The
chordal Bayes net and the associated square root information matrix R resulting from elim-
inating the factor graph using the elimination ordering l1,l2,x1,x2,x3. The last variable to
be eliminated, here x3, is called the root. (c) The Bayes tree and the associated square
root information matrix R describing the clique structure in the chordal Bayes net. The
association of cliques and their conditional densities with rows in the R factor is indicated
by color.
1. Building the Bayes Tree and Incremental Inference
As was shown by Ref. 3, the Bayes net resulting from elimination and factorization is chordal
and can be converted into a new and totally different tree-structured graphical model in which
optimization and marginalization are much easier. Additionally, the process of adding new
measurements to the tree only affect the cliques involving the relevant variables and the root
(see Fig. 6). The modified part of the tree is converted into a factor graph, and new factors
are added to it. We then convert it back to the Bayes tree form, reattaching the branches
to the updated component. In this way, it is not necessary to modify the rest of the data
set and significantly improve efficiency of the process.
6 of 12

7. Figure 6: Updating a Bayes tree with a new factor, based on the example in Fig. 5(c). The
affected part of the Bayes tree is highlighted for the case of adding a new factor between x1
and x3. Note that the right branch is not affected by the change. (Top right) The factor
graph generated from the affected part of the Bayes 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.
IV. Implementation
The following section presents and analyzes two examples which utilize the iSAM2 algo-
rithm for smoothing and mapping. The first example is taken from the GTSAM toolbox,
and presents results using SAM by incorporating range-only data. The second example is a
structure from motion application of the iSAM2 algorithm. The simulations are carried out
in Matlab using the GTSAM toolbox.
A. Range Odometry
The following example presents results using iSAM2 by incorporating range-only data col-
lected from radio-based sensors.
7 of 12

8. 1. Data Description
The range-only data is collected from a radio-based system that utilizes ultra-wide band
signals to compute the distance between two homogeneous nodes by measuring the difference
of arrival times. The locations of the stationary radio nodes were manually surveyed to a 2cm
accuracy using available GPS. The dataset is comprised of three kinds of data: the ground-
truth path of the robot from GPS and inertial sensors, the path using dead reckoning,
and the range measurements to the stationary radio nodes. The path from dead reckoning
is computed by integrating over time incremental measurements of change in the robot’s
heading from a KVH gyro (with a drift rate of 30 deg/hr) and incremental traveled distance
measurements from the wheel encoders. The robot traveled 1.3 km, receiving 1,816 range
measurements.
The Data is structured as follows:
• GT: Groundtruth path from GPS : Time (sec) Xpose (m) Ypose (m) Heading (rad)
• DR: Odometry Input (delta distance traveled and delta heading change): Time (sec)
Delta Dist. Trav. (m) Delta Heading (rad)
• DRp: Dead Reckoned Path from Odometry : Time (sec) Xpose (m) Ypose (m) Heading
(rad)
• TL: Surveyed Node Locations : Time (sec) Xpose (m) Ypose (m)
2. Simulation
Fig. 7 presents the application of the iSAM2 algorithm in a SLAM problem. The figure
presents the dead reckoning path, which highlights the effect of heading error by turning
in the same direction repeatedly. Furthermore, the figure presents the estimation and the
ground truth of both the landmarks(L0,L1,L5,L6) and the 2D poses.
A similar simulation was run, using the SAM algorithm. The execution time of iSAM
was two times faster (20 vs 40 seconds). Choleskey factorization was used because of its
superior speed in contrast to the QR factorization.
Fig. 8 and Fig. 9 present the Baeys tree evolution after 24 and 350 measurements respec-
tively.
8 of 12

9. X [m]
-100 -80 -60 -40 -20 0 20 40
Y[m]
-10
0
10
20
30
40
50
60
70 Dead Reckonong Path (Odometry)
Estimated Pose
Estimated Landmark
Groundtruth path from GPS
True Node Locations
Figure 7: Robot Path
L0,16,L6,L1,7,11
9 : 7,11,L1 3 : 7,L0,L1,L6 13 : 11,16,L0 L5 : 7,16,L0,L1,L6
8 : 7,9 10 : 9,11 2 : 3,L1 5 : 3,7,L0
1 : 2,L1
0 : 1
4 : 3,5 6 : 5,7
12 : 11,13 15 : 13,16
14 : 13,15
20 : 16,L0,L1,L5,L6
17 : 16,20,L1 24 : 20,L0,L1,L5,L6
19 : 17,20
18 : 17,19
22 : 20,24,L0
21 : 20,22 23 : 22,24
Figure 8: Baeys tree after 24 measurements
L0,16,L6,L1,7,11
9 : 7,11,L1 3 : 7,L0,L1,L6 13 : 11,16,L0 L5 : 7,16,L0,L1,L6
8 : 7,9 10 : 9,11 2 : 3,L1 5 : 3,7,L0
1 : 2,L1
0 : 1
4 : 3,5 6 : 5,7
12 : 11,13 15 : 13,16
14 : 13,15
20 : 16,L0,L1,L5,L6
17 : 16,20,L1 24 : 20,L0,L1,L5,L6
19 : 17,20
18 : 17,19
22 : 20,24,L0 28 : 24,L0,L1,L5,L6
21 : 20,22 23 : 22,24 26 : 24,28,L1 34 : 28,L0,L1,L5,L6
25 : 24,26 27 : 26,28 33 : 28,34,L0,L5 38 : 34,L0,L1,L5,L6
30 : 28,33,L0
29 : 28,30 32 : 30,33
31 : 30,32
36 : 34,38,L6 43 : 38,L0,L1,L5,L6
35 : 34,36 37 : 36,38 41 : 38,43,L5 47 : 43,L0,L1,L5,L6
40 : 38,41 42 : 41,43
39 : 38,40
45 : 43,47,L6 51 : 47,L0,L1,L5,L6
44 : 43,45 46 : 45,47 49 : 47,51,L5 57 : 51,L0,L1,L5,L6
48 : 47,49 50 : 49,51 53 : 51,57,L6 61 : 57,L0,L1,L5,L6
52 : 51,53 56 : 53,57
55 : 53,56
54 : 53,55
59 : 57,61,L1 66 : 61,L0,L1,L5,L6
58 : 57,59 60 : 59,61 63 : 61,66,L0 70 : 66,L0,L1,L5,L6
62 : 61,63 65 : 63,66
64 : 63,65
67 : 66,70,L1 74 : 70,L0,L1,L5,L6
69 : 67,70
68 : 67,69
72 : 70,74,L0 78 : 74,L0,L1,L5,L6
71 : 70,72 73 : 72,74 76 : 74,78,L1 83 : 78,L0,L1,L5,L6
75 : 74,76 77 : 76,78 80 : 78,83,L0 86 : 83,L0,L1,L5,L6
79 : 78,80 82 : 80,83
81 : 80,82
84 : 83,86,L1 91 : 86,L0,L1,L5,L6
85 : 84,86 89 : 86,91,L0 95 : 91,L0,L1,L5,L6
88 : 86,89 90 : 89,91
87 : 86,88
93 : 91,95,L1 99 : 95,L0,L1,L5,L6
92 : 91,93 94 : 93,95 97 : 95,99,L0 104 : 99,L0,L1,L5,L6
96 : 95,97 98 : 97,99 101 : 99,104,L1 108 : 104,L0,L1,L5,L6
100 : 99,101 103 : 101,104
102 : 101,103
106 : 104,108,L0 112 : 108,L0,L1,L5,L6
105 : 104,106 107 : 106,108 109 : 108,112,L1 116 : 112,L0,L1,L5,L6
111 : 109,112
110 : 109,111
114 : 112,116,L0 120 : 116,L0,L1,L5,L6
113 : 112,114 115 : 114,116 118 : 116,120,L1 125 : 120,L0,L1,L5,L6
117 : 116,118 119 : 118,120 122 : 120,125,L0 129 : 125,L0,L1,L5,L6
121 : 120,122 124 : 122,125
123 : 122,124
126 : 125,129,L1 133 : 129,L0,L1,L5,L6
128 : 126,129
127 : 126,128
131 : 129,133,L0 137 : 133,L0,L1,L5,L6
130 : 129,131 132 : 131,133 135 : 133,137,L1 142 : 137,L0,L1,L5,L6
134 : 133,135 136 : 135,137 139 : 137,142,L0 145 : 142,L0,L1,L5,L6
138 : 137,139 141 : 139,142
140 : 139,141
143 : 142,145,L1 150 : 145,L0,L1,L5,L6
144 : 143,145 147 : 145,150,L0 154 : 150,L0,L1,L5,L6
146 : 145,147 149 : 147,150
148 : 147,149
151 : 150,154,L1 158 : 154,L0,L1,L5,L6
153 : 151,154
152 : 151,153
156 : 154,158,L0 162 : 158,L0,L1,L5,L6
155 : 154,156 157 : 156,158 160 : 158,162,L1 166 : 162,L0,L1,L5,L6
159 : 158,160 161 : 160,162 164 : 162,166,L0 170 : 166,L0,L1,L5,L6
163 : 162,164 165 : 164,166 168 : 166,170,L1 175 : 170,L0,L1,L5,L6
167 : 166,168 169 : 168,170 173 : 170,175,L0 181 : 175,L0,L1,L5,L6
172 : 170,173 174 : 173,175
171 : 170,172
179 : 175,181,L6 185 : 181,L0,L1,L5,L6
178 : 175,179 180 : 179,181
177 : 175,178
176 : 175,177
183 : 181,185,L5 192 : 185,L0,L1,L5,L6
182 : 181,183 184 : 183,185 187 : 185,192,L6 196 : 192,L0,L1,L5,L6
186 : 185,187 191 : 187,192
190 : 187,191
189 : 187,190
188 : 187,189
193 : 192,196,L1 200 : 196,L0,L1,L5,L6
195 : 193,196
194 : 193,195
198 : 196,200,L0 204 : 200,L0,L1,L5,L6
197 : 196,198 199 : 198,200 202 : 200,204,L1 209 : 204,L0,L1,L5,L6
201 : 200,202 203 : 202,204 206 : 204,209,L0 212 : 209,L0,L1,L5,L6
205 : 204,206 208 : 206,209
207 : 206,208
210 : 209,212,L1 217 : 212,L0,L1,L5,L6
211 : 210,212 215 : 212,217,L0 221 : 217,L0,L1,L5,L6
214 : 212,215 216 : 215,217
213 : 212,214
218 : 217,221,L1 225 : 221,L0,L1,L5,L6
220 : 218,221
219 : 218,220
223 : 221,225,L0 229 : 225,L0,L1,L5,L6
222 : 221,223 224 : 223,225 227 : 225,229,L1 234 : 229,L0,L1,L5,L6
226 : 225,227 228 : 227,229 231 : 229,234,L0 237 : 234,L0,L1,L5,L6
230 : 229,231 233 : 231,234
232 : 231,233
235 : 234,237,L1 242 : 237,L0,L1,L5,L6
236 : 235,237 240 : 237,242,L0 248 : 242,L0,L1,L5,L6
239 : 237,240 241 : 240,242
238 : 237,239
244 : 242,248,L1 252 : 248,L0,L1,L5,L6
243 : 242,244 247 : 244,248
246 : 244,247
245 : 244,246
250 : 248,252,L5 256 : 252,L0,L1,L5,L6
249 : 248,250 251 : 250,252 254 : 252,256,L6 269 : 256,L0,L1,L5,L6
253 : 252,254 255 : 254,256 265 : 256,269,L0,L5 275 : 269,L0,L1,L5,L6
259 : 256,265,L5 267 : 265,269,L5
258 : 256,259 264 : 259,265
257 : 256,258 263 : 259,264
262 : 259,263
261 : 259,262
260 : 259,261
266 : 265,267 268 : 267,269
271 : 269,275,L6 279 : 275,L0,L1,L5,L6
270 : 269,271 274 : 271,275
273 : 271,274
272 : 271,273
277 : 275,279,L1 284 : 279,L0,L1,L5,L6
276 : 275,277 278 : 277,279 282 : 279,284,L0 288 : 284,L0,L1,L5,L6
281 : 279,282 283 : 282,284
280 : 279,281
285 : 284,288,L1 292 : 288,L0,L1,L5,L6
287 : 285,288
286 : 285,287
290 : 288,292,L0 296 : 292,L0,L1,L5,L6
289 : 288,290 291 : 290,292 294 : 292,296,L1 302 : 296,L0,L1,L5,L6
293 : 292,294 295 : 294,296 301 : 296,302,L0,L5 307 : 302,L0,L1,L5,L6
298 : 296,301,L0
297 : 296,298 300 : 298,301
299 : 298,300
304 : 302,307,L6 311 : 307,L0,L1,L5,L6
303 : 302,304 306 : 304,307
305 : 304,306
309 : 307,311,L5 315 : 311,L0,L1,L5,L6
308 : 307,309 310 : 309,311 313 : 311,315,L6 319 : 315,L0,L1,L5,L6
312 : 311,313 314 : 313,315 317 : 315,319,L5 324 : 319,L0,L5,L6
316 : 315,317 318 : 317,319 321 : 319,324,L6 326 : 324,L5,L6
320 : 319,321 323 : 321,324
322 : 321,323
325 : 324,326 330 : 326,L6
329 : 326,330
328 : 326,329
327 : 326,328
Figure 9: Baeys tree after 350 measurements
9 of 12

10. B. Visual SLAM
The following example presents a simple visual SLAM example for a Structure From Motion
problem. Structure from Motion (SFM) is a technique to recover a 3D reconstruction of
the environment from corresponding visual features in a collection of unordered images (in
our case, they are ordered). In GTSAM this is done using the factor graph framework. In
particular, there is a projection factor that calculates the reprojection error f(xi, pj; zij, K)
for a given camera pose xi and point pj (points are landmarks). The factor is parameterized
by the 2D measurement zij, and known calibration parameters K.
The challenging aspects of SFM are: (a) data association, and (b) initialization. GTSAM
does neither of these things for you: it simply provides the bundle adjustment optimization.
In the example, we simply assume the data association is known, and we initialize with the
ground truth.
In the current simulation, twenty cameras are arranged in a circle, observing 3 vertices
that are arranged in a triangle. The following figures present the poses and the landmarks
after 2,3 and 20 measurements, together with their appropriate Bayes tree.
The camera is rendered with color-coded axes (RGB for XYZ in the following figures),
and the viewing direction is along the positive Z-axis. The ellipses in the following figures
are the 3D error covariance ellipses for both cameras and points.
The camera calibration type we used is the standard, no-radial distortion, 5 parameter
calibration matrix.
(a)
x2,x1,l3
l1 : x1,x2 l2 : x1,x2
(b)
Figure 10: SFM after 2 measurements: (a)Poses and Covariance; (b)Bayes Tree.
10 of 12

11. (a)
l1,x2,l3,l2,x3
x1 : l1,l2,l3,x2
(b)
Figure 11: SFM after 3 measurements: (a)Poses and Covariance; (b)Bayes Tree.
(a)
l1,x19,l3,l2,x20
x18 : l1,l2,l3,x19
x17 : l1,l2,l3,x18
x16 : l1,l2,l3,x17
x15 : l1,l2,l3,x16
x14 : l1,l2,l3,x15
x13 : l1,l2,l3,x14
x12 : l1,l2,l3,x13
x11 : l1,l2,l3,x12
x10 : l1,l2,l3,x11
x9 : l1,l2,l3,x10
x8 : l1,l2,l3,x9
x7 : l1,l2,l3,x8
x6 : l1,l2,l3,x7
x5 : l1,l2,l3,x6
x4 : l1,l2,l3,x5
x3 : l1,l2,l3,x4
x2 : l1,l2,l3,x3
x1 : l1,l2,l3,x2
(b)
Figure 12: SFM after 20 measurements: (a)Poses and Covariance; (b)Bayes Tree.
V. Conclusions
The papers on iSAM and iSAM2 present a fast incremental solution to the SLAM prob-
lem. By avoiding marginalization using either a sparse square root information matrix
11 of 12

12. representation in iSAM or a graphical representation in iSAM2. The main disadvantage of
iSAM being the periodic batch step necessary for variable reordering and relinearization has
been alleviated by utilizing a novel data structure called the Bayes Tree. This new graphical
approach provides a better understanding of matrix factorization in terms of probability
densities and significantly facilitates the process of marginalization and optimization.
Two main limitations of iSAM2 can be observed. Firstly, the fast growth of the Bayes
Tree when operating on a long and complex mission. The incremental method requires only
part of the structure to be modified at every step, but the update affects more variables as the
tree grows. The implications of this being updates can affect large segments of the structure,
leading to large computation costs. Secondly, the elimination process that constitutes the
dominating part of how time is spent in iSAM2 is suboptimal due to use of heuristic methods
of reordering such as COLAMD.
References
1
Dellaert, F. and Kaess, M., “Square Root SAM: Simultaneous localization and mapping via square root
information smoothing,” The International Journal of Robotics Research, Vol. 25, No. 12, 2006, pp. 1181–
1203.
2
Kaess, M., Ranganathan, A., and Dellaert, F., “iSAM: Incremental smoothing and mapping,” Robotics,
IEEE Transactions on, Vol. 24, No. 6, 2008, pp. 1365–1378.
3
Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J. J., and Dellaert, F., “iSAM2: Incremen-
tal smoothing and mapping using the Bayes tree,” The International Journal of Robotics Research, 2011,
pp. 0278364911430419.
12 of 12