《视觉SLAM十四讲》学习笔记-Bundle Adjustment (BA) 问题

• BA本质是把PnP问题当成李代数上的非线性最小二乘问题。
• 线性问题的方式：先求相机位姿，再求空间点位置

问题：给定n$n$个三维空间点P$P$和投影p$p$，希望计算相机的位姿R$\mathbf{R}$和t⃗ $\overrightarrow{t}$,对应的李代数为ξ$\xi$. 假设：Pi=[Xi,Yi,Zi]T${\mathbf{P}}_i=[X_i,Y_i,Z_i{]}^T$,则像素位置与空间位置的关系为：

s⎡⎣⎢uivi1⎤⎦⎥=Kexp(ξ∧)⎡⎣⎢⎢⎢XiYiZi1⎤⎦⎥⎥⎥$$s\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\mathbf{K}\exp ({\xi }^{\wedge })\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]$$

写在矩阵形式为：

siu⃗ i=Kexp(ξ∧)Pi$$s_i{\overrightarrow{u}}_i=\mathbf{K}\exp ({\xi }^{\wedge }){\mathbf{P}}_i$$

上式中隐含有一个齐次坐标到非齐次坐标的转换。把误差求和，构建最小二乘问题，寻找最好的相机位姿：

ξ∗=argminξ12∑i=1n|u⃗ i−1siKexp(ξ∧)Pi|22$${\xi }^{\ast }=\arg \underset{\xi }{min}\frac{1}{2}\sum _{i=1}^n|{\overrightarrow{u}}_i-\frac{1}{s_i}\mathbf{K}\exp ({\xi }^{\wedge }){\mathbf{P}}_i{|}_2^2$$

上式为重投影误差.
应用G-N或L-M等方法之前，要知道误差项关于变量的导数，即：

e(x⃗ +△x⃗ )≈e(x)+J△x$$e(\overrightarrow{x}+\mathrm{△}\overrightarrow{x})\approx e(x)+\mathbf{J}\mathrm{△}x$$

这里：
* e$e$的意义为像素坐标误差，为2维
* x$x$为相机位姿，6维变量
* J$\mathbf{J}$为2×6$2\times 6$的矩阵，需要进一步推导。

设变换到相机坐标系下的空间点坐标为P′${\mathbf{P}}^{\prime }$：

P′=(exp(ξ∧)P)1:3=[X′,Y′,Z′]⊤$${\mathbf{P}}^{\prime }={\left(\exp ({\xi }^{\wedge })\mathbf{P}\right)}_{1:3}=[X^{\prime },Y^{\prime },Z^{\prime }{]}^{\mathrm{\top }}$$

则相机投影模型为：

su⃗ =KP′$$s\overrightarrow{u}=\mathbf{K}{\mathbf{P}}^{\prime }$$

即：

⎡⎣⎢susvs⎤⎦⎥=⎡⎣⎢fx000fy0cxcy1⎤⎦⎥[X′Y′Z′]$$\left[\begin{array}{c}su\\ sv\\ s\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{X}^{\prime }& Y^{\prime }& Z^{\prime }\end{array}\right]$$

所以：

u=fxX′Z′+cx,   v=fyY′Z′+cy$$u=f_x\frac{X^{\prime }}{Z^{\prime }}+c_x,v=f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y$$

考虑对ξ∧${\xi }^{\wedge }$的左乘扰动量δξ$\delta \xi$关于e$e$的变化，根据链式法则有：

∂e∂δξ=limδξ→0e(δξ⊕ξ)δξ=∂e∂P′∂P′∂δξ$$\frac{\mathrm{\partial }e}{\mathrm{\partial }\delta \xi }=\underset{\delta \xi \to 0}{lim}\frac{e\left(\delta \xi \oplus \xi \right)}{\delta \xi }=\frac{\mathrm{\partial }e}{\mathrm{\partial }{\mathbf{P}}^{\prime }}\frac{\mathrm{\partial }{\mathbf{P}}^{\prime }}{\mathrm{\partial }\delta \xi }$$

其中符号⊕$\oplus$为李代数上的左乘扰动。所以有：

∂e∂P′=−[∂u∂X′∂v∂X′∂u∂Y′∂v∂Y′∂u∂Z′∂v∂Z′]=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyY′Z′2⎤⎦⎥$$\frac{\mathrm{\partial }e}{\mathrm{\partial }{\mathbf{P}}^{\prime }}=-\left[\begin{array}{ccc}\frac{\mathrm{\partial }u}{\mathrm{\partial }{X}^{\prime }}& \frac{\mathrm{\partial }u}{\mathrm{\partial }{Y}^{\prime }}& \frac{\mathrm{\partial }u}{\mathrm{\partial }{Z}^{\prime }}\\ \frac{\mathrm{\partial }v}{\mathrm{\partial }{X}^{\prime }}& \frac{\mathrm{\partial }v}{\mathrm{\partial }{Y}^{\prime }}& \frac{\mathrm{\partial }v}{\mathrm{\partial }{Z}^{\prime }}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]$$

式(???)$\text{(???)}$的第二项为变换后的点关于李代数的导数：

∂TP∂δξ=(TP)⊙=[I0⊤−P′∧0⊤]$$\frac{\mathrm{\partial }\mathbf{T}\mathbf{P}}{\mathrm{\partial }\delta \xi }=(\mathbf{T}\mathbf{P}{)}^{\odot }=\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}^{\prime \wedge }\\ 0^{\mathrm{\top }}& 0^{\mathrm{\top }}\end{array}\right]$$

又由于：

∂P′∂δξ=[I,−P′∧]$$\frac{\mathrm{\partial }{\mathbf{P}}^{\prime }}{\mathrm{\partial }\delta \xi }=[\mathbf{I},-{\mathbf{P}}^{\prime \wedge }]$$

将这两项相乘后雅可比矩阵为：

∂e∂δξ=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyY′Z′2−fxX′Y′Z′2−fy−fyY′2Z′2fx+fxX2Z′2fyX′Y′Z′2−fxY′Z′fyX′Z′⎤⎦⎥$$\frac{\mathrm{\partial }e}{\mathrm{\partial }\delta \xi }=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^2}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$

该矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系.

另一方面，除了要优化位姿，也得优化特征点的空间位置，即需要讨论e$e$关于空间点P$\mathbf{P}$的导数。
利用链式法则，有：

∂e∂P=∂e∂P′∂P′∂P$$\frac{\mathrm{\partial }e}{\mathrm{\partial }\mathbf{P}}=\frac{\mathrm{\partial }e}{\mathrm{\partial }{\mathbf{P}}^{\prime }}\frac{\mathrm{\partial }{\mathbf{P}}^{\prime }}{\mathrm{\partial }\mathbf{P}}$$

第一项与前面的式子相同。第二项按照定义有：

P′=exp(ξ∧)P=RP+t⃗ $${\mathbf{P}}^{\prime }=\exp \left({\xi }^{\wedge }\right)\mathbf{P}=\mathbf{R}\mathbf{P}+\overrightarrow{t}$$

对P$\mathbf{P}$求导后只剩下R$\mathbf{R}$,所以：

∂e∂P=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyY′Z′2⎤⎦⎥R$$\frac{\mathrm{\partial }e}{\mathrm{\partial }\mathbf{P}}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\mathbf{R}$$

上式即为相机方程关于相机特征点的导数矩阵方程。