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

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

问题：给定 n n 个三维空间点 P P 和投影 p p ，希望计算相机的位姿 R R 和 t⃗  t → ,对应的李代数为 ξ ξ . 假设： Pi=[Xi,Yi,Zi]T P i = [ X i , Y i , Z i ] T ,则像素位置与空间位置的关系为：

s⎡⎣⎢uivi1⎤⎦⎥=Kexp(ξ∧)⎡⎣⎢⎢⎢XiYiZi1⎤⎦⎥⎥⎥ s [ u i v i 1 ] = K exp ⁡ ( ξ ∧ ) [ X i Y i Z i 1 ]

写在矩阵形式为：

siu⃗ i=Kexp(ξ∧)Pi s i u → i = K exp ⁡ ( ξ ∧ ) P i

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

ξ∗=argminξ12∑i=1n|u⃗ i−1siKexp(ξ∧)Pi|22 ξ ∗ = arg ⁡ min ξ 1 2 ∑ i = 1 n | u → i − 1 s i K exp ⁡ ( ξ ∧ ) P i | 2 2

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

e(x⃗ +△x⃗ )≈e(x)+J△x e ( x → + △ x → ) ≈ e ( x ) + J △ x

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

设变换到相机坐标系下的空间点坐标为 P′ P ′ ：

P′=(exp(ξ∧)P)1:3=[X′,Y′,Z′]⊤ P ′ = ( exp ⁡ ( ξ ∧ ) P ) 1 : 3 = [ X ′ , Y ′ , Z ′ ] ⊤

则相机投影模型为：

su⃗ =KP′ s u → = K P ′

即：

⎡⎣⎢susvs⎤⎦⎥=⎡⎣⎢fx000fy0cxcy1⎤⎦⎥[X′Y′Z′] [ s u s v s ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X ′ Y ′ Z ′ ]

所以：

u=fxX′Z′+cx,   v=fyY′Z′+cy u = f x X ′ Z ′ + c x ,       v = f y Y ′ Z ′ + c y

考虑对 ξ∧ ξ ∧ 的左乘扰动量 δξ δ ξ 关于 e e 的变化，根据链式法则有：

∂e∂δξ=limδξ→0e(δξ⊕ξ)δξ=∂e∂P′∂P′∂δξ ∂ e ∂ δ ξ = lim δ ξ → 0 e ( δ ξ ⊕ ξ ) δ ξ = ∂ e ∂ P ′ ∂ P ′ ∂ δ ξ

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

∂e∂P′=−[∂u∂X′∂v∂X′∂u∂Y′∂v∂Y′∂u∂Z′∂v∂Z′]=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyY′Z′2⎤⎦⎥ ∂ e ∂ P ′ = − [ ∂ u ∂ X ′ ∂ u ∂ Y ′ ∂ u ∂ Z ′ ∂ v ∂ X ′ ∂ v ∂ Y ′ ∂ v ∂ Z ′ ] = − [ f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ]

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

∂TP∂δξ=(TP)⊙=[I0⊤−P′∧0⊤] ∂ T P ∂ δ ξ = ( T P ) ⊙ = [ I − P ′ ∧ 0 ⊤ 0 ⊤ ]

又由于：

∂P′∂δξ=[I,−P′∧] ∂ P ′ ∂ δ ξ = [ I , − P ′ ∧ ]

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

∂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′⎤⎦⎥ ∂ e ∂ δ ξ = − [ f x Z ′ 0 − f x X ′ Z ′ 2 − f x X ′ Y ′ Z ′ 2 f x + f x X 2 Z ′ 2 − f x Y ′ Z ′ 0 f y Z ′ − f y Y ′ Z ′ 2 − f y − f y Y ′ 2 Z ′ 2 f y X ′ Y ′ Z ′ 2 f y X ′ Z ′ ]

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

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

∂e∂P=∂e∂P′∂P′∂P ∂ e ∂ P = ∂ e ∂ P ′ ∂ P ′ ∂ P

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

P′=exp(ξ∧)P=RP+t⃗  P ′ = exp ⁡ ( ξ ∧ ) P = R P + t →

对 P P 求导后只剩下 R R ,所以：

∂e∂P=−⎡⎣⎢fxZ′00fyZ′−fxX′Z′2−fyY′Z′2⎤⎦⎥R ∂ e ∂ P = − [ f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ] R

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