03 图优化
文章目录
-
- 03 图优化
-
- 3.1 基本概念
- 3.2 SLAM 与图优化
- 3.3 核函数
- 3.4 总结
- 3.5 BA 与图优化
03 图优化
3.1 基本概念
（1）将优化问题用图（Graph）的形式表达，即为图优化。
（2）图由顶点（Vertex）和边（edge）组成。
3.2 SLAM 与图优化
（1）假设机器人在 k k k 时刻，处于 x k x_k xk 位置，进行了一次观测，得到观测数据 z k z_k zk，即
z k = h ( x k ) (1) z_k = h(x_k) \tag{1} zk=h(xk)(1)
在视觉 SLAM 中， x k x_k xk 就是空间点坐标， z k z_k zk 是观测图像。假设 x = [ X , Y , Z ] T \boldsymbol{x}=[X, Y, Z]^\mathrm{T} x=[X,Y,Z]T， z = [ u , v ] T \boldsymbol{z}=[u, v]^\mathrm{T} z=[u,v]T，则
z = K ( R x + t ) (2) \boldsymbol{z} = \boldsymbol{K}(\boldsymbol{Rx+t}) \tag{2} z=K(Rx+t)(2)
但由于误差的存在，一定有
e k = z k − h ( x k ) (3) e_k = z_k - h(x_k) \tag{3} ek=zk−h(xk)(3)
我们的目标就是优化 h ( ⋅ ) h(\cdot) h(⋅) 的参数（这里就是 R \boldsymbol{R} R、 t \boldsymbol{t} t），使得 e e e 最小。
（2）具体来说，对一个带有n条边的图，其目标函数可以写成：
min x ∑ k = 1 n e k ( x k , z k ) T Ω k e k ( x k , z k ) (4) \min _x \sum_{k=1}^n \boldsymbol{e_k}\left(\boldsymbol{x_k}, \boldsymbol{z_k}\right)^T \boldsymbol{\Omega_k} \boldsymbol{e_k}\left(\boldsymbol{x_k}, \boldsymbol{z_k}\right) \tag{4} xmink=1∑nek(xk,zk)TΩkek(xk,zk)(4)
这里是所有误差的矢量平方和， Ω k \boldsymbol{\Omega_k} Ωk 为信息矩阵，等于协方差矩阵的逆，表示不同误差项的权重系数。
对于视觉 SLAM，即
e k ( x k , T k , z k ) = ( z k − K ( R x k + t ) ) T Ω k ( z k − K ( R x k − t ) ) (5) e_k\left(x_k, T_k, z_k\right)=\left(z_k-K\left(R x_k+t\right)\right)^T \Omega_k\left(z_k-K\left(R x_k-t\right)\right) \tag{5} ek(xk,Tk,zk)=(zk−K(Rxk+t))TΩk(zk−K(Rxk−t))(5)
对于第 k k k 条边 e k ( x k ) e_k(x_k) ek(xk)，假设初始点为 x ~ k ′ \tilde{x}_{k_{\prime}} x~k′，增量为 Δ x \Delta x Δx，则
e k ( x ~ k + Δ x ) ≈ e k ( x ~ k ) + d e k d x k Δ x = e k + J k Δ x (6) e_k\left(\tilde{x}_k+\Delta x\right) \approx e_k\left(\tilde{x}_k\right)+\frac{d e_k}{d x_k} \Delta x=e_k+J_k \Delta x \tag{6} ek(x~k+Δx)≈ek(x~k)+dxkdekΔx=ek+JkΔx(6)
J k J_k Jk 为导数（雅克比矩阵）。
那么，对第 k k k 条边的目标函数项
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 (7) \begin{aligned} F_k\left(\tilde{x}_k+\Delta x\right) &=e_k\left(\tilde{x}_k+\Delta x\right)^T \Omega_k e_k\left(\tilde{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+2 e_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{aligned} \tag{7} 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Δx=Ck+2bkΔx+ΔxTHkΔx(7)
我们把与 Δ x \Delta x Δx 无关的整理成常数项 C k C_k Ck ，把一次项系数写成 2 b k 2b_k 2bk ，二次项则为 H k H_k Hk（即 Hessian 矩阵）。
C k C_k Ck 就是 x ~ k ′ \tilde{x}_{k_{\prime}} x~k′ 对应的目标函数项，则从 x ~ k ′ \tilde{x}_{k_{\prime}} x~k′ 到 x ~ k ′ + Δ x \tilde{x}_{k_{\prime}}+\Delta x x~k′+Δx ，目标函数项的增量为
Δ F k = 2 b k Δ x + Δ x T H k Δ x (8) \Delta F_k=2 b_k \Delta x+\Delta x^T H_k \Delta x \tag{8} ΔFk=2bkΔx+ΔxTHkΔx(8)
我们希望求出使 Δ F k \Delta F_k ΔFk 最小的 Δ x \Delta x Δx，故直接求导，并令导数为零
d F k d Δ x = 2 b k + 2 H k Δ x = 0 ⇒ H k Δ x = − b k (9) \frac{d F_k}{d \Delta x}=2 b_k+2 H_k \Delta x=0 \Rightarrow H_k \Delta x=-b_k \tag{9} dΔxdFk=2bk+2HkΔx=0⇒HkΔx=−bk(9)
如果同时考虑所有的边，可以直接去掉下标，即
H Δ x = − b (10) \boldsymbol{H}\Delta \boldsymbol{x} = -\boldsymbol{b} \tag{10} HΔx=−b(10)
只需求解出增量 Δ x \Delta \boldsymbol{x} Δx 即可，这和高斯牛顿法类似。
3.3 核函数
如果不小心加入某些错误的边，或者是误差很大的边，这时仍按照原来的方法求解，就会使最终的结果产生较大偏差。所以引入核函数，减小那些误差很大的边的影响，从而使优化结果更鲁棒。
3.4 总结
-
选择你想要的图里的节点与边的类型，确定它们的参数化形式；
-
往图里加入实际的节点和边；
-
选择初值，开始迭代；
-
每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
-
求解稀疏线性方程HkΔx=−bk，得到梯度方向；
-
继续用GN或LM进行迭代。如果迭代结束，返回优化值。
实际上，g2o能帮我们做好第3-6步，只需要做前两步即可。
3.5 BA 与图优化
关于 BA，见后端 1.
两张图像，对应的特征点分别为
z 1 = { z 1 1 , z 1 2 , … , z 1 N } , z 2 = { z 2 1 , z 2 2 , … , z 2 N } (11) \boldsymbol{z}_1=\left\{\boldsymbol{z}_1^1, \boldsymbol{z}_1^2, \ldots, \boldsymbol{z}_1^N\right\}, \boldsymbol{z}_2=\left\{\boldsymbol{z}_2^1, \boldsymbol{z}_2^2, \ldots, \boldsymbol{z}_2^N\right\} \tag{11} z1={z11,z12,…,z1N},z2={z21,z22,…,z2N}(11)
假设特征点 z 1 j \boldsymbol{z}_1^j z1j 和 z 2 j \boldsymbol{z}_2^j z2j 在世界坐标系中对应的坐标为 X j \boldsymbol{X}^j Xj，则有
s 1 [ z 1 j 1 ] = K X j , s 2 [ z 2 j 1 ] = K ( R X j + t ) (12) s_1\left[\begin{array}{c} \boldsymbol{z}_1^j \\ 1 \end{array}\right]=\boldsymbol{K X}^j, \quad s_2\left[\begin{array}{c} \boldsymbol{z}_2^j \\ 1 \end{array}\right]=\boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{X}^j+\boldsymbol{t}\right) \tag{12} s1[z1j1]=KXj,s2[z2j1]=K(RXj+t)(12)
为表达简洁，我们以第一个相机的坐标系为世界坐标系。
式（12）不可避免存在误差，由此构建优化方程
min X j , R , t ∥ 1 s 1 K X j − [ z 1 j , 1 ] T ∥ 2 + ∥ 1 s 2 K ( R X j + t ) − [ z 2 j , 1 ] T ∥ 2 (13) \min _{\boldsymbol{X}^j, \boldsymbol{R}, \boldsymbol{t}}\left\|\frac{1}{s_1} \boldsymbol{K} \boldsymbol{X}^j-\left[\boldsymbol{z}_1^j, 1\right]^\mathrm{T}\right\|^2+\left\|\frac{1}{s_2} \boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{X}^j+\boldsymbol{t}\right)-\left[\boldsymbol{z}_2^j, 1\right]^\mathrm{T}\right\|^2 \tag{13} Xj,R,tmin s11KXj−[z1j,1]T 2+ s21K(RXj+t)−[z2j,1]T 2(13)
综合所有误差
min X j , R , t ∑ j = 1 N ∥ 1 s 1 K X j − [ z 1 j , 1 ] T ∥ 2 + ∥ 1 s 2 K ( R X j + t ) − [ z 2 j , 1 ] T ∥ 2 (14) \min _{\boldsymbol{X}^j, \boldsymbol{R}, \boldsymbol{t}}\sum_{j=1}^N\left\|\frac{1}{s_1} \boldsymbol{K} \boldsymbol{X}^j-\left[\boldsymbol{z}_1^j, 1\right]^\mathrm{T}\right\|^2+\left\|\frac{1}{s_2} \boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{X}^j+\boldsymbol{t}\right)-\left[\boldsymbol{z}_2^j, 1\right]^\mathrm{T}\right\|^2 \tag{14} Xj,R,tminj=1∑N s11KXj−[z1j,1]T 2+ s21K(RXj+t)−[z2j,1]T 2(14)
这里， z 1 \boldsymbol{z}_1 z1、 z 2 \boldsymbol{z}_2 z2 为已知量， X \boldsymbol{X} X、 R \boldsymbol{R} R、 t \boldsymbol{t} t 为待优化变量。