Vins中的FeatureTracker::readImage(const cv::Mat _img, double _cur

该函数读取图像Mat以及当前世界_cur_time，并对其进行处理，这一段判断是否对图像利用opencv进行处理，从而消除其明暗变化，并且将传入的时间记录为当前时间。

    cv::Mat img;TicToc t_r;cur_time = _cur_time;if (EQUALIZE){cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));TicToc t_c;clahe->apply(_img, img);//ROS_DEBUG("CLAHE costs: %fms", t_c.toc());}elseimg = _img;

接下来是图像的一个保存，如果是处理第一帧图像，prev_img = cur_img = forw_img = img, 三个图像都是第一帧图像，第二帧图像进入后perv和cur是上一帧图像，forw_img是最新进入的图像

    if (forw_img.empty()){prev_img = cur_img = forw_img = img;}else{forw_img = img;}forw_pts.clear();

forw_pts记录新的特征点，因此对特征点集进行清空

forw_pts.clear();

cur_pts是记录的上一帧已经处理的图像的特征点，当第一帧图像进入时，cur_pts还为空，因此不会进入这一处理函数。如果是上一帧的img已经存在并且提取了特征点，就可以进行光流跟踪。cur_img是上一处理过的图像，cur_pts是对那张图像处理获得的二维点，forw_img是最新进来的图像，forw_pts是利用光流获得的跟踪点，status会记录是否跟踪成功。

    if (cur_pts.size() > 0){TicToc t_o;vector<uchar> status;vector<float> err;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());}

进行光流跟踪后，会判断新的点是否在图像内，如果距离边界小于1个像素，也会被排除

        for (int i = 0; i < int(forw_pts.size()); i++)if (status[i] && !inBorder(forw_pts[i]))status[i] = 0;bool inBorder(const cv::Point2f &pt)
{const int BORDER_SIZE = 1;int img_x = cvRound(pt.x);int img_y = cvRound(pt.y);return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;
}

如果status为0，也就是未成功跟踪，或者出界的点，将会被从prev_pts,cur_pts,forw_pts,ids,cur_un_pts,track_cnt中进行移除

        reduceVector(prev_pts, status);reduceVector(cur_pts, status);reduceVector(forw_pts, status);reduceVector(ids, status);reduceVector(cur_un_pts, status);reduceVector(track_cnt, status);

对所有未移除的点，track_nt记录跟踪成功的次数，需要对其进行+1

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

如果PUB_THIS_FRAME,

    if (PUB_THIS_FRAME){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;

利用F矩阵来排除异常点

void FeatureTracker::rejectWithF()
{if (forw_pts.size() >= 8){//ROS_DEBUG("FM ransac begins");TicToc t_f;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;cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status); //排除异常点int size_a = cur_pts.size();reduceVector(prev_pts, status);reduceVector(cur_pts, status);reduceVector(forw_pts, status);reduceVector(cur_un_pts, status);reduceVector(ids, status);reduceVector(track_cnt, status);//ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);//ROS_DEBUG("FM ransac costs: %fms", t_f.toc());}
}

利用F矩阵来排除异常点，函数liftProjective是对图像的二维点去畸变，然后投影到归一化平面，其在camera.cc中是一个纯虚函数，在不同相机类,PinholeCamera.cc等中进行了具体实现，就是去畸变，然后投影。之后利用小孔成像，重新计算x和y，再利用opencv的函数寻找异常点

void FeatureTracker::rejectWithF()
{if (forw_pts.size() >= 8){//ROS_DEBUG("FM ransac begins");TicToc t_f;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;cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status); //排除异常点int size_a = cur_pts.size();reduceVector(prev_pts, status);reduceVector(cur_pts, status);reduceVector(forw_pts, status);reduceVector(cur_un_pts, status);reduceVector(ids, status);reduceVector(track_cnt, status);//ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);//ROS_DEBUG("FM ransac costs: %fms", t_f.toc());}
}

接下来会寻找新的特征点，例如总特征点数为150，上一帧还剩下120个特征点，那么就需要重新寻找新的30个特征点，新的30个特征点需要与已找到的特征点尽量分开，因此会通过setMask函数来对已有的点进行记录

void FeatureTracker::setMask()
{if(FISHEYE)mask = fisheye_mask.clone();elsemask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));// prefer to keep features that are tracked for long timevector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;for (unsigned int i = 0; i < forw_pts.size(); i++) //对最新跟踪的点放入到cnt_pts_id中cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_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)  //对cnt_pts_id利用跟踪的次数进行排序{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){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); //将特征点附近设置为0}}
}

检测新的角点

        TicToc t_t;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);}elsen_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;

其中addPoints会将新检测的点添加到forw_pts中，ids全部设置为-1，track_cnt则会设置为1，因此是第一次被追踪

void FeatureTracker::addPoints()
{for (auto &p : n_pts){forw_pts.push_back(p);ids.push_back(-1);track_cnt.push_back(1);}
}

对当前的点计算计算速度与无畸变的投影

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 velocityif (!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));}elsepts_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));}}prev_un_pts_map = cur_un_pts_map;
}

经过处理后获得了以下数据：

    vector<cv::Point2f> prev_pts, cur_pts, forw_pts; //前，中，后三帧图像的特征点vector<cv::Point2f> prev_un_pts, cur_un_pts; //去畸变后的点vector<cv::Point2f> pts_velocity; //点的速度vector<int> ids; //点的idvector<int> track_cnt; //点跟踪的次数map<int, cv::Point2f> cur_un_pts_map; //mapmap<int, cv::Point2f> prev_un_pts_map; //map

Vins中的FeatureTracker::readImage(const cv::Mat _img, double _cur_time)函数相关推荐

OpenCv中的cv::Mat::create（）函数，cvRound()，cvFloor()，cvCeil()函数的详解l
文件说明: cv::create()函数的详解函数原型: inline void Mat::create(int _rows, int _cols, int _type) inline void M ...
【OpenCV3】cv::Mat中的数据按行列写入txt文件中
在使用opencv进行图像处理的过程中,经常会涉及到将文件中的数据读入到cv::Mat中,或者将cv::Mat中的数据写入到txt文件中. 下面就介绍一种我常用的将cv::Mat中的数据写入到txt文 ...
QImage/cv::Mat/HObject的图像格式互相转换，4字节对齐
QImage/cv::Mat互相转换 QImage ImgChange::cvMat2QImage(const Mat &mat) // Mat 改成 QImage {if (mat.type ...
VI ORB-SLAM初始化与VINS初始化对比(将vi orb-slam初始化方法移植到vins中)
初始化时需要求出的变量:相机和imu外参r t.重力g.尺度s.陀螺仪和加速度计偏置ba bg. 下面对两种算法初始化的详细步骤进行对比: 求陀螺仪偏置bg 求解公式相同,求解方法不同.公式如下,VI ...
【OpenCV3】cv::Mat的定义与初始化
cv::Mat是OpenCV2和OpenCV3中基本的数据类型,在cv::Mat类中,关于cv::Mat的定义和初始化有多种不同的形式,这里,将对其进行详尽的总结和介绍. 一.无数据拷贝的cv::Ma ...
QImage 与 cv::Mat 之间的相互转换
近期做图像处理方面的项目比較多,非常多算法自己从头写的话太浪费时间,并且自己写的也不一定完好,早就听说OpenCV在图像处理算法方面功能非常强大,一直没时间学习,这次正好项目用到了.暂时抱佛脚学习些O ...
cv mat的shape_pybind11—opencv图像处理(numpy数据交换)
前言 C++ opencv中图像和矩阵的表示采用Mat类,比如imread()读取的结果就是返回一个Mat对象.对于python而言,numpy 通常用于矩阵运算, 矩阵,图像表示为numpy.nda ...
【OpenCV3】cv::Mat类成员函数详解
cv::Mat为OpenCV2和OpenCV3中最重要的类,可以毫不夸张得说,掌握了cv::Mat的操作,就掌握了OpenCV大半,这里将详细的介绍下cv::Mat类的成员函数及其使用. 1.clon ...
cv::Mat两张图片的叠加方法
适用jpg图片的叠加 void mergeImage(cv::Mat &img1, cv::Mat img2, int x, int y) {if(x < 0 || y < 0)r ...

Vins中的FeatureTracker::readImage(const cv::Mat _img, double _cur_time)函数

Vins中的FeatureTracker::readImage(const cv::Mat _img, double _cur_time)函数相关推荐

最新文章

热门文章