12 #include <mrpt/3rdparty/do_opencv_includes.h>
28 template <
typename FEATLIST>
30 FEATLIST& featureList,
const CImage& cur_gray,
31 const float minimum_KLT_response,
const unsigned int KLT_response_half_win,
32 const unsigned int max_x,
const unsigned int max_y);
37 const float minimum_KLT_response,
const unsigned int KLT_response_half_win,
38 const unsigned int max_x,
const unsigned int max_y)
40 for (
auto& ft : featureList)
45 const unsigned int x = ft.keypoint.pt.x;
46 const unsigned int y = ft.keypoint.pt.y;
47 if (x > KLT_response_half_win && y > KLT_response_half_win &&
48 x < max_x && y < max_y)
50 ft.response = cur_gray.
KLT_response(x, y, KLT_response_half_win);
54 if (ft.response < minimum_KLT_response)
67 template <
class FEAT_LIST>
69 FEAT_LIST& featureList,
const CImage& cur_gray,
70 const float minimum_KLT_response,
const unsigned int KLT_response_half_win,
71 const unsigned int max_x_,
const unsigned int max_y_)
73 if (featureList.empty())
return;
75 using pixel_coord_t =
typename FEAT_LIST::feature_t::pixel_coord_t;
76 const auto half_win =
static_cast<pixel_coord_t
>(KLT_response_half_win);
77 const auto max_x =
static_cast<pixel_coord_t
>(max_x_);
78 const auto max_y =
static_cast<pixel_coord_t
>(max_y_);
80 for (
int N = featureList.size() - 1; N >= 0; --N)
82 typename FEAT_LIST::feature_t& ft = featureList[N];
86 if (ft.pt.x > half_win && ft.pt.y > half_win && ft.pt.x < max_x &&
90 cur_gray.
KLT_response(ft.pt.x, ft.pt.y, KLT_response_half_win);
94 if (ft.response < minimum_KLT_response)
110 const float minimum_KLT_response,
const unsigned int KLT_response_half_win,
111 const unsigned int max_x,
const unsigned int max_y)
113 trackFeatures_checkResponses_impl_simple<TKeyPointList>(
114 featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
120 const float minimum_KLT_response,
const unsigned int KLT_response_half_win,
121 const unsigned int max_x,
const unsigned int max_y)
123 trackFeatures_checkResponses_impl_simple<TKeyPointfList>(
124 featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
128 template <
typename FEATLIST>
130 FEATLIST& featureList,
const CImage& cur_gray);
136 for (
auto& ft : featureList)
141 const size_t patch_width = ft.patch->getWidth();
142 const size_t patch_height = ft.patch->getHeight();
143 if (patch_width > 0 && patch_height > 0)
147 const int offset = (int)patch_width / 2;
150 *ft.patch,
round(ft.keypoint.pt.x) - offset,
151 round(ft.keypoint.pt.y) - offset, patch_width,
154 catch (std::exception&)
164 [[maybe_unused]]
const CImage& cur_gray)
171 [[maybe_unused]]
const CImage& cur_gray)
176 template <
typename FEATLIST>
179 const std::vector<size_t>& sorted_indices,
const size_t nNewToCheck,
180 const size_t maxNumFeatures,
const float minimum_KLT_response_to_add,
181 const double threshold_sqr_dist_to_add_new,
const size_t patchSize,
187 const std::vector<size_t>& sorted_indices,
const size_t nNewToCheck,
188 const size_t maxNumFeatures,
const float minimum_KLT_response_to_add,
189 const double threshold_sqr_dist_to_add_new,
const size_t patchSize,
193 const int offset = (int)patchSize / 2 + 1;
194 const int w_off = int(imgSize.
x - offset);
195 const int h_off = int(imgSize.
y - offset);
197 for (
size_t i = 0; i < nNewToCheck && featureList.
size() < maxNumFeatures;
200 const TKeyPoint& feat = new_feats[sorted_indices[i]];
202 if (feat.
response < minimum_KLT_response_to_add)
continue;
204 double min_dist_sqr =
square(10000);
206 if (!featureList.
empty())
212 if (min_dist_sqr > threshold_sqr_dist_to_add_new &&
213 feat.
pt.x > offset && feat.
pt.y > offset && feat.
pt.x < w_off &&
233 round(feat.
pt.y) - offset, patchSize, patchSize);
241 template <
class FEAT_LIST>
244 const std::vector<size_t>& sorted_indices,
const size_t nNewToCheck,
245 const size_t maxNumFeatures,
const float minimum_KLT_response_to_add,
246 const double threshold_sqr_dist_to_add_new,
247 [[maybe_unused]]
const size_t patchSize,
252 featureList.getVector());
254 for (
size_t i = 0; i < nNewToCheck && featureList.size() < maxNumFeatures;
257 const TKeyPoint& feat = new_feats[sorted_indices[i]];
258 if (feat.
response < minimum_KLT_response_to_add)
break;
261 double min_dist_sqr = std::numeric_limits<double>::max();
263 if (!featureList.empty())
269 if (min_dist_sqr > threshold_sqr_dist_to_add_new)
272 featureList.emplace_back(feat.
pt.x, feat.
pt.y);
276 typename FEAT_LIST::feature_t& newFeat = featureList.back();
278 newFeat.ID = ++max_feat_ID_at_input;
291 const std::vector<size_t>& sorted_indices,
const size_t nNewToCheck,
292 const size_t maxNumFeatures,
const float minimum_KLT_response_to_add,
293 const double threshold_sqr_dist_to_add_new,
const size_t patchSize,
296 trackFeatures_addNewFeats_simple_list<TKeyPointList>(
297 featureList, new_feats, sorted_indices, nNewToCheck, maxNumFeatures,
298 minimum_KLT_response_to_add, threshold_sqr_dist_to_add_new, patchSize,
299 cur_gray, max_feat_ID_at_input);
304 const std::vector<size_t>& sorted_indices,
const size_t nNewToCheck,
305 const size_t maxNumFeatures,
const float minimum_KLT_response_to_add,
306 const double threshold_sqr_dist_to_add_new,
const size_t patchSize,
309 trackFeatures_addNewFeats_simple_list<TKeyPointfList>(
310 featureList, new_feats, sorted_indices, nNewToCheck, maxNumFeatures,
311 minimum_KLT_response_to_add, threshold_sqr_dist_to_add_new, patchSize,
312 cur_gray, max_feat_ID_at_input);
316 template <
typename FEATLIST>
318 FEATLIST& trackedFeats,
const size_t img_width,
const size_t img_height,
319 const int MIN_DIST_MARGIN_TO_STOP_TRACKING);
321 template <
typename FEATLIST>
323 FEATLIST& trackedFeats,
const size_t img_width,
const size_t img_height,
324 const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
326 if (trackedFeats.empty())
return 0;
328 std::vector<size_t> survival_idxs;
329 const size_t N = trackedFeats.size();
332 survival_idxs.reserve(N);
333 for (
size_t i = 0; i < N; i++)
335 const typename FEATLIST::feature_t& ft = trackedFeats[i];
341 const int x = ft.pt.x;
342 const int y = ft.pt.y;
343 if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
344 y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
345 x >
static_cast<int>(
346 img_width - MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
347 y >
static_cast<int>(
348 img_height - MIN_DIST_MARGIN_TO_STOP_TRACKING))
353 if (!eras) survival_idxs.push_back(i);
357 const size_t N2 = survival_idxs.size();
358 const size_t n_removed = N - N2;
359 for (
size_t i = 0; i < N2; i++)
361 if (survival_idxs[i] != i)
362 trackedFeats[i] = trackedFeats[survival_idxs[i]];
364 trackedFeats.resize(N2);
371 const size_t img_height,
const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
373 return trackFeatures_deleteOOB_impl_simple_feat<TKeyPointList>(
374 trackedFeats, img_width, img_height, MIN_DIST_MARGIN_TO_STOP_TRACKING);
379 const size_t img_height,
const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
381 return trackFeatures_deleteOOB_impl_simple_feat<TKeyPointfList>(
382 trackedFeats, img_width, img_height, MIN_DIST_MARGIN_TO_STOP_TRACKING);
387 CFeatureList& trackedFeats,
const size_t img_width,
const size_t img_height,
388 const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
390 auto itFeat = trackedFeats.
begin();
391 size_t n_removed = 0;
392 while (itFeat != trackedFeats.
end())
399 const float x = itFeat->keypoint.pt.x;
400 const float y = itFeat->keypoint.pt.y;
401 if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
402 y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
403 x > (img_width - MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
404 y > (img_height - MIN_DIST_MARGIN_TO_STOP_TRACKING))
411 itFeat = trackedFeats.
erase(itFeat);
424 [[maybe_unused]]
const CImage& old_img,
425 [[maybe_unused]]
const CImage& new_img,
443 template <
typename FEATLIST>
445 const CImage& old_img,
const CImage& new_img, FEATLIST& featureList)
449 const size_t img_width = new_img.
getWidth();
450 const size_t img_height = new_img.
getHeight();
455 if (!featureList.empty()) max_feat_ID_at_input = featureList.getMaxID();
459 m_timlog.enter(
"CGenericFeatureTracker.to_grayscale");
464 m_timlog.leave(
"CGenericFeatureTracker.to_grayscale");
469 m_newly_detected_feats.clear();
471 m_timlog.enter(
"CGenericFeatureTracker.trackFeatures_impl");
473 trackFeatures_impl(prev_gray, cur_gray, featureList);
475 m_timlog.leave(
"CGenericFeatureTracker.trackFeatures_impl");
480 const int check_KLT_response_every =
481 extra_params.getWithDefaultVal(
"check_KLT_response_every", 1);
482 const float minimum_KLT_response =
483 extra_params.getWithDefaultVal(
"minimum_KLT_response", 30.f);
484 const unsigned int KLT_response_half_win =
485 extra_params.getWithDefaultVal(
"KLT_response_half_win", 8U);
487 if (check_KLT_response_every > 0 &&
488 ++m_check_KLT_counter >=
size_t(check_KLT_response_every))
490 m_timlog.enter(
"CGenericFeatureTracker.check_KLT_responses");
491 m_check_KLT_counter = 0;
493 const unsigned int max_x = img_width - KLT_response_half_win;
494 const unsigned int max_y = img_height - KLT_response_half_win;
497 featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
500 m_timlog.leave(
"CGenericFeatureTracker.check_KLT_responses");
508 const bool remove_lost_features =
509 extra_params.getWithDefaultVal(
"remove_lost_features", 0) != 0;
511 if (remove_lost_features)
513 m_timlog.enter(
"CGenericFeatureTracker.OOB_remove");
515 static const int MIN_DIST_MARGIN_TO_STOP_TRACKING = 10;
518 featureList, img_width, img_height,
519 MIN_DIST_MARGIN_TO_STOP_TRACKING);
521 m_timlog.leave(
"CGenericFeatureTracker.OOB_remove");
523 last_execution_extra_info.num_deleted_feats = nRemoved;
527 last_execution_extra_info.num_deleted_feats = 0;
533 const int update_patches_every =
534 extra_params.getWithDefaultVal(
"update_patches_every", 0);
536 if (update_patches_every > 0 &&
537 ++m_update_patches_counter >=
size_t(update_patches_every))
540 m_timlog,
"CGenericFeatureTracker.update_patches");
542 m_update_patches_counter = 0;
552 const bool add_new_features =
553 extra_params.getWithDefaultVal(
"add_new_features", 1) != 0;
554 const double threshold_dist_to_add_new =
555 extra_params.getWithDefaultVal(
"add_new_feat_min_separation", 15);
559 if (add_new_features)
562 m_timlog,
"CGenericFeatureTracker.add_new_features");
566 if (m_newly_detected_feats.empty())
577 m_newly_detected_feats.clear();
578 m_newly_detected_feats.reserve(new_feats.
size());
582 m_newly_detected_feats.push_back(
TKeyPoint(f.keypoint));
587 const size_t N = m_newly_detected_feats.size();
588 last_execution_extra_info.raw_FAST_feats_detected = N;
591 const size_t desired_num_features = extra_params.getWithDefaultVal(
592 "desired_num_features_adapt",
593 size_t((img_width * img_height) >> 9));
594 updateAdaptiveNewFeatsThreshold(N, desired_num_features);
598 const unsigned int max_x = img_width - KLT_response_half_win;
599 const unsigned int max_y = img_height - KLT_response_half_win;
600 for (
size_t i = 0; i < N; i++)
602 const unsigned int x = m_newly_detected_feats[i].pt.x;
603 const unsigned int y = m_newly_detected_feats[i].pt.y;
604 if (x > KLT_response_half_win && y > KLT_response_half_win &&
605 x < max_x && y < max_y)
606 m_newly_detected_feats[i].response =
609 m_newly_detected_feats[i].response = 0;
616 std::vector<size_t> sorted_indices(N);
617 for (
size_t i = 0; i < N; i++) sorted_indices[i] = i;
620 sorted_indices.begin(), sorted_indices.end(),
627 const size_t nNewToCheck = std::min(
size_t(1500), N);
628 const double threshold_sqr_dist_to_add_new =
629 square(threshold_dist_to_add_new);
630 const size_t maxNumFeatures =
631 extra_params.getWithDefaultVal(
"add_new_feat_max_features", 100U);
632 const size_t patchSize =
633 extra_params.getWithDefaultVal(
"add_new_feat_patch_size", 0U);
634 const float minimum_KLT_response_to_add =
635 extra_params.getWithDefaultVal(
"minimum_KLT_response_to_add", 70.f);
639 featureList, m_newly_detected_feats, sorted_indices, nNewToCheck,
640 maxNumFeatures, minimum_KLT_response_to_add,
641 threshold_sqr_dist_to_add_new, patchSize, cur_gray,
642 max_feat_ID_at_input);
650 internal_trackFeatures<TKeyPointList>(old_img, new_img, featureList);
656 internal_trackFeatures<TKeyPointfList>(old_img, new_img, featureList);
660 const size_t nNewlyDetectedFeats,
const size_t desired_num_features)
662 const size_t hysteresis_min_num_feats = desired_num_features * 0.9;
663 const size_t hysteresis_max_num_feats = desired_num_features * 1.1;
665 if (nNewlyDetectedFeats < hysteresis_min_num_feats)
666 m_detector_adaptive_thres = std::max(
668 m_detector_adaptive_thres - 1.0,
669 m_detector_adaptive_thres * 0.8));
670 else if (nNewlyDetectedFeats > hysteresis_max_num_feats)
671 m_detector_adaptive_thres = std::max(
672 m_detector_adaptive_thres + 1.0, m_detector_adaptive_thres * 1.2);