萌新学VINS-Fusion（一）------特征跟踪

VINS-FUSION代码心得

新人小白，第一次写博客，主要相当于自己做一个关于学习VINS的笔记，不喜勿喷，转载请注明出处。

其实我之前也尝试着在VINS-MONO基础上改写双目的，快改完的时候，沈老师把VINS-FUSION开源了，那我们就一起来学习一下吧。（本文章仅仅是个人的粗浅理解，不到位或者有错误，请大家批评指正）
PS：我之前开源了自己剥除了ROS后的VINS-MONO，并且将位置约束（GPS）作为一个factor添加到优化中去的代码，写的也比较简陋。 https://github.com/grn213331/remove_ros_VINS-position-constraint
等我把我写的双目代码改好，也可能会放到github上，大家可以互相交流。

本文面向已经阅读过vins-mono的盆友，如果你还没看过，建议看以下博客，我最开始看的
https://blog.csdn.net/wangshuailpp/article/details/78461171

官方README

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

官方简介：VINS-Fusion is an optimization-based multi-sensor state estimator, which achieves accurate self-localization for autonomous applications (drones, cars, and AR/VR). VINS-Fusion is an extension of VINS-Mono, which supports multiple visual-inertial sensor types (mono camera + IMU, stereo cameras + IMU, even stereo cameras only). We also show a toy example of fusing VINS with GPS.

这段话意思就是VINS-Fusion是在VINS-Mono上的扩展，VINS-Mono只支持IMU+单目，VINS-Fusion新加了双目+IMU，只有双目，甚至还和GPS做了融合。

main函数

我们以rosNodeTest.cpp文件中的main函数作为程序入口进行讲解。

程序开始首先是ros节点的初始化，之后检查参数，然后读取参数。

紧接着是四个节点订阅，对应四个回调函数，得到四种数据，分别是imu、feature（这个我还没看到是在哪里播发的）、左相机照片、右相机照片。之后开启处理数据线程。

    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    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);
	
	std::thread sync_thread{sync_process};//开启处理数据线程

回调函数主要就是把左右img和imu数据存储到相应的缓冲区内。在sync_process才是对其进行的处理。其中双目代码如下

        if(STEREO)
        {
            cv::Mat image0, image1;
            std_msgs::Header header;
            double time = 0;
            m_buf.lock();
            if (!img0_buf.empty() && !img1_buf.empty())
            {
                double time0 = img0_buf.front()->header.stamp.toSec();
                double time1 = img1_buf.front()->header.stamp.toSec();
                if(time0 < time1)
                {
                    img0_buf.pop();
                    printf("throw img0\n");
                }
                else if(time0 > time1)
                {
                    img1_buf.pop();
                    printf("throw img1\n");
                }
                else
                {
                    time = img0_buf.front()->header.stamp.toSec();
                    header = img0_buf.front()->header;
                    image0 = getImageFromMsg(img0_buf.front());
                    img0_buf.pop();
                    image1 = getImageFromMsg(img1_buf.front());
                    img1_buf.pop();
                    //printf("find img0 and img1\n");
                }
            }
            m_buf.unlock();
            if(!image0.empty())
                estimator.inputImage(time, image0, image1);

当判断为双目的时候，首先判断img0_buf、img1_buf缓冲区和时标是否异常，正常的话，通过getImageFromMsg（）函数获取图片，该函数主要功能就是把ros中sensor_msgs::ImageConstPtr类型的图片转换成opencv中的标准图像格式cv::Mat输出。最后把两个cv::Mat格式的图片通过estimator.inputImage（）函数传入，之后便进行特征提取。
（需要注意，VINS-Mono里面feature_tracker和estimator是完全分开的，而这里feature_tracker包含在estimator里，唯一的差别在于减少了前端跟踪和后端之间的rostopic的发布和订阅）

feature_tracker

主要就是在feature_tracker.cpp中实现的，单目跟踪方面几乎和VINS-MONO（为了方便后面称VM）一样，只不过是把VM中的一系列小函数整合到了一个trackImage函数中。

为了得到更好的前后帧之间的跟踪，加入了FLOW_BACK，即反向的光流跟踪，通过上一帧跟踪到的点，反向跟踪回去，差异大的点剔除。如代码所示

if(hasPrediction)//这个判断我也不知道是干嘛的，代码里好像hasPrediction一直==false
        {
            cur_pts = predict_pts;
            cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1, 
            cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
            
            int succ_num = 0;
            for (size_t i = 0; i < status.size(); i++)
            {
                if (status[i])
                    succ_num++;
            }
            if (succ_num < 10)
               cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
        }
        else
            cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);//光流跟踪，上一帧到当前帧
        // reverse check
        if(FLOW_BACK)//反向跟踪
        {
            vector reverse_status;
            vector reverse_pts = prev_pts;
            cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1, 
            cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);//反向光流
            //cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3); 
            for(size_t i = 0; i < status.size(); i++)
            {
                if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
                //当正向和反向都跟踪成功，并且反向跟踪回来的点与前一帧的点像素小于0.5时，跟踪成功（提高跟踪精度）
                {
                    status[i] = 1;
                }
                else
                    status[i] = 0;
            }
        }

主要添加部分是在左右图像的匹配上，方法也是光流跟踪，由左图跟踪到右图，得到右图的匹配点，在反过来光流，剔除掉偏差大的点，但是这里并没有像orb-slam那样直接就三角化出深度，而是把点保存下来，放到后面计算。如代码所示

        //printf("stereo image; track feature on right image\n");
        vector reverseLeftPts;
        vector status, statusRightLeft;
        vector err;
        // cur left ---- cur right
        cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
        //当前帧光流到当前右侧图片，找到右帧图片的匹配点
        // reverse check cur right ---- cur left
        if(FLOW_BACK)
        {
            cv::calcOpticalFlowPyrLK(rightImg, cur_img, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);
            for(size_t i = 0; i < status.size(); i++)
            {
                if(status[i] && statusRightLeft[i] && inBorder(cur_right_pts[i]) && distance(cur_pts[i], reverseLeftPts[i]) <= 0.5)
                //从右帧图像反向光流回来，并进行判断，以提高左右视图特征点的匹配精度
                    status[i] = 1;
                else
                    status[i] = 0;
            }
        }

跟踪完成后，构建一个特征容器
map>>> featureFrame
第一个int为feature的id，vector里面的int为相机id（0为左图，1为右图），后面的Eigen::Matrix类型里面包含该特征点在该相机下的信息
分别为:归一化平面坐标（x,y,z=1），像素坐标（u,v），像素移动速度（v_x,v_y），共七维。

将featureFrame加入到featurebuf中，传到后端进行图像处理。

前端追踪就到此结束了，后端还没看完后续更。