文章目录
一. 总览
GTSAM(Georgia Tech Smoothing and Mapping)是基于因子图的C++库,它由佐治亚理工学院的教授和学生们创造。它可以解决slam和sfm的问题,当然它也可以解决简单或者更加复杂的估计问题。
因子图是一种图模型,非常适合于复杂的估计问题的建模,比如SLAM或者是SFM( Structure from Motion)。贝叶斯网络属于一种无环图模型,如果对贝叶斯网络感兴趣可以看本人写过的一篇文章。
参考网站:
官网教程:https://gtsam.org/tutorials/intro.html
github: https://github.com/borglab/gtsam
iSAM1:http://www.cs.cmu.edu/~kaess/pub/Kaess08tro.pdf
iSAM2:https://www.cs.cmu.edu/~kaess/pub/Kaess12ijrr.pdf
因子图的三个基本组成部分:
- 因子图(factor graph):它属于一个二分图,由因子和变量连接而成。
- 变量(variables):估计问题中的未知随机变量。
- 因子(factors):非线性因子表示变量之间的约束,在slam中,可能为landmark或者odometry的读数。
二. 贝叶斯网络和因子图
- 贝叶斯网络
我们下面先来讲解一个贝叶斯网络例子:
上图的贝叶斯网络由三步的隐马尔可夫模型组成。上面的马尔科夫链由一个先验概率和两个转移概率组成:
P
(
X
1
)
,
P
(
X
2
∣
X
1
)
,
P
(
X
3
∣
X
2
)
P(X_1),P(X_2|X_1),P(X_3|X_2)
P(X1),P(X2∣X1),P(X3∣X2)。给定一组测量值
z
1
,
z
2
,
z
3
z_1,z_2,z_3
z1,z2,z3,则
X
1
,
X
2
,
X
3
X_1,X_2,X_3
X1,X2,X3的后验概率最大的表达为:
P
(
X
1
,
X
2
,
X
3
∣
Z
1
=
z
1
,
Z
2
=
z
2
,
Z
3
=
z
3
)
P(X_1,X_2,X_3|Z_1=z_1,Z_2=z_2,Z_3=z_3)
P(X1,X2,X3∣Z1=z1,Z2=z2,Z3=z3)。由贝叶斯法则可知,最大化后验概率正比于似然估计与先验概率的乘积,即:
P
(
X
∣
Z
)
=
P
(
Z
∣
X
)
(
P
X
)
P
(
Z
)
∝
P
(
Z
∣
X
)
P
(
X
)
P(X|Z) = \frac{P(Z|X)(PX)}{P(Z)} ∝P(Z|X)P(X)
P(X∣Z)=P(Z)P(Z∣X)(PX)∝P(Z∣X)P(X)
其中P(Z|X)表示似然估计,P(X)表示先验概率。
在本例中,我们定义似然估计
L
(
X
t
;
z
)
∝
P
(
Z
t
=
z
∣
X
t
)
L(X_t;z) ∝ P(Z_t=z|X_t)
L(Xt;z)∝P(Zt=z∣Xt),所以上图中的最大似然估计:
P
(
X
1
,
X
2
,
X
3
∣
Z
1
,
Z
2
,
Z
3
)
∝
P
(
X
1
)
P
(
X
2
∣
X
1
)
P
(
X
3
∣
X
2
)
L
(
X
1
;
z
1
)
L
(
X
2
;
z
2
)
L
(
X
3
;
z
3
)
P ( X_1,X_2,X_3|Z_1,Z_2,Z_3 ) ∝ P(X_1)P(X_2|X_1)P(X_3|X_2)L(X_1;z_1)L(X_2;z_2)L(X3;z_3 )
P(X1,X2,X3∣Z1,Z2,Z3)∝P(X1)P(X2∣X1)P(X3∣X2)L(X1;z1)L(X2;z2)L(X3;z3)
- 因子图
上图中的因子图由三步的隐马尔可夫模型组成。其中未知变量(variables)为: X 1 , X 2 , X 3 X_1,X_2,X_3 X1,X2,X3。后验概率最大化模型:
f ( X 1 , X 2 , X 3 ) = ∏ f i ( χ i ) f(X_1,X_2,X_3) =∏f_i(\chi_i) f(X1,X2,X3)=∏fi(χi)
上图的因子图中黑块表示因子(factors),对于每一个因子 f i f_i fi,都依赖于变量。
三. 机器人运动建模
3.1 使用因子图建模
在深入slam之前,我们先来考虑以下一个马尔科夫链的机器人运动模型。
x
1
,
x
2
,
x
3
x_1,x_2,x_3
x1,x2,x3:三个变量,表示机器人随时间变化的位姿。
一元因子(unary factor):
f
0
(
x
1
)
f_0(x_1)
f0(x1),组成了我们的
x
1
x_1
x1的先验知识。
二元因子(binary factor):
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)
f1(x1,x2,o1),f2(x2,x3,o2),表示连续位姿的相关性。其中
o
1
,
o
2
o_1,o_2
o1,o2表示里程计的测量值。
3.2 建立因子图
上图中的因子图用GTSAM中的C++语言表示如下:
//创建一个空的非线性因子图
NonlinearFactorGraph graph;
// 添加一个高斯先验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));//对角线高斯模型0.3m,0.1弧度
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
//添加两个里程因子
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));
当运行这个例子时候(make OdometryExample.run),将会输出:
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.3 因子图与变量
GTSAM中的两个重要思想:
- 因子图体现在联合概率P(X|Z)中,而不仅仅是最新的位姿,其中X表示整个轨迹 { x 1 , x 2 , x 3 } \{x_1,x_2,x_3\} {x1,x2,x3}
- 在GTSAM中仅仅指明了概率密度P(X|Z),和派生出来的概念。
应该把因子图当作一个函数:f(X) ∝ P(X|Z).
3.4 GTSAM中的非线性优化
下面创建了一些变量(values),并且进行了初始估计来进行最大后验估计估计X。
// 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();
相应的输出为:
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)
3.5 全后验推论
GTSAM也可以用于计算每一个位姿的协方差矩阵,认识到因子图编码了后验概率P(X|Z),对于每个
x
x
x的均值
u
u
u和协方差
Σ
Σ
Σ,都有一个边缘后验概率弥补
P
(
x
∣
Z
)
P(x|Z)
P(x∣Z)。这仅仅是一个近似,因为最简单的里程计因子也是非线性的,GTSAM只计算高斯近似下的后验。
下面的代码将复原后验边缘概率。
// 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
需要注意的是,这里的协方差矩阵是由相对坐标给出的,而不是绝对值。
四. 机器人定位
4.1 一元测量因子
我们增加一个测量因子(unary measurement factors)帮助我们实时的进行机器人定位。
在第三部分的举例中,对实际的机器人并不实用,因为它只有里程计的测量因子。而在上图中,我们使用了一元测量因子来处理外部测量(省略了先验
f
0
(
x
1
)
f_0(x_1)
f0(x1)),这三个一元测量因子分别为:
f
1
(
x
1
;
z
1
)
,
f
2
(
x
2
;
z
2
)
,
f
3
(
x
3
,
z
3
)
f_1(x_1;z_1),f_2(x_2;z_2),f_3(x_3,z_3)
f1(x1;z1),f2(x2;z2),f3(x3,z3)。这里的
z
t
z_t
zt仅仅依赖于当前的机器人定位,可以是GPS、激光测距仪等。
4.2 自定义因子
在GTSAM中,我们使用NoiseModelFactor1<T>定义一元因子,它利用了高斯噪声模型:
L
(
q
;
m
)
=
e
x
p
{
−
1
2
∣
∣
h
(
q
)
−
m
∣
∣
Σ
2
}
=
f
(
q
)
L(q;m)=exp\{-\frac{1}{2}||h(q)-m||_Σ^2\}=f(q)
L(q;m)=exp{−21∣∣h(q)−m∣∣Σ2}=f(q)
其中:
m:测量值;q未知变量;h(q)表示测量函数。Σ表示噪声协方差。由上面的概念
L
(
q
;
m
)
L(q;m)
L(q;m)表示似然估计,它仅仅是q的估计,所以可以表示为
f
(
q
)
f(q)
f(q),实际上它就是
P
(
m
∣
q
)
P(m|q)
P(m∣q)。
下面举例怎么自定义一元因子(UnaryFactor),这里用到了GPS-like测量似然。
class UnaryFactor: public NoiseModelFactor1<Pose2> {
double mx_, my_; ///< X and Y measurements
public:
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const
{
if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
};
上式中evaluateError表示评价误差:
E
(
q
)
=
h
(
q
)
−
m
E(q) = h(q)-m
E(q)=h(q)−m,H表示
h
(
q
)
h(q)
h(q)的雅可比矩阵。在这个例子中
h
h
h表示二维函数:
h
(
q
)
=
[
q
x
q
y
]
h(q)=\begin{bmatrix}q_x\\q_y\end{bmatrix}
h(q)=[qxqy]
如果位姿是三维
q
=
(
q
x
,
q
y
,
q
θ
)
q= (q_x,q_y,q_\theta)
q=(qx,qy,qθ),那么它的雅可比矩阵为:
H
=
(
1
0
0
0
1
0
)
H = \begin{pmatrix}1&0&0\\0&1&0\end{pmatrix}
H=(100100)
4.3 使用自定义因子
下列代码片段描述了怎么创建和增加自定义因子到因子图中。
// add unary measurement factors, like GPS, on all three poses
noiseModel::Diagonal::shared_ptr unaryNoise =
noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
上面的代码中首先创建了一个噪声模型,指定了两个标准偏差 m x m_x mx和 m y m_y my。第4-6行中使用了自定义一元因子模型UnaryFactor,并且加入因子图中。
4.4 全后验推论
Final Result:
Values with 3 values:
Value 1: (-1.5e-14, 1.3e-15, -1.4e-16)
Value 2: (2, 3.1e-16, -8.5e-17)
Value 3: (4, -6e-16, -8.2e-17)
x1 covariance:
0.0083 4.3e-19 -1.1e-18
4.3e-19 0.0094 -0.0031
-1.1e-18 -0.0031 0.0082
x2 covariance:
0.0071 2.5e-19 -3.4e-19
2.5e-19 0.0078 -0.0011
-3.4e-19 -0.0011 0.0082
x3 covariance:
0.0083 4.4e-19 1.2e-18
4.4e-19 0.0094 0.0031
1.2e-18 0.0031 0.018
它与3.5中的全后验相比,这三个值并没有很大的突变。
五. PoseSLAM
5.1 闭环约束
除了轮子里程计之外,常用的测距仪由2D激光雷达,它提供了位姿之间的里程约束和闭环约束。
闭环约束的因子图如上,下面我们用代码来实现一下:
NonlinearFactorGraph graph;
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// Add odometry factors
noiseModel::Diagonal::shared_ptr model =
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// Add the loop closure constraint
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
代码中,创建了先验的一元因子 f 0 ( x 1 ) f_0(x_1) f0(x1),和四个随里程运动的二元因子 f t ( x t , x t + 1 ) f_t(x_t,x_{t+1}) ft(xt,xt+1)。最后还创建了一个闭环约束 f 5 ( x 5 , x 2 ) f_5(x_5,x_2) f5(x5,x2)。
5.2 使用Matlab接口
GTSAM中提供了matlab接口。
5.3 读取和优化姿态图
上图展示了曼哈顿的100个位姿图,初始位姿为绿色的,利用协方差优化后的为蓝色的。
代码为:
5.4 3D中的poseSLAM
GTSAM支持四元数(quaternions )和3*3旋转矩阵的形式。通过GTSAM_USE_QUATERNIONS进行选择。
3D中地图中绿色为里程计的初始估计,优化后的路径为红色。
以下也是matlab的代码:
%% Initialize graph, initial estimate, and odometry noise
datafile = findExampleDataFile('w100.graph');
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
[graph,initial] = load2D(datafile, model);
%% Add a Gaussian prior on pose x_0
priorMean = Pose2(0, 0, 0);
priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
graph.add(PriorFactorPose2(0, priorMean, priorNoise));
%% Optimize using Levenberg-Marquardt optimization and get marginals
optimizer = LevenbergMarquardtOptimizer(graph, initial);
result = optimizer.optimizeSafely;
marginals = Marginals(graph, result);
%% Initialize graph, initial estimate, and odometry noise
datafile = findExampleDataFile('sphere2500.txt');
model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
[graph,initial] = load3D(datafile, model, true, 2500);
plot3DTrajectory(initial, 'g-', false); % Plot Initial Estimate
%% Read again, now with all constraints, and optimize
graph = load3D(datafile, model, false, 2500);
graph.add(NonlinearEqualityPose3(0, initial.atPose3(0)));
optimizer = LevenbergMarquardtOptimizer(graph, initial);
result = optimizer.optimizeSafely();
plot3DTrajectory(result, 'r-', false); axis equal;
六. 基于Landmark的slam
6.1 基础
上图基于landmark的slam中,位姿是基于马尔可夫链,landmarks则是多个位姿下的观测值。
上图优化的结构中,绿色的椭圆为位姿,路标表示蓝色的协方差。matlab代码如下;
% Create graph container and add factors to it
graph = NonlinearFactorGraph;
% Create keys for variables
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2);
% Add prior
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
% add directly to graph
graph.add(PriorFactorPose2(i1, priorMean, priorNoise));
% Add odometry
odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
% Add bearing/range measurement factors
degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise));
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
6.2 Keys和Symbols
在GTSAM中我们使用Key类型表示变量(32或64位的整形)。symbol函数则可以创建一个大的keys。在上面的代码中我们使用’x’表示位姿,‘I’表示landmarks。
6.3 A Larger Example
上图中表示100个位姿和差不多30个landmarks(这个例子在PlanarSLAMExample_graph.m中)
6.4 A Real-World Example
上图是悉尼的维多利亚公园的数据集。蓝色的表示协方差(Kaess and Dellaert, 2009).,绿色是更早时候的算法 (Kaess et al., 2008).。
七. iSAM: Incremental Smoothing and Mapping
GTSAM中提供了iSAM和iSAM2算法,具体算法可以查看相关的论文(Kaess et al. (2008); Kaess et al. (2012) )。iSAM提供了高效的更新地图的方法。
下列代码位iSAM实例:
int relinearizeInterval = 3;
NonlinearISAM isam(relinearizeInterval);
// ... first frame initialization omitted ...
// Loop over the different poses, adding the observations to iSAM
for (size_t i = 1; i < poses.size(); ++i) {
// Add factors for each landmark observation
NonlinearFactorGraph graph;
for (size_t j = 0; j < points.size(); ++j) {
graph.add(
GenericProjectionFactor<Pose3, Point3, Cal3_S2>
(z[i][j], noise,Symbol('x', i), Symbol('l', j), K)
);
}
// Add an initial guess for the current pose
Values initialEstimate;
initialEstimate.insert(Symbol('x', i), initial_x[i]);
// Update iSAM with the new factors
isam.update(graph, initialEstimate);
}
这段代码是一个典型iSAM循环:
- 给新测量创建新的因子。GenericProjectionFactor<Pose3, Point3, Cal3_S2>。
- 给所有的新引进变量创建新的估计。initialEstimate.insert(Symbol(‘x’, i), initial_x[i]);
- 更新iSAM。isam.update(graph, initialEstimate);