/** ** search matches by guide descriptor match ** **/ void KeyFrame::searchByDes(std::vector<cv::Point2f> &measurements_old, std::vector<cv::Point2f> &measurements_old_norm, const std::vector<BRIEF::bitset> &descriptors_old, const std::vector<cv::KeyPoint> &keypoints_old) { printf("loop_match before cur %d %d, old %d\n", window_descriptors.size(), measurements.size(), descriptors_old.size()); std::vector<int> dis_cur_old; std::vector<uchar> status; for(int i = 0; i < window_descriptors.size(); i++) { int bestDist = 256; int bestIndex = -1; for(int j = 0; j < descriptors_old.size(); j++) { int dis = HammingDis(window_descriptors[i], descriptors_old[j]); if(dis < bestDist) { bestDist = dis; bestIndex = j; } } if(bestDist < 256) { measurements_old.push_back(keypoints_old[bestIndex].pt); dis_cur_old.push_back(bestDist); } } rejectWithF(measurements_old, measurements_old_norm); printf("loop_match after cur %d %d, old %d\n", window_descriptors.size(), measurements.size(), descriptors_old.size()); }
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; }