GTSAM Tutorial学习笔记
Posted 超小黑
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了GTSAM Tutorial学习笔记相关的知识，希望对你有一定的参考价值。
GTSAM Tutorial学习笔记
GTSAM Tutorial学习笔记
为了学习LIO-SAM，我快速过了一遍《机器人感知：因子图在SLAM中的应用》以及董靖大佬在泡泡机器人分享的《GTSAM Tutorial》，本博客内容主要是《GTSAM Tutorial》的学习笔记，并对GTSAM在LIO-SAM中的实际应用进行一些简单分析，如果对GTSAM有基本了解的同学可以直接跳到第三部分。
1. 基本原理
在下图是一个典型SLAM场景 以上是关于GTSAM Tutorial学习笔记的主要内容，如果未能解决你的问题，请参考以下文章 Python 2.7.8 学习笔记（001）python manuals/the python tutorial Python 2.7.8 学习笔记（002）python manuals/the python tutorial -- 1. Whetting Your Appetite 深度学习 Deep LearningUFLDL 最新Tutorial 学习笔记 2：Logistic Regression 深度学习 Deep Learning UFLDL 最新Tutorial 学习笔记 4：Debugging: Gradient Checking
其中，机器人对特征点的观测可以构建为如上左图所示的一个贝叶斯网络，该贝叶斯网络中
x
i
x_i
xi为机器人状态，
z
i
z_i
zi为机器人观测值，
l
i
l_i
li为特征点。这样一个贝叶斯网络的联合分布概率可以通过如下公式表示：
P
(
X
,
L
,
Z
)
=
P
(
x
0
)
∏
i
=
1
M
P
(
x
i
∣
x
i
−
1
,
u
i
)
∏
k
=
1
K
P
(
z
k
∣
x
i
k
,
l
j
k
)
P(X, L, Z)=P\\left(x_0\\right) \\prod_i=1^M P\\left(x_i \\mid x_i-1, u_i\\right) \\prod_k=1^K P\\left(z_k \\mid x_i_k, l_j_k\\right)
P(X,L,Z)=P(x0)i=1∏MP(xi∣xi−1,ui)k=1∏KP(zk∣xik,ljk)其中，
P
(
x
0
)
P\\left(x_0\\right)
P(x0)为先验状态概率分布，
P
(
x
i
∣
x
i
−
1
,
u
i
)
P\\left(x_i \\mid x_i-1, u_i\\right)
P(xi∣xi−1,ui)表示已知状态
x
i
−
1
x_i-1
xi−1和控制量
u
i
u_i
ui分布的情况下，
x
i
\\boldsymbolx_i
xi的概率分布，具体为：
x
i
=
f
i
(
x
i
−
1
,
u
i
)
+
w
i
⇔
x_i=f_i\\left(x_i-1, u_i\\right)+w_i \\quad \\Leftrightarrow
xi=fi(xi−1,ui)+wi⇔
P
(
x
i
∣
x
i
−
1
,
u
i
)
∝
exp
−
1
2
∥
f
i
(
x
i
−
1
,
u
i
)
−
x
i
∥
Λ
i
2
P\\left(x_i \\mid x_i-1, u_i\\right) \\propto \\exp -\\frac12\\left\\|f_i\\left(x_i-1, u_i\\right)-x_i\\right\\|_\\Lambda_i^2
P(xi∣xi−1,ui)∝exp−21∥fi(xi−1,ui)−xi∥Λi2
P
(
z
k
∣
x
i
k
,
l
j
k
)
P\\left(z_k \\mid x_i_k, l_j_k\\right)
P(zk∣xik,ljk)标示已知状态
x
i
k
\\boldsymbolx_i_k
xik和
l
j
k
l_j_k
ljk的分布的情况下，
z
k
z_k
zk的概率分布，具体为：
z
k
=
h
k
(
x
i
k
,
l
j
k
)
+
v
k
⇔
z_k=h_k\\left(x_i_k, l_j_k\\right)+v_k \\quad \\Leftrightarrow
zk=hk(xik,ljk)+vk⇔
P
(
z
k
∣
x
i
k
,
l
j
k
)
∝
exp
−
1
2
∥
h
k
(
x
i
k
,
l
j
k
)
−
z
k
∥
Σ
k
2
P\\left(z_k \\mid x_i_k, l_j_k\\right) \\propto \\exp -\\frac12\\left\\|h_k\\left(x_i_k, l_j_k\\right)-z_k\\right\\|_\\Sigma_k^2
P(zk∣xik,ljk)∝exp−21∥hk(