VINS-Fusion代码阅读（五）

对应解析13页，四、前端视觉处理
主要包括特征点检测和特征点跟踪两部分，似乎是基于openCV实现的。openCV2.4官方文档，（暂时未找到openCV3.4有类似的文档）
这一节在解析中的内容较少（可能崔神认为比较简单），然而于我个人而言，它依然是个新东西。所以，仍然得仔细推敲。
（发现VINS-Mono和VINS-Fusion之中的文件组织结构也略有差别，之后进行一下详细对比，先主要阅读VINS-Fusion的代码）

featureTracker文件夹下有个feature_tracker.h文件，其中定义了一个FeatureTracker类。
发现通篇仅有一个成员函数FeatureTracker::trackImage()中调用了cv::goodFeaturesToTracke()函数来检测特征点，因此，从该函数开始分析：
输入参数：_cur_time，_img，_img1
返回参数：map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> （乍看起来可复杂，细细分析之）首先是一个map，键值对中的值是一个vector，其中每个元素是一个pair。具体什么含义？

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1 = cv::Mat());

（洋洋洒洒写了200行左右，看来并不简单）
TicToc是一个类，感觉和计时有关，使用std::chrono实现（The elements in this header deal with time.）暂时不展开。
predict_pts，cur_pts：类数据成员，vector<cv::Point2f> predict_pts，cur_pts;
hadPrediction：数据成员，bool hasPrediction;，初始值为什么？(254行赋值为hasPrediction = false;)

如果值为true，则调用openCV的函数进行光流跟踪：
发现该cv::calcOpticalFlowPyrLK()函数所需参数好多。因为是核心，此处展开来讲（calcOpticalFlowPyrLK官方介绍）：
函数功能：Calculates an optical flow for a sparse feature set using the iterative Lucas-Kanade method with pyramids.
使用带金字塔的迭代Lucas Kanade方法 计算稀疏特征集的光流。
C++ API:
void calcOpticalFlowPyrLK(InputArray prevImg, InputArray nextImg, InputArray prevPts, InputOutputArray nextPts, OutputArray status, OutputArray err, Size winSize=Size(21,21), int maxLevel=3, TermCriteria criteria=TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01), int flags=0, double minEigThreshold=1e-4 )
详细解释每个参数的含义：
prevImg – first 8-bit input image or pyramid constructed by buildOpticalFlowPyramid().

nextImg – second input image or pyramid of the same size and the same type as prevImg.

prevPts – vector of 2D points for which the flow needs to be found; point coordinates must be single-precision floating-point numbers.
需要找到流的二维点（存放在vector里），点坐标必须是单精度浮点型数字。

nextPts – output vector of 2D points containing the calculated new positions of input features in the second image; when OPTFLOW_USE_INITIAL_FLOW flag is passed, the vector must have the same size as in the input.
在第二个图像中，计算得到的输入特征所在的新位置的二维点（存放在vector里）；当标志位为OPTFLOW_USE_INITIAL_FLOW时，该vector必须与输入中的vector大小相同。

status – output status vector (of unsigned chars); each element of the vector is set to 1 if the flow for the corresponding features has been found, otherwise, it is set to 0.

err – output vector of errors; each element of the vector is set to an error for the corresponding feature, type of the error measure can be set in flags parameter; if the flow wasn’t found then the error is not defined (use the status parameter to find such cases).

winSize – size of the search window at each pyramid level.

maxLevel – 0-based maximal pyramid level number; if set to 0, pyramids are not used (single level), if set to 1, two levels are used, and so on; if pyramids are passed to input then algorithm will use as many levels as pyramids have but no more than maxLevel.

criteria – parameter, specifying the termination criteria of the iterative search algorithm (after the specified maximum number of iterations criteria.maxCount or when the search window moves by less than criteria.epsilon.
算法终止条件，1.最大迭代次数次数maxCount；2.精度criteria.epsilon

flags –
OPTFLOW_USE_INITIAL_FLOW uses initial estimations, stored in nextPts; if the flag is not set, then prevPts is copied to nextPts and is considered the initial estimate.
OPTFLOW_LK_GET_MIN_EIGENVALS use minimum eigen values as an error measure (see minEigThreshold description); if the flag is not set（默认度量）, then L1 distance between patches around the original and a moved point, divided by number of pixels in a window, is used as a error measure.
作为误差度量

minEigThreshold – the algorithm calculates the minimum eigen value （最小特征值）of a 2x2 normal matrix of optical flow equations (this matrix is called a spatial gradient matrix in [Bouguet00]), divided by number of pixels in a window; if this value is less than minEigThreshold, then a corresponding feature is filtered out and its flow is not processed, so it allows to remove bad points and get a performance boost（剔除坏点，提高性能）.

有了这些理解，看一下代码中调用的时候，传入参数表示的含义：
prev_img, cur_img：上一帧图片，当前帧图片；
prev_pts, cur_pts：特征点在上一帧中的位置（等待找到对应的流），特整点在当前帧中的位置（用以保存该算法计算结果）；
status：vector<uchar> status;
err：vector<float> err;
cv::Size(21, 21)：在每层金字塔中搜索窗的大小（$21\times 21$）；
1： 对应两层；
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01)：终止条件——迭代次数 和 精度；
cv::OPTFLOW_USE_INITIAL_FLOW：保持和原来一样多特征点数。

然后，统计跟踪成功的点数（即status为1的个数）
【如果】跟踪成功的点数少于10个，对应一些处理。（再执行一次，这次修改传入参数中金字塔的层数为4层）

============ 分割线 =====================
假设跟踪成功的点的数目足够多了，进行下一步处理操作：
FLOW_BACK：暂时未找到该变量定义的地方。。
但是，其对应函数块的含义是清晰的，即，当FLOW_BACK为true时，再调用cv::calcOpticalFlowPyrLK()进行一次反向的光流跟踪（传入参数时将两幅图像、特征点做对应调换）

符合以下要求的才认为是跟踪成功的点：
status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5

=============== 分割线 ==================
将跟踪失败的点删除（即当status[i]值为0，删除各vector中下标i对应的元素）：
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ids, track_cnt：FeatureTracker类的数据成员vector<int> ids;和vector<int> track_cnt;

============ 分割线 ====================
vectortrack_cnt中所有的计数值，统一 +1。
调用一个setMask()成员函数，其主要完成了按照track_cnt对cur_pts, ids, track_cnt的排序，并且创建了一个mask，以点cur_pts为圆心，填充了一个给定大小的黑色圆，其余位置为白色。
详细的介绍放在了 VINS-Fusion代码阅读（六）里。

调用另外一个openCV函数cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);，可以猜测其想实现的功能为，在图片的其他位置，再检测出给定数目MAX_CNT - cur_pts.size()的特征点，放在n_pts中。
同样，详细的介绍放在了 VINS-Fusion代码阅读（六）里。

这样，将新检测到的特征点n_pts添加到cur_pts中去，并赋予相应的ids和track_cnt。

++++++++++++++ 新分割线 +++++++++++++++++
下一步，调用undistortedPts成员函数，该函数比较简单，主要想实现的功能是
将像素坐标系下的坐标，转换为归一化相机坐标系下的坐标？即cur_un_pts为归一化相机坐标系下的坐标。为什么使用的是m_camera[0]？
cur_un_pts = undistortedPts(cur_pts, m_camera[0]);
其中，vector<camodocal::CameraPtr> m_camera;是一个vector，其中元素为CameraPtr类型，下一步需要了解一下该类型中成员函数liftProjective()的功能与实现。

vector<cv::Point2f> FeatureTracker::undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam)
{
    vector<cv::Point2f> un_pts;
    for (unsigned int i = 0; i < pts.size(); i++)
    {
        Eigen::Vector2d a(pts[i].x, pts[i].y);
        Eigen::Vector3d b;
        cam->liftProjective(a, b);
        un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
    }
    return un_pts;
}

接下来，计算pts_velocity，其为当前帧相对于前一帧 特征点沿x,y方向的像素移动速度。
pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map);
cur_un_pts_map中存放ids[i]和cur_un_pts[i]构成的键值对。
在prev_un_pts_map非空的情况下，对每一个cur_un_pts_map中的键值对都去寻找是否在prev_un_pts_map中存在，若存在像素移动速度不难计算；若不存在则为0；
如果prev_un_pts_map是空的情况下，置零。

vector<cv::Point2f> FeatureTracker::ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts, 
                                            map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts)
{
    vector<cv::Point2f> pts_velocity;
    cur_id_pts.clear();
    for (unsigned int i = 0; i < ids.size(); i++)
    {
        cur_id_pts.insert(make_pair(ids[i], pts[i]));
    }

    // caculate points velocity
    if (!prev_id_pts.empty())
    {
        double dt = cur_time - prev_time;
        
        for (unsigned int i = 0; i < pts.size(); i++)
        {
            std::map<int, cv::Point2f>::iterator it;
            it = prev_id_pts.find(ids[i]);
            if (it != prev_id_pts.end())
            {
                double v_x = (pts[i].x - it->second.x) / dt;
                double v_y = (pts[i].y - it->second.y) / dt;
                pts_velocity.push_back(cv::Point2f(v_x, v_y));
            }
            else
                pts_velocity.push_back(cv::Point2f(0, 0));

        }
    }
    else
    {
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            pts_velocity.push_back(cv::Point2f(0, 0));
        }
    }
    return pts_velocity;
}

解析（14页）关于这一部分——前段视觉处理就结束了；代码中，由于支持双目相机因此下面对于双目相机的另一幅图像_img1进行处理。

++++++++++++ 分割线 +++++++++++++++++++++
if(!_img1.empty() && stereo_cam){ }
不太理解的是，为什么双目的时候，光流跟踪是在左右两幅图像之间进行？
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);
不同之处：使用的是m_camera[1]
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_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;
prevLeftPtsMap[ids[i]] = cur_pts[i]; ID和cur_pts像素位置的点云图（用在了绘图展示中）

最后，返回值map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame
其中Eigen::Matrix<double, 7, 1> xyz_uv_velocity;包含跟踪点归一化相机坐标系下的坐标3，像素平面上的坐标2，像素移动速度2。
featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
此处很神奇，我们可以看出没有使用make_pair，更没有使用vector.push_back()，简洁的一句.emplace_back()就搞定了！了解其使用！

同样地，对右边一个相机有相同的操作，featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);不同的是camera_id，左边是0，右边是1。

============== 下面是源代码 可以不看了呀！==============

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;  // 注意此处是一个浅拷贝
    
    cur_pts.clear(); // vector的clear成员函数

    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)
               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<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++)
            {
                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++;

    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;
        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;
            cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %f ms", t_t.toc());

        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
            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;
    }
    if(SHOW_TRACK)
        drawTrack(cur_img, rightImg, ids, cur_pts, cur_right_pts, prevLeftPtsMap);

    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);
    }

    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;
}

PS. 如何使用QT打开已有的ROS工作空间？
目前使用的方法仅仅可以打开并查看，其中一系列的“未找到定义”的错误。当然也无法编译。
相关的文章主要查看了：
创客空间——ROS与C++入门教程-搭建开发环境(QT+ros_qtc_plugin)，通过最后新建工作空间的方法。
QT打开ROS工作空间时遇到的问题和解决方法，按照该方法执行了，但依然未能成功。