VINS-FUSION代码阅读VIO2

添加链接描述IMU数据处理
VINS-Fusion代码按执行顺序阅读（一）
VINS-Fusion代码按执行顺序阅读（二）
VINS-Fusion代码按执行顺序阅读（三）
VINS-Fusion代码按执行顺序阅读（四）
使用自己的INDEMIND相机来运行ORBSLAM2单目,双目和深度模式(小觅相机和realsense通用)
INDEMIND 双目相机使用教程
INDEMIND官网SDK
VINS-FUSION 源码 双目 单线程 按执行顺序阅读
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(1)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(2)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(3）
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(4）
【泡泡读者来稿】VINS 论文推导及代码解析（一）
【泡泡读者来稿】VINS 论文推导及代码解析（二）
【泡泡读者来稿】VINS 论文推导及代码解析（三）
【泡泡读者来稿】VINS 论文推导及代码解析（四）

注释代码转自蒙牛铁观音

VINS代码阅读

在rosNodeTest.cpp中的sync_process()中estimator.inputImage(time, image0, image1);转到estimator中

void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
    inputImageCnt++;
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
    TicToc featureTrackerTime;

    if(_img1.empty())
        featureFrame = featureTracker.trackImage(t, _img);
    else
        featureFrame = featureTracker.trackImage(t, _img, _img1);
    //printf("featureTracker time: %f\n", featureTrackerTime.toc());

    if (SHOW_TRACK)
    {
        cv::Mat imgTrack = featureTracker.getTrackImage();
        pubTrackImage(imgTrack, t);
    }
    
    if(MULTIPLE_THREAD)  
    {     
        if(inputImageCnt % 2 == 0)
        {
            mBuf.lock();
            featureBuf.push(make_pair(t, featureFrame));
            mBuf.unlock();
        }
    }
    else
    {
        mBuf.lock();
        featureBuf.push(make_pair(t, featureFrame));
        mBuf.unlock();
        TicToc processTime;
        processMeasurements();
        printf("process time: %f\n", processTime.toc());
    }
    
}

1.首先判断是单目还是双目，然后经过featureTracker.trackImage得到featureframe
2.if (SHOW_TRACK)是否展示轨迹
3.if(MULTIPLE_THREAD)多线程不会
4.featureBuf.push(make_pair(t, featureFrame));将featureFrame放入队列。
5.processMeasurements();这里执行这个线程（处理全部量测得线程IMU预积分、特征点处理）
**

1.FeatureTracker::trackImage

**
对图像进行处理（图像处理、区域Mark、检测特征点、计算像素速度、基于角点特征的金字塔LK光流跟踪-前后帧、左右帧跟踪），返回featureFrame。

1.前后帧跟踪----正向反向状态都为1且distance<0.5
2.setMark(特征周围MIN_DIST设为0)追踪到的点为0，否则为255
  shi-Tomasi角点   goodFeaturesToTrack(若当前图像的特征点小于规定max，则进行提取)
3.角点去畸变cam->lifeProjective(a,b) 调用图像像素投影函数将特征点坐标映射到空间坐标
 在cataCamera.cc中(鱼眼相机标定去畸变过程)
 undistorttedPts
4.左右帧跟踪
5.当前角点的速度ptsVelocity
6.数据结构featureFrame
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>
map<feature_id,>;pair<camera_id,>;Matrix<x,y,z,p.u,p.v,vel_x,vel_y>

注释代码转自蒙牛铁观音

// 对图片进行一系列操作，返回特征点featureFrame。
// 其中还包含了：图像处理、区域mask、检测特征点、计算像素速度等
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)
{
    TicToc t_r; // 定义一个时间，为了计时
    cur_time = _cur_time;
    cur_img = _img;
    row = cur_img.rows;
    col = cur_img.cols;
    cv::Mat rightImg = _img1;
    /*//TODO: 这里使用来做图像处理的！！！
    {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//createCLAHE 直方图均衡
        clahe->apply(cur_img, cur_img);
        if(!rightImg.empty())
            clahe->apply(rightImg, rightImg);
    }
    */
    cur_pts.clear();

    // --------------如果上一帧有特征点，就直接进行LK追踪
    if (prev_pts.size() > 0) 
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;

        if(hasPrediction)
        {
            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)//小于10时，好像会扩大搜索，输入的基于最大金字塔层次数为3
               cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
        }
        else 
            //如果没有进行预测的话，直接是基于最大金字塔层次数为3
            cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
        
        
        // reverse check 方向检查
        if(FLOW_BACK)//
        {
            vector<uchar> reverse_status;
            vector<cv::Point2f> 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++)
            {
                //如果前后都能找到，并且找到的点的距离小于0.5
                if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
                {
                    status[i] = 1;
                }
                else
                    status[i] = 0;
            }
        }
        
        for (int i = 0; i < int(cur_pts.size()); i++)
            if (status[i] && !inBorder(cur_pts[i]))// 如果这个点不在图像内，则剔除
                status[i] = 0;
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
        printf("track cnt %d\n", (int)ids.size());
    }

    for (auto &n : track_cnt)
        n++;

    // --------------对所有的帧，先计算一个mask，然后检测器特征点是否够多，不够多则提取新的，存入到cur_pts
    if (1)
    {
        // rejectWithF();
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        setMask();
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        ROS_DEBUG("detect feature begins");
        TicToc t_t;

        // 如果当前图像的特征点cur_pts数目小于规定的最大特征点数目MAX_CNT，则进行提取
        int n_max_cnt = MAX_CNT - static_cast<int>(cur_pts.size()); // 在图像上所需要提取的特征点最大值
        if (n_max_cnt > 0)
        {
            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1)
                cout << "mask type wrong " << endl;
            /* goodFeaturesToTrack
            _image：8位或32位浮点型输入图像，单通道
            _corners：保存检测出的角点
            maxCorners：角点数目最大值，如果实际检测的角点超过此值，则只返回前maxCorners个强角点
            qualityLevel：角点的品质因子
            minDistance：对于初选出的角点而言，如果在其周围minDistance范围内存在其他更强角点，则将此角点删除
            _mask：指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI
            blockSize：计算协方差矩阵时的窗口大小
            useHarrisDetector：指示是否使用Harris角点检测，如不指定，则计算shi-tomasi角点
            harrisK：Harris角点检测需要的k值 */
            cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
            // mask 这里肯定是指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %f ms", t_t.toc());

        // 对于所有新提取出来的特征点，加入到cur_pts
        for (auto &p : n_pts)
        {
            cur_pts.push_back(p);
            ids.push_back(n_id++);
            track_cnt.push_back(1);
        }
        //printf("feature cnt after add %d\n", (int)ids.size());
    }

    cur_un_pts = undistortedPts(cur_pts, m_camera[0]); //当前帧不失真的点
    pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map);

    // ---------------如果是双目的
    if(!_img1.empty() && stereo_cam) 
        // 把左目的点在右目上找到，然后计算右目上的像素速度。
    {
        ids_right.clear();
        cur_right_pts.clear();
        cur_un_right_pts.clear();
        right_pts_velocity.clear();
        cur_un_right_pts_map.clear();

        // --------------------如果当前帧非空
        if(!cur_pts.empty())
        {
            //printf("stereo image; track feature on right image\n"); //在右侧图像上追踪特征

            vector<cv::Point2f> reverseLeftPts;
            vector<uchar> status, statusRightLeft; //左右目的状态
            vector<float> err;

            /*光流跟踪是在左右两幅图像之间进行cur left ---- cur right
            prevImg	第一幅8位输入图像 或 由buildOpticalFlowPyramid()构造的金字塔。
            nextImg	第二幅与preImg大小和类型相同的输入图像或金字塔。
            prevPts	光流法需要找到的二维点的vector。点坐标必须是单精度浮点数。
            nextPts	可以作为输入，也可以作为输出。包含输入特征在第二幅图像中计算出的新位置的二维点（单精度浮点坐标）的输出vector。当使用OPTFLOW_USE_INITIAL_FLOW 标志时，nextPts的vector必须与input的大小相同。
            status	输出状态vector(类型：unsigned chars)。如果找到了对应特征的流，则将向量的每个元素设置为1；否则，置0。
            err	误差输出vector。vector的每个元素被设置为对应特征的误差，可以在flags参数中设置误差度量的类型；如果没有找到流，则未定义误差（使用status参数来查找此类情况）。
            winSize	每级金字塔的搜索窗口大小。
            maxLevel	基于最大金字塔层次数。如果设置为0，则不使用金字塔（单级）；如果设置为1，则使用两个级别，等等。如果金字塔被传递到input，那么算法使用的级别与金字塔同级别但不大于MaxLevel。
            criteria	指定迭代搜索算法的终止准则（在指定的最大迭代次数标准值（criteria.maxCount）之后，或者当搜索窗口移动小于criteria.epsilon。）
            flags 操作标志，可选参数：
            OPTFLOW_USE_INITIAL_FLOW：使用初始估计，存储在nextPts中；如果未设置标志，则将prevPts复制到nextPts并被视为初始估计。
            OPTFLOW_LK_GET_MIN_EIGENVALS：使用最小本征值作为误差度量（见minEigThreshold描述）；如果未设置标志，则将原始周围的一小部分和移动的点之间的 L1 距离除以窗口中的像素数，作为误差度量。
            minEigThreshold	
            算法所计算的光流方程的2x2标准矩阵的最小本征值（该矩阵称为[Bouguet00]中的空间梯度矩阵）÷ 窗口中的像素数。如果该值小于MinEigThreshold，则过滤掉相应的特征，相应的流也不进行处理。因此可以移除不好的点并提升性能。 */
            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;
                }
            }

            ids_right = ids;
            reduceVector(cur_right_pts, status);
            reduceVector(ids_right, status);
            // 仅保留左右目都有的点only keep left-right pts
            /*
            reduceVector(cur_pts, status);
            reduceVector(ids, status);
            reduceVector(track_cnt, status);
            reduceVector(cur_un_pts, status);
            reduceVector(pts_velocity, status);
            */


            cur_un_right_pts = undistortedPts(cur_right_pts, m_camera[1]);//右目的归一化平面点
            right_pts_velocity = ptsVelocity(ids_right, cur_un_right_pts, cur_un_right_pts_map, prev_un_right_pts_map);
        }
        prev_un_right_pts_map = cur_un_right_pts_map;
    }

    // 展示轨迹 featureFrame是在这构造的
    if(SHOW_TRACK)
        drawTrack(cur_img, rightImg, ids, cur_pts, cur_right_pts, prevLeftPtsMap);


    // ----------------------构建特征点featureFrame，并加入特征点
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    prev_un_pts_map = cur_un_pts_map;
    prev_time = cur_time;
    hasPrediction = false;
    prevLeftPtsMap.clear();
    for(size_t i = 0; i < cur_pts.size(); i++)
        prevLeftPtsMap[ids[i]] = cur_pts[i];

    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
    for (size_t i = 0; i < ids.size(); i++)
    {
        int feature_id = ids[i];
        double x, y ,z;
        x = cur_un_pts[i].x;
        y = cur_un_pts[i].y;
        z = 1;
        double p_u, p_v;
        p_u = cur_pts[i].x;
        p_v = cur_pts[i].y;
        int camera_id = 0;//左目的特征点
        double velocity_x, velocity_y;
        velocity_x = pts_velocity[i].x;
        velocity_y = pts_velocity[i].y;

        Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
        xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
        featureFrame[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
        // 特征点的id，相机id（0或1） 和 xyz_uv_velocity（特征点空间坐标，像素坐标和像素速度）
    }

    // ---------
    if (!_img1.empty() && stereo_cam)
    {
        for (size_t i = 0; i < ids_right.size(); i++)
        {
            int feature_id = ids_right[i];
            double x, y ,z;
            x = cur_un_right_pts[i].x;
            y = cur_un_right_pts[i].y;
            z = 1;
            double p_u, p_v;
            p_u = cur_right_pts[i].x;
            p_v = cur_right_pts[i].y;
            int camera_id =  1;//右目的特征点
            double velocity_x, velocity_y;
            velocity_x = right_pts_velocity[i].x;
            velocity_y = right_pts_velocity[i].y;

            Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
            xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
            featureFrame[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
        }
    }
    // printf("feature track whole time %f\n", t_r.toc());
    return featureFrame;
}