![《视觉SLAM十四讲》学习笔记-Bundle Adjustment (BA) 问题_第1张图片](http://img.e-com-net.com/image/info8/70868178851c4fe78f8350e76880e305.jpg)
- 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
上式即为
相机方程关于相机特征点的导数矩阵方程
。