Vins-mono是香港科技大学开源的一个VIO算法 是用紧耦合方法实现的，通过单目+IMU恢复出尺度，效果非常棒。

1.特征点跟踪

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

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

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

2.初始化

2.1外参中的旋转

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

特别的：手眼标定：

其实就是AX=XB  X是手眼的外参（对于VINS问题，也就是IMU和Camera的外参）

A是IMU在相邻时间的变换   B是摄像头的变换

从而：

2.2 SFM

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

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

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

2.3 计算陀螺仪的偏移

重构：

1——框架解析

     写在前面：本文整和自己的思路，希望对学习VINS或者VIO的同学有所帮助，如果你觉得文章写的对你的理解有一点帮助，可以推荐给周围的小伙伴们当然，如果你有任何问题想要交流，欢迎随时探讨。话不多说，下面上正文。   

    VINS代码主要包含在两个文件中，分别是feature_tracker和vins_estimate，feature_tracker就像文件的名字一样，总体的作用是接收图像，使用KLT光流算法跟踪；vins_estimate包含相机和IMU数据的前端预处理（也就是预积分过程）、单目惯性联合初始化（在线的标定过程）、基于滑动窗口的BA联合优化、全局的图优化和回环检测等。要想真正的理解一个SLAM框架，必须真正搞懂其对应的算法模型，然后才能研究其代码逻辑，最后做到相得益彰的效果，因此本次讲解主要是结合论文中的理论知识这和两个文件中的代码进行详细的探讨。整体的框架都比较熟悉，如下图所示，第一部分是Measuremen Preprocessing：观测值数据预处理，包含图像数据跟踪IMU数据预积分；第二部分是Initialization：初始化，包含单纯的视觉初始化和视觉惯性联合初始化；第三部分Local Visual-Inertia BA and Relocalization：局部BA联合优化和重定位，包含一个基于滑动窗口的BA优化模型；第四部分Global Pose Graph Optimization：全局图优化，只对全局的位姿进行优化；第五部分Loop detection：回环检测。

2——单目视觉跟踪

一．Feature_tracker文件夹中

         首先讲第一部分，也就是纯粹的图像处理部分内容，在论文中的第IV点观测值预处理的A部分视觉前端处理，为了更好的理解代码，有必要将论文中的相关内容和大家讨论一番。

         论文内容：每当进入新的图像，都会使用KLT稀疏光流法进行跟踪，同时提取100-300个角点信息，我的理解是角点是用来建立图像，光流跟踪是用来快速定位。同时在这里还进行了关键帧的选取（注意这一过程在代码中是由vins_estimate文件中实现的），主要是两个剔除关键帧的策略，分别是平均视差法和跟踪质量法。平均视差法：如果当前帧的和上一个关键帧跟踪点的平均视差超出了一个设定的阈值，就将当前帧设为关键帧。这里有一个问题，就是旋转和平移都会产生视差（不只是平移哦），当出现纯旋转的时候特征点无法被三角化，无法计算出旋转值，也就无法计算跟踪点间的平均视差，为了解决这一问题，采用短时的陀螺仪观测值来补偿旋转，从而计算出视差，这一过程只应用到平均视差的计算，不会影响真实的旋转结果。

         具体代码实现：主要负责图像角点提取和光流跟踪，只有一个主线程。主要是三个源程序，分别是feature_tracker、feature_tracker_node以及parameters。feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。

1.      feature_tracker_node.cpp系统入口

（1）      main()函数

步骤1：readParameters(n);读取参数，是config->euroc->euroc_config.yaml中的一些配置参数。

步骤2: trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);在这里NUM_OF_CAM设置成常量1，只有一个摄像头（单目），读取相机内参。

步骤3:判断是否加入鱼眼mask来去除边缘噪声

步骤4: ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);订阅话题和发布话题，监听IMAGE_TOPIC（/cam0/image_raw），有图像发布到这个话题上的时候，执行回调函数，这里直接进入到img_callback函数中接收图像，前端视觉的算法基本在这个回调函数中。

1）  img_callback(const sensor_msgs::ImageConstPtr &img_msg)接收图像

步骤1: 频率控制，保证每秒钟处理的image不多于FREQ，这里将平率控制在10hz以内。

步骤2: 处理单目相机

步骤2.1: trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW *(i + 1)));读取到的图像数据存储到trackerData中，读取完之后如果图像太亮或太黑（EQUALIZE=1），使用createCLAHE对图像进行自适应直方图均衡化，如果图像正常，设置成当前图像。在读取图像的时候进行光流跟踪和特征点的提取。FeatureTracker类中处理的主要函数就是readImage()，这里涉及到3个img(prev_img, cur_img, forw_img)和pts(prev_pts,cur_pts, forw_pts)，两者是相似的。刚开始看不是太好理解，cur和forw分别是LK光流跟踪的前后两帧，forw才是真正的“当前”帧，cur实际上是上一帧，而prev是上一次发布的帧，它实际上是光流跟踪以后，prev和forw根据Fundamental Matrix做RANSAC剔除outlier用的，也就是rejectWithF()函数. readImage()的处理流程为:

①先调用cv::CLAHE对图像做直方图均衡化(如果EQUALIZE=1，表示太亮或则太暗)

②调用calcOpticalFlowPyrLK()跟踪cur_pts到forw_pts,根据status,把跟踪失败的点剔除(注意:prev, cur,forw, ids, track_cnt都要剔除),这里还加了个inBorder判断,把跟踪到图像边缘的点也剔除掉.

③如果不需要发布特征点,则到这步就完了,把当前帧forw赋给上一帧cur, 然后退出.如果需要发布特征点(PUB_THIS_FRAME=1), 则执行下面的步骤

④先调用rejectWithF()对prev_pts和forw_pts做ransac剔除outlier.(实际就是调用了findFundamentalMat函数), 在光流追踪成功就记被追踪+1，数值代表被追踪的次数，数值越大，说明被追踪的就越久

⑤调用setMask(), 先对跟踪点forw_pts按跟踪次数降排序, 然后依次选点, 选一个点, 在mask中将该点周围一定半径的区域设为0, 后面不再选取该区域内的点. 有点类似与non-max suppression, 但区别是这里保留track_cnt最高的点.

⑥在mask中不为0的区域,调用goodFeaturesToTrack提取新的角点n_pts, 通过addPoints()函数push到forw_pts中, id初始化-1,track_cnt初始化为1.

整体来说需要注意的是：光流跟踪在②中完成，角点提取在⑥中完成

步骤2.2:判断是否需要显示畸变。

步骤2.3:将特征点矫正（相机模型camodocal）后归一化平面的3D点（此时没有尺度信息，3D点p.z=1），像素2D点，以及特征的id，封装成ros的sensor_msgs::PointCloud消息类型的feature_points实例中；将图像封装到cv_bridge::CvImageConstPtr类型的ptr实例中

步骤3: 发布消息的数据

pub_img.publish(feature_points);

pub_match.publish(ptr->toImageMsg())

将处理完的图像信息用PointCloud实例feature_points和Image的实例ptr消息类型，发布到"feature"和"feature_img"的topic（此步骤在main函数中完成）

    至此，已经将图像数据包装成特征点数据和图像数据发布出来了，下面就是在开一个线程，发布一个话题，接收这两种消息，也就是下面的vins_esitimate文件中做的事