vins 解读_VINS-mono详细解读

最新推荐文章于 2024-08-08 08:27:07 发布

清水33

最新推荐文章于 2024-08-08 08:27:07 发布

阅读量1k

点赞数 3

文章标签： vins 解读

本文链接：https://blog.csdn.net/weixin_30969157/article/details/111973181

版权

本文详细解读了VINS-mono单目视觉惯性里程计的实现原理，包括特征点跟踪、初始化过程（外参旋转计算、SFM、陀螺仪偏移、速度重力尺度对齐）、地图点优化及回环检测。通过对源码的分析，阐述了算法如何通过融合单目图像和IMU数据来获取精确的尺度和姿态估计。

摘要由CSDN通过智能技术生成

VINS-mono详细解读

极品巧克力

前言

Vins-mono是香港科技大学开源的一个VIO算法，https://github.com/HKUST-Aerial-Robotics/VINS-Mono，是用紧耦合方法实现的，通过单目+IMU恢复出尺度，效果非常棒。

感谢他们开源，我从中学到了非常多的知识。源码总共有15000多行，我在通读完程序之后，结合参考文献，把程序背后的算法理论都推导了一遍，总结成了本文，与各位分享。

本文目标读者：对vins-mono有一定了解的SLAM算法工程师。由于程序里有非常多的实现细节，建议读者在读本文前，先读一遍vins-mono的程序。

1.特征点跟踪

首先用cv::goodFeaturesToTrack在第一帧图像上面找最强的150个特征点，非极大值抑制半径为30。新的特征点都有自己的新的对应的id。然后在下一帧过来时，对这些特征点用光流法进行跟踪，在下一帧上找匹配点。然后对前后帧中这些匹配点进行校正。先对特征点进行畸变校正，再投影到以原点为球心，半径为1的球面上，再延伸到深度归一化平面上，获得最终校正后的位置。对于每对匹配点，基于校正后的位置，用F矩阵加ransac来筛选。然后再在匹配上的特征点之外的区域，用cv::goodFeaturesToTrack搜索最强的新的特征点，把特征点数量补上150个。

最后，把剩下的这些特征点，把图像点投影回深度归一化平面上，再畸变校正，再投影到球面上，再延伸到深度归一化平面上，得到校正后的位置。把校正后的位置发送出去。

特征点跟踪和匹配，就是前一帧到这一帧的，一帧帧继承下去。或者生成新的特征点。

2.初始化

2.1外参中的旋转

用机器人手眼标定的方法，计算出外参中的旋转。

其中

是陀螺仪预积分得到的，

是用8点法对前后帧对应的特征点进行计算得到的。详细见《Monocular Visual-Inertial State Estimation With Online Initialization and Camera-IMU Extrinsic Calibration》。

2.2 SFM

先在关键帧窗口里面，找到第l帧，第l帧与最后一帧有足够的像素位移，并且能用8点法算出旋转和位移。以l帧的姿态为世界坐标系。先从l帧开始与最后一帧进行三角定位，再用pnp估计出下一帧的位姿，下一帧再与最后一帧三角定位得出更多的三维点。重复到倒数第二帧。从l帧开始往第一帧，逐渐帧pnp，再与第l帧进行三角定位得到更多的三维点。每帧pnp时的位姿初值都用上一个关键帧的的位姿。剩下的那些还没有被三角定位的特征点，通过它被观察到的第一帧和最后一帧进行三角定位。

固定住l帧的位置和姿态，固定住最后一帧的位置。因为这时候的图像位姿和点的位置都不太准，所以用ceres统一一起优化图像位姿和三维点位置，优化重投影误差。优化的测量值是，特征点在每帧中被观察到的位置，可以转成重投影误差约束。有关的自变量是，每帧图像的位姿，特征点的三维坐标。

优化完成之后，即用ceres优化出这些关键帧的位姿和地图点后，再用pnp算出在这段时间区域内的所有图像的位姿。每个图像的计算都用下一个关键帧的位姿来当pnp的初值。

程序里面没有求雅克比，而是用自动求导的方法。

2.3 计算陀螺仪的偏移

在2.1中已经根据连续图像的相对旋转算出相机和IMU间的外参旋转了，现在要再根据上一节2.2中的SFM算出来的各帧图像的相对旋转来计算出陀螺仪的偏移。

就是根据前后帧之间的根据陀螺仪预积分出来的旋转

与基于SFM图像算出来的旋转转换到IMU坐标系的相对旋转

之间的向量差的两倍。

在程序里面，每次算出的图像的姿态

，都会转换成

。然后在计算相对IMU的姿态时，就用

。