视觉SLAM十四讲 第七讲 视觉里程计1 Bundle Adjustment公式推导

PnP问题可构建成一个定义于李代数上的非线性最小二乘问题。
线性方法往往是先求位姿，再求空间点位置。非线性优化把它们都看成优化变量，放在一起优化。在PnP中，这个Bundle Adjustment问题，是一个最小化重投影误差（Reprojection error)的问题。
n个三维空间点P及其投影p，希望计算相机的位姿 $\boldsymbol{R,t}$R,t ，它的李代数为 $\boldsymbol\xi$ξ 。假设某空间点坐标为 $P_i=[X_i,Y_i,Z_i]^T$Pi​=[Xi​,Yi​,Zi​]T ,投影像素坐标为 $\boldsymbol{u_i}=[u_i,v_i]$ui​=[ui​,vi​] 。像素位置于空间点位置关系如下：
$s_i\left[ \begin{matrix}u_i\\v_i\\1\end{matrix}\right]=Kexp(\xi^{\wedge})\left[ \begin{matrix}X_i\\Y_i\\Z_i\\1\end{matrix}\right]$si​⎣⎡​ui​vi​1​⎦⎤​=Kexp(ξ∧)⎣⎢⎢⎡​Xi​Yi​Zi​1​⎦⎥⎥⎤​
除了用 $\xi$ξ表示相机位姿之外，别的都和之前的定义保持一致。写成矩阵形式就是：
$s_iu_i=Kexp(\xi^{\wedge})P_i$si​ui​=Kexp(ξ∧)Pi​
中间隐含齐次坐标到非齐次坐标的转换。由于相机位姿未知及观测点的噪声，该等式存在一个误差，因此，把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：
$\xi^{*}=arg\min \limits_{\xi} \frac{1}{2}\sum_{i=1}^n\left \| u_i-\frac{1}{s_i}Kexp(\xi^{\wedge})P_i\right\|_2^2$ξ∗=argξmin​21​∑i=1n​∥∥∥​ui​−si​1​Kexp(ξ∧)Pi​∥∥∥​22​
该问题的误差项，是将像素坐标（观测到的投影位置）于3D点安装当前估计的位姿进行投影得到的位置相比较得到的误差，作以称为重投影误差。使用齐次坐标时，这个坐标有3维。不过，由于 $\boldsymbol{u}$u 最后一维为1，该维度的误差一直为零，因而使用非齐次坐标，于是误差就只有两维了。

通过特征匹配知道了 $p_1,p_2$p1​,p2​ 是同一个空间点P的投影，但不知道相机的位姿。在初始值中，P的投影 $\hat{p}_2$p^​2​ 与实际的 $p_2$p2​ 之间有一定的距离。于是调整相机的位姿，使得这个距离变小。由于调整需要考虑很多个点，最好每个点的误差通常不会精确为零。
每个误差项关于优化变量的导数，线性化：
$e(x+\Delta{x})\approx e(x)+J\Delta{x}$e(x+Δx)≈e(x)+JΔx
当 e 为像素坐标（2维）， x 为相机坐标(6维)时， J 是一个 $2\times6$2×6 的矩阵。J 的形式推导如下：
变换到相机坐标系下的空间点坐标为$P^{'}$P′ ，取出前三维：
$P'=(exp(\xi^{\wedge})P)_{1:3}=[X',Y',Z']^T$P′=(exp(ξ∧)P)1:3​=[X′,Y′,Z′]T
相机投影模型相对于 P’ 为
$s\boldsymbol u=KP'$su=KP′
展开：
$\left[ \begin{matrix}su\\sv\\s\end{matrix}\right]=\left[ \begin{matrix}fx&0&c_x\\0&f_y&c_y\\0&0&1\end{matrix}\right]\left[\begin{matrix}X'\\Y'\\Z'\end{matrix}\right]$⎣⎡​susvs​⎦⎤​=⎣⎡​fx00​0fy​0​cx​cy​1​⎦⎤​⎣⎡​X′Y′Z′​⎦⎤​
利用第三行消去s（实际上就是 P’ 的距离），得：
$u=f_x\frac{X'}{Z'}+c_x,v=f_y\frac{X'}{Z'}+c_y$u=fx​Z′X′​+cx​,v=fy​Z′X′​+cy​
求误差时，可以把这里的u,v与实际的测量值进行比较，求差。定义了中间变量后对 $\xi^{\wedge}$ξ∧ 左乘扰动量 $\delta \xi$δξ ，然后考虑 e 的变化关于扰动量的导数。利用链式法则：
$\frac{\partial e}{\partial \delta\xi}=\lim_{\partial \xi \to 0}\frac{e(\delta \xi\oplus\xi)}{\delta \xi}=\frac{\partial e}{\partial P'}\frac{\partial P'}{\partial \delta \xi}$∂δξ∂e​=lim∂ξ→0​δξe(δξ⊕ξ)​=∂P′∂e​∂δξ∂P′​
$\oplus$⊕ 指李代数上的左乘扰动。第一项是误差关于投影点的导数：
$\frac{\partial e}{\partial P'}=-\left[ \begin{matrix}\frac{\partial u}{\partial X'}&\frac{\partial u}{\partial Y'}&\frac{\partial u}{\partial Z'}\\\frac{\partial v}{\partial X'}&\frac{\partial v}{\partial Y'}&\frac{\partial v}{\partial Z'}\end{matrix}\right]=-\left[ \begin{matrix}\frac{f_x}{Z'}&0&\frac{f_xX'}{Z'^2}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\end{matrix}\right]$∂P′∂e​=−[∂X′∂u​∂X′∂v​​∂Y′∂u​∂Y′∂v​​∂Z′∂u​∂Z′∂v​​]=−[Z′fx​​0​0Z′fy​​​Z′2fx​X′​−Z′2fy​Y′​​]
第二项为变换到相机坐标系下的空间点 P’ 关于李代数的导数
$\frac{\partial(TP)}{\partial \delta \xi}=(TP)^\odot=\left[ \begin{matrix}I&-P'^{\wedge}\\0^T&0^T\end{matrix}\right]$∂δξ∂(TP)​=(TP)⊙=[I0T​−P′∧0T​]
在 P’ 的定义中，去除前三维得：
$\frac{\partial P'}{\partial \delta \xi}=\left[ \begin{matrix}I&-P'^{\wedge}\end{matrix}\right]$∂δξ∂P′​=[I​−P′∧​]
将这两项相乘，就得到了 2\times6 的雅可比矩阵：
$\frac{\partial e}{\partial \delta\xi}=-\left[ \begin{matrix}\frac{f_x}{Z'}&0&\frac{f_xX'}{Z'^2}&-\frac{f_xX'Y'}{Z'^2}&f_x+\frac{f_xX'^2}{Z'^2}&-\frac{f_xY'}{Z'}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}&-f_y-\frac{f_yY'^2}{Z'^2}&\frac{f_yX'Y'}{Z'^2}&\frac{f_yX'}{Z'}\end{matrix}\right]$∂δξ∂e​=−[Z′fx​​0​0Z′fy​​​Z′2fx​X′​−Z′2fy​Y′​​−Z′2fx​X′Y′​−fy​−Z′2fy​Y′2​​fx​+Z′2fx​X′2​Z′2fy​X′Y′​​−Z′fx​Y′​Z′fy​X′​​]
这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。保留了前面的负号，是因为误差由观测值减预测值定义。可以反过来，定义成“预测值减观测值”的形式，此时，去掉负号即可。如果 se(3) 的定义形式是旋转在前，平移在后，只要把这个矩阵的前3列与后3列对调即可。
除了优化位姿，还需要优化特征点的空间位置，需要讨论 e 关于空间点 P 的导数。仍用链式法则：
$\frac{\partial e}{\partial P}=\frac{\partial e}{P'}\frac{\partial P'}{\partial P}$∂P∂e​=P′∂e​∂P∂P′​
第一项前面已经推导：
$\frac{\partial e}{\partial P'}=-\left[ \begin{matrix}\frac{\partial u}{\partial X'}&\frac{\partial u}{\partial Y'}&\frac{\partial u}{\partial Z'}\\\frac{\partial v}{\partial X'}&\frac{\partial v}{\partial Y'}&\frac{\partial v}{\partial Z'}\end{matrix}\right]=-\left[ \begin{matrix}\frac{f_x}{Z'}&0&\frac{f_xX'}{Z'^2}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\end{matrix}\right]$∂P′∂e​=−[∂X′∂u​∂X′∂v​​∂Y′∂u​∂Y′∂v​​∂Z′∂u​∂Z′∂v​​]=−[Z′fx​​0​0Z′fy​​​Z′2fx​X′​−Z′2fy​Y′​​]
关于第二项，按照定义：
$P'=exp(\xi^{\wedge})P=RP+t$P′=exp(ξ∧)P=RP+t
P’ 对 P 求导后仅剩下 R ，于是：
$\frac{\partial e}{\partial P}=-\left[ \begin{matrix}\frac{f_x}{Z'}&0&\frac{f_xX'}{Z'^2}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\end{matrix}\right]R$∂P∂e​=−[Z′fx​​0​0Z′fy​​​Z′2fx​X′​−Z′2fy​Y′​​]R
因此，观测相机方程关于相机位姿和特征点的两个导数矩阵都求到了，在优化过程中提供了重要的梯度方向，指导优化的迭代。
使用BA优化：