VINS-Fusion代码阅读(六)

上一篇太长了,把其中调用的FeatureTracker类里面的setMask()成员函数单独拿出来介绍。
row, colFeatureTracker类的数据成员,被赋值为row = cur_img.rows; col = cur_img.cols;
首先,建立一个和cur_img同样大小的灰度(CV_8UC1)“图片”,白色(255);

把已经得到的信息,组织成一个新的数据结构
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;
其中的每一个元素make_pair(track_cnt[i], make_pair(cur_pts[i], ids[i]))
track_cnt, cur_pts, ids分别对应:点的跟踪计数、点的位置和点的ID;
并按照track_cnt排序,由多到少;
信息保存到cnt_pts_id,此时将track_cnt, cur_pts, ids清空。

按顺序遍历cnt_pts_id的每个元素,按顺序将对应信息存入track_cnt, cur_pts, ids
同时,cv::circle(mask, it.second.first, MIN_DIST, 0, -1);查看openCV官方文档如下:

C++ API:
void circle(Mat& img, Point center, int radius, const Scalar& color, int thickness=1, int lineType=8, int shift=0)
前几个参数比较直观,介绍最后几个:
thickness – Thickness of the circle outline, if positive. Negative thickness means that a filled circle is to be drawn. (-1表示填充)

那么源代码的含义就是,在maskcur_pts位置上,画一个半径为MIN_DIST(具体值怎么设定?)的颜色为黑色(0)的填充圆。

【注意】:maskFeatureTracker类的数据成员,因此结果得以保存!

void FeatureTracker::setMask()
{
    mask = cv::Mat(row, col, CV_8UC1, cv::Scalar(255));

    // prefer to keep features that are tracked for long time
    vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;

    for (unsigned int i = 0; i < cur_pts.size(); i++)
        cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(cur_pts[i], ids[i])));

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

    cur_pts.clear();
    ids.clear();
    track_cnt.clear();

    for (auto &it : cnt_pts_id)
    {
        if (mask.at<uchar>(it.second.first) == 255)
        {
            cur_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);
        }
    }
}
  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值