An HMM with observed measurements, unrolled over time, represented as a
factor graph.
\end_layout
\end_inset
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset ERT
status open
\begin_layout Plain Layout
\backslash
noindent
\end_layout
\end_inset
This motivates a different graphical model, a
\series bold
factor graph
\series default
, in which we only represent the unknown variables
\begin_inset Formula $X_{1}$
\end_inset
,
\begin_inset Formula $X_{2}$
\end_inset
, and
\begin_inset Formula $X_{3}$
\end_inset
, connected to factors that encode probabilistic information on them, as
in Figure
\begin_inset CommandInset ref
LatexCommand ref
reference "fig:HMM-FG"
\end_inset
.
To do maximum a-posteriori (MAP) inference, we then maximize the product
\begin_inset Formula
\[
f(X_{1},X_{2},X_{3})=\prod f_{i}(\mathcal{X}_{i})
\]
\end_inset
i.e., the value of the factor graph.
It should be clear from the figure that the connectivity of a factor graph
encodes, for each factor
\begin_inset Formula $f_{i}$
\end_inset
, which subset of variables
\begin_inset Formula $\mathcal{X}_{i}$
\end_inset
it depends on.
In the examples below, we use factor graphs to model more complex MAP inference
problems in robotics.
\end_layout
\begin_layout Section
\begin_inset CommandInset label
LatexCommand label
name "sec:Robot-Localization"
\end_inset
Modeling Robot Motion
\end_layout
\begin_layout Subsection
Modeling with Factor Graphs
\end_layout
\begin_layout Standard
Before diving into a SLAM example, let us consider the simpler problem of
modeling robot motion.
This can be done with a
\emph on
continuous
\emph default
Markov chain, and provides a gentle introduction to GTSAM.
\end_layout
\begin_layout Standard
\begin_inset Float figure
placement h
wide false
sideways false
status open
\begin_layout Plain Layout
\align center
\begin_inset Graphics
filename images/FactorGraph.pdf
scale 80
BoundingBox 40bp 585bp 300bp 625bp
clip
\end_inset
\begin_inset Caption Standard
\begin_layout Plain Layout
\begin_inset CommandInset label
LatexCommand label
name "fig:OdometryFG"
\end_inset
Factor graph for robot localization.
\end_layout
\end_inset
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset ERT
status open
\begin_layout Plain Layout
\backslash
noindent
\end_layout
\end_inset
The factor graph for a simple example is shown in Figure
\begin_inset CommandInset ref
LatexCommand ref
reference "fig:OdometryFG"
\end_inset
.
There are three variables
\begin_inset Formula $x_{1}$
\end_inset
,
\begin_inset Formula $x_{2}$
\end_inset
, and
\begin_inset Formula $x_{3}$
\end_inset
which represent the poses of the robot over time, rendered in the figure
by the open-circle variable nodes.
In this example, we have one
\series bold
unary factor
\series default
\begin_inset Formula $f_{0}(x_{1})$
\end_inset
on the first pose
\begin_inset Formula $x_{1}$
\end_inset
that encodes our prior knowledge about
\begin_inset Formula $x_{1}$
\end_inset
, and two
\series bold
binary factors
\series default
that relate successive poses, respectively
\begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$
\end_inset
and
\begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$
\end_inset
, where
\begin_inset Formula $o_{1}$
\end_inset
and
\begin_inset Formula $o_{2}$
\end_inset
represent odometry measurements.
\end_layout
\begin_layout Subsection
Creating a Factor Graph
\end_layout
\begin_layout Standard
The following C++ code, included in GTSAM as an example, creates the factor
graph in Figure
\begin_inset CommandInset ref
LatexCommand ref
reference "fig:OdometryFG"
\end_inset
:
\end_layout
\begin_layout Standard
\begin_inset CommandInset include
LatexCommand lstinputlisting
filename "Code/OdometryExample.cpp"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/OdometryExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:OdometryExample},language={C++},numbers=left"
\end_inset
\end_layout
\begin_layout Standard
\begin_inset ERT
status open
\begin_layout Plain Layout
\backslash
noindent
\end_layout
\end_inset
Above, line 2 creates an empty factor graph.
We then add the factor
\begin_inset Formula $f_{0}(x_{1})$
\end_inset
on lines 5-8 as an instance of
\series bold
\emph on
PriorFactor<T>
\series default
\emph default
, a templated class provided in the slam subfolder, with
\series bold
\emph on
T=Pose2
\series default
\emph default
.
Its constructor takes a variable
\series bold
\emph on
Key
\series default
\emph default
(in this case 1), a mean of type
\series bold
\emph on
Pose2,
\series default
\emph default
created on Line 5, and a noise model for the prior density.
We provide a diagonal Gaussian of type
\series bold
\emph on
noiseModel::Diagonal
\series default
\emph default
by specifying three standard deviations in line 7, respectively 30 cm.
\begin_inset space ~
\end_inset
on the robot's position, and 0.1 radians on the robot's orientation.
Note that the
\series bold
\emph on
Sigmas
\series default
\emph default
constructor returns a shared pointer, anticipating that typically the same
noise models are used for many different factors.
\end_layout
\begin_layout Standard
Similarly, odometry measurements are specified as
\series bold
\emph on
Pose2
\series default
\emph default
on line 11, with a slightly different noise model defined on line 12-13.
We then add the two factors
\begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$
\end_inset
and
\begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$
\end_inset
on lines 14-15, as instances of yet another templated class,
\series bold
\emph on
BetweenFactor<T>
\series default
\emph default
, again with
\series bold
\emph on
T=Pose2
\series default
\emph default
.
\end_layout
\begin_layout Standard
When running the example (
\emph on
make OdometryExample.run
\emph default
on the command prompt), it will print out the factor graph as follows:
\family typewriter
\size small
\begin_inset CommandInset include
LatexCommand verbatiminput
filename "Code/OdometryOutput1.txt"
\end_inset
\end_layout
\begin_layout Subsection
Factor Graphs versus Values
\end_layout
\begin_layout Standard
At this point it is instructive to emphasize two important design ideas
underlying GTSAM:
\end_layout
\begin_layout Enumerate
The factor graph and its embodiment in code specify the joint probability
distribution
\begin_inset Formula $P(X|Z)$
\end_inset
over the
\emph on
entire
\emph default
trajectory
\begin_inset Formula $X\define\{x_{1},x_{2},x_{3}\}$
\end_inset
of the robot, rather than just the last pose.
This
\emph on
smoothing
\emph default
view of the world gives GTSAM its name:
\begin_inset Quotes eld
\end_inset
smoothing and mapping
\begin_inset Quotes erd
\end_inset
.
Later in this document we will talk about how we can also use GTSAM to
do filtering (which you often do
\emph on
not
\emph default
want to do) or incremental inference (which we do all the time).
\end_layout
\begin_layout Enumerate
A factor graph in GTSAM is just the specification of the probability density
\begin_inset Formula $P(X|Z)$
\end_inset
, and the corresponding
\series bold
\emph on
FactorGraph
\series default
\emph default
class and its derived classes do not ever contain a
\begin_inset Quotes eld
\end_inset
solution
\begin_inset Quotes erd
\end_inset
.
Rather, there is a separate type
\series bold
\emph on
Values
\series default
\emph default
that is used to specify specific values for (in this case)
\begin_inset Formula $x_{1}$
\end_inset
,
\begin_inset Formula $x_{2}$
\end_inset
, and
\begin_inset Formula $x_{3}$
\end_inset
, which can then be used to evaluate the probability (or, more commonly,
the error) associated with particular values.
\end_layout
\begin_layout Standard
The latter point is often a point of confusion with beginning users of GTSAM.
It helps to remember that when designing GTSAM we took a functional approach
of classes corresponding to mathematical objects, which are usually immutable.
You should think of a factor graph as a
\emph on
function
\emph default
to be applied to values -as the notation
\begin_inset Formula $f(X)\propto P(X|Z)$
\end_inset
implies- rather than as an object to be modified.
\end_layout
\begin_layout Subsection
Non-linear Optimization in GTSAM
\end_layout
\begin_layout Standard
The listing below creates a
\series bold
\emph on
Values
\series default
\emph default
instance, and uses it as the initial estimate to find the maximum a-posteriori
(MAP) assignment for the trajectory
\begin_inset Formula $X$
\end_inset
:
\end_layout
\begin_layout Standard
\begin_inset CommandInset include
LatexCommand lstinputlisting
filename "Code/OdometryOptimize.cpp"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/OdometryExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:OdometryOptimize},language={C++},numbers=left"
\end_inset
\end_layout
\begin_layout Standard
\begin_inset ERT
status open
\begin_layout Plain Layout
\backslash
noindent
\end_layout
\end_inset
Lines 2-5 in Listing
\begin_inset CommandInset ref
LatexCommand ref
reference "listing:OdometryOptimize"
\end_inset
create the initial estimate, and on line 8 we create a non-linear Levenberg-Mar
quardt style optimizer, and call
\series bold
\emph on
optimize
\series default
\emph default
using default parameter settings.
The reason why GTSAM needs to perform non-linear optimization is because
the odometry factors
\begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$
\end_inset
and
\begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$
\end_inset
are non-linear, as they involve the orientation of the robot.
This also explains why the factor graph we created in Listing
\begin_inset CommandInset ref
LatexCommand ref
reference "listing:OdometryExample"
\end_inset
is of type
\series bold
\emph on
NonlinearFactorGraph
\series default
\emph default
.
The optimization class linearizes this graph, possibly multiple times,
to minimize the non-linear squared error specified by the factors.
\end_layout
\begin_layout Standard
The relevant output from running the example is as follows:
\family typewriter
\size small
\begin_inset CommandInset include
LatexCommand verbatiminput
filename "Code/OdometryOutput2.txt"
\end_inset
\end_layout
\begin_layout Standard
\begin_inset ERT
status open
\begin_layout Plain Layout
\backslash
noindent
\end_layout
\end_inset
It can be seen that, subject to very small tolerance, the ground truth
solution
\begin_inset Formula $x_{1}=(0,0,0)$
\end_inset
,
\begin_inset Formula $x_{2}=(2,0,0)$
\end_inset
, and
\begin_inset Formula $x_{3}=(4,0,0)$
\end_inset
is recovered.
\end_layout
\begin_layout Subsection
\begin_inset CommandInset label
LatexCommand label
name "sub:Full-Posterior-Inference"
\end_inset
Full Posterior Inference
\end_layout
\begin_layout Standard
GTSAM can also be used to calculate the covariance matrix for each pose
after incorporating the information from all measurements
\begin_inset Formula $Z$
\end_inset
.
Recognizing that the factor graph encodes the
\series bold
posterior density
\series default
\begin_inset Formula $P(X|Z)$
\end_inset
, the mean
\begin_inset Formula $\mu$
\end_inset
together with the covariance
\begin_inset Formula $\Sigma$
\end_inset
for each pose
\begin_inset Formula $x$
\end_inset
approximate the
\series bold
marginal posterior density
\series default
\begin_inset Formula $P(x|Z)$
\end_inset
.
Note that this is just an approximation, as even in this simple case the
odometry factors are actually non-linear in their arguments, and GTSAM
only computes a Gaussian approximation to the true underlying posterior.
\end_layout
\begin_layout Standard
The following C++ code will recover the posterior marginals:
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={C++},numbers=left,caption={Excerpt from examples/OdometryExample.cpp},label={listing:OdometryMarginals}"
is a (possibly nonlinear) measurement function, and
\begin_inset Formula $\Sigma$
\end_inset
is the noise covariance.
Note that
\begin_inset Formula $m$
\end_inset
is considered
\emph on
known
\emph default
above, and the likelihood
\begin_inset Formula $L(q;m)$
\end_inset
\begin_inset Note Note
status open
\begin_layout Plain Layout
of
\begin_inset Formula $q$
\end_inset
given
\begin_inset Formula $m$
\end_inset
\end_layout
\end_inset
will only ever be evaluated as a function of
\begin_inset Formula $q$
\end_inset
, which explains why it is a unary factor
\begin_inset Formula $f(q)$
\end_inset
.
It is always the unknown variable
\begin_inset Formula $q$
\end_inset
that is either likely or unlikely, given the measurement.
\end_layout
\begin_layout Standard
\series bold
Note:
\series default
many people get this backwards, often misled by the conditional density
notation
\begin_inset Formula $P(m|q)$
\end_inset
.
In fact, the likelihood
\begin_inset Formula $L(q;m)$
\end_inset
is
\emph on
defined
\emph default
as any function of
\begin_inset Formula $q$
\end_inset
proportional to
\begin_inset Formula $P(m|q)$
\end_inset
.
\end_layout
\begin_layout Standard
Listing
\begin_inset CommandInset ref
LatexCommand vref
reference "listing:LocalizationFactor"
\end_inset
shows an example on how to define the custom factor class
\series bold
\emph on
UnaryFactor
\series default
\emph default
which implements a
\begin_inset Quotes eld
\end_inset
GPS-like
\begin_inset Quotes erd
\end_inset
measurement likelihood:
\end_layout
\begin_layout Standard
\begin_inset CommandInset include
LatexCommand lstinputlisting
filename "Code/LocalizationFactor.cpp"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/LocalizationExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:LocalizationFactor},language={C++},numbers=left"
\end_inset
\end_layout
\begin_layout Standard
\begin_inset ERT
status open
\begin_layout Plain Layout
\backslash
noindent
\end_layout
\end_inset
In defining the derived class on line 1, we provide the template argument
\series bold
\emph on
Pose2
\series default
\emph default
to indicate the type of the variable
\begin_inset Formula $q$
\end_inset
, whereas the measurement is stored as the instance variables
\series bold
\emph on
mx_
\series default
\emph default
and
\series bold
\emph on
my_
\series default
\emph default
, defined on line 2.
The constructor on lines 5-6 simply passes on the variable key
\begin_inset Formula $j$
\end_inset
and the noise model to the superclass, and stores the measurement values
provided.
The most important function to has be implemented by every factor class
is
\series bold
\emph on
evaluateError
\series default
\emph default
, which should return
\begin_inset Formula
\[
E(q)\define h(q)-m
\]
\end_inset
which is done on line 12.
Importantly, because we want to use this factor for nonlinear optimization
(see e.g.,
\begin_inset CommandInset citation
LatexCommand citealt
key "Dellaert06ijrr"
\end_inset
for details), whenever the optional argument
\begin_inset Formula $H$
\end_inset
is provided, a
\series bold
\emph on
Matrix
\series default
\emph default
reference, the function should assign the
\series bold
Jacobian
\series default
of
\begin_inset Formula $h(q)$
\end_inset
to it, evaluated at the provided value for
\begin_inset Formula $q$
\end_inset
.
This is done for this example on line 11.
In this case, the Jacobian of the 2-dimensional function
\begin_inset Formula $h$
\end_inset
, which just returns the position of the robot,
\begin_inset Formula
\[
h(q)=\left[\begin{array}{c}
q_{x}\\
q_{y}
\end{array}\right]
\]
\end_inset
with respect the 3-dimensional pose
\begin_inset Formula $q=\left(q_{x},q_{y},q_{\theta}\right)$
\end_inset
, yields the following simple
\begin_inset Formula $2\times3$
\end_inset
matrix:
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
H=\left[\begin{array}{ccc}
1 & 0 & 0\\
0 & 1 & 0
\end{array}\right]
\]
\end_inset
\end_layout
\begin_layout Subsection
Using Custom Factors
\end_layout
\begin_layout Standard
The following C++ code fragment illustrates how to create and add custom
factors to a factor graph:
\end_layout
\begin_layout Standard
\begin_inset CommandInset include
LatexCommand lstinputlisting
filename "Code/LocalizationExample2.cpp"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/LocalizationExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:LocalizationExample2},language={C++},numbers=left"
\end_inset
\begin_inset ERT
status open
\begin_layout Plain Layout
\backslash
noindent
\end_layout
\end_inset
In Listing
\begin_inset CommandInset ref
LatexCommand vref
reference "listing:LocalizationExample2"
\end_inset
, we create the noise model on line 2-3, which now specifies two standard
deviations on the measurements
\begin_inset Formula $m_{x}$
\end_inset
and
\begin_inset Formula $m_{y}$
\end_inset
.
On lines 4-6 we create
\series bold
\emph on
shared_ptr
\series default
\emph default
versions of three newly created
\series bold
\emph on
UnaryFactor
\series default
\emph default
instances, and add them to graph.
GTSAM uses shared pointers to refer to factors in factor graphs, and
\series bold
\emph on
boost::make_shared
\series default
\emph default
is a convenience function to simultaneously construct a class and create
a
\series bold
\emph on
shared_ptr
\series default
\emph default
to it.
\begin_inset Note Note
status collapsed
\begin_layout Plain Layout
and on lines 4-6 we add three newly created
\series bold
\emph on
UnaryFactor
\series default
\emph default
instances to the graph.
\end_layout
\end_inset
We obtain the factor graph from Figure
\begin_inset CommandInset ref
LatexCommand vref
reference "fig:LocalizationFG"
\end_inset
.
\family typewriter
\size small
\begin_inset Note Note
status collapsed
\begin_layout Plain Layout
The relevant output from running the example is as follows:
\family typewriter
\size small
\begin_inset CommandInset include
LatexCommand verbatiminput
filename "Code/LocalizationOutput4.txt"
\end_inset
\end_layout
\end_inset
\end_layout
\begin_layout Subsection
Full Posterior Inference
\end_layout
\begin_layout Standard
The three GPS factors are enough to fully constrain all unknown poses and
tie them to a
\begin_inset Quotes eld
\end_inset
global
\begin_inset Quotes erd
\end_inset
reference frame, including the three unknown orientations.
If not, GTSAM would have exited with a singular matrix exception.
The marginals can be recovered exactly as in Section
\begin_inset CommandInset ref
LatexCommand ref
reference "sub:Full-Posterior-Inference"
\end_inset
, and the solution and marginal covariances are now given by the following:
\size footnotesize
\begin_inset CommandInset include
LatexCommand verbatiminput
filename "Code/LocalizationOutput5.txt"
\end_inset
\end_layout
\begin_layout Standard
Comparing this with the covariance matrices in Section
\begin_inset CommandInset ref
LatexCommand ref
reference "sub:Full-Posterior-Inference"
\end_inset
, we can see that the uncertainty no longer grows without bounds as measurement
uncertainty accumulates.
Instead, the
\begin_inset Quotes eld
\end_inset
GPS
\begin_inset Quotes erd
\end_inset
measurements more or less constrain the poses evenly, as expected.
\end_layout
\begin_layout Standard
\begin_inset Float figure
placement h
wide false
sideways false
status open
\begin_layout Plain Layout
\align center
\begin_inset Float figure
wide false
sideways false
status open
\begin_layout Plain Layout
\align center
\begin_inset Graphics
filename images/Odometry.pdf
width 80text%
BoundingBox 70bp 310bp 525bp 500bp
clip
\end_inset
\end_layout
\begin_layout Plain Layout
\begin_inset Caption Standard
\begin_layout Plain Layout
Odometry marginals
\end_layout
\end_inset
\end_layout
\end_inset
\end_layout
\begin_layout Plain Layout
\align center
\begin_inset space ~
\end_inset
\end_layout
\begin_layout Plain Layout
\align center
\begin_inset Float figure
wide false
sideways false
status open
\begin_layout Plain Layout
\align center
\begin_inset Graphics
filename images/Localization.pdf
width 80text%
BoundingBox 70bp 310bp 525bp 500bp
clip
\end_inset
\end_layout
\begin_layout Plain Layout
\begin_inset Caption Standard
\begin_layout Plain Layout
Localization Marginals
\end_layout
\end_inset
\end_layout
\end_inset
\begin_inset Caption Standard
\begin_layout Plain Layout
\begin_inset CommandInset label
LatexCommand label
name "fig:CompareMarginals"
\end_inset
Comparing the marginals resulting from the
\begin_inset Quotes eld
\end_inset
odometry
\begin_inset Quotes erd
\end_inset
factor graph in Figure
\begin_inset CommandInset ref
LatexCommand ref
reference "fig:OdometryFG"
\end_inset
and the
\begin_inset Quotes eld
\end_inset
localization
\begin_inset Quotes erd
\end_inset
factor graph in Figure
\begin_inset CommandInset ref
LatexCommand ref
reference "fig:LocalizationFG"
\end_inset
.
\end_layout
\end_inset
\end_layout
\end_inset
\end_layout
\begin_layout Standard
It helps a lot when we view this graphically, as in Figure
\begin_inset CommandInset ref
LatexCommand ref
reference "fig:CompareMarginals"
\end_inset
, where I show the marginals on position as covariance ellipses that contain
68.26% of all probability mass.
For the odometry marginals, it is immediately apparent from the figure
that (1) the uncertainty on pose keeps growing, and (2) the uncertainty
on angular odometry translates into increasing uncertainty on y.
The localization marginals, in contrast, are constrained by the unary factors
and are all much smaller.
In addition, while less apparent, the uncertainty on the middle pose is
actually smaller as it is constrained by odometry from two sides.
\end_layout
\begin_layout Standard
You might now be wondering how we produced these figures.
The answer is via the MATLAB interface of GTSAM, which we will demonstrate
in the next section.
\end_layout
\begin_layout Standard
\begin_inset Newpage pagebreak
\end_inset
\end_layout
\begin_layout Section
\begin_inset CommandInset label
LatexCommand label
name "sec:Pose2SLAM"
\end_inset
\begin_inset CommandInset label
LatexCommand label
name "sec:WithMarginals"
\end_inset
PoseSLAM
\end_layout
\begin_layout Subsection
Loop Closure Constraints
\end_layout
\begin_layout Standard
The simplest instantiation of a SLAM problem is
\series bold
PoseSLAM
\series default
, which avoids building an explicit map of the environment.
The goal of SLAM is to simultaneously localize a robot and map the environment
given incoming sensor measurements
\begin_inset CommandInset citation
LatexCommand citep
key "DurrantWhyte06ram"
\end_inset
.
Besides wheel odometry, one of the most popular sensors for robots moving
on a plane is a 2D laser-range finder, which provides both odometry constraints
between successive poses, and loop-closure constraints when the robot re-visits
a previously explored part of the environment.
\end_layout
\begin_layout Standard
\begin_inset Float figure
placement h
wide false
sideways false
status open
\begin_layout Plain Layout
\align center
\begin_inset Graphics
filename images/FactorGraph3.pdf
scale 80
BoundingBox 40bp 585bp 330bp 710bp
clip
\end_inset
\begin_inset Caption Standard
\begin_layout Plain Layout
\begin_inset CommandInset label
LatexCommand label
name "fig:Pose2SLAM"
\end_inset
Factor graph for PoseSLAM.
\end_layout
\end_inset
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset ERT
status open
\begin_layout Plain Layout
\backslash
noindent
\end_layout
\end_inset
A factor graph example for PoseSLAM is shown in Figure
\begin_inset CommandInset ref
LatexCommand ref
reference "fig:Pose2SLAM"
\end_inset
.
The following C++ code, included in GTSAM as an example, creates this factor
graph in code:
\end_layout
\begin_layout Standard
\begin_inset CommandInset include
LatexCommand lstinputlisting
filename "Code/Pose2SLAMExample.cpp"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},caption={Excerpt from examples/Pose2SLAMExample.cpp},captionpos=b,frame=single,identifierstyle={\\bfseries},label={listing:Pose2SLAMExample},language={C++},numbers=left"
\end_inset
\end_layout
\begin_layout Standard
\begin_inset ERT
status open
\begin_layout Plain Layout
\backslash
noindent
\end_layout
\end_inset
As before, lines 1-4 create a nonlinear factor graph and add the unary
factor
\begin_inset Formula $f_{0}(x_{1})$
\end_inset
.
As the robot travels through the world, it creates binary factors
\begin_inset Formula $f_{t}(x_{t},x_{t+1})$
\end_inset
corresponding to odometry, added to the graph in lines 6-12 (Note that
M_PI_2 refers to pi/2).
But line 15 models a different event: a
\series bold
loop closure
\series default
.
For example, the robot might recognize the same location using vision or
a laser range finder, and calculate the geometric pose constraint to when
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/Pose2SLAMExample.m},label={listing:Pose2SLAMExample-MATLAB}"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/Pose2SLAMExample\\_graph.m},label={listing:Pose2SLAMExample-graph}"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/Pose3SLAMExample\\_graph.m},label={listing:Pose3SLAMExample-graph-1}"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/PlanarSLAMExample.m},label={listing:PlanarSLAMExample}"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={Matlab},numbers=left,caption={Excerpt from matlab/gtsam\\_examples/SFMExample.m},label={listing:SFMExample}"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={C++},numbers=left,caption={Excerpt from examples/VisualISAMExample.cpp},label={listing:iSAMExample}"
