本周的目标是orb-slam2中的PoseOptimization函数，这个函数算是orb-slam2中Traking进程频繁调用且是入门级的优化问题。看代码的时候多次看到g2o库，这个库是以图优化为核心的优化库。
图优化并不是和高斯牛顿法一样的优化迭代法，其核心是使用图的形式来描述优化问题。
优化问题在上周对高斯-牛顿公式推导时已经接触，由目标函数、优化变量和约束组成，而在slam中很少具有有约束的优化问题（还没接触到），比如机器人的运动是无约束的（想怎么旋转平移都可以。优化问题可以表示为函数形式：
m
i
n
x
F
(
x
)
min_xF(x)
minxF(x)
x就是优化变量，
F
(
x
)
F(x)
F(x)是目标函数。优化问题可以理解为求出x的一个确切值使得
F
(
x
)
F(x)
F(x)求得最小值。从导数的角度看，F对x求导为0时可能会出现最小值。但在前一周的学习中可以得到一个结论：x的精确值无法求得，只能进行迭代，逐步逼近，当差值在可接受的范围内的时候，课近似得到x的值。
图是由顶点（Vertex）和边（Edge）组成的结构，表示为：G={V,E}其中V为顶点集，E为边集。
怎样去理解一个图
我们将上图看成机器人移动的俯视图，它首先向右移动再向下运动。换句话讲：顶点通过边的变化变成了另一个顶点，变量通过函数方程获得了结果。
对于PoseOptimization函数来说，其传入的最主要的三个参数为：相机位姿状态、观察到的点的二维坐标、对应点的三维点坐标。其函数的目标是根据二维点和三位点的坐标来计算机器人运动的位姿，也就是对位姿状态进行优化，点坐标不进行优化。
我们用图来表示这个问题，由二维点、三维点和估计方程构成了前一次和后一次位姿中间的边，位姿在三者的作用下校正了自己的位姿状态。所以在图上看也就是一个闭环，边多少取决于匹配的点有多少。
其实图优化能更好的去理解我们要解决的问题，知道了顶点和边是什么，也就知道了要解决的问题是什么。
二维点的观测方程可以表示为：
z
=
C
(
R
x
+
t
)
z = C ( R \mathbf{x} + t )
z=C(Rx+t)
C为相机内参，R,t为旋转和平移。
但是z不可能精确的等于后一项，于是存在误差项e
z
=
C
(
R
x
+
t
)
+
e
z = C ( R \mathbf{x} + t )+e
z=C(Rx+t)+e
于是乎误差函数
e
e
e就出现了，求x令其最小。
e
k
=
z
k
−
C
(
R
x
k
+
t
)
{e_k} = {z_k} - C\left( {R{x_k}+t} \right)
ek=zk−C(Rxk+t)
将目标函数改写成：
min
x
∑
k
=
1
n
e
k
(
x
k
,
z
k
)
T
Ω
k
e
k
(
x
k
,
z
k
)
\min\limits_{x} \sum\limits_{k = 1}^n {{e_k}{{\left( {{x_k},{z_k}} \right)}^T}{\Omega _k}{e_k}\left( {{x_k},{z_k}} \right)}
xmink=1∑nek(xk,zk)TΩkek(xk,zk)
e
k
{e_k}
ek表示为由x和z构成的函数，Ω 是信息矩阵，个人理解为：在传入的信息中可能可靠性不同，比如图片边缘的点往往不可靠，或者是orb-slam2特征提取时判定的“远点”，其信息可靠度远比不上清晰的“近点”。
优化函数算是一个由矢量去求标量的过程，传入的坐标、位姿无一例外都是矩阵（矢量），而要求得的是要误差最小，最接近0。所以在表达式中出现平方项。
接下来的问题就很直接了，泰勒展开–求导–求出迭代公式：
F
k
(
x
~
k
+
Δ
x
)
=
e
k
(
x
~
k
+
Δ
x
)
T
Ω
k
e
k
(
x
~
k
+
Δ
x
)
≈
(
e
k
+
J
k
Δ
x
)
T
Ω
k
(
e
k
+
J
Δ
x
)
=
e
k
T
Ω
k
e
k
+
2
e
k
T
Ω
k
J
k
Δ
x
+
Δ
x
T
J
k
T
Ω
k
J
k
Δ
x
=
C
k
+
2
b
k
Δ
x
+
Δ
x
T
H
k
Δ
x
\begin{array}{lll}{F_k}\left( {{{\widetilde x}_k} + \Delta x} \right) &=& {e_k}{\left( {{{\widetilde x}_k} + \Delta x} \right)^T}{\Omega _k}{e_k}\left( {{{\widetilde x}_k} + \Delta x} \right)\\ & \approx & {\left( {{e_k} + {J_k}\Delta x} \right)^T}{\Omega _k}\left( {{e_k} + J\Delta x} \right)\\ &=& e_k^T{\Omega _k}{e_k} + 2e_k^T{\Omega _k}{J_k}\Delta x + \Delta {x^T}J_k^T{\Omega _k}{J_k}\Delta x\\ &=& {C_k} + 2{b_k}\Delta x + \Delta {x^T}{H_k}\Delta x \end{array}
Fk(x
k+Δx)=≈==ek(x
k+Δx)TΩkek(x
k+Δx)(ek+JkΔx)TΩk(ek+JΔx)ekTΩkek+2ekTΩkJkΔx+ΔxTJkTΩkJkΔxCk+2bkΔx+ΔxTHkΔx
将公式进行整理，我们要求的是
Δ
x
\Delta x
Δx，那就求出
Δ
F
k
\Delta F_k
ΔFk:
Δ
F
k
=
2
b
k
Δ
x
+
Δ
x
T
H
k
Δ
x
.
\Delta F_k = 2b_k \Delta x + \Delta x^T H_k \Delta x.
ΔFk=2bkΔx+ΔxTHkΔx.
求导，令导数为0：
d
F
k
d
Δ
x
=
2
b
+
2
H
k
Δ
x
=
0
⇒
H
k
Δ
x
=
−
b
k
\frac{{d{F_k}}}{{d\Delta x}} = 2b + 2{H_k}\Delta x = 0 \Rightarrow {H_k}\Delta x = - b_k
dΔxdFk=2b+2HkΔx=0⇒HkΔx=−bk
即：
H
k
Δ
x
=
−
b
k
H_k \Delta x = -b_k
HkΔx=−bk
接下来的事就交由高斯牛顿或是LM非线性迭代法来求解问题了。
总结一下：
图优化最重要的是找边和顶点，找到了这些也就知道怎样去求解问题，求解的具体细节交给LM迭代完成。
