从零开始手写VIO 第5期(Lesson 3)

从零开始手写VIO 第5期(Lesson 3)

呀，之前有别的事情，所以更新被耽误了，本文是VIO课程的第三节课，权当自己的总结，由于github可能比较慢，所以放在了gitee，链接参见之前的博文，欢迎大家参考，更新不易，如需转载，请著名出处，谢谢。

课程内容回顾

   第三节主要讲解了VIO中基于优化的 IMU 与视觉信息融合，首先回顾了最小二乘问题，包括最速下降法，牛顿法，高斯牛顿，LM等；然后讲解了VIO残差函数的构建，以及预计分量的构建和模型；最后推导了残差Jacobian

1 作业1

作业1是编程题，主要是记录下每一次LM迭代的 $\mu$值，用于绘制最后的曲线；最后就是根据
《The Levenberg-Marquardt method for nonlinear least squares curve-fitting problems》里面的三种方法替换LM里的$\mu$的迭代过程。
这里记录一下结果：

实现其他更优秀的阻尼因子策略，并给出实验对比

作业中实现了论文里的三种方法，根据参数进行选择即可，以exp函数为例，设置噪声方差sigma = 0.1，得到结果如下：

• 方法1： Levenberg-Marquardt lambda update

  代码实现在函数Problem::IsGoodStepInLM()中lambda_update_type_ == 0的条件

  结果:

  优化结果	Lambda曲线

• 方法2： Quadratic update

  代码实现在函数Problem::IsGoodStepInLM()中lambda_update_type_ == 1的条件

  结果:

  优化结果	Lambda曲线

• 方法3： Nielsen’s lambda update equations

  代码实现在函数Problem::IsGoodStepInLM()中lambda_update_type_ == 2的条件

  结果:

  优化结果	Lambda曲线

  结果说明： 对比发现在指数函数情况下，三种方法都能得到相近的解，Levenberg-Marquardt lambda update和Nielsen's lambda update equations方法的迭代次数少，收敛快，但是Lambda变化曲线跳动较大，Quadratic update方法迭代次数多，但是Lambda变化更加平滑；此外，将噪声方差设置为1的时候，也得到相似的结果

2 作业2 公式推导

2.1 $f_{15}$推导

根据PPT的公式有
$$\begin{gathered} \mathbf{a}=\frac{1}{2}(q_{b_i{b}_k}({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a)+q_{b_i{b}_{k+1}}({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)) \\ \omega =\frac{1}{2}({\omega }^{b_k}-{\mathbf{b}}_k^g)+({\omega }^{b_{k+1}}-{\mathbf{b}}_k^g))=\frac{1}{2}({\omega }^{b_k}+{\omega }^{b_{k+1}})-{\mathbf{b}}_k^g \\ {\alpha }_{b_i{b}_{k+1}}={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\ast \delta t+\frac{1}{2}\mathbf{a}\ast {\delta t}^2 \\ 而{\mathbf{b}}_k^g与\omega 相关,与其它的无关，所以 \end{gathered}$$
$$\begin{gathered} f_{15}=\frac{\partial {\alpha }_{b_i{b}_{k+1}}}{\partial \delta b_k^g}=\frac{\partial \frac{1}{2}\mathbf{a}\ast {\delta t}^2}{\partial \delta b_k^g}=\frac{\partial \frac{1}{4}(q_{b_i{b}_k}({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a)+q_{b_i{b}_{k+1}}({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a))\ast {\delta t}^2}{\partial \delta b_k^g} \\ =\frac{1}{4}\frac{\partial q_{b_i{b}_{k+1}}({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta b_k^g} \\ =\frac{1}{4}\frac{\partial q_{b_i{b}_k}⨂\left[\begin{array}{c}1\\ 0.5\ast (\omega -\delta b_k^g)\ast \delta t\end{array}\right]({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta b_k^g} \\ =\frac{1}{4}\frac{\partial R_{b_i{b}_k}exp([(\omega -\delta b_k^g)\ast \delta t{]}_{\times })({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta b_k^g} \\ =\frac{1}{4}\frac{\partial R_{b_i{b}_k}exp([\omega \ast \delta t{]}_{\times })exp([-J_r(\omega \delta t)\ast \delta b_k^g\ast \delta t){]}_{\times })({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta b_k^g} \\ \approx \frac{1}{4}\frac{\partial R_{b_i{b}_k}exp([\omega \ast \delta t{]}_{\times })exp([-\delta b_k^g\ast \delta t){]}_{\times })({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta b_k^g} \\ =-\frac{1}{4}\frac{\partial R_{b_i{b}_{k+1}}[({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2{]}_{\times }(-\delta b_k^g\ast \delta t)}{\partial \delta b_k^g} \\ =-\frac{1}{4}{R}_{b_i{b}_{k+1}}[({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2{]}_{\times }(-\delta t) \end{gathered}$$

2.2 $g_{12}$推导

根据PPT公式有
$$\begin{gathered} \mathbf{a}=\frac{1}{2}(q_{b_i{b}_k}({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a)+q_{b_i{b}_{k+1}}({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)) \\ \omega =\frac{1}{2}(\widetilde{{\omega }^{b_k}}+{\mathbf{n}}_k^g-{\mathbf{b}}_k^g)+(\widetilde{{\omega }^{b_{k+1}}}+{\mathbf{n}}_{k+1}^g-{\mathbf{b}}_k^g)) \\ 所以,在{\mathbf{n}}_k^g上有一个扰动，相当于加上一个\delta {\mathbf{n}}_k^g \\ {\alpha }_{b_i{b}_{k+1}}={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\ast \delta t+\frac{1}{2}\mathbf{a}\ast {\delta t}^2 \\ 而{\mathbf{n}}_k^g与\omega 相关,与其它的无关，所以 \end{gathered}$$

$$\begin{gathered} g_{12}=\frac{\partial {\alpha }_{b_i{b}_{k+1}}}{\partial \delta {\mathbf{n}}_k^g}=\frac{\partial \frac{1}{2}\mathbf{a}\ast {\delta t}^2}{\partial \delta {\mathbf{n}}_k^g}=\frac{\partial \frac{1}{4}(q_{b_i{b}_k}({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a)+q_{b_i{b}_{k+1}}({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a))\ast {\delta t}^2}{\partial \delta {\mathbf{n}}_k^g} \\ =\frac{1}{4}\frac{\partial q_{b_i{b}_{k+1}}({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta {\mathbf{n}}_k^g} \\ =\frac{1}{4}\frac{\partial q_{b_i{b}_k}⨂\left[\begin{array}{c}1\\ 0.5\ast (\omega +0.5\ast \delta {\mathbf{n}}_k^g)\ast \delta t\end{array}\right]({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta {\mathbf{n}}_k^g} \\ =\frac{1}{4}\frac{\partial R_{b_i{b}_k}exp([(\omega +0.5\ast \delta {\mathbf{n}}_k^g)\ast \delta t{]}_{\times })({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta {\mathbf{n}}_k^g} \\ =\frac{1}{4}\frac{\partial R_{b_i{b}_k}exp([\omega \ast \delta t{]}_{\times })exp([J_r(\omega \delta t)\ast (0.5\ast \delta {\mathbf{n}}_k^g)\ast \delta t{]}_{\times })({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta {\mathbf{n}}_k^g} \\ \approx \frac{1}{4}\frac{\partial R_{b_i{b}_k}exp([\omega \ast \delta t{]}_{\times })exp([(0.5\ast \delta {\mathbf{n}}_k^g)\ast \delta t{]}_{\times })({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2}{\partial \delta {\mathbf{n}}_k^g} \\ =-\frac{1}{4}\frac{\partial R_{b_i{b}_{k+1}}[({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2{]}_{\times }(0.5\ast \delta {\mathbf{n}}_k^g\ast \delta t)}{\partial \delta {\mathbf{n}}_k^g} \\ =-\frac{1}{4}{R}_{b_i{b}_{k+1}}[({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a)\ast {\delta t}^2{]}_{\times }(\frac{1}{2}\delta t) \end{gathered}$$

3 证明题

证明式(9)

证明：

根据 Levenberg-Marquardt Method方法，可以得到：
$$({\mathbf{J}}^T\mathbf{J}+\mu I)\Delta {\mathbf{x}}_{lm}=-{\mathbf{J}}^T\mathbf{f},\mu \ge 0$$
将$J^{T}J$进行特征值分解，可以得到${\mathbf{J}}^T\mathbf{J}=\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^T$,

其中
$$\begin{gathered} \mathbf{V}=\left[\begin{array}{c}{\mathbf{v}}_1,...,{\mathbf{v}}_n\end{array}\right]为正交矩阵，{\mathbf{v}}_1,...,{\mathbf{v}}_n为特征向量 \\ \mathbf{\Lambda }=\left[\begin{array}{ccc}{\lambda }_1& & \\ & ...& \\ & & {\lambda }_n\end{array}\right],为对角矩阵，对角线上的元素为特征值 \end{gathered}$$
所以有：
$$\begin{gathered} ({\mathbf{J}}^T\mathbf{J}+\mu \mathbf{I})\Delta {\mathbf{x}}_{lm}=(\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^T+\mathbf{V}\mu {\mathbf{V}}^T)\Delta {\mathbf{x}}_{lm} \\ =(\left[\begin{array}{c}{\mathbf{v}}_1,...,{\mathbf{v}}_n\end{array}\right]\left[\begin{array}{ccc}{\lambda }_1+\mu & & \\ & ...& \\ & & {\lambda }_n+\mu \end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_1^T\\ ...\\ {\mathbf{v}}_n^T\end{array}\right])\Delta {\mathbf{x}}_{lm}=-{\mathbf{J}}^T\mathbf{f}=-{\mathbf{F}}^{\prime T} \end{gathered}$$
所以可以得到：
$$\begin{gathered} ==>\left[\begin{array}{ccc}{\lambda }_1+\mu & & \\ & ...& \\ & & {\lambda }_n+\mu \end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_1^T\\ ...\\ {\mathbf{v}}_n^T\end{array}\right]\Delta {\mathbf{x}}_{lm}=-\left[\begin{array}{c}{\mathbf{v}}_1^T\\ ...\\ {\mathbf{v}}_n^T\end{array}\right]{\mathbf{F}}^{\prime T} \\ ==>\left[\begin{array}{c}{\mathbf{v}}_1^T\\ ...\\ {\mathbf{v}}_n^T\end{array}\right]\Delta {\mathbf{x}}_{lm}=-{\left[\begin{array}{ccc}{\lambda }_1+\mu & & \\ & ...& \\ & & {\lambda }_n+\mu \end{array}\right]}^{-1}\left[\begin{array}{c}{\mathbf{v}}_1^T\\ ...\\ {\mathbf{v}}_n^T\end{array}\right]{\mathbf{F}}^{\prime T} \\ ==>\Delta {\mathbf{x}}_{lm}=-\left[\begin{array}{c}{\mathbf{v}}_1,...,{\mathbf{v}}_n\end{array}\right]{\left[\begin{array}{ccc}{\lambda }_1+\mu & & \\ & ...& \\ & & {\lambda }_n+\mu \end{array}\right]}^{-1}\left[\begin{array}{c}{\mathbf{v}}_1^T\\ ...\\ {\mathbf{v}}_n^T\end{array}\right]{\mathbf{F}}^{\prime T} \\ =-\sum _{j=1}^n\frac{{\mathbf{v}}_j{\mathbf{v}}_j^T}{{\lambda }_j+\mu }{\mathbf{F}}^{\prime T} \\ 因为{\mathbf{v}}_j是列向量，{\mathbf{F}}^{\prime T}是列向量，所以有{\mathbf{v}}_j^T{\mathbf{F}}^{\prime T}=c是一个数，所以{\mathbf{v}}_j(v_j^T{\mathbf{F}}^{\prime T})=({\mathbf{v}}_j^T{\mathbf{F}}^{\prime T}){\mathbf{v}}_j \\ -\sum _{j=1}^n\frac{{\mathbf{v}}_j{\mathbf{v}}_j^T}{{\lambda }_j+\mu }{\mathbf{F}}^{\prime T}=-\sum _{j=1}^n\frac{{\mathbf{v}}_j^T{\mathbf{F}}^{\prime T}}{{\lambda }_j+\mu }{\mathbf{v}}_j \end{gathered}$$
所以式(9)得证。

总结

参考链接

[1] 深蓝学院SLAM/VIO课程