【从零开始手写 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 R exp(\omega^{\wedge})$R←Rexp(ω∧)
$或 \ q \leftarrow q \otimes [1, \frac{1}{2}\omega]^T$或 q←q⊗[1,21​ω]T
请编程验证对于小量 $\omega = [0.01, 0.02, 0.03]^T$ω=[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}$dRd(R−1p)​
$\frac{d \ ln(R_1 R_2^{-1})^{\vee}}{dR_2}$dR2​d ln(R1​R2−1​)∨​

答：

$\begin{aligned} \frac{d(R^{-1}p)}{dR} &= \frac{\partial(R^{-1}p)}{\partial \psi} \\ &= \lim \limits_{\psi \to 0} \frac{[Rexp(\psi^{\wedge})]^{-1} p - R^{-1} p}{\psi} \\ &= \lim \limits_{\psi \to 0} \frac{exp(\psi^{\wedge})^{-1} R^{-1} p - R^{-1} p}{\psi} \\ &= \lim \limits_{\psi \to 0} \frac{exp(-\psi^{\wedge}) R^{-1} p - R^{-1} p}{\psi} \\ &\approx \lim \limits_{\psi \to 0} \frac{[I - \psi^{\wedge}] R^{-1} p - R^{-1} p}{\psi} \\ &= \lim \limits_{\psi \to 0} \frac{R^{-1} p - \psi^{\wedge} R^{-1} p - R^{-1} p}{\psi} \\ &= \lim \limits_{\psi \to 0} \frac{- \psi^{\wedge} R^{-1} p}{\psi} \\ &= \lim \limits_{\psi \to 0} \frac{(R^{-1} p)^{\wedge} \psi}{\psi} \\ &= (R^{-1} p)^{\wedge} \end{aligned}$dRd(R−1p)​​=∂ψ∂(R−1p)​=ψ→0lim​ψ[Rexp(ψ∧)]−1p−R−1p​=ψ→0lim​ψexp(ψ∧)−1R−1p−R−1p​=ψ→0lim​ψexp(−ψ∧)R−1p−R−1p​≈ψ→0lim​ψ[I−ψ∧]R−1p−R−1p​=ψ→0lim​ψR−1p−ψ∧R−1p−R−1p​=ψ→0lim​ψ−ψ∧R−1p​=ψ→0lim​ψ(R−1p)∧ψ​=(R−1p)∧​

$\begin{aligned} \frac{d \ ln(R_1 R_2^{-1})^{\vee}}{dR_2} &= \lim \limits_{\psi \to 0} \frac{ln[R_1(R_2 exp(\psi^{\wedge}))^{-1}]^{\vee} - ln(R_1 R_2^{-1})^{\vee}}{\psi} \\ &= \lim \limits_{\psi \to 0} \frac{ln[R_1exp(-\psi^{\wedge})R_2^{-1}]^{\vee} - ln(R_1 R_2^{-1})^{\vee}}{\psi} \\ &= \lim \limits_{\psi \to 0} \frac{ln[R_1exp(-\psi^{\wedge})R_2^T]^{\vee} - ln(R_1 R_2^T)^{\vee}}{\psi} \\ &= \lim \limits_{\psi \to 0} \frac{ln[R_1 R_2^T R_2exp(-\psi^{\wedge})R_2^T]^{\vee} - ln(R_1 R_2^T)^{\vee}}{\psi} \\ &= \lim \limits_{\psi \to 0} \frac{ln[R_1 R_2^Texp(-(R_2 \psi)^{\wedge})]^{\vee} - ln(R_1 R_2^T)^{\vee}}{\psi} \\ &\approx \lim \limits_{\psi \to 0} \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} \\ &= \lim \limits_{\psi \to 0} \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{aligned}$dR2​d ln(R1​R2−1​)∨​​=ψ→0lim​ψln[R1​(R2​exp(ψ∧))−1]∨−ln(R1​R2−1​)∨​=ψ→0lim​ψln[R1​exp(−ψ∧)R2−1​]∨−ln(R1​R2−1​)∨​=ψ→0lim​ψln[R1​exp(−ψ∧)R2T​]∨−ln(R1​R2T​)∨​=ψ→0lim​ψln[R1​R2T​R2​exp(−ψ∧)R2T​]∨−ln(R1​R2T​)∨​=ψ→0lim​ψln[R1​R2T​exp(−(R2​ψ)∧)]∨−ln(R1​R2T​)∨​≈ψ→0lim​ψln(R1​R2T​)∨+Jr−1​(ln(R1​R2T​)∨)[−(R2​ψ)]−ln(R1​R2T​)∨​=ψ→0lim​ψ−Jr−1​(ln(R1​R2T​)∨)R2​ψ​=−Jr−1​(ln(R1​R2T​)∨)R2​=−Jr−1​(ln(R1​R2−1​)∨)R2​​