void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { 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()); } else img = _img; if (forw_img.empty()) { prev_img = cur_img = forw_img = img; } else { forw_img = img; } forw_pts.clear(); 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()); } for (auto &n : track_cnt) n++; 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; 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(); 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; }
void FeatureTracker::readImage(const cv::Mat &_img) { cv::Mat img; TicToc t_r; 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()); } else img = _img; if (forw_img.empty()) { prev_img = cur_img = forw_img = img; } else { forw_img = img; } forw_pts.clear(); if (cur_pts.size() > 0) { TicToc t_o; vector<uchar> status; vector<float> err; // Status will give us infor whether the feature is being tracked // Forw_img is the new image and forw_pts are the "KLT" points. // We will later track more points that are not tracked by KLT 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; // Resizing vectors so that only the good status (tracked) features are in the vectors // The remaining features will be tracked/added by KLT reduceVector(prev_pts, status); reduceVector(cur_pts, status); reduceVector(forw_pts, status); reduceVector(ids, status); reduceVector(track_cnt, status); ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc()); } if (PUB_THIS_FRAME) { rejectWithF(); for (auto &n : track_cnt) n++; 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>(forw_pts.size()); // If > 0, meaning not all points are tracked, so we will ty // to track more points 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; // There are features that are not tracked by KLT cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask); } else n_pts.clear(); ROS_DEBUG("detect feature costs: %fms", t_t.toc()); ROS_DEBUG("add feature begins"); TicToc t_a; // The newly added "good features" are assigned -1 ID (will update later). // These are new observation points addPoints(); ROS_DEBUG("selectFeature costs: %fms", t_a.toc()); prev_img = forw_img; prev_pts = forw_pts; } cur_img = forw_img; cur_pts = forw_pts; }