Learn and use GTSAM official documents

Overview

Factor graph is a graphical model suitable for modeling complex estimation problems, such as SLAM and SFM problems. Factor graphs differ from Bayesian networks (directed acyclic graphs) in that factor graphs are bipartite graphs composed of factors connected to variables. Variables represent unknown random variables in an estimation problem, while factors represent probabilistic constraints on these variables derived from measurements or prior knowledge.

1. Factor diagram

Insert image description here
The above figure is a Bayesian network of a three-step Hidden Markov Model (HMM). In a Bayesian network, each node is associated with a conditional probability density: a Markov chain encodes the prior P(X1) and the conditional probabilities P(X2|X1) and P(X3|X2), and the observation Zt depends only on In state Xt, it is modeled as P(Zt|Xt) based on conditional probability. Given the known observations z1, z2, z3, we can maximize the posterior probability P(X1, , X2, > Note: My understanding is that the above formula The actual meaning of is that the observation z is most likely to be seen at that x. Maximize the posterior probability according to the following formula:
Insert image description here

The following figure shows a factor graph:

2. Robot motion modeling

1.Motion modeling

Insert image description here
The above figure shows a factor graph based on Markov chain, where x1, x2, x3 are the robot's pose variables, the black dots represent the factors, and o1, o2 are the measurement values of the odometer. (This is the motion equation of the robot, and IMU is usually used as the measurement of the motion equation in laser slam).

2. Create a simple factor graph

// Create an empty nonlinear factor graph
NonlinearFactorGraph graph;

// Add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));

// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

It will return a:

Factor Graph:
size: 3
Factor 0: PriorFactor on 1
  prior mean: (0, 0, 0)
  noise model: diagonal sigmas [0.3; 0.3; 0.1];
Factor 1: BetweenFactor(1,2)
  measured: (2, 0, 0)
  noise model: diagonal sigmas [0.2; 0.2; 0.1];
Factor 2: BetweenFactor(2,3)
  measured: (2, 0, 0)
  noise model: diagonal sigmas [0.2; 0.2; 0.1];

3.Omitted

4. Nonlinear optimization in GTSAM

Use NonlinearFactorGraph to store factors and Values to store variables in gtsam:

// create (deliberately inaccurate) initial estimate
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));

// optimize using Levenberg-Marquardt optimization
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

The above code will return the initial pose and the optimized pose:

Initial Estimate:
Values with 3 values:
Value 1: (0.5, 0, 0.2)
Value 2: (2.3, 0.1, -0.2)
Value 3: (4.1, 0.1, 0.1)

Final Result:
Values with 3 values:
Value 1: (-1.8e-16, 8.7e-18, -9.1e-19)
Value 2: (2, 7.4e-18, -2.5e-18)
Value 3: (4, -1.8e-18, -3.1e-18)

Reference link:https://gtsam.org/tutorials/intro.html