采用逆深度参数表达的BA问题导数推导
由于大部分的slam算法均采用逆深度参数表达地图点的结构,但是网上对该方法的介绍比较少因此本文将详细说明其推导过程。逆深度参数表达具有优化变量少、能表达非常远的点以及分布接近高斯分布等优势,这也是大家选择逆深度参数的原因。
图结构表达
从上图可以看出一个约束项Ep12连接三个节点分别是当前帧、参考帧和逆深度参数表达的地图点。这是因为在逆深度参数的表达下需要当前帧、参考帧位姿及地图点才能共同确定误差函数,这将在下文中看到。
优化节点
优化节点分为两类,关键帧的相机位姿和地图点坐标。其中关键帧的相机位姿维护SE(3)表达的状态,而地图点则是逆深度参数。
相机位姿
状态量采用四元数+平移向量的方式表达,即[q|t],对于微小的角度更新方法如下:
tk+1=tk+δtqk+1=qk⊕δq
其中⊕代表的是四元数更新,一般采用四阶Runge-Kunta法更新四元数。
∂δξ∂pf=(TP)⊙=[I0T−pf∧0T]
其中pf为当前坐标系下的XYZ表达地图点,导数的具体求解过程参考附录。
逆深度参数地图点
逆深度参数采用MSCKF表示:
pf=⎣⎡xfyfzf⎦⎤=ρ1⎣⎡αβ1⎦⎤
λ=⎣⎡αβρ⎦⎤=⎣⎡xf/zfyf/zf1/zf⎦⎤
∂λ∂pf=⎣⎢⎡ρ1000ρ10−ρ2α−ρ2β−ρ21⎦⎥⎤
其中pf为参考坐标系下地图点的三维坐标,xf,yf,zf分别为对应数值,λ为逆深度参数表达的坐标而二者的导数关系也一并给出。推导公式参考附录。
边约束项
误差计算的思路在于把参考帧下的地图点转换到当前帧坐标系下,在投影到当前帧像素平面上与对应的特征点坐标求差值。因此误差函数为:
e=[umvm]−[uv]
[umvm]=H(Twc−1TwfG(λ))
其中[umvm]是重投影的点,H函数根据相机模型将相机坐标系下的点投影到像素坐标系中,而G函数则将逆深度参数转换为参考帧下的3d坐标,对于Twc,Twf则分别是世界系下当前相机帧的位姿和参考帧的相机位姿。
更进一步,将误差函数展开成为更加具体的形式。
[umvm]=[fzcxc+cxfzcyc+cy]
pc=⎣⎡xcyczc⎦⎤=Rwc−1pw−Rwc−1twc
pw=Rwfpf+twf
pf=ρ1⎣⎡αβ1⎦⎤
将误差函数通过一步步转换成为逆深度参数坐标,在求导时便可轻易的采用链式法则进行计算。
对地图点的导数
∂λ∂e=∂pc∂e∂pf∂pc∂λ∂pf
由于∂λ∂pf已知,因此只需要求解∂pc∂e,∂pf∂pc
∂pc∂e=−[∂xc∂u∂xc∂v∂yc∂u∂yc∂v∂zc∂u∂zc∂v]=−[zcf00zcf−zc2fxc−zc2fyc]
∂pf∂pc=Rcf
对当前帧的导数
∂δξc∂e=∂pc∂e∂δξc∂pc
其中,∂pc∂e在对点求导时已求得,而∂δξc∂pc只要带入可得
∂δξc∂pc=[I0T−pc∧0T]
在计算时通常会直接删除下面为零的部分,以便和前面矩阵维度一致。
对参考帧的导数
∂δξf∂e=∂pf∂e∂δξf∂pf
同样,∂pf∂e在对点求导时已经求得了
∂δξf∂pc=[I0T−pf∧0T]
附录
向量的求导
∂[xy]∂[ab]=[∂x∂a∂x∂b∂y∂a∂y∂b]
对微小的扰动δξ求导