/**
** 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;
}
