void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{cv::Mat img;TicToc t_r;cur_time = _cur_time;//如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理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;if (forw_img.empty()){//如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据//将读入的图像赋给当前帧forw_img，同时还赋给prev_img、cur_imgprev_img = cur_img = forw_img = img;}else{//否则，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据forw_img = img;}//此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除forw_pts.clear();if (cur_pts.size() > 0){TicToc t_o;vector<uchar> status;vector<float> err;//调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts//status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被追踪到的点标记为0cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);//将位于图像边界外的点标记为0for (int i = 0; i < int(forw_pts.size()); i++)if (status[i] && !inBorder(forw_pts[i]))status[i] = 0;//根据status,把跟踪失败的点剔除//不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts和cur_pts中剔除//prev_pts和cur_pts中的特征点是一一对应的//记录特征点id的ids，和记录特征点被跟踪次数的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);ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());}//光流追踪成功,特征点被成功跟踪的次数就加1//数值代表被追踪的次数，数值越大，说明被追踪的就越久for (auto &n : track_cnt)n++;//PUB_THIS_FRAME=1 需要发布特征点if (PUB_THIS_FRAME){//通过基本矩阵剔除outliersrejectWithF();ROS_DEBUG("set mask begins");TicToc t_m;setMask();//保证相邻的特征点之间要相隔30个像素,设置maskROS_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>(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;//添将新检测到的特征点n_pts添加到forw_pts中，id初始化-1,track_cnt初始化为1.addPoints();ROS_DEBUG("selectFeature costs: %fms", t_a.toc());}//当下一帧图像到来时，当前帧数据就成为了上一帧发布的数据prev_img = cur_img;prev_pts = cur_pts;prev_un_pts = cur_un_pts;//把当前帧的数据forw_img、forw_pts赋给上一帧cur_img、cur_ptscur_img = forw_img;cur_pts = forw_pts;//根据不同的相机模型去畸变矫正和转换到归一化坐标系上，计算速度undistortedPoints();prev_time = cur_time;
}