【从零开始手写 VIO】习题

第1节概述与课程介绍

1. VIO 文献阅读

视觉与IMU进行融合之后有何优势？
答：视觉提供丰富的场景信息，可以用来建立3D模型，定位和重定位；IMU提供自我运动的信息，可以用来恢复尺度信息，估计重力方向和速度信息等。

有哪些常见的视觉 + IMU 融合方案？有没有工业界应用的例子？
答：MSCKF，ROVIO，Okvis，VI-ORB，VINS-Mono等；谷歌tango里面的算法据说用的是MSCKF，还有AR/VR设备和无人机里面会用VIO。

在学术界， VIO 研究有哪些新进展？有没有将学习方法用到 VIO 中的例子？
答：2020最新的应该是ORB-SLAM3，还有多基元的VIO；个人觉得语义定位的VIO运用了学习方法。

2. 四元数和李代数更新

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

答：

方法一：使用李群计算旋转矩阵

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>

int main(int argc, char **argv) {
    Eigen::Vector3d vec(1, 2, 3);
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 4, vec / vec.norm()).toRotationMatrix();
    Eigen::Quaterniond q(R);
    Sophus::SO3d SO3_R(R);
    std::cout << "R is" << std::endl<< R << std::endl << std::endl;

    Eigen::Vector3d w(0.01, 0.02, 0.03);
    Sophus::SO3d R_updated = SO3_R * Sophus::SO3d::exp(w);
    std::cout << "Rotation updated = \n" << R_updated.matrix() << std::endl << std::endl;

    Eigen::Quaterniond q_updated = q * Eigen::Quaterniond(1, w.x() / 2, w.y() / 2, w.z() / 2);
    Eigen::Matrix3d R_q_updated = Eigen::Matrix3d(q_updated.normalized());
    std::cout << "Quaternion updated = \n" << R_q_updated << std::endl << std::endl;

    std::cout << "Difference = \n" << R_updated.matrix() - R_q_updated << std::endl << std::endl;

    return 0;
}

方法二：使用罗德里格斯公式计算旋转矩阵

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc, char **argv) {
    Eigen::Vector3d vec(1, 2, 3);
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 4, vec / vec.norm()).toRotationMatrix();
    Eigen::Quaterniond q(R);
    std::cout << "Before updating, R = " << std::endl<< R << std::endl << std::endl;

    Eigen::Vector3d w(0.01, 0.02, 0.03);
    double theta = w.norm();
    Eigen::Vector3d u_w = w / theta;
    Eigen::Matrix3d u_w_skew;
    u_w_skew << 0,          -u_w(2),    u_w(1),
                u_w(2),     0,          -u_w(0),
                -u_w(1),    u_w(0),     0;
    Eigen::Matrix3d R_w = cos(theta) * Eigen::Matrix3d::Identity() + (1 - cos(theta)) * u_w * u_w.transpose() + sin(theta) * u_w_skew;
    Eigen::Matrix3d R_updated = R * R_w;
    std::cout << "After rotation updating, R = " << std::endl << R_updated.matrix() << std::endl << std::endl;

    Eigen::Quaterniond q_updated = q * Eigen::Quaterniond(1, w.x() / 2, w.y() / 2, w.z() / 2);
    q_updated = q_updated.normalized();
    Eigen::Matrix3d R_q_updated = q_updated.toRotationMatrix();
    std::cout << "After quaternion updating, R = " << std::endl << R_q_updated << std::endl << std::endl;

    std::cout << "Difference = \n" << R_updated - R_q_updated << std::endl << std::endl;

    return 0;
}

3. 其他导数

使用右乘 so(3)，推导以下导数：
$$\frac{d(R^{-1}p)}{dR}$$
$$\frac{dln(R_1{R}_2^{-1}{)}^{\vee }}{d{R}_2}$$

答：

$$\begin{array}{rl}\frac{d(R^{-1}p)}{dR}& =\frac{\partial (R^{-1}p)}{\partial \psi }\\ & =\underset{\psi \to 0}{\lim }\frac{[Rexp({\psi }^{\wedge }){]}^{-1}p-R^{-1}p}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{exp({\psi }^{\wedge }{)}^{-1}{R}^{-1}p-R^{-1}p}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{exp(-{\psi }^{\wedge })R^{-1}p-R^{-1}p}{\psi }\\ & \approx \underset{\psi \to 0}{\lim }\frac{[I-{\psi }^{\wedge }]R^{-1}p-R^{-1}p}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{R^{-1}p-{\psi }^{\wedge }{R}^{-1}p-R^{-1}p}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{-{\psi }^{\wedge }{R}^{-1}p}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{(R^{-1}p{)}^{\wedge }\psi }{\psi }\\ & =(R^{-1}p{)}^{\wedge }\end{array}$$

$$\begin{array}{rl}\frac{dln(R_1{R}_2^{-1}{)}^{\vee }}{d{R}_2}& =\underset{\psi \to 0}{\lim }\frac{ln[R_1(R_{2}exp({\psi }^{\wedge }){)}^{-1}{]}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{ln[R_{1}exp(-{\psi }^{\wedge })R_2^{-1}{]}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{ln[R_{1}exp(-{\psi }^{\wedge })R_2^T{]}^{\vee }-ln(R_1{R}_2^T{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{ln[R_1{R}_2^T{R}_{2}exp(-{\psi }^{\wedge })R_2^T{]}^{\vee }-ln(R_1{R}_2^T{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{ln[R_1{R}_2^{T}exp(-(R_2\psi {)}^{\wedge }){]}^{\vee }-ln(R_1{R}_2^T{)}^{\vee }}{\psi }\\ & \approx \underset{\psi \to 0}{\lim }\frac{ln(R_1{R}_2^T{)}^{\vee }+J_r^{-1}(ln(R_1{R}_2^T{)}^{\vee })[-(R_2\psi )]-ln(R_1{R}_2^T{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{-J_r^{-1}(ln(R_1{R}_2^T{)}^{\vee })R_2\psi }{\psi }\\ & =-J_r^{-1}(ln(R_1{R}_2^T{)}^{\vee })R_2\\ & =-J_r^{-1}(ln(R_1{R}_2^{-1}{)}^{\vee })R_2\end{array}$$