Deep blue college - handwritten VIO operations - the first chapter

一、VIO文献阅读

阅读VIO 相关综述文献[a],回答以下问题：

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

   • Vision sensors work well in most texture-rich scenes,But if you encounter a white wall,或者纯色的一些特殊场景,基本上无法提取特征,也就无法工作;And the frequency should not be too high,15-60Hz居多.
   • IMUDrift exists,随着时间的推移,Errors accumulate more and more,但在短时间内,IMU的精度很高,典型的6轴IMUfrequency greater than or equal to100Hz.

   我的理解是,Because of the visual andIMUEach has its own advantages and disadvantages,Combine the advantages of two sensors,取长补短.因此,The advantage of fusion is：

   • 视觉与IMUintegration can be usedIMU较高的采样频率,进而提高系统的输出频率.

   • 视觉与IMUThe fusion can improve the robustness of vision,如视觉SLAM因为某些运动或场景出现的错误结果.

   • 视觉与IMUfusion can be effectively eliminatedIMU的积分漂移.

   • 视觉与IMUThe fusion can be correctedIMU的Bias.

   • 单目与IMU的融合可以有效解决单目尺度不可观测的问题.

   • 可以应对快速的运动变化,相机在快速运动过程中会出现运动模糊.

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

   常见的融合方案：
   

   2020年推出的ORB-SLAM3方案,Binocular is provided+IMU的融合方案,并且ORB-SLAM3It was also introduced in the Deep Blue Academy Open Class.

   工业界应用

   VIOMainly the application of autonomous driving andAR、VR领域;比如可以使用手机进行AR换鞋（京东淘宝上有些店铺是支持AR试鞋的）.以硬件为主打的XR头盔设备,目前在工业领域都一些落地的场景,如西门子的AR头盔的工业展示,飞机驾驶员的培训.

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

​ 目前VIO和其他传感器的融合也是一个比较受欢迎的研究热点,比如VIO和激光雷达的融合,和GPS的融合.

• 2019, RAL, Visual-Inertial Localization with Prior LiDAR Map Constraints --视觉 VIO 和激光地图.
• 2018, arxiv, A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors --VINS-Mono 的扩展版,能 融合 GPS、单目、双目等信息.
• 2017, ICRA, Vins on wheels --系统分析了 VIO + 轮速计系统的可观性.
• 2019, ICRA, Visual-Odometric Localization and Mapping for Ground Vehicles Using SE(2)-XYZ Constraints --SE2-XYZ 的模型来对地 面Wheel speed robot进行参数化.

VIO+深度学习

• VINet : Visual-inertial odometry as a sequence-to-sequence learning problem.就整体而言,VINet是首次使用DL的框架来解决VIO问题,目前所披露的实验表现出了一定的实用价值.
• Unsupervised Deep Visual-Inertial Odometry with Online Error Correction for RGB-D Imagery.此方法可以在没有Camera-IMU外参的情况下基于学习进行VIO网络学习整合IMU测量并生成估计轨迹,然后根据相对于像素坐标的空间网格的缩放图像投影误差的雅可比行列式在线校正.

参考博客：视觉惯性里程计 综述

题目：A review of visual inertial odometry from filtering and optimisation perspectives

期刊：Advanced Robotics | Volume 29,2015 - Issue 20

Author and research institution information：

翻译：VIO综述论文

二、四元数和李代数更新

课件提到了可以使用四元数或旋转矩阵存储旋转变量.当我们用计算出来的$\omega$对某旋转更新时,有两种不同方式:

$$\begin{gathered} R\leftarrow R\exp ({\omega }^{\wedge }) \\ 或q\leftarrow q\otimes [1,\frac{1}{2}\omega {]}^T \end{gathered}$$
请编程验证对于小量$\omega$ = [0.01, 0.02, 0.03]T,两种方法得到的结果非常接近,实践当中可视为等同.因此,在后文提到旋转时,我们并不刻意区分旋转本身是q 还是R,也不区分其更新方式为上式的哪一种.

代码编写

Program according to the formula,Refer to the textbookp48和p87页代码中对于eigen库和sophus库的使用.

The rotation vector refers to the example in the textbook,Set it to Edgez轴旋转45度.Use the rotation matrix to construct the corresponding quaternion,利用 Sophus::SO3d::hatThe antisymmetric matrix of the rotation vector can be found.

具体代码如下：

int main()
{
    //旋转矩阵
    Matrix3d rotation_matrix = Matrix3d::Identity();
    AngleAxisd rotation_vector(M_PI/4, Vector3d(0,0,1));  //表示沿Z轴旋转45度
    rotation_matrix = rotation_vector.toRotationMatrix();

    cout<<"R: "<<endl<<rotation_matrix<<endl<<endl;

    //Constructs the corresponding quaternion
    Eigen::Quaterniond q(rotation_matrix);
    cout << "q :"<< endl << q.coeffs().transpose() <<endl<<endl;

    //使用对数映射获得它的李代数
    Sophus::SO3d SO3_R(rotation_matrix);
    cout << "so3 :"<< endl << SO3_R.log().transpose() <<endl<<endl;

    //更新量omega
    Eigen::Vector3d omega(0.01, 0.02, 0.03); //更新量

    // 李代数更新
    Matrix3d skew_symmetric_matrix = Sophus::SO3d::hat(omega).matrix();
    cout<<"SO3 hat= "<<endl<< skew_symmetric_matrix<<endl<<endl;
    Sophus::SO3d SO3_updated = SO3_R * Sophus::SO3d::exp(omega);
    cout<<"SO3 updated = "<<endl<<(SO3_R * Sophus::SO3d::exp(omega)).matrix()<<endl<<endl;

    // 四元数更新
    Eigen::Quaterniond q_update(1, omega(0)/2, omega(1)/2, omega(2)/2);
    Eigen::Quaterniond q_updated = q * q_update.normalized();
    cout<<"Quaterniond updated = "<< endl << q_updated.toRotationMatrix() <<endl<<endl;

    //衡量两种方法之间的差距
    cout<<"disparity = "<< endl <<SO3_updated.matrix().transpose()*q_updated.toRotationMatrix()<<endl;
}

最终运行结果截图：

从实验结果可以看出,obtained in two different ways$\omega$There is very little difference in the results of updating a certain rotation,（这里需要注意的一点是,Because the rotation matrix is ​​on the Lie group,Lie groups do not have addition and subtraction,So the first one can be usedRThe inverse or transpose of the matrix is ​​multiplied by the secondR矩阵,closer to the identity matrix,It means that the difference between the two matrices is smaller）差距在$10^{-6}$以内,Therefore it is not deliberately distinguished that the rotation itself isq 还是R.

三、其他导数

使用右乘$\mathfrak{s}\mathfrak{o}(3)$,推导以下导数：

1.

$$\frac{d({\mathbf{R}}^{-1}p)}{d\mathbf{R}}$$

右扰动：
$$\begin{array}{rl}\frac{d(R^{-1}p)}{dR}& =\frac{d(R^{-1}p)}{d\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(R\exp ({\phi }^{\wedge }){)}^{-1}p-R^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(\exp ({\phi }^{\wedge }){)}^{-1}{R}^{-1}p-R^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\exp (-{\phi }^{\wedge })R^{-1}p-R^{-1}p}{\phi }\\ 泰勒展开& \approx \underset{\phi \to 0}{\lim }\frac{(I-{\phi }^{\wedge })R^{-1}p-R^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-{\phi }^{\wedge }{R}^{-1}p}{\phi }\\ 根据slam课程中表1中提供的性质u^{\wedge }v=-v^{\wedge }u可得：& =\underset{\phi \to 0}{\lim }\frac{(R^{-1}p{)}^{\wedge }\phi }{\phi }\\ & =(R^{-1}p{)}^{\wedge }\end{array}$$

2.

$$\frac{dln({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee }}{d{\mathbf{R}}_2}$$

这里需要用BCHLinear approximation：
$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{\mathcal{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& 当{\varphi }_1为小量时\\ {\mathcal{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& 当{\varphi }_2为小量时\end{array}$$
The following properties are also used：
$$\begin{gathered} (\mathbf{C}u{)}^{\wedge }\equiv \mathbf{C}{u}^{\wedge }{\mathbf{C}}^T \\ \exp ((\mathbf{C}u{)}^{\wedge })\equiv \mathbf{C}\exp (u^{\wedge }){\mathbf{C}}^T \\ 旋转矩阵R^{-1}=R^T \end{gathered}$$

Calculate the above formula by means of right perturbation：
$$\begin{array}{rl}\frac{d\ln (R_1{R}_2^{-1}{)}^{\vee }}{d{R}_2}& =\underset{{\phi }_2\to 0}{\lim }\frac{\ln (R_1(R_2\exp ({\phi }_2^{\wedge }){)}^{-1}{)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ & =\underset{{\phi }_2\to 0}{\lim }\frac{\ln (R_1\exp (-{\phi }_2^{\wedge })R_2^{-1}{)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ & =\underset{{\phi }_2\to 0}{\lim }\frac{\ln (R_1{R}_2^{-1}{R}_2\exp (-{\phi }_2^{\wedge })R_2^T{)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ & =\underset{{\phi }_2\to 0}{\lim }\frac{\ln (R_1{R}_2^{-1}\exp ((-R_2{\phi }_2{)}^{\wedge }){)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ BCH近似& \approx \frac{{\mathcal{J}}_r(\ln (R_1{R}_2^{-1}{)}^{\vee }{)}^{-1}(-R_2{\phi }_2)+\ln (R_1{R}_2^{-1}{)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ & =-{\mathcal{J}}_r(\ln (R_1{R}_2^{-1}{)}^{\vee }{)}^{-1}\cdot R_2\end{array}$$

代码地址：https://gitee.com/ximing689/vio-learning.git