VINS-FUSION 源码 双目 单线程 按执行顺序阅读

1. 概述

本文依照代码(双目不含imu，只开单线程)执行顺序写成，内容为主要代码加注释的方式，个别会有文字说明，VINS-FUSION开源代码地址为：

https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

错的地方欢迎指正
转载注明来源 - by.十里桃园

吐槽下小觅，只会在知乎抖机灵卖产品，真正的干货基本没，抖机灵抖得多了就很烦了。
再吐槽下vins。vslam的前端在光流法，以至于此算法的天花板颇低。
有两个先天弱点：1，特征点无描述，导致无法用某一帧定位在全图的位置 2，用LK光流跟踪，转弯废

2. 程序流程图(不含回环检测)

   程序位姿计算主要分为两个模块，暂不包含回环检测模块：
   1，前端视觉跟踪及角点信息计算
   2，位姿计算模块

3. 程序入口：
   程序入口：VINS-Fusion/vins-estimator/src/rosNodeTest.cpp

• 3.1 estimator 类初始化

rosNodeTest.cpp:

Estimator estimator;

 

Estimator.cpp:

Estimator::Estimator(): f_manager{Rs}

{

 ROS_INFO("init begins");

initThreadFlag = false; // 是否初始化线程 false则表示还未初始化线程数

clearState();//清除了状态，为诸多变量赋初值

}

• 3.2数据队列queue初始化

rosNodeTest.cpp:

queue<sensor_msgs::PointCloudConstPtr>
feature_buf;//存储feature

queue<sensor_msgs::ImageConstPtr>
img0_buf;//存储image0 左侧图像

queue<sensor_msgs::ImageConstPtr>
img1_buf;//存储image1 左侧图像

• 3.3 main函数

rosNodeTest.cpp:

readParameters(config_file);

estimator.setParameter();//读取并配置文件参数

 

ros::Subscriber
sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);//源数据为特征点

ros::Subscriber
sub_img0 = n.subscribe(IMAGE0_TOPIC, 100, img0_callback);//左侧图像

ros::Subscriber
sub_img1 = n.subscribe(IMAGE1_TOPIC, 100, img1_callback);//右侧图像

ros::Subscriber sub_restart = n.subscribe("/vins_restart", 100,
restart_callback); //restart

void restart_callback(const std_msgs::BoolConstPtr &restart_msg)

{

    if (restart_msg->data
== true)

    {

        ROS_WARN("restart the estimator!");

        estimator.clearState();//清除状态 赋初始值

        estimator.setParameter();//根据配置文件赋值

    }

    return;

}

 

std::thread sync_thread{sync_process};//入口

main函数为程序入口程序，值得注意的是sync_process为程序入口。

4. 接受源图像数据及处理接口

接受源图像数据定义在synv_process函数中，并送入estimator进行位姿估计和后续运算。其接受图像代码块如下：

rosNodeTest.cpp:

double time0 =
img0_buf.front()->header.stamp.toSec();

                double time1 =
img1_buf.front()->header.stamp.toSec();

                // 0.003s sync tolerance  双摄像头同步 阈值0.003

                if(time0 < time1 - 0.003)

                {

                    img0_buf.pop();

                    printf("throw img0\n");

                }

                else if(time0 > time1 + 0.003) 

                {

                    img1_buf.pop();

                    printf("throw img1\n");

                }//双摄像头同步 购买的摄像头已经做了同步 ros消息时间戳完全相同

 

                else

                {

                    time =
img0_buf.front()->header.stamp.toSec();

                    header =
img0_buf.front()->header;

                    image0 =
getImageFromMsg(img0_buf.front());//接收到的左图像   1通道  

                    img0_buf.pop();

                    image1 =
getImageFromMsg(img1_buf.front());//接收到的右图像   1通道 

                    img1_buf.pop();

                    //printf("find img0 and img1\n");

               
}

在接受到源图像之后，送入estimator：

  rosNodeTest.cpp:
  if(!image0.empty())
     estimator.inputImage(time, image0,
  image1);//双目图像送入estimator

5. 角点检测和视觉跟踪

• 5.1 外层调用接口

VINS-FUISON调用的OPENCV的角点检测和光流法跟踪函数，其外层调用接口如下：

estimator.cpp:
Estimator::inputImage()

map<int, vector<pair<int, Eigen::Matrix<double, 7,
1>>>> featureFrame;//map 里面的pair为 <int, Eigen::Matrix<double, 7, 1>>类型 存储featureFrame map

 

if(_img1.empty())

        featureFrame =
featureTracker.trackImage(t, _img);//角点检测和跟踪 单目

    else

       
featureFrame = featureTracker.trackImage(t, _img, _img1);//角点检测和跟踪 双目

 

if (SHOW_TRACK)//是否发布TrackImage话题 RVIZ里面的 track_image窗口

{  

     cv::Mat imgTrack =
featureTracker.getTrackImage();

         //featureTracker.showUndistortion();//显示去畸变的图像，dataset可以正常显示 ，摄像头有黑白间隔异常

     pubTrackImage(imgTrack, t);

 }

• 5.2 角点检测

VINS-FUSION采用的为Shi-Tomasi角点。

feature_tracker.cpp:
FeatureTracker::setMask ()

cv::circle(mask, it.second.first, MIN_DIST,
0, -1);//把特征点周围MIN_DIST的点设置为0

feature_tracker.cpp:
FeatureTracker::trackImage()

cv::goodFeaturesToTrack(cur_img,
n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);

//cur_img
当前输入的单通道图像;

//n_pts 输出的被提取的角点坐标;

//MAX_CNT为设置的最大角点个数程序中会按照角点强度降序排序，超过MAX_CNT的角点将被舍弃;

//0.01为角点强度的阈值系数，若检测出所有角点中的最大强度是max，那么强度值小于max*qualityLevel的角点被舍弃

//MIN_DIST为角点与其邻域强角点之间的欧式距离，与邻域强角点距离小于minDistance的角点将被舍弃;

//mask : 感兴趣区域，

• 5.3 光流法跟踪

VINS-FUSION采用基于角点特征的金字塔LK光流跟踪算法。VINS所定义的跟踪包含两种模式：前后帧跟踪、左右帧间跟踪

前后帧跟踪，以前帧为基础，跟踪当前帧：

feature_tracker.cpp:
FeatureTracker::trackImage()

cv::calcOpticalFlowPyrLK(prev_img,
cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);

                   //输入：prev_img 第一张图像  
cur_img： 当前图像    
prev_pts第一张图像中的点

                   //输出   
nextPts   当前图像的点  


                   //金字塔三层

if(FLOW_BACK)// 反向的光流跟踪，通过上一帧跟踪到的点，反向跟踪回去，差异大的点剔除。 默认打开->bot.yaml

        {

            vector<uchar> reverse_status;

            vector<cv::Point2f>
reverse_pts = prev_p