void FeatureTracker::ransac(std::vector<cv::Point2f> prev, std::vector<cv::Point2f> curr)
{
//ROS_INFO("ransac begin!");
if((int)curr.size() < 4 )
return;
vector<uchar> status;
// ransac after undistort
std::vector<cv::Point2f> prev_un = undistortedPoints(prev);
std::vector<cv::Point2f> curr_un = undistortedPoints(curr);
//cv::findFundamentalMat(prev_un, curr_un, cv::FM_RANSAC, ransac_thres, 0.99, status);
cv::findFundamentalMat(prev_un, curr_un, cv::FM_RANSAC, ransac_thres, 0.99, status);
int num = curr.size();
cur_pts.clear();
cur_ids.clear();
cur_track_cnt.clear();
for(unsigned int i = 0; i < curr.size(); i++)
{
//cout<<"prev_points "<<prev[i].x<<" "<<prev[i].y<<" curr points "<<curr[i].x<<" "<<curr[i].y<<"status "<<(int)status[i]<<endl;
if((int)status[i] == 1)
{
if(curr[i].y < 5 || curr[i].y > ROW - 5 || curr[i].x < 5 || curr[i].x > COL - 5 ||
(curr[i].x - COL / 2.0) * (curr[i].x - COL / 2.0) + (curr[i].y - ROW / 2.0) * (curr[i].y - ROW / 2.0) > 345 * 345 )
continue;
cur_pts.push_back(curr[i]);
cur_ids.push_back(ransac_ids[i]);
cur_track_cnt.push_back(ransac_track_cnt[i]);
}
}
ROS_INFO("RANSAC delete %d features" , num - (int)cur_pts.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;
}
