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
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}"
lstparams "aboveskip=10pt,basicstyle={\\ttfamily\\small},captionpos=b,frame=single,identifierstyle={\\bfseries},language={C++},numbers=left,caption={Excerpt from examples/LocalizationExample.cpp},label={listing:LocalizationFactor}"
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
, 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}"
