《视觉SLAM进阶：从零开始手写VIO》（一）

最新推荐文章于 2023-10-24 14:54:02 发布

原创最新推荐文章于 2023-10-24 14:54:02 发布 · 1.8k 阅读

16 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶

SLAM 专栏收录该内容

82 篇文章

订阅专栏

本文介绍了视觉SLAM与IMU融合的优势，如互补运动估计误差，提高定位稳定性，并列举了学术界与工业界的融合方案，如MSCKF和ARKit。同时，通过代码展示了四元数和李代数在位姿更新中的应用，强调了融合技术在解决视觉SLAM问题中的重要性。

《视觉SLAM进阶：从零开始手写VIO》第一讲作业

文章目录

- 《视觉SLAM进阶：从零开始手写VIO》第一讲作业

1 视觉与IMU融合后优势互补！

IMU虽然可以测得加速度和角速度，但这些量都存在明显的漂移，使得积分两次得到的位姿数据非常不可靠。但是，对于短时间内的快速运动，IMU能够提供一些较好的估计。这正是相机的弱点。当运动过快时，（卷帘快门的）相机会出现运动模糊，或者两帧之间重叠区域太少以至于无法进行特征匹配，所以纯视觉SLAM非常害怕快速的运动。而有IMU，即使在相机数据无效的那段时间内，我们还能保持一个较好的位姿估计，这是纯视觉SLAM无法做到的。额外要提的一点就是IMU的加速度计容易出现飘移，但是陀螺仪多角度的测量还是很准确的，有的时候会利用这个特性。
相比于IMU，相机数据基本不会有漂移。如果相机放在原地固定不动，那么（在静态场景下）视觉SLAM 的位姿估计也是固定不动的。所以，相机数据可以有效地估计并修正IMU读数中的漂移，使得在慢速运动后的位姿估计依然有效。
当图像发生变化时，本质上我们没法知道是相机自身发生了运动，还是外界条件发生了变化，所以纯视觉SLAM 难以处理动态的障碍物。而IMU能够感受到自己的运动信息，从某种程度上减轻动态物体的影响。现在的扫地机器人基本采用LDS等类似的传感器来实现避障的功能。
对于单目视觉SLAM，存在尺度不确定性，融合IMU后可以恢复尺度。
纯视觉SLAM在容易受弱纹理场景和光照变化的影响，在定位失败时，可以依靠IMU进行短暂的定位。

综上，视觉与IMU融合之后会弥补各自的劣势，可利用视觉定位信息来估计IMU的零偏，减少IMU由零偏导致的发散和累积误差；IMU可以为视觉提供快速运动时的定位，以及因为某种因素（场景特征点较少，光照变化较大等）定位失败时。

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

学术界应用

MSCKF
OKVIS
ROVIO
VIORB
VINS-Mono，VINS-Mobile，VINS-Fusion

工业界应用

Google：Tango，ARCore
Apple：ARKit
Microsoft：HoloLens
百度：DuMix AR

3. 四元数和李代数更新，位姿变化的求导及其连续变化下的表现形式

在这里插入图片描述

//验证在一个小量的变化下 采用旋转矩阵和四元数的差异大小
#include <iostream>

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

#include "sophus/so3.h"
#include "sophus/se3.h"

using namespace std; 

int main( int argc, char** argv )
{
    // 沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    
    cout << "R * R^t ?= I"<< endl << R * R.transpose() <<endl;

    Sophus::SO3 SO3_R(R);               // 从旋转矩阵构造Sophus::SO(3)
    Eigen::Quaterniond q(R);            // 从旋转矩阵构造四元数

    // 李代数更新
    Eigen::Vector3d update_so3(0.01, 0.02, 0.03); //更新量
    Sophus::SO3 SO3_updated = SO3_R * Sophus::SO3::exp(update_so3);
    cout<<"SO3 updated = "<< endl << SO3_updated.matrix() <<endl;
    
    //四元数更新
    Eigen::Quaterniond q_update(1, update_so3(0)/2, update_so3(1)/2, update_so3(2)/2); 
    Eigen::Quaterniond q_updated = (q * q_update).normalized(); //四元数归一化
    cout<<"q2R = "<< endl << q_updated.toRotationMatrix() <<endl;

    return 0;
}

在这里插入图片描述