如有错漏，请评论或者私信指出，感谢！！
GTSAM简介
GTSAM（Georgia Tech Smoothing and Mapping）是基于因子图的C++库，它由佐治亚理工学院的教授和学生们创造。它可以解决slam和sfm的问题，当然它也可以解决简单或者更加复杂的估计问题。
因子图是一种图模型，非常适合于复杂的估计问题的建模，比如SLAM或者是SFM( Structure from Motion)。
以下内容基于官方文档：https://gtsam.org/tutorials/intro.html#magicparlabel-65377
根据下面的例子，可以让你理解在程序中使用gtsam库时的基本步骤。读代码时能够了解代码含义（对于读代码来说勉强够用，要自己写的话还需要深入学习，建议阅读官方文档，比大部分教程好用，但是需要概率论与数理统计的基础知识）
下面，由一个简单例子（官方文档中的），来讲解一下gtsam库，对一个问题，从构建因子图–>初始化–>求解–>后处理的整个过程。
3自由度（x, y, θ \theta θ ）小车运动模型
现在想象有一个小车，在二维平面上运动，他有几个自由度呢？答案是三个，x方向，y方向以及旋转，这三个变量足够描述小车从一个位置到另一个位置的变换了，我们现在假定小车从初始位置x1，在x方向移动了两次，每次移动了两米来到了x3位置，当然这是我们假设的，实际情况我们不能直接知道其真实运动情况，只能通过里程计输出以及其前一状态等信息来估计位姿变换的值。这个过程可以用以下的图表示
o 1 o_1 o 1 和o 2 o_2 o 2 是运动过程中，里程计的测量值，变量x 1 , x 2 , x 3 x_1,x_2,x_3 x 1 ,x 2 ,x 3 是对应位置的位姿向量，特别注意的是，这里位姿包含 x , y , θ x,y,\theta x ,y ,θ三个变量的。构建成 因子图的表示方法，如下：
其中，一维因子f 0 ( x 1 ) f_0(x_1)f 0 (x 1 )是我们需要我们在初始化给定的，相当于一个先验，类似于非线性优化提供的初始值，初值的好坏在一定程度上决定了优化结果的好坏，在实际的SLAM程序中，有一些其他的方法可以给非线性优化提供初始值，这里不详细说了。同时，图中还有两个二维因子f i ( x i , x i + 1 ; o 1 ) i = 2 , 3 f_i(x_i,x_{i+1};o_1) i=2,3 f i (x i ,x i +1 ;o 1 )i =2 ,3，怎么区分这里的一维因子和二维因子呢？我不保证正确的认为，是和变量variable有关的，因子和几个变量有关，就是几维因子。
再更加直观的解释一下：上面图中的x 1 , x 2 , x 3 x_1,x_2,x_3 x 1 ,x 2 ,x 3 就是我们要优化的变量；f 0 ( x 1 ) f_0(x_1)f 0 (x 1 )，是我们的先验因子；f 1 ( x 1 , x 2 ; o 1 ) , f 2 ( x 2 , x 3 ; o 2 ) f_1(x_1,x_2;o_1), f_2(x_2,x_3;o_2)f 1 (x 1 ,x 2 ;o 1 ),f 2 (x 2 ,x 3 ;o 2 )，称为二维因子（二元因子），以f 1 ( x 1 , x 2 ; o 1 ) f_1(x_1,x_2;o_1)f 1 (x 1 ,x 2 ;o 1 )为例，o 1 o_1 o 1 表示机器人从x1运动到x2位置时，对应里程计的输出（可以称为一次观测），这个输出和x1和x2相关。
好了，根据上面的描述，我们将一个模型抽象成了因子图的模式，下面使用c++代码来处理求解上面这个问题？请你带着这个问题来阅读： 要求解什么呢？
- 第一步 创建一个因子图(实例化一个因子图)
//实例化一个空的因子图
NonlinearFactorGraph graph;
// 添加一个先验，上面提到的x1
Pose2 priorMean(0.0, 0.0, 0.0);//全部置0即可
//添加先验的噪声
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
//将一维因子先验及噪声添加到图中
graph.add(PriorFactor(1, priorMean, priorNoise));
// 添加二维因子
Pose2 odometry(2.0, 0.0, 0.0);//两次位姿变换都是x方向移动2m
noiseModel::Diagonal::shared_ptr odometryNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));//两次移动噪声相同
graph.add(BetweenFactor(1, 2, odometry, odometryNoise));//添加二维因子
graph.add(BetweenFactor(2, 3, odometry, odometryNoise));//添加二维因子
参数解释一下，其中的Pose2就是描述变换的数据类型；在添加因子的时候的数字为Key，指代的是第几个变量，二维因子在添加的时候，两个数字表示的是该二维因子相关的两个变量；上面的噪声项意思是在x,y方向上偏离20cm在旋转上偏离0.1弧度。 上面程序相当于构建了如下的一个因子图：
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];
- 因子图和求解 到这里，实际上我们只是构建了一个因子图模型，还不能够进行求解，这一点确实令人困惑。我们可以理解为，上述的模型（忽略噪声的话）是一个理想情况下，我们想让机器人移动的一个轨迹，实际求解位姿变量还需要我们使用GTSAM库提供的另一个类 Values，并且需要我们自己给每一个变量提供一个估计的初始值。 （这是我的理解，原文放在下面，大家可以自己理解一下）
At this point it is instructive to emphasize two important design ideas underlying GTSAM:
-
The factor graph and its embodiment in code specify the joint probability distribution P(X|Z)P(X|Z) over the entire trajectory X={x1,x2,x3}X={x1,x2,x3} of the robot, rather than just the last pose. This smoothing view of the world gives GTSAM its name: “smoothing and mapping”. Later in this document we will talk about how we can also use GTSAM to do filtering (which you often do not want to do) or incremental inference (which we do all the time).
-
A factor graph in GTSAM is just the specification of the probability density P(X|Z)P(X|Z), and the corresponding FactorGraph class and its derived classes do not ever contain a “solution”. Rather, there is a separate type Values that is used to specify specific values for (in this case) x1x1, x2x2, and x3x3, which can then be used to evaluate the probability (or, more commonly, the error) associated with particular values. 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 function to be applied to values -as the notation f(X)∝P(X|Z)f(X)∝P(X|Z) implies- rather than as an object to be modified.
-
第二步 求解 及 求解
// 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();
上面的代码很明了，我就不过多解释了。值得一提的是，这里的初始估计值（代码的3-5行），是我们故意编的与解相近的值， 更复杂时，我们可以想象，机器人继续向右运动（变量增多），添加估计值时，可以根据上一次优化求得的上一个位姿的值加上里程计的输出来估计当前位姿。 非线性优化结果：
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)
可以看到，结果和我们假设的真实结果x 1 ( 0 , 0 , 0 ) , x 2 ( 2 , 0 , 0 ) , x 3 ( 4 , 0 , 0 ) x_1(0,0,0),x_2(2,0,0),x_3(4,0,0)x 1 (0 ,0 ,0 ),x 2 (2 ,0 ,0 ),x 3 (4 ,0 ,0 )非常相近。
* 总结 上面虽然只用了两步就将问题解决了，实际上，可以分成三步
1. 构建因子图：包含因子的设定及噪声的设定
2. 输入预测值：为优化提供一个良好的初值
3. 非线性优化求解：利用gtsam给定的相应的求解器进行求解，不同的求解器需要设定的参数不同
* 再补充一点 : – ) GTSAM 还可以提供完整的后验推理，即整合来自所有的测量信息（在本例子中就是里程计测量信息）后，GTSAM可以计算每个变量（x 1 , x 2 , x 3 x_1,x_2,x_3 x 1 ,x 2 ,x 3 ）之间的协方差矩阵。需要注意的是，因子图对后验密度P(X|Z)进行了编码，每个估计位姿的均值以及协方差矩阵（方差）,近似于边缘后验概率P ( x i ∣ Z ) P(x_i|Z)P (x i ∣Z )，但是这只是一个估计值，因为非线性系统的原因。
// Query the marginals
cout.precision(2);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
输出:
x1 covariance:
0.09 1.1e-47 5.7e-33
1.1e-47 0.09 1.9e-17
5.7e-33 1.9e-17 0.01
x2 covariance:
0.13 4.7e-18 2.4e-18
4.7e-18 0.17 0.02
2.4e-18 0.02 0.02
x3 covariance:
0.17 2.7e-17 8.4e-18
2.7e-17 0.37 0.06
8.4e-18 0.06 0.03
相对抽象的数学推导
对上面的例子进行相对抽象的数学推导:
我们把里程计的输出称为观测记为Z ( o 1 , o 2 ) Z(o_1,o_2)Z (o 1 ,o 2 ),需要提醒的是，o 1 , o 2 o_1,o_2 o 1 ,o 2 本身也是向量。对应的位姿变量记为X ( x 1 , x 2 , x 3 ) X(x_1,x_2,x_3)X (x 1 ,x 2 ,x 3 )
我们做的是根据观测Z去求X，转换为状态估计问题，即是求相应的X使得在观测Z的情况下最大的P ( X ∣ Z ) P(X|Z)P (X ∣Z )，又由贝叶斯公式可得：
P ( X ∣ Z ) ∝ P ( Z ∣ X ) P ( X ) P(X|Z) \propto P(Z|X)P(X)P (X ∣Z )∝P (Z ∣X )P (X )
其中P(X）表示的先验，在实际问题中不存在，我们舍去，从而把上面的问题简化成了
X ∗ = a r g m a x ( x ) P ( Z ∣ X ) X^ = {argmax}(x) P(Z|X)X ∗=a r g m a x (x )P (Z ∣X )
即极大似然估计问题， 到这里，我们就知道了整个过程到底要干什么，下面我们讲一下怎么干的问题。*
Original: https://blog.csdn.net/weixin_45703465/article/details/123723712
Author: 山海里啊有星辰
Title: 【以例为引】gtsam简单入门（上）–理论和认识
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/651326/
转载文章受原作者版权保护。转载请注明原作者出处！