17 #include <mrpt/3rdparty/do_opencv_includes.h>
36 template <
typename FEATLIST>
38 const CImage& old_img,
const CImage& new_img, FEATLIST& featureList)
43 const int window_width = extra_params.getWithDefaultVal(
"window_width", 15);
44 const int window_height =
45 extra_params.getWithDefaultVal(
"window_height", 15);
47 const int LK_levels = extra_params.getWithDefaultVal(
"LK_levels", 3);
48 const int LK_max_iters = extra_params.getWithDefaultVal(
"LK_max_iters", 10);
49 const int LK_epsilon = extra_params.getWithDefaultVal(
"LK_epsilon", 0.1);
50 const float LK_max_tracking_error =
51 extra_params.getWithDefaultVal(
"LK_max_tracking_error", 150.0f);
58 const size_t img_width = old_img.
getWidth();
59 const size_t img_height = old_img.
getHeight();
61 const size_t nFeatures = featureList.size();
70 std::vector<cv::Point2f> points_prev(nFeatures), points_cur;
71 std::vector<uchar> status(nFeatures);
72 std::vector<float> track_error(nFeatures);
74 for (
size_t i = 0; i < nFeatures; ++i)
76 points_prev[i].x = featureList.getFeatureX(i);
77 points_prev[i].y = featureList.getFeatureY(i);
83 cv::calcOpticalFlowPyrLK(
84 prev, cur, points_prev, points_cur, status, track_error,
85 cv::Size(window_width, window_height), LK_levels,
87 cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS,
88 LK_max_iters, LK_epsilon));
90 for (
size_t i = 0; i < nFeatures; ++i)
92 const bool trck_err_too_large =
93 track_error[i] > LK_max_tracking_error;
95 if (status[i] == 1 && !trck_err_too_large && points_cur[i].x > 0 &&
96 points_cur[i].y > 0 && points_cur[i].x < img_width &&
97 points_cur[i].y < img_height)
100 featureList.setFeatureXf(i, points_cur[i].x);
101 featureList.setFeatureYf(i, points_cur[i].y);
106 featureList.setFeatureX(i, -1);
107 featureList.setFeatureY(i, -1);
108 featureList.setTrackStatus(
114 featureList.mark_as_outdated();
118 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
127 trackFeatures_impl_templ<TKeyPointList>(old_img, new_img, featureList);
133 trackFeatures_impl_templ<TKeyPointfList>(old_img, new_img, featureList);