vins-mono学习笔记-特征追踪部分

我是参考大佬的文章学习的，建议直接看他的：【SLAM】VINS-MONO解析——前端_featuretracker trackerdata[num_of_cam];-CSDN博客

1.vins-mono的启动

roslaunch vins_estimator euroc.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag

1.1.euroc.launch

作用：启动feature_tracker、vins_estimator和pose_graph三个节点，一些参数的设置。

node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log"

tip:node name-节点名，pkg-功能包名字，type-可执行文件名字，output-输出方式(log为输出到日志，screen为输出到终端屏幕)

FeatureTracker::readImage

光流追踪

cv::calcOpticalFlowPyrLK是OpenCV中的函数，用于计算两幅图像之间的光流。它接受当前图像 (cur_img)、下一帧图像 (forw_img)，以及一些初始特征点 (cur_pts)，并计算对应的下一帧的特征点 (forw_pts)。

status 是输出参数，表示每个特征点是否成功跟踪的状态。如果 status[i] 为非零，则表示第 i 个特征点成功跟踪，否则失败。

代码通过遍历 forw_pts，检查跟踪状态和图像边界，将无效的特征点（追踪失败或者特征点位于图像边界，inBorder()用来判断是否在图像边界）标记为失败。

通过 reduceVector 函数，根据跟踪状态删除无效的点和相关信息。

最后，输出光流跟踪的计算时间。toc()用于返回当前时间与刚开始记时时间的差值

if (cur_pts.size() > 0)
{
    // 计时器，用于测量运行时间
    TicToc t_o;

    // 定义光流跟踪的输出状态和误差
    vector<uchar> status;
    vector<float> err;

    // 使用cv::calcOpticalFlowPyrLK计算当前图像(cur_img)和下一帧图像(forw_img)之间的光流
    cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);

    // 遍历光流跟踪结果，检查状态和图像边界
    for (int i = 0; i < int(forw_pts.size()); i++)
        if (status[i] && !inBorder(forw_pts[i]))
            status[i] = 0;

    // 根据状态删除无效的点和相关信息
    reduceVector(prev_pts, status);
    reduceVector(cur_pts, status);
    reduceVector(forw_pts, status);
    reduceVector(ids, status);
    reduceVector(cur_un_pts, status);
    reduceVector(track_cnt, status);

    // 输出光流跟踪的计算时间
    ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}

边缘点剔除

rejectWithF()主要是利用了cv::findFundamentalMat()这个函数来进一步剔除outlier。这个函数的功能对应在《SLAM十四讲》第七讲2D-2D对极几何相关知识点，两帧上的一系列对应的特征点能够复原出两帧之间的相对位姿变化，也就是基础矩阵E。但是这些特征点中肯定会有一些outlier，所以通过这个opencv的函数，能够巧妙地剔除这些outlier。补充一点，如果函数传入的是归一化坐标，那么得到的是本质矩阵E，如果传入的是像素坐标，那么得到的是基础矩阵。

if (PUB_THIS_FRAME)
    {
        rejectWithF();//剔除outliners
        setMask();
        int n_max_cnt = MAX_CNT - static_cast<int>(forw_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;
            if (mask.size() != forw_img.size())
                cout << "wrong size " << endl;
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        addPoints();
    }

rejecttWithF()

剔除外点

for循环中是将图像特征点坐标先转换为归一坐标再变为像素坐标。转换两遍的原因按大佬说的应该是在liftProjective()中，在某些相机模型时，liftProjective()还有去畸变的作用

计算出像素坐标后，findFundamentalMat()可以通过输入像素坐标和其他参数计算基础矩阵，status存放了内外点信息（这里没有存放基础矩阵的对象，这里只需要输出status就行？）

 if (forw_pts.size() >= 8)
    {
        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        //坐标转换
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            Eigen::Vector3d tmp_p;
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);//将像素坐标变为归一化坐标
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;           //计算像素坐标
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
        }

        vector<uchar> status;
        //通过使用 OpenCV 提供的 cv::findFundamentalMat 函数，计算归一化坐标下的 Fundamental Matrix（基本矩阵）。这里采用了 RANSAC 算法来剔除外点。
        //在这里，F_THRESHOLD 是 RANSAC 阈值，0.99 是置信度。
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();
        //利用status剔除外点
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(cur_un_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
    }

setMask()

这个函数的操作对象是forw_pts！这里，需要再次明确一下forw_pts里面是什么，它是通过光流法在当前帧中仍然能追踪到的那些特征点在当前帧中的像素坐标！所以forw_pts里面放着的都是老特征点。这里干了2件事，首先对他们根据追踪到的次数进行排序，从而确定了优先级；然后再把特征点周围的密集的特征点剔除掉。所以说，排序的作用就是为了给剔除操作提供优先级信息！因为追踪次数多的点在前面，所以在剔除的时候，这些时间长的特征点能够保留下来，而在这些老特征点周围比较近的距离内那些比较新的特征点就剔除掉，从而一方面保证了特征点的稳定，另一方面让特征点的分布更加均匀。
 

void FeatureTracker::setMask()
{
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));

    //保存跟踪次数更多的特征点
    vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;  //存放cnt（跟踪次数），pts，id

    for (unsigned int i = 0; i < forw_pts.size(); i++)
        cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));          //将数据放入cnt_pts_id中

    //按跟踪次数进行倒序
    sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
         {
            return a.first > b.first;
         });

    //清空容器
    forw_pts.clear();
    ids.clear();
    track_cnt.clear();

    for (auto &it : cnt_pts_id)
    {
        if (mask.at<uchar>(it.second.first) == 255)  //该位置的灰度值为255则将数据记录，并以它为圆心画圆来标记此处已选择的特征点，一段距离内不再选择特征点
        {
            forw_pts.push_back(it.second.first);
            ids.push_back(it.second.second);
            track_cnt.push_back(it.first);
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }
}

特征点添加

empty()检测是否为空；type()检测类型是否为CV_8UC1,8位单通道图像，用于表示二值图像,掩码（Mask）通常是一个与图像具有相同尺寸的二值图像，其中的像素值用来指示是否对对应位置的像素进行某种操作。掩码的目的是限制或过滤对图像的处理，只对掩码中特定数值（通常是非零值）的像素进行操作，而对其他像素不产生影响。

cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask)

• forw_img: 当前帧图像。
• n_pts: 输出的特征点坐标。
• MAX_CNT - forw_pts.size(): 要检测的特征点的最大数量，其中 forw_pts.size() 是已有特征点的数量。目的是为了保证总的特征点数量不超过 MAX_CNT。
• 0.01: 角点检测的质量水平，值越大表示检测到的特征点质量越高。
• MIN_DIST: 特征点之间的最小距离，确保检测到的特征点之间的距离足够远。

 if (n_max_cnt > 0)
        {
            //检测图像是否可以添加特征点
            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1)
                cout << "mask type wrong " << endl;
            if (mask.size() != forw_img.size())
                cout << "wrong size " << endl;
            //添加特征点，输出为n_pts(特征点坐标)，0.01为角点质量,MIN_DIST为特征点之间最小距离
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;
        addPoints();            //将特征点加入到特征点集合
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());

数据更新

    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    cur_img = forw_img;
    cur_pts = forw_pts;
    undistortedPoints();
    prev_time = cur_time;

FeatureTracker::undistortedPoints()

这个函数两个作用，第一个是获取forw时刻去畸变的归一化坐标(这个是要发布到rosmsg里的points数据)，另一个是获取forw时刻像素运动速度。

void FeatureTracker::undistortedPoints()    //对特征点的图像坐标根据不同的相机模型进行去畸变矫正和深度归一化，计算每个角点的速度
{
    cur_un_pts.clear();
    cur_un_pts_map.clear();
    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
    for (unsigned int i = 0; i < cur_pts.size(); i++)
    {
        Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
        Eigen::Vector3d b;
        m_camera->liftProjective(a, b);
        cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
        cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
        //printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
    }
    // caculate points velocity
    if (!prev_un_pts_map.empty())
    {
        double dt = cur_time - prev_time;
        pts_velocity.clear();
        for (unsigned int i = 0; i < cur_un_pts.size(); i++)
        {
            if (ids[i] != -1)
            {
                std::map<int, cv::Point2f>::iterator it;
                it = prev_un_pts_map.find(ids[i]);
                if (it != prev_un_pts_map.end())
                {
                    double v_x = (cur_un_pts[i].x - it->second.x) / dt;
                    double v_y = (cur_un_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
            {
                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));
        }
    }

寻找ids[i](特征点标识) ，如果没有it就指向prev_un_pts_map的尾端迭代器prev_un_pts_map.end()

如果it == prev_un_pts_map.end()表示prev_un_pts_map中没有ids[i]

it = prev_un_pts_map.find(ids[i]);
if (it != prev_un_pts_map.end())

更新特征点ID

//更新每个特征点的ID，直到更新完返回false
    for (unsigned int i = 0;; i++)
    {
        bool completed = false;
        for (int j = 0; j < NUM_OF_CAM; j++)
            if (j != 1 || !STEREO_TRACK)
                completed |= trackerData[j].updateID(i);
        if (!completed)
            break;
    }

bool FeatureTracker::updateID(unsigned int i)
{
    if (i < ids.size())
    {
        if (ids[i] == -1)
            ids[i] = n_id++;
        return true;
    }
    else
        return false;
}

特征点发布

pub_count++;
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);        //特征点归一化坐标
        sensor_msgs::ChannelFloat32 id_of_point;
        sensor_msgs::ChannelFloat32 u_of_point;                 //像素坐标x
        sensor_msgs::ChannelFloat32 v_of_point;                 //像素坐标y
        sensor_msgs::ChannelFloat32 velocity_x_of_point;
        sensor_msgs::ChannelFloat32 velocity_y_of_point;

        feature_points->header = img_msg->header;
        feature_points->header.frame_id = "world";

        vector<set<int>> hash_ids(NUM_OF_CAM);

vector<数据类型> 容器名(元素个数)

发布特征点信息话题

 for (int i = 0; i < NUM_OF_CAM; i++)
        {
            auto &un_pts = trackerData[i].cur_un_pts;       //归一化坐标
            auto &cur_pts = trackerData[i].cur_pts;             //像素坐标
            auto &ids = trackerData[i].ids;                                 //特征点id
            auto &pts_velocity = trackerData[i].pts_velocity;               //特征点速度
            for (unsigned int j = 0; j < ids.size(); j++)
            {
                if (trackerData[i].track_cnt[j] > 1)
                {
                    int p_id = ids[j];
                    hash_ids[i].insert(p_id);
                    geometry_msgs::Point32 p;
                    p.x = un_pts[j].x;
                    p.y = un_pts[j].y;
                    p.z = 1;

                    feature_points->points.push_back(p);
                    id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
                    u_of_point.values.push_back(cur_pts[j].x);
                    v_of_point.values.push_back(cur_pts[j].y);
                    velocity_x_of_point.values.push_back(pts_velocity[j].x);
                    velocity_y_of_point.values.push_back(pts_velocity[j].y);
                }
            }
        }
        //将特征点id、xy速度、xy坐标整合为点云话题类型feature_points中
        feature_points->channels.push_back(id_of_point);
        feature_points->channels.push_back(u_of_point);
        feature_points->channels.push_back(v_of_point);
        feature_points->channels.push_back(velocity_x_of_point);
        feature_points->channels.push_back(velocity_y_of_point);
        ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
        // skip the first image; since no optical speed on frist image
        //跳过第一帧，发布后面帧的点云信息
        if (!init_pub)
        {
            init_pub = 1;
        }
        else
            pub_img.publish(feature_points);

发布标出特征点的图像话题 

 if (SHOW_TRACK)
        {
            ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
            //cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
            cv::Mat stereo_img = ptr->image;

            for (int i = 0; i < NUM_OF_CAM; i++)
            {
                cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
                cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);

                for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
                {
                    //画圆标记出特征点
                    double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
                    cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
                }
            }
            pub_match.publish(ptr->toImageMsg());
        }

至此，featrue_track_node.cpp阅读完毕，待修改完善