Factor Graphs and GTSAM

Factor Graphs and GTSAM:

A Hands-on Introduction

Frank Dellaert

Technical Report number GT-RIM-CP&R-2012-002

September 2012

Overview

In this document I provide a hands-on introduction to both factor graphs and GTSAM.

Factor graphs are graphical models (Koller and Friedman, 2009) that are well suited to mod-

many examples from both robotics and vision.

The GTSAM toolbox (GTSAM stands for “Georgia Tech Smoothing and Mapping”) toolbox is

a BSD-licensed C++ library based on factor graphs, developed at the Georgia Institute of Technol-

ogy by myself, many of my students, and collaborators. It provides state of the art solutions to the

SLAM and SFM problems, but can also be used to model and solve both simpler and more com-

plex estimation problems. It also provides a MATLAB interface which allows for rapid prototype

development, visualization, and user interaction.

GTSAM exploits sparsity to be computationally efficient. Typically measurements only provide

information on the relationship between a handful of variables, and hence the resulting factor graph

will be sparsely connected. This is exploited by the algorithms implemented in GTSAM to reduce

2 Modeling Robot Motion 4

2.1 Modeling with Factor Graphs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.2 Creating a Factor Graph . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.3 Factor Graphs versus Values . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.4 Non-linear Optimization in GTSAM . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.5 Full Posterior Inference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Robot Localization 8

3.1 Unary Measurement Factors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.2 Defining Custom Factors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.3 Using Custom Factors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

5 Landmark-based SLAM 18

5.1 Basics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5.2 Of Keys and Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

5.3 A Larger Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

5.4 A Real-World Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

6 Structure from Motion 22

7 More Applications 24

7.1 Conjugate Gradient Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

7.2 Visual Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

7.3 Visual SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

Figure 1 shows the Bayes network for a hidden Markov model (HMM) over three time steps.

In a Bayes net each node is associated with a conditional density: the top Markov chain encodes

Figure 2: An HMM with observed measurements, unrolled over time, represented as a factor graph.

This motivates a different graphical model, a factor graph, in which we only represent the un-

i.e., the value of the factor graph. It should be clear from the figure that the connectivity of a factor

it depends. In the examples below,

we use factor graphs to model more complex MAP inference problems in robotics.

3

2.1 Modeling with Factor Graphs

Before diving into a SLAM example, let us consider the simpler problem of modeling robot motion,

which can be done with a continuous Markov chain, and provides a gentle introduction to GTSAM.

, and two binary factors that relate successive poses, respectively

noise model for the prior density. We provide a diagonal Gaussian of type noiseModel::Diagonal

by specifying three standard deviations in line 7, respectively 30 cm. on the robot’s position, and

0.1 radians on the robot’s orientation. Note that the Sigmas constructor returns a shared pointer,

anticipating that typically the same noise models are used for many different factors.

Similarly, odometry measurements are specified as Pose2 on line 11, with a slightly different

lines 14-15, as instances of yet another templated class, BetweenFactor<T>, again with T=Pose2.

When running the example (make LocalizationExample.run on the command prompt), will

print out the factor graph as follows: