从零开始手写VIO第一章作业

1. VIO文献阅读

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

视觉和IMU定位方案存在一定的互补性，可以充分发挥各自的长处，建立鲁棒性更强的SLAM方案。
单一的传感器不能适用所有的场景，比如视觉传感器在大多数纹理丰富的场景中效果很好，但是如果遇到玻璃，白墙等特征较少的场景，基本上无法工作，而通过多个传感器的融合可以达到理想的定位效果。虽然 IMU 长时间使用有非常大的累积误差，但是在短时间内，其相对位移数据又有很高的精度，所以当视觉传感器失效时，融合 IMU 数据，能够提高其定位的精度。与此同时，视觉和惯性测量的互补特性， 使它们特别适合融合， 而鲁棒性和准确的定位与地图绘制是任何移动机器人都需要解决的主要需求。此外， 这两种传感器在大多数智能手机中都有，融合可以有效解决移动手机上的视觉-惯性同时定位与制图。
视觉与 IMU 的融合可以借助 IMU 较高的采样频率，进而提高系统的输出频率；视觉与 IMU 的融合可以提高视觉的鲁棒性，如视觉 SLAM 因为某些运动或场景出现的错误结果；视觉与 IMU 的融合可以有效的消除 IMU 的积分漂移；视觉与 IMU 的融合能够校正 IMU 的 Bias；单目与 IMU 的融合可以有效解决单目尺度不可观测的问题。

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

VINS，OKVIS VI-ORB ，MSCKF， ICE-BA ，ROVIO，大疆无人机Mavic Air,Mavic 2,精灵4用的VIO方案用来提高定位精度，并且为避障构建地图，小觅智能推出的VIO双目相机，视觉定位的扫地机器人，苹果的 ARKit 和谷歌的 ARCore等。

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

VIO研究的新进展方面参考链接如下链接：SLAM 论文阅读和分类整理，涉及到VIO初始化和外参标定，时间戳标定，自监督在线标定，VIO系统可观性分析，VO前端，后端等。
看见同事做过基于深度学习的单目深度估计，深度学习做深度估计最经典的两篇文章应该属于Eigen组2014,2015年的两篇工作，还有就是基于Fully Convolutional Networks的深度预测。还有了解到的在传统slam中加入语义信息，像图像语义分割，语义地图构建等，需要借助深度学习方面的内容来解决。

2.四元数和李代数更新

代码思路如下：基于随机数生成绕随机轴旋转45°的旋转向量，生成10000个基于四元数更新和旋转向量更新的值，并保存数据，用matlab分析两种云算方式之间的差异。主要代码如下：

#include <iostream>
#include <cmath>
#include<stdio.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/so3.h"
#include "sophus/se3.h"

using namespace std; 
int main( int argc, char** argv )
{
    char sinsdata_file_name[100];
    // file name
    time_t currtime =time(NULL);
    tm *sins_p =localtime(&currtime);
    sprintf(sinsdata_file_name,"../data/sins_data%04d%02d%02d%02d%02d%02d.txt",1900+sins_p->tm_year,1+sins_p->tm_mon,sins_p->tm_mday,sins_p->tm_hour,sins_p->tm_min,sins_p->tm_sec);
    FILE * sins_filestream = fopen(sinsdata_file_name,"w+");
    if (sins_filestream==NULL)
    {
        printf("can't create file to save sins data!\n");
    }

    for(int i=0;i<10000;i++)
    {
        Eigen::Vector3d random_vector = Eigen::Vector3d::Random();
        random_vector.normalize();
        Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, random_vector).toRotationMatrix(); //
        //cout<<"Eigen::Matrix3d R = "<<R<<endl;

        Sophus::SO3 SO3_R(R);
        Eigen::Vector3d update_so3(0.01,0.02, 0.03);
        Sophus::SO3 SO3_updated1 = SO3_R*Sophus::SO3::exp(update_so3);
        Eigen::Vector3d so3_updated1 = SO3_updated1.log();

        Eigen::Quaterniond q(R);
        //cout<<"q.coeffs() = "<<q.coeffs()<<endl;  //x,y,z,w

        Eigen::Quaterniond q_add(1,0.5*update_so3(0),0.5*update_so3(1),0.5*update_so3(2));  // w,x,y,z
        q_add.normalize();
        //cout<<"q_add.coeffs() = "<<q_add.coeffs()<<endl;  //x,y,z,w

        Eigen::Quaterniond q_updata = q*q_add;
        q_updata = q_updata.normalized();
        //cout<<"q_updata.coeffs() = "<<q_updata.coeffs()<<endl;
        Sophus::SO3 SO3_updated2(q_updata);
        Eigen::Vector3d so3_updated2 = SO3_updated2.log();

        cout<<"比较SO3 updated1 和SO3_updated2："<<endl;
        cout<<"SO3 updated1 = "<<so3_updated1.transpose()<<endl;
        cout<<"SO3_updated2 = "<<so3_updated2.transpose()<<endl<<endl;

        // write data
        fprintf(sins_filestream,"%f ",so3_updated1(0));
        fprintf(sins_filestream,"%f ",so3_updated1(1));
        fprintf(sins_filestream,"%f ",so3_updated1(2));
        fprintf(sins_filestream,"%f ",so3_updated2(0));
        fprintf(sins_filestream,"%f ",so3_updated2(1));
        fprintf(sins_filestream,"%f\r\n",so3_updated2(2));

    }

    // close file
    fclose(sins_filestream);
    return 0;
}

代码中需要注意的是Eigen::AngleAxisd函数中的轴向量为单位向量，如下图所示：

还有就是四元数Eigen::Quaterniond的输入参数顺序，如下图所示，先是实部，再是虚部。总之，要多查文档。
运行结果如下：

Matlab代码如下：

Matlab运行的结果如下，以下结果是两者so3形式的差异，可以看到两种计算方式的差异量级在
$10^{-6}$10−6。

3. 证明其它导数

3.1 第一题

$\frac{d \ln \left(R_{1} R_{2}^{-1}\right)^{v}}{d R_{2}}$dR2​dln(R1​R2−1​)v​
证明：采用扰动模型的右乘形式，设右扰动$\nabla R_{2}$∇R2​对应的李代数为$\varphi_{2}$φ2​，然后对$\varphi_{2}$φ2​求导，过程如下所示：

$\begin{aligned} \frac{d \ln \left(R_{1} R_{2}^{-1}\right)^{\mathrm{v}}}{d R_{2}} &=\lim _{\varphi_{2} \rightarrow 0} \frac{\operatorname{In}\left(R_{1}\left(R_{2} \cdot \exp \left(\varphi_{2}^{\wedge}\right)\right)^{-1}\right)^{\mathrm{V}}-\operatorname{In}\left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}}{\varphi_{2}} \\ &=\lim _{\varphi_{2} \rightarrow 0} \frac{\ln \left(R_{1} \cdot \exp \left(-\varphi_{2}^{\wedge}\right) \cdot R_{2}^{-1}\right)^{\mathrm{V}}-\operatorname{In}\left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}}{\varphi_{2}} \\ &=\lim _{\varphi_{2} \rightarrow 0} \frac{\ln \left(R_{1} \cdot R_{2}^{-1} \cdot R_{2} \cdot \exp \left(-\varphi_{2}^{\wedge}\right) \cdot R_{2}^{-1}\right)^{\mathrm{V}}-\ln \left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}}{\varphi_{2}} \\ &=\lim _{\varphi_{2} \rightarrow 0} \frac{\ln \left(R_{1} \cdot R_{2}^{-1} \cdot \exp \left(\left(-R_{2} \varphi_{2}\right)^{\wedge}\right)\right)^{\mathrm{V}}-\operatorname{In}\left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}}{\varphi_{2}}\\ &=\lim _{\varphi_{2} \rightarrow 0} \frac{\ln \left(R_{1} \cdot R_{2}^{-1} \cdot \exp \left(\left(-R_{2} \varphi_{2}\right)^{\wedge}\right)\right)^{\vee}-\ln \left(R_{1} R_{2}^{-1}\right)^{\vee}}{\varphi_{2}}\\ &{=\lim _{\varphi_{2} \rightarrow 0} \frac{\ln \left(\exp \left(\ln \left(\left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}\right) \cdot \exp \left(\left(-R_{2} \varphi_{2}\right)^{\wedge}\right)\right)^{\mathrm{V}}-\operatorname{In}\left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}\right.}{\varphi_{2}}} \\ &{=\lim _{\varphi_{2} \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}+J_{r}\left(\ln \left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}\right)^{-1} \cdot\left(-R_{2} \varphi_{2}\right)-\ln \left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}}{\varphi_{2}}} \\ &{=-J_{r}\left(\ln \left(R_{1} R_{2}^{-1}\right)^{\mathrm{V}}\right)^{-1} \cdot R_{2} \leftrightarrow} \end{aligned}$dR2​dln(R1​R2−1​)v​​=φ2​→0lim​φ2​In(R1​(R2​⋅exp(φ2∧​))−1)V−In(R1​R2−1​)V​=φ2​→0lim​φ2​ln(R1​⋅exp(−φ2∧​)⋅R2−1​)V−In(R1​R2−1​)V​=φ2​→0lim​φ2​ln(R1​⋅R2−1​⋅R2​⋅exp(−φ2∧​)⋅R2−1​)V−ln(R1​R2−1​)V​=φ2​→0lim​φ2​ln(R1​⋅R2−1​⋅exp((−R2​φ2​)∧))V−In(R1​R2−1​)V​=φ2​→0lim​φ2​ln(R1​⋅R2−1​⋅exp((−R2​φ2​)∧))∨−ln(R1​R2−1​)∨​=φ2​→0lim​φ2​ln(exp(ln((R1​R2−1​)V)⋅exp((−R2​φ2​)∧))V−In(R1​R2−1​)V​=φ2​→0lim​φ2​ln(R1​R2−1​)V+Jr​(ln(R1​R2−1​)V)−1⋅(−R2​φ2​)−ln(R1​R2−1​)V​=−Jr​(ln(R1​R2−1​)V)−1⋅R2​↔​

需要注意的是从第二个等号，如果采用如下变换，就会变成左扰动模式，因为$\exp \left(-R_{2} \varphi_{2}\right)^{\wedge}$exp(−R2​φ2​)∧为小量，这一点需要注意。
$\begin{aligned} &=\lim _{\varphi_{2} \rightarrow 0} \frac{\operatorname{In}\left(R_{1} \cdot \exp \left(-\varphi_{2}^{\wedge}\right) \cdot R_{2}^{-1}\right)^{\vee}-\operatorname{In}\left(R_{1} R_{2}^{-1}\right)^{\vee}}{\varphi_{2}}\\ &=\lim _{\varphi_{2} \rightarrow 0} \frac{\operatorname{In}\left(R_{1} \cdot \exp \left(-\varphi_{2}^{\wedge}\right) \cdot R_{1}^{T} \cdot R_{1} \cdot R_{2}^{-1}\right)^{\vee}-\operatorname{In}\left(R_{1} R_{2}^{-1}\right)^{\vee}}{\varphi_{2}}\\ &=\lim _{\varphi_{2} \rightarrow 0} \frac{\operatorname{In}\left(\exp \left(\left(-R_{1} \varphi_{2}\right)^{\wedge}\right) \cdot R_{1} R_{2}^{-1}\right)^{\vee}-\operatorname{In}\left(R_{1} R_{2}^{-1}\right)^{\vee}}{\varphi_{2}} \end{aligned}$​=φ2​→0lim​φ2​In(R1​⋅exp(−φ2∧​)⋅R2−1​)∨−In(R1​R2−1​)∨​=φ2​→0lim​φ2​In(R1​⋅exp(−φ2∧​)⋅R1T​⋅R1​⋅R2−1​)∨−In(R1​R2−1​)∨​=φ2​→0lim​φ2​In(exp((−R1​φ2​)∧)⋅R1​R2−1​)∨−In(R1​R2−1​)∨​​
第一个等号到第二个等号用到了矩阵的逆的性质，推导过程中用到了$S O(3)$SO(3)上的伴随性质，公式如下所示：
$\operatorname{Rexp}\left(p^{\wedge}\right) R^{T}=\exp \left((R p)^{\wedge}\right)$Rexp(p∧)RT=exp((Rp)∧)
另外推导中还用到了旋转矩阵的性质，$log$log函数的性质，以及$exp$exp函数的性质等。

3.2 第二题

$\frac{d\left(R^{-1} p\right)}{d R}$dRd(R−1p)​
证明：采用扰动模型的右乘形式，设右扰动$\nabla R_{2}$∇R2​对应的李代数为$\varphi$φ，然后对$\varphi$φ求导，过程如下所示：
$\begin{aligned} \frac{d\left(R^{-1} p\right)}{d R} &=\lim _{\varphi \rightarrow 0} \frac{\left(R \cdot \exp \left(\varphi^{\wedge}\right)\right)^{-1} \cdot p-R^{-1} p}{\varphi} \\ &=\lim _{\varphi \rightarrow 0} \frac{\left(\exp \left(\varphi^{\wedge}\right)\right)^{-1} R^{-1} \cdot p-R^{-1} p}{\varphi} \\ &=\lim _{\varphi \rightarrow 0} \frac{\exp \left(-\varphi^{\wedge}\right) \cdot R^{-1} \cdot p-R^{-1} p}{\varphi} \\ &=\lim _{\varphi \rightarrow 0} \frac{\left(I-\varphi^{\wedge}\right) \cdot R^{-1} \cdot p-R^{-1} \cdot p}{\varphi}\\ &=\lim _{\varphi \rightarrow 0} \frac{\left(I-\varphi^{\wedge}\right) \cdot R^{-1} \cdot p-R^{-1} \cdot p}{\varphi}\\ &=\lim _{\varphi \rightarrow 0} \frac{-\varphi^{\wedge} R^{-1} p}{\varphi}\\ &=\lim _{\varphi \rightarrow 0} \frac{\varphi \cdot\left(R^{-1} p\right)^{\wedge}}{\varphi}\\ &=\left(R^{-1} p\right)^{\wedge}\\ \end{aligned}$dRd(R−1p)​​=φ→0lim​φ(R⋅exp(φ∧))−1⋅p−R−1p​=φ→0lim​φ(exp(φ∧))−1R−1⋅p−R−1p​=φ→0lim​φexp(−φ∧)⋅R−1⋅p−R−1p​=φ→0lim​φ(I−φ∧)⋅R−1⋅p−R−1⋅p​=φ→0lim​φ(I−φ∧)⋅R−1⋅p−R−1⋅p​=φ→0lim​φ−φ∧R−1p​=φ→0lim​φφ⋅(R−1p)∧​=(R−1p)∧​

推导中的最后一步用到了叉乘的性质，即：

$a^{\wedge} b=-b^{\wedge} a$a∧b=−b∧a