-
VINS-Mono-后端优化 （二：预积分残差雅可比推导）
这里是求预积分对约束的参数块进行求导，有这个雅可比矩阵才能进行优化步长的计算，这个是预积分这个约束因子对各个优化变量的求导，后面还有相机的观测残差块中的 θ \theta θ 是3维的，但是参数块中的四元数是4维的，因为相减后残差只剩虚部了，但是参数是从4个参数变过来的
预积分的残差具体如下，总共有15维的自由度，即y有15维
而参数块 x x x ， 维护的是 k k k 和 k + 1 k+1 k+1 时刻的 P , Q , V , B a , B g P,Q,V,Ba,Bg P,Q,V,Ba,Bg
P是3维，Q是四元数有4维，因为是过参数化的形式，而 V , B a , B g V,Ba,Bg V,Ba,Bg 总共是9维的参数块
所以整个参数块 x x x 的大小为 7+9残差对残差参数块的求导
[ ∂ e ∂ P k ∂ e ∂ V k ∂ e ∂ P k + 1 ∂ e ∂ V k + 1 ∂ e 1 ∂ P k ∂ e 1 ∂ V k ∂ e 1 ∂ P k + 1 ∂ e 1 ∂ V k + 1 ⋮ ]∂Pk∂e∂Pk∂e1⋮∂Vk∂e∂Vk∂e1∂Pk+1∂e∂Pk+1∂e1∂Vk+1∂e∂Vk+1∂e1 [ ∂ e ∂ P k ∂ e ∂ V k ∂ e ∂ P k + 1 ∂ e ∂ V k + 1 ∂ e 1 ∂ P k ∂ e 1 ∂ V k ∂ e 1 ∂ P k + 1 ∂ e 1 ∂ V k + 1 ⋮ ]
这个矩阵有15行，因为误差矩阵 e e e 是15维的(残差分别是 α , β , θ , B a , B g 构成，各自都是 3 个维度 \alpha,\beta,\theta,B_{a},B_{g}构成，各自都是3个维度 α,β,θ,Ba,Bg构成，各自都是3个维度)，参数块 P P P 是7维，参数块 V V V 是9维
所以把这个雅可比矩阵分块成了 15 × 7 15×7 15×7 ， 15 × 9 15×9 15×9 ， 15 × 7 15×7 15×7 ， 15 × 9 15×9 15×9 的形式
误差矩阵的维度和参数是不同的，求导就是对构成这个误差函数的里面的全部变量进行求导由于我们维护的是 R w c R_{wc} Rwc ，所以我们的扰动是右乘的，十四讲里面维护的是 R c w R_{cw} Rcw 所以才使用左乘
对这个误差矩阵进行求导的时候，也可以按照误差参数块进行分别求导的，15=3*5，前三行雅可比使用位移的函数对 P k , V k , P k + 1 , V k + 1 P_{k},V_{k},P_{k+1},V_{k+1} Pk,Vk,Pk+1,Vk+1 进行求导
对位置 δ α \delta\alpha δα 进行求导
以下示例都是对 k k k 时刻的状态量进行求导， k + 1 k+1 k+1 时刻的同理
即使用 δ α b k + 1 b k = … \delta\alpha^{b_{k}}_{b_{k+1}}=\dots δαbk+1bk=… 分别对 P , Q , V , B a , B g P,Q,V,Ba,Bg P,Q,V,Ba,Bg 进行求导位置误差 δ α \delta\alpha δα 对平移 P b k w P^{w}_{b_{k}} Pbkw 的求导
代码中的 Q i Q_{i} Qi 是 R b k w R^{w}_{b_{k}} Rbkw ，所以代码中要取逆
∂ δ α b k + 1 b k ∂ P b k w = − R w b k \frac{\partial\delta\alpha^{b_{k}}_{b_{k+1}}}{\partial P^{w}_{b_{k}}}=-R^{b_{k}}_{w} ∂Pbkw∂δαbk+1bk=−Rwbk
位置 δ α \delta\alpha δα 对旋转 R w b k R^{b_{k}}_{w} Rwbk 进行求导
接下来是对旋转 R w b k R^{b_{k}}_{w} Rwbk 进行求导，由于代码中维护的是 R b k w R^{w}_{b_{k}} Rbkw ，所以这里的公式推导要取逆，方便代码的维护，这样是一个旋转方向的问题，如果直接左乘的话旋转方向就是反过来的了，这样操作的话旋转方向就是按照代码中维护的量的方向来进行操作
后面一串相乘后就是一个向量，当作向量 a a a
∂ δ α b k + 1 b k ∂ R w b k = l i m ϕ → 0 ( R b k w e x p ( ϕ ∧ ) ) − 1 ⋅ a − R w b k ⋅ a ϕ \frac{\partial\delta\alpha^{b_{k}}_{b_{k+1}}}{\partial R^{b_{k}}_{w}}=lim_{\phi\rightarrow0}\frac{(R^{w}_{b_{k}}exp(\phi^{\wedge}))^{-1}·a-R^{b_{k}}_{w}·a}{\phi} ∂Rwbk∂δαbk+1bk=limϕ→0ϕ(Rbkwexp(ϕ∧))−1⋅a−Rwbk⋅a有公式 ( A ⋅ B ) − 1 = B − 1 ⋅ A − 1 (A·B)^{-1}=B^{-1}·A^{-1} (A⋅B)−1=B−1⋅A−1
对旋转向量 ϕ \phi ϕ 取逆，相当于是换了一个旋转方向，所以 ϕ − 1 = − ϕ \phi^{-1}=-\phi ϕ−1=−ϕ
= ( I − ϕ ∧ ) R w b k ⋅ a − R w b k ⋅ a =(I-\phi^{\wedge})R^{b_{k}}_{w}·a-R^{b_{k}}_{w}·a =(I−ϕ∧)Rwbk⋅a−Rwbk⋅a
= − ϕ ∧ ⋅ R w b k ⋅ a =-\phi^{\wedge}·R^{b_{k}}_{w}·a =−ϕ∧⋅Rwbk⋅a叉乘有一个性质， a ⃗ × b ⃗ = − b ⃗ × a ⃗ \vec{a}×\vec{b}=-\vec{b}×\vec{a} a×b=−b×a
a ⃗ × b ⃗ = a ∧ ⋅ b \vec{a}×\vec{b}=a^{\wedge}·b a×b=a∧⋅b
− b ⃗ × a ⃗ = − b ∧ ⋅ a -\vec{b}×\vec{a}=-b^{\wedge}·a −b×a=−b∧⋅a
a ∧ ⋅ b = − b ∧ ⋅ a a^{\wedge}·b=-b^{\wedge}·a a∧⋅b=−b∧⋅a则上面有
= ( R w b k ⋅ a ) ∧ ⋅ ϕ =(R^{b_{k}}_{w}·a)^{\wedge}·\phi =(Rwbk⋅a)∧⋅ϕ然后约掉分母上的 ϕ \phi ϕ
∂ δ α b k + 1 b k ∂ R w b k = ( R w b k ⋅ a ) ∧ \frac{\partial\delta\alpha^{b_{k}}_{b_{k+1}}}{\partial R^{b_{k}}_{w}}=(R^{b_{k}}_{w}·a)^{\wedge} ∂Rwbk∂δαbk+1bk=(Rwbk⋅a)∧对速度 δ β \delta\beta δβ 进行求导
也是使用 δ β b k + 1 b k = … \delta\beta^{b_{k}}_{b_{k+1}}=\dots δβbk+1bk=… 分别对 P , Q , V , B a , B w P,Q,V,Ba,Bw P,Q,V,Ba,Bw 进行求导
速度 δ β \delta\beta δβ 对位置 P b k w P^{w}_{b_{k}} Pbkw 求导
由于公式里面不含位置，所以这块导数 = 0
速度 δ β \delta\beta δβ 对旋转 R w b k R^{b_{k}}_{w} Rwbk 求导
整体公式结构和上面的位移对旋转求导一致，所以求导结果也是 ( R w b k ⋅ a ) ∧ (R^{b_{k}}_{w}·a)^{\wedge} (Rwbk⋅a)∧
对旋转 δ θ \delta\theta δθ 进行求导
分别对 P , Q , V , B a , B g P,Q,V,Ba,Bg P,Q,V,Ba,Bg 进行求导
由于公式中不包含平移和速度量，所以对应的雅可比也为0
δ θ = 2 ⋅ ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w \delta\theta=2·(\gamma^{b_{k}}_{b_{k+1}})^{-1}\otimes(q^{w}_{b_{k}})^{-1}\otimes q^{w}_{b_{k+1}} δθ=2⋅(γbk+1bk)−1⊗(qbkw)−1⊗qbk+1w
k对应代码中的 i ，k+1对应代码中的 j旋转 δ θ \delta\theta δθ 对 q b k w q^{w}_{b_{k}} qbkw 进行求导
就是在右边加一个扰动，扰动为 [ 1 θ 2 ] T [1 \ \frac{\theta}{2}]^{T} [1 2θ]T
∂ δ θ ∂ q b k w = ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ⊗ [ 1 θ 2 ] ) − 1 ⊗ q b k + 1 w − ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w θ \frac{\partial \delta\theta}{\partial q^{w}_{b_{k}}}=\frac{(\gamma^{b_{k}}_{b_{k+1}})^{-1}\otimes(q^{w}_{b_{k}}\otimes)^{-1}\otimes q^{w}_{b_{k+1}}-(\gamma^{b_{k}}_{b_{k+1}})^{-1}\otimes(q^{w}_{b_{k}})^{-1}\otimes q^{w}_{b_{k+1}}}{\theta} ∂qbkw∂δθ=θ(γbk+1bk)−1⊗(qbkw⊗[12θ])−1⊗qbk+1w−(γbk+1bk)−1⊗(qbkw)−1⊗qbk+1w[ 1 θ 2 ] 把逆乘进去，对于扰动那里其实就是把虚部 n ⃗ \vec n n 变一个旋转方向，所以是取个负号
= ( γ b k + 1 b k ) − 1 ⊗ [ 1 − θ 2 ] ⊗ q b k w − 1 ⊗ q b k + 1 w − ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w =(\gamma^{b_{k}}_{b_{k+1}})^{-1}\otimes\otimes q^{w-1}_{b_{k}}\otimes q^{w}_{b_{k+1}}-(\gamma^{b_{k}}_{b_{k+1}})^{-1}\otimes(q^{w}_{b_{k}})^{-1}\otimes q^{w}_{b_{k+1}} =(γbk+1bk)−1⊗[1−2θ]⊗qbkw−1⊗qbk+1w−(γbk+1bk)−1⊗(qbkw)−1⊗qbk+1w[ 1 − θ 2 ] 这里会用一个四元数的性质
a ⊗ b = [ a ] L ⋅ b = [ b ] R ⋅ a a\otimes b=[a]_{L}·b=[b]_{R}·a a⊗b=[a]L⋅b=[b]R⋅a
具体看这篇文章讲解VINS-Mono-IMU预积分 （二：连续时间的PVQ积分+四元数求导）公式变成
= 2 [ q b k w − 1 ⊗ q b k + 1 w ] R ⋅ [ [ ( γ b k + 1 b k ) − 1 ] L ⋅ [ 1 − θ 2 ] ] − [ q b k w − 1 ⊗ q b k + 1 w ] R ⋅ [ [ ( γ b k + 1 b k ) − 1 ] L ⋅ [ 1 0 ⋮ ] ] =2[q^{w-1}_{b_{k}}\otimes q^{w}_{b_{k+1}}]_{R}·\end{bmatrix}-[q^{w-1}_{b_{k}}\otimes q^{w}_{b_{k+1}}]_{R}·\begin{bmatrix}[(\gamma^{b_{k}}_{b_{k+1}})^{-1}]_{L}·\begin{bmatrix}1\\-\frac{\theta}{2} \end{bmatrix} \end{bmatrix} =2[qbkw−1⊗qbk+1w]R⋅[[(γbk+1bk)−1]L⋅[1−2θ]]−[qbkw−1⊗qbk+1w]R⋅ [(γbk+1bk)−1]L⋅ 10⋮ \begin{bmatrix}[(\gamma^{b_{k}}_{b_{k+1}})^{-1}]_{L}·\begin{bmatrix}1\\ 0 \\\vdots \end{bmatrix}
= 2 [ q b k w − 1 ⊗ q b k + 1 w ] R ⋅ [ ( γ b k + 1 b k ) − 1 ] L ⋅ [ [ 1 − θ 2 ] − [ 1 0 ⋮ ] ] =2[q^{w-1}_{b_{k}}\otimes q^{w}_{b_{k+1}}]_{R}·[(\gamma^{b_{k}}_{b_{k+1}})^{-1}]_{L}·-\begin{bmatrix} \begin{bmatrix}1\\-\frac{\theta}{2} \end{bmatrix} \end{bmatrix} =2[qbkw−1⊗qbk+1w]R⋅[(γbk+1bk)−1]L⋅ [1−2θ]− 10⋮ [ 1 0 ⋮ ]
= [ q b k w − 1 ⊗ q b k + 1 w ] R ⋅ [ ( γ b k + 1 b k ) − 1 ] L ⋅ [ 0 − θ ] x y z θ =\frac{[q^{w-1}_{b_{k}}\otimes q^{w}_{b_{k+1}}]_{R}·[(\gamma^{b_{k}}_{b_{k+1}})^{-1}]_{L}·_{xyz}}{\theta} =θ[qbkw−1⊗qbk+1w]R⋅[(γbk+1bk)−1]L⋅[0−θ]xyz[ 0 − θ ] 把 θ \theta θ 约掉
= − [ q b k w − 1 ⊗ q b k + 1 w ] R ⋅ [ ( γ b k + 1 b k ) − 1 ] L =-[q^{w-1}_{b_{k}}\otimes q^{w}_{b_{k+1}}]_{R}·[(\gamma^{b_{k}}_{b_{k+1}})^{-1}]_{L} =−[qbkw−1⊗qbk+1w]R⋅[(γbk+1bk)−1]L
这里乘完后的结果只有四元数的虚部 x y z xyz xyz
所以把四元数进行矩阵化的公式会有点变动
原本的
[ q ] L = q w ⋅ I + [ 0 − q T q q × ] [q]_{L}=q_{w}·I+[q]L=qw⋅I+[0q−qTq×][ 0 − q T q q × ]
[ q ] R = q w ⋅ I + [ 0 − q T q − q × ] [q]_{R}=q_{w}·I+[q]R=qw⋅I+[0q−qT−q×][ 0 − q T q − q × ]
当只取虚部的时候
[ q ] L = q w ⋅ I + q × [q]_{L}=q_{w}·I+q_{×} [q]L=qw⋅I+q×
[ q ] R = q w ⋅ I − q × [q]_{R}=q_{w}·I-q_{×} [q]R=qw⋅I−q×对于一个四元数取逆的时候
q = [ c o s θ 2 n ⃗ ⋅ s i n θ 2 ] q=q=[cos2θn⋅sin2θ][ c o s θ 2 n → · s i n θ 2 ]
其实就是把旋转轴的方向换成反方向，实部是不变的，只有虚部会反方向
q − 1 = [ c o s θ 2 − n ⃗ ⋅ s i n θ 2 ] q^{-1}=q−1=[cos2θ−n⋅sin2θ][ c o s θ 2 − n → · s i n θ 2 ]
则
[ q − 1 ] L = q w ⋅ I − q × = [ q ] R [q^{-1}]_{L}=q_{w}·I-q_{×}=[q]_{R} [q−1]L=qw⋅I−q×=[q]R
[ q − 1 ] R = q w ⋅ I + q × = [ q ] L [q^{-1}]_{R}=q_{w}·I+q_{×}=[q]_{L} [q−1]R=qw⋅I+q×=[q]L我们推导的公式是这样的
= − [ q b k w − 1 ⊗ q b k + 1 w ] R ⋅ [ ( γ b k + 1 b k ) − 1 ] L =-[q^{w-1}_{b_{k}}\otimes q^{w}_{b_{k+1}}]_{R}·[(\gamma^{b_{k}}_{b_{k+1}})^{-1}]_{L} =−[qbkw−1⊗qbk+1w]R⋅[(γbk+1bk)−1]L代码中用了上面的变换关系，代码中的公式是这样的
其实就是对四元数取个逆就可以进行左右乘矩阵的变换= − [ q b k + 1 w − 1 ⊗ q b k w ] L ⋅ [ ( γ b k + 1 b k ) ] R =-[q^{w-1}_{b_{k+1}}\otimes q^{w}_{b_{k}}]_{L}·[(\gamma^{b_{k}}_{b_{k+1}})]_{R} =−[qbk+1w−1⊗qbkw]L⋅[(γbk+1bk)]R
γ \gamma γ 是预积分量来的
在计算这个雅可比前也还是会用实时修正的零偏对预积分量进行调整，调整后才进入雅可比的计算对零偏进行求导
上面的公式中没有包含零偏的项，这里要用到预积分一阶近似更新公式
用这个近似公式代替前面的预积分，再对零偏进行求导平移 δ α \delta\alpha δα 对 k/i 时刻的 b a 、 b w b_{a}、b_{w} ba、bw的扰动
其实就是对预积分量进行一个扰动，此时预积分量前面的参数都等于是常数直接为0
公式为
− [ ( α ^ b k + 1 b k + J b a α Δ b a ) − α b k + 1 b k ] Δ b a = − J b a α \frac{-[(\hat \alpha^{b_{k}}_{b_{k+1}}+J^{\alpha}_{b_{a}}\Delta b_{a})-\alpha^{b_{k}}_{b_{k+1}}]}{\Delta b_{a}}=-J^{\alpha}_{b_{a}} Δba−[(α^bk+1bk+JbaαΔba)−αbk+1bk]=−Jbaα对 b g b_{g} bg 求导也是同理的， α 和 β 都是一样的建模方式 \alpha 和 \beta 都是一样的建模方式 α和β都是一样的建模方式，结果也是一样的
上面是对 i / k 时刻的零偏的求导，这里的零偏也是第 i 时刻的δ b a \delta b_{a} δba 对 i时刻ba的求导就是 − I -I −I， b g b_{g} bg 同理
由于预积分量中零偏的建模中都是假设零偏与 k k k 时刻有关，与 k + 1 k+1 k+1 时刻无关的，因为假设预积分过程中零偏是不会变的，虽然有联合优化零偏，但是零偏是通过一阶近似的方式加入到第 k k k 时刻的零偏中，所以 α , β , θ \alpha,\beta,\theta α,β,θ 对于 k + 1 k+1 k+1 时刻的零偏求导都是 0
θ \theta θ 对陀螺仪零偏求导
2 ( γ b k + 1 b k [ 1 1 2 J b w γ δ b w k ] ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w − 2 ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w 2(\gamma^{b_{k}}_{b_{k+1}}
)^{-1}\otimes(q^{w}_{b_{k}})^{-1}\otimes q^{w}_{b_{k+1}}-2(\gamma^{b_{k}}_{b_{k+1}})^{-1}\otimes(q^{w}_{b_{k}})^{-1}\otimes q^{w}_{b_{k+1}} 2(γbk+1bk[121Jbwγδbwk])−1⊗(qbkw)−1⊗qbk+1w−2(γbk+1bk)−1⊗(qbkw)−1⊗qbk+1w[ 1 1 2 J b w γ δ b w k ] 解开逆
= 2 [ 1 − 1 2 J b w γ δ b w k ] ⊗ ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w − 2 ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w =2\otimes(\gamma^{b_{k}}_{b_{k+1}})^{-1}\otimes(q^{w}_{b_{k}})^{-1}\otimes q^{w}_{b_{k+1}}-2(\gamma^{b_{k}}_{b_{k+1}})^{-1}\otimes(q^{w}_{b_{k}})^{-1}\otimes q^{w}_{b_{k+1}} =2[1−21Jbwγδbwk]⊗(γbk+1bk)−1⊗(qbkw)−1⊗qbk+1w−2(γbk+1bk)−1⊗(qbkw)−1⊗qbk+1w[ 1 − 1 2 J b w γ δ b w k ] = [ 0 − J b w γ δ b w k ] ⊗ ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w =
\otimes(\gamma^{b_{k}}_{b_{k+1}})^{-1}\otimes(q^{w}_{b_{k}})^{-1}\otimes q^{w}_{b_{k+1}} =[0−Jbwγδbwk]⊗(γbk+1bk)−1⊗(qbkw)−1⊗qbk+1w[ 0 − J b w γ δ b w k ] = [ ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w ] R ⋅ [ 0 − J b w γ δ b w k ] ∂ b w k =\frac{
_{R}·[ ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w ] }{\partial b_{w_{k}}} =∂bwk[(γbk+1bk)−1⊗(qbkw)−1⊗qbk+1w]R⋅[0−Jbwγδbwk][ 0 − J b w γ δ b w k ] 约掉 b w k b_{w_{k}} bwk
= [ ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w ] R ⋅ − J b w γ =_{R}·-J^{\gamma}_{b_{w}} =[(γbk+1bk)−1⊗(qbkw)−1⊗qbk+1w]R⋅−Jbwγ[ ( γ b k + 1 b k ) − 1 ⊗ ( q b k w ) − 1 ⊗ q b k + 1 w ] = − [ ( ⊗ q b k + 1 w ) − 1 ⊗ q b k w ⊗ γ b k + 1 b k ] L ⋅ J b w γ =-
_{L}·J^{\gamma}_{b_{w}} =−[(⊗qbk+1w)−1⊗qbkw⊗γbk+1bk]L⋅Jbwγ[ ( ⊗ q b k + 1 w ) − 1 ⊗ q b k w ⊗ γ b k + 1 b k ] 最后这样的形式就和代码中的公式一致了
-
相关阅读:
【Rust】包和模块，文档注释，Rust格式化输出
超详细关于JSR303服务端校验&&拦截器入门到案例使用
一文详解DevExpress的HTML & CSS模板如何实现集合渲染
根据java文法生成对应的词法分析器Content description
阿里云数据盘挂载目录
通俗的解释什么是Docker，一文搞懂
达梦：dmfldr 数据装载
Chapter7: SpringBoot与数据访问
嵌入式软件开发工程师未来的薪资待遇是什么情况
【LeetCode力扣】75 快速排序的子过程partition（荷兰国旗问题）
- 原文地址：https://blog.csdn.net/zysss_/article/details/134293190