GTSAM 是一个在机器人领域和计算机视觉领域用于平滑（smoothing）和建图（mapping）的C++库。它与g2og2o不同的是，g2og2o采用稀疏矩阵的方式求解一个非线性优化问题，而GTSAM是采用因子图（factor graphs）和贝叶斯网络（Bayes networks）的方式最大化后验概率。
https://www.bilibili.com/video/av52770741?from=search&seid=1073002253619093972
可以在C++中使用
提供了C++转化 .M 文件的工具
提供了一些factor
可以自己定义factor
4.0提供了自动微分工具，可以求解链式微分
4.0提供了python中使用
在gtsam中优化李群这种非 空间的量
对李群变量所在的流形上操作。
学习笔记：https://blog.csdn.net/qq_27262241/article/details/84291471
视觉惯性里程计 VIO - Visual Inertial Odometry 视觉−惯性导航融合SLAM方案
https://blog.csdn.net/xiaoxiaowenqiang/article/details/81192045
slam问题可以看做是一个贝叶斯网络的问题。
根据各个观测值估计机器人的位置、姿态和和路标点的位置。
第一个问题，这个观测值是如何转化为概率的，还是观测值和概率之间的关系问题。
1.build factor graph
2.give initial values(this is a little bit tricky and highly application-related, design your strategybased on your application!)
3.optimize
4.optional post process,like caculate marginal distributions.
/**
* @file Pose2SLAMExample.cpp
* @brief 2D SLAM example
* @date Nov 7, 2016
* @author Jing Dong
*/
/**
* A simple 2D pose-graph SLAM
* The robot moves from x1 to x5, with odometry information between each pair.
* the robot moves 5 each step, and makes 90 deg right turns at x3 - x5
* At x5, there is a *loop closure* between x2 is avaible
* The graph strcuture is shown:
*
* p-x1 - x2 - x3
* | |
* x5 - x4
*/
// In planar cases we use Pose2 variables (x, y, theta) to represent the robot poses in SE(2)
#include <gtsam/geometry/Pose2.h>
// class for factor graph, a container of various factors
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// class for graph nodes values, a container of various geometric types
// here Values is used as a container of SE(2)
#include <gtsam/nonlinear/Values.h>
// symbol class is used to index varible in values
// e.g. pose varibles are generally indexed as 'x' + number, and landmarks as 'l' + numbers
#include <gtsam/inference/Symbol.h>
// Factors used in this examples
// PriorFactor gives the prior distribution over a varible
// BetweenFactor gives odometry constraints
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// optimizer class, here we use Gauss-Newton
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the
// (appoximated / linearized) marginal covariance of desired variables
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Create a factor graph container
NonlinearFactorGraph graph;
// 添加 初始位置的factor
//Add a prior on the first pose, setting it to the origin
// The prior is needed to fix/align the whole trajectory at world frame
// A prior factor consists of a mean value and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Sigmas(Vector3(1.0, 1.0, 0.1));
graph.add(PriorFactor<Pose2>(Symbol('x', 1), Pose2(0, 0, 0), priorModel));
// odometry measurement noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr odomModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));
// Pose2(5, 0, 0) 表示 是 两个状态之间的转化（里程计）
// Add odometry factors
// Create odometry (Between) factors between consecutive poses
// robot makes 90 deg right turns at x3 - x5
graph.add(BetweenFactor<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(5, 0, 0), odomModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(5, 0, -M_PI_2), odomModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 3), Symbol('x', 4), Pose2(5, 0, -M_PI_2), odomModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 4), Symbol('x', 5), Pose2(5, 0, -M_PI_2), odomModel));
// loop closure measurement noise model
noiseModel::Diagonal::shared_ptr loopModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));
// Add the loop closure constraint
graph.add(BetweenFactor<Pose2>(Symbol('x', 5), Symbol('x', 2), Pose2(5, 0, -M_PI_2), loopModel));
// print factor graph
graph.print("\nFactor Graph:\n");
//设置图的初始值 ，
// initial varible values for the optimization
// add random noise from ground truth values
Values initials;
initials.insert(Symbol('x', 1), Pose2(0.2, -0.3, 0.2));
initials.insert(Symbol('x', 2), Pose2(5.1, 0.3, -0.1));
initials.insert(Symbol('x', 3), Pose2(9.9, -0.1, -M_PI_2 - 0.2));
initials.insert(Symbol('x', 4), Pose2(10.2, -5.0, -M_PI + 0.1));
initials.insert(Symbol('x', 5), Pose2(5.1, -5.1, M_PI_2 - 0.1));
// print initial values
initials.print("\nInitial Values:\n");
// Use Gauss-Newton method optimizes the initial values
GaussNewtonParams parameters;
// print per iteration
parameters.setVerbosity("ERROR");
// optimize!
GaussNewtonOptimizer optimizer(graph, initials, parameters);
Values results = optimizer.optimize();
// print final values
results.print("Final Result:\n");
//求解边界分别并画出协方差矩阵
// Calculate marginal covariances for all poses
Marginals marginals(graph, results);
// print marginal covariances
cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
cout << "x4 covariance:\n" << marginals.marginalCovariance(Symbol('x', 4)) << endl;
cout << "x5 covariance:\n" << marginals.marginalCovariance(Symbol('x', 5)) << endl;
return 0;
}