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

最新推荐文章于 2022-06-21 16:19:55 发布

Jevin-L

最新推荐文章于 2022-06-21 16:19:55 发布

阅读量1k

点赞数 1

分类专栏：深蓝学院手写vio作业文章标签：计算机视觉 SLAM 机器人手写VIO SLAM

本文链接：https://blog.csdn.net/hitljy/article/details/107320682

版权

深蓝学院手写vio作业专栏收录该内容

7 篇文章 16 订阅

订阅专栏

1.VIO文献阅读

在这里插入图片描述

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

PPT中各自优劣：

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

以下摘自《视觉 SLAM 十四讲》：

IMU虽然可以测得角速度和加速度，但这些量都存在明显的漂移（Drift），使得积分两次得到的位姿数据非常不可靠。好比说，我们将IMU放在桌上不动，用它的读数积分得到的位姿也会漂出十万八千里。但是，对于短时间内的快速运动，IMU能够提供一些较好的估计。这正是相机的弱点。当运动过快时，（卷帘快门的）相机会出现运动模糊，或者两帧之间重叠区域太少以至于无法进行特征匹配，所以纯视觉SLAM非常害怕快速的运动。而有了IMU，即370第14讲SLAM：现在与未来使在相机数据无效的那段时间内，我们还能保持一个较好的位姿估计，这是纯视觉SLAM无法做到的。
相比于IMU，相机数据基本不会有漂移。如果相机放在原地固定不动，那么（在静态场景下）视觉SLAM的位姿估计也是固定不动的。所以，相机数据可以有效地估计并修正IMU读数中的漂移，使得在慢速运动后的位姿估计依然有效。
当图像发生变化时，本质上我们没法知道是相机自身发生了运动，还是外界条件发生了变化，所以纯视觉SLAM难以处理动态的障碍物。而IMU能够感受到自己的运动信息，从某种程度上减轻动态物体的影响。
总而言之，我们看到IMU为快速运动提供了较好的解决方式，而相机又能在慢速运动下解决IMU的漂移问题——在这个意义下，它们二者是互补的。

整体上，视觉和 IMU 定位方案存在一定互补性质：
• IMU 适合计算短时间、快速的运动；
• 视觉适合计算长时间、慢速的运动。
同时，可利用视觉定位信息来估计 IMU 的零偏，减少 IMU 由零偏导致的发散和累积误差；反之， IMU 可以为视觉提供快速运动时的定位

视觉与IMU融合之后会弥补各自的劣势，可利用视觉定位信息来减少IMU由零偏导致的发散和累积误差；IMU可以为视觉提供快速运动时的定位，减少因为外界影响定位失败，同时有效解决单目尺度不可观测的问题

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

VINS (单目+IMU、双目+IMU)
OKVIS (单目+IMU、双目+IMU)
ROVIO (单目+IMU）
RKSLAM (单目+IMU）
ORB_SLAM-IMU（单目+IMU）

AR/VR，自动驾驶，无人机，手机、无人机拍照防抖

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

参考知乎：一些基于深度学习的视觉里程计/SLAM开源代码

2. 四元数和李代数更新

在这里插入图片描述

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/so3.hpp>

using namespace std;

int main()
{
    Eigen::Vector3d vec(1,2,3);
    cout << "vec :"<< endl << vec.transpose() <<endl<<endl;
    cout << "vecnormd :"<< endl << (vec/vec.norm()).transpose() <<endl<<endl;
    Eigen::AngleAxisd angelaxis(M_PI/4, vec/vec.norm());

    // 待旋转矩阵
    Eigen::Matrix3d R = angelaxis.toRotationMatrix();
    cout << "R :"<< endl << R <<endl<<endl;

    Sophus::SO3d SO3_R(R);               // 从旋转矩阵构造Sophus::SO(3)
    Eigen::Quaterniond q(R);            // 从旋转矩阵构造四元数
    cout << "so3 :"<< endl << SO3_R.log().transpose() <<endl<<endl;
    cout << "q :"<< endl << q.coeffs().transpose() <<endl<<endl;

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

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

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

    cout<<"error = "<< endl <<SO3_updated.matrix()-q_updated.toRotationMatrix()<<endl;
}

在这里插入图片描述

3. 其他导数

在这里插入图片描述
李群性质：
$\begin{array}{c} \boldsymbol{C}=\exp \left(\phi^{\wedge}\right) \equiv \sum_{n=0}^{\infty} \frac{1}{n !}\left(\phi^{\wedge}\right)^{n} \\ \equiv \cos \phi \mathbf{1}+(1-\cos \phi) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \phi \boldsymbol{a}^{\wedge} \\ \approx \mathbf 1+\phi^{\wedge} \end{array} \\ \boldsymbol{C}^{-1} \equiv \boldsymbol{C}^{\mathrm{T}} \equiv \sum_{n=0}^{\infty} \frac{1}{n !}\left(-\boldsymbol{\phi}^{\wedge}\right)^{n} \approx \mathbf{1}-\boldsymbol{\phi}^{\wedge}$

$\begin{aligned} \frac{d\left(R^{-1} p\right)}{d R} &=\lim _{\phi \rightarrow 0} \frac{\left(R \cdot \exp \left(\phi^{\wedge}\right)\right)^{-1} \cdot p-R^{-1} p}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{\left(\exp \left(\phi^{\wedge}\right)\right)^{-1} R^{-1} \cdot p-R^{-1} p}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{\exp \left(-\phi^{\wedge}\right) \cdot R^{-1} \cdot p-R^{-1} p}{\phi} \\ &\approx\lim _{\phi \rightarrow 0} \frac{\left(I-\phi^{\wedge}\right) \cdot R^{-1} \cdot p-R^{-1} \cdot p}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{-\phi^{\wedge} R^{-1} p}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{\phi \cdot\left(R^{-1} p\right)^{\wedge}}{\phi} \\ &=\left(R^{-1} p\right)^{\wedge} \end{aligned}$

李群性质：
$(\boldsymbol{C u})^{\wedge} \equiv \boldsymbol{C} \boldsymbol{u}^{\wedge} \boldsymbol{C}^{\mathrm{T}} \\ \exp \left((\boldsymbol{C} \boldsymbol{u})^{\wedge}\right) \equiv \boldsymbol{C} \exp \left(\boldsymbol{u}^{\wedge}\right) \boldsymbol{C}^{\mathrm{T}} \\ \ln { \left( {\exp \left( { \bm{\phi} _1^ \wedge } \right)\exp \left( {\bm{\phi} _2^ \wedge } \right)} \right) ^ \vee } \approx \left\{ \begin{array}{l} {\bm{J}_l}{\left( {{\bm{\phi} _2}} \right)^{ - 1}}{ \bm{\phi} _1} + {\bm{\phi} _2} \quad \text{当} \bm{\phi}_1 \text{为小量},\\ {\bm{J}_r}{\left( {{\bm{\phi} _1}} \right)^{ - 1}}{\bm{\phi} _2} + {\bm{\phi} _1} \quad \text{当} \bm{\phi}_2 \text{为小量}. \end{array} \right.$

$\begin{aligned} \frac{d \ln \left(R_{1} R_{2}^{-1}\right)^{\vee}}{d R_{2}} &=\lim _{\phi \rightarrow 0} \frac{\ln\left(R_{1}\left(R_{2} \cdot \exp \left(\phi^{\wedge}\right)\right)^{-1}\right)^{\vee}-\ln\left(R_{1} R_{2}^{-1}\right)^{\vee}}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1} \cdot \exp \left(-\phi^{\wedge}\right) \cdot R_{2}^{-1}\right)^{\vee}-\ln\left(R_{1} R_{2}^{-1}\right)^{\vee}}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1} \cdot R_{2}^{-1} \cdot R_{2} \cdot \exp \left(-\phi^{\wedge}\right) \cdot R_{2}^{-1}\right)^{\vee}-\ln \left(R_{1} R_{2}^{-1}\right)^{\vee}}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1} \cdot R_{2}^{-1} \cdot \exp \left(\left(-R_{2} \phi\right)^{\wedge}\right)\right)^{\vee}-\ln \left(R_{1} R_{2}^{-1}\right)^{\vee}}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{\ln \left(\exp \left(\ln \left(\left(R_{1} R_{2}^{-1}\right)^{\vee}\right) \cdot \exp \left(\left(-R_{2} \phi\right)^{\wedge}\right)\right)^{\vee}- \ln \left(R_{1} R_{2}^{-1}\right)^{\vee}\right.}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{J_{r}\left(\ln \left(R_{1} R_{2}^{-1}\right)^{\vee}\right)^{-1} \cdot\left(-R_{2} \phi\right)+\ln \left(R_{1} R_{2}^{-1}\right)^{\vee}-\ln \left(R_{1} R_{2}^{-1}\right)^{\vee}}{\phi} \\ &=-J_{r}\left(\ln \left(R_{1} R_{2}^{-1}\right)^{\vee}\right)^{-1} \cdot R_{2} \end{aligned}$