深蓝学院《从零开始手写VIO》作业一

1. VIO文献阅读

阅读VIO相关综述文献如A review of visual inertial odometry from filtering and
optimisation
perspectives，回答以下问题：
视觉与IMU进行融合之后有何优势？有哪些常见的视觉+IMU融合方案？有没有工业界应用的例子？
在学术界，VIO研究有哪些新进展？有没有将学习方法用到VIO中的例子？

1.1 视觉与IMU进行融合之后有何优势？

我觉得这个问题在高博的课上已经回答得很清楚了，IMU和视觉定位方案优劣势对比：

方案	IMU	视觉
优势	快速响应，不受成像质量影响，角速度普遍比较准确，可估计绝对尺度	不产生漂移，直接测量旋转与平移
劣势	存在零偏，低精度IMU积分定位发散，高精度价格昂贵	受图像遮挡、运动物体干扰，单目无尺度，单目纯旋转无法估计，快速运动时丢失

那么用自己的话总结一下：
（1）IMU具备的快速响应的特点正好可以弥补视觉在快速运动时容易丢失的缺陷；
（2）低精度IMU积分定位容易发散，而视觉定位在静止时不会产生漂移；
（3）单目相机无法进行绝对尺度估计，而IMU正好可以；
综上所述，IMU和视觉可以达到优势互补，在达到同等定位精度或者鲁棒性的情况下，IMU和视觉的组合可以极大的降低成本，提高性能！

1.2 有哪些常见的视觉+IMU融合方案？有没有工业界应用的例子？

常见的视觉+IMU融合方案有：
VINS (单目+IMU、双目+IMU)
OKVIS (单目+IMU、双目+IMU)
ROVIO (单目+IMU）
RKSLAM (单目+IMU）
ORB_SLAM-IMU（单目+IMU）

工业界应用的例子：
工业界里面在AR/VR，自动驾驶，无人机等很多地方都有用到的，太多了，这里举两个例子好了
凌感马赓宇：基于视觉+惯性传感器的空间定位方法
全球首款采用VIO定位技术的Mark无人机

参考：
快看，那个学SLAM 的崩溃了!
SLAM中VIO的优势及入门姿势

1.3 在学术界，VIO研究有哪些新进展？有没有将学习方法用到VIO中的例子？

有篇文章叫VINet : Visual-inertial odometry as a sequence-to-sequence learning problem就是将VIO与深度学习结合，而知乎上有大佬对此做出了解释深度学习SLAM ：最新的基于深度学习的deepvo，VINet，大家怎样评价？

在HKUST Aerial Robotics Group——Publications里面可以关注到沈老师那边的最新进展，像Real‐time dense mapping for online processing and navigation这篇文章就是基于VIO（但不是完全基于）进行了大尺度的稠密3D重建

2. 四元数和李代数更新

课件提到了可以使用四元数或旋转矩阵存储旋转变量。当我们用计算出来的 ωωω 对某旋转更新时，有两种不同方式：R←Rexp⁡(ω∧)q←q[1,12ω]T\begin{array}{l}{\mathbf{R} \leftarrow \mathbf{R} \exp \left(\boldsymbol{\omega}^{\wedge}\right)} \\ {\mathbf{q} \leftarrow \mathbf{q}\left[1, \frac{1}{2} \boldsymbol{\omega}\right]^{\mathrm{T}}}\end{array} R←Rexp(ω∧)q←q[1,21ω]T请编程验证对于小量 ω=[0.01,0.02,0.03]Tω = [0.01, 0.02, 0.03]^Tω=[0.01,0.02,0.03]T，两种方法得到的结果非常接近，实践当中可视为等同。因此，在后文提到旋转时，我们并不刻意区分旋转本身是 qqq 还是 RRR，也不区分其更新方式为上式的哪一种。

我的验证思路是这样的，分别计算Eigen下的旋转矩阵相乘，Eigen下四元数相乘，Sophus下李代数扰动的三个结果进行比较（不知道用库是不是算偷懒，应该不会让自己写四元数相乘来验证这么狠吧…），代码如下：

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/so3.h>using namespace std;int main()
{//生成原始旋转矩阵R和四元数qEigen::Matrix3d R = Eigen::Matrix3d::Identity();Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));R = rotation_vector.toRotationMatrix();Eigen::Quaterniond q;q = R;cout<<"R is"<<endl<<R<<endl<<endl;//生成小量w的李代数Eigen::Vector3d w;w<<0.1,0.2,0.3;//将李代数w转换成旋转矩阵R_w和四元数q_wSophus::SO3 SO3_w = Sophus::SO3::exp(w);Eigen::Matrix3d R_w = SO3_w.matrix();Eigen::Quaterniond q_w;q_w = R_w;//[验证方式一]：在Eigen下进行计算Eigen::Matrix3d R_res = R*R_w;Eigen::Quaterniond q_res = q*q_w;Eigen::Matrix3d R_q_res = q_res.toRotationMatrix();cout<<"the result of calculation by R is:"<<endl<<R_res<<endl<<endl;cout<<"the result of calculation by q is:"<<endl<<R_q_res<<endl<<endl;//[验证方式二]：在Sophus下进行计算Sophus::SO3 SO3_R(R);Sophus::SO3 SO3_res = SO3_R*SO3_w;cout<<"the result of calculation by so3 is:"<<endl<<SO3_res.matrix()<<endl<<endl;
}

结果如下

R is0.707107 -0.707107         00.707107  0.707107         00         0         1the result of calculation by R is:
0.447473 -0.87239 0.196733
0.875884 0.471934 0.100523
-0.18054 0.127335  0.97529the result of calculation by q is:
0.447473 -0.87239 0.196733
0.875884 0.471934 0.100523
-0.18054 0.127335  0.97529the result of calculation by so3 is:
0.447473 -0.87239 0.196733
0.875884 0.471934 0.100523
-0.18054 0.127335  0.97529

结果可以看出无论是通过旋转矩阵还是四元数还是李代数，乘以小量的结果都是一致的，因此符合题意

做题中间稍微耽误了点时间的地方是安装Sophus库的时候先安装的最新版本，但是新版本的类和旧版本有了较大改动，不太熟悉，因此又卸了重装…

3. 其他导数

使用右乘so(3)\mathfrak{s o}(3)so(3)，推导以下导数d(R−1p)dR\frac{\mathrm{d}\left(\mathbf{R}^{-1} \mathbf{p}\right)}{\mathrm{d} \mathbf{R}} dRd(R−1p)dln⁡(R1R2−1)dR2\frac{\operatorname{dln}\left(\mathbf{R}_{1} \mathbf{R}_{2}^{-1}\right)}{\mathrm{d} \mathbf{R}_{2}} dR2dln(R1R2−1)

（1）证明如下d(R−1p)dR=lim⁡φ→0(R⁡exp⁡(φ∧))−1p−R−1pφ=(exp⁡(φ∧))−1R−1p−R−1pφ=((exp⁡(φ∧))−1−1)R−1pφ≈(1−φ∧−1)R−1pφ=−φ∧R−1pφ=(R−1p)∧φφ=(R−1p)∧\begin{aligned} \frac{d\left(R^{-1} p\right)}{d R}&=\lim _{\varphi \rightarrow 0} \frac{\left(\operatorname{R} \exp\left(\varphi^{\wedge}\right)\right)^{-1} p-R^{-1} p}{\varphi} \\&=\frac{\left(\exp \left(\varphi^{\wedge}\right)\right)^{-1} R^{-1} p-R^{-1} p}\varphi \\&=\frac{\left(\left(\exp \left(\varphi^{\wedge}\right)\right)^{-1}-1\right) R^{-1} p}{\varphi} \\&\approx\frac{\left(1-\varphi^{\wedge}-1\right) R^{-1} p}{\varphi} \\&=\frac{-\varphi^{\wedge} R^{-1} p}{\varphi} \\&=\frac{\left(R^{-1} p\right)^{\wedge} \varphi}{\varphi} \\&=\left(R^{-1} p\right)^{\wedge} \end{aligned} dRd(R−1p)=φ→0limφ(Rexp(φ∧))−1p−R−1p=φ(exp(φ∧))−1R−1p−R−1p=φ((exp(φ∧))−1−1)R−1p≈φ(1−φ∧−1)R−1p=φ−φ∧R−1p=φ(R−1p)∧φ=(R−1p)∧其中第二行到第三行是《机器人学中的状态估计》的第220页的性质推导：C−1≡CT≡∑n=0∞1n!(−ϕ∧)n≈1−ϕ∧C^{-1} \equiv C^{\mathrm{T}} \equiv \sum_{n=0}^{\infty} \frac{1}{n !}\left(-\phi^{\wedge}\right)^{n} \approx 1-\phi^{\wedge} C−1≡CT≡n=0∑∞n!1(−ϕ∧)n≈1−ϕ∧倒数第二行到倒数第一行是叉乘的性质

（2）证明如下：
这里我改用了尝试两种推导方案：
方案一：
dln⁡(R1R2−1)dR2=lim⁡ϕ→0ln⁡(R1(R2exp⁡(ϕ∧))−1)−ln⁡(R1R2−1)ϕ=lim⁡ϕ→0ln⁡(R1(exp⁡(ϕ∧))−1R2−1)−ln⁡(R1R2−1)ϕ=lim⁡ϕ→0ln⁡((exp⁡(ϕ∧)R1−1)−1R2−1)−ln⁡(R1R2−1)ϕ=lim⁡ϕ→0ln⁡((R1−1R1exp⁡(ϕ∧)R1−1)−1R2−1)−ln⁡(R1R2−1)ϕ=lim⁡ϕ→0ln⁡((R1−1exp⁡((R1ϕ)∧))−1R2−1)−ln⁡(R1R2−1)ϕ=lim⁡ϕ→0ln⁡(exp⁡((R1ϕ)∧)−1R1R2−1)−ln⁡(R1R2−1)ϕ≈lim⁡ϕ→0ln⁡(exp⁡(−(R1ϕ)∧)R1R2−1)−ln⁡(R1R2−1)ϕ=lim⁡ϕ→0Jl−1(ln⁡(R1R2−1))(−(R1ϕ))+ln⁡(R1R2−1)−ln⁡(R1R2−1)ϕ=Jl−1(ln⁡(R1R2−1))(−R1)\begin{aligned} \frac{d \ln \left(R_{1} R_{2}^{-1}\right)}{d R_{2}} &=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1}\left(R_{2} \exp \left(\phi^{\wedge}\right)\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1}\left(\exp \left(\phi^{\wedge}\right)\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(\left(\exp \left(\phi^{\wedge}\right) R_{1}^{-1}\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(\left(R_{1}^{-1} R_{1} \exp \left(\phi^{\wedge}\right) R_{1}^{-1}\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(\left(R_{1}^{-1} \exp \left(\left(R_{1} \phi\right)^{\wedge}\right)\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(\exp \left(\left(R_{1} \phi\right)^{\wedge}\right)^{-1} R_{1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&\approx\lim _{\phi \rightarrow 0} \frac{\ln \left(\exp \left(-\left(R_{1} \phi\right)^{\wedge}\right) R_{1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{J_{l}^{-1}\left(\ln \left(R_{1} R_{2}^{-1}\right)\right)\left(-\left(R_{1} \phi\right)\right)+\ln \left(R_{1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=J_{l}^{-1}\left(\ln \left(R_{1} R_{2}^{-1}\right)\right)\left(-R_{1}\right) \end{aligned} dR2dln(R1R2−1)=ϕ→0limϕln(R1(R2exp(ϕ∧))−1)−ln(R1R2−1)=ϕ→0limϕln(R1(exp(ϕ∧))−1R2−1)−ln(R1R2−1)=ϕ→0limϕln((exp(ϕ∧)R1−1)−1R2−1)−ln(R1R2−1)=ϕ→0limϕln((R1−1R1exp(ϕ∧)R1−1)−1R2−1)−ln(R1R2−1)=ϕ→0limϕln((R1−1exp((R1ϕ)∧))−1R2−1)−ln(R1R2−1)=ϕ→0limϕln(exp((R1ϕ)∧)−1R1R2−1)−ln(R1R2−1)≈ϕ→0limϕln(exp(−(R1ϕ)∧)R1R2−1)−ln(R1R2−1)=ϕ→0limϕJl−1(ln(R1R2−1))(−(R1ϕ))+ln(R1R2−1)−ln(R1R2−1)=Jl−1(ln(R1R2−1))(−R1)

方案二：dln⁡(R1R2−1)dR2=lim⁡ϕ→0ln⁡(R1(R2exp⁡(ϕ∧))−1)−ln⁡(R1R2−1)ϕ=lim⁡ϕ→0ln⁡(R1(exp⁡(ϕ∧))−1R2−1)−ln⁡(R1R2−1)ϕ=lim⁡φ→0ln⁡(R1R2−1R2(exp⁡(ϕ∧))−1R2−1)−ln⁡(R1R2−1)ϕ=lim⁡ϕ→0ln⁡(R1R2−1(R2(exp⁡(ϕ∧))R2−1)−1)−ln⁡(R1R2−1)ϕ=lim⁡θ→0ln⁡(R1R2−1(R2(exp⁡(ϕ∧))R2T)−1)−ln⁡(R1R2−1)ϕ=lim⁡t→0ln⁡(R1R2−1(exp⁡(R2ϕ)∧)−1)−ln⁡(R1R2−1)ϕ=lim⁡t→0ln⁡(R1R2−1exp⁡(−(R2ϕ)∧))−ln⁡(R1R2−1)ϕ=lim⁡ϕ→0ln⁡(R1R2−1)−Jr−1(ln⁡(R1R2−1))R2ϕ−ln⁡(R1R2−1)ϕ=Jr−1(ln⁡(R1R2−1))(−R2)\begin{aligned} \frac{d \ln \left(R_{1} R_{2}^{-1}\right)}{d R_{2}} &=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1}\left(R_{2} \exp \left(\phi^{\wedge}\right)\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1}\left(\exp \left(\phi^{\wedge}\right)\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\varphi \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1} R_{2}\left(\exp \left(\phi^{\wedge}\right)\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\left(R_{2}\left(\exp \left(\phi^{\wedge}\right)\right) R_{2}^{-1}\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\theta \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\left(R_{2}\left(\exp \left(\phi^{\wedge}\right)\right) R_{2}^{T}\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{t \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\left(\exp \left(R_{2}\phi\right)^{\wedge}\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{t \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\exp \left(-\left(R_{2}\phi\right)^{\wedge}\right)\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\right)-J_{r}^{-1}(\ln \left(R_{1} R_{2}^{-1}\right)) R_{2} \phi-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=J_{r}^{-1}\left(\ln \left(R_{1} R_{2}^{-1}\right)\right)\left(-R_{2}\right) \end{aligned} dR2dln(R1R2−1)=ϕ→0limϕln(R1(R2exp(ϕ∧))−1)−ln(R1R2−1)=ϕ→0limϕln(R1(exp(ϕ∧))−1R2−1)−ln(R1R2−1)=φ→0limϕln(R1R2−1R2(exp(ϕ∧))−1R2−1)−ln(R1R2−1)=ϕ→0limϕln(R1R2−1(R2(exp(ϕ∧))R2−1)−1)−ln(R1R2−1)=θ→0limϕln(R1R2−1(R2(exp(ϕ∧))R2T)−1)−ln(R1R2−1)=t→0limϕln(R1R2−1(exp(R2ϕ)∧)−1)−ln(R1R2−1)=t→0limϕln(R1R2−1exp(−(R2ϕ)∧))−ln(R1R2−1)=ϕ→0limϕln(R1R2−1)−Jr−1(ln(R1R2−1))R2ϕ−ln(R1R2−1)=Jr−1(ln(R1R2−1))(−R2)