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

《视觉SLAM十四讲》学习笔记-Bundle Adjustment (BA) 问题_第1张图片

  • 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 ,则像素位置与空间位置的关系为:

suivi1=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ξ12i=1n|u⃗ i1siKexp(ξ)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)+Jx 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[XYZ] [ s u s v s ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X ′ Y ′ Z ′ ]

所以:
u=fxXZ+cx,   v=fyYZ+cy u = f x X ′ Z ′ + c x ,       v = f y Y ′ Z ′ + c y

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

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

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

eP=[uXvXuYvYuZvZ]=fxZ00fyZfxXZ2fyYZ2 ∂ 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)=[I0P0] ∂ T P ∂ δ ξ = ( T P ) ⊙ = [ I − P ′ ∧ 0 ⊤ 0 ⊤ ]

又由于:
Pδξ=[I,P] ∂ P ′ ∂ δ ξ = [ I , − P ′ ∧ ]

将这两项相乘后雅可比矩阵为:
eδξ=fxZ00fyZfxXZ2fyYZ2fxXYZ2fyfyY2Z2fx+fxX2Z2fyXYZ2fxYZfyXZ ∂ 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 的导数。
利用链式法则,有:

eP=ePPP ∂ e ∂ P = ∂ e ∂ P ′ ∂ P ′ ∂ P

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

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

P P 求导后只剩下 R R ,所以:
eP=fxZ00fyZfxXZ2fyYZ2R ∂ e ∂ P = − [ f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ] R

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

你可能感兴趣的:(slam)