该博客详细介绍了VINS-Mono后端优化中预积分残差雅可比矩阵的推导过程，涉及位移、旋转和平移的微分，以及对四元数、速度、零偏的求导。内容涵盖了从3D旋转和平移的误差到四元数的处理，以及预积分的线性化和零偏的修正。
这里是求预积分对约束的参数块进行求导，有这个雅可比矩阵才能进行优化步长的计算，这个是预积分这个约束因子对各个优化变量的求导，后面还有相机的观测
残差块中的 θ \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 ⋮ ] \begin{bmatrix} \frac{\partial e}{\partial P_{k}} &\frac{\partial e}{\partial V_{k}}&\frac{\partial e}{\partial P_{k+1}}&\frac{\partial e}{\partial V_{k+1}}\\ \frac{\partial e_{1}}{\partial P_{k}} &\frac{\partial e_{1}}{\partial V_{k}}&\frac{\partial e_{1}}{\partial P_{k+1}}&\frac{\partial e_{1}}{\partial V_{k+1}}\\ \vdots \end{bmatrix}
∂Pk∂e∂Pk∂e1⋮∂Vk∂e∂Vk∂e1∂Pk+1∂e∂Pk+1∂e1∂Vk+1∂e∂Vk+1∂e1
这个矩阵有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
最低0.47元/天 解锁文章
扫一扫
870
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
