MRPT  2.0.4
tracking.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/3rdparty/do_opencv_includes.h>
13 #include <mrpt/math/ops_matrices.h>
15 #include <mrpt/vision/tracking.h>
16 
17 using namespace mrpt;
18 using namespace mrpt::vision;
19 using namespace mrpt::img;
20 using namespace mrpt::tfest;
21 using namespace mrpt::math;
22 using namespace std;
23 
24 // ------------------------------- internal helper templates
25 // ---------------------------------
26 namespace mrpt::vision::detail
27 {
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);
33 
34 template <>
36  CFeatureList& featureList, const CImage& cur_gray,
37  const float minimum_KLT_response, const unsigned int KLT_response_half_win,
38  const unsigned int max_x, const unsigned int max_y)
39 {
40  for (auto& ft : featureList)
41  {
42  if (ft.track_status != status_TRACKED)
43  continue; // Skip if it's not correctly tracked.
44 
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)
49  { // Update response:
50  ft.response = cur_gray.KLT_response(x, y, KLT_response_half_win);
51 
52  // Is it good enough?
53  // http://grooveshark.com/s/Goonies+Are+Good+Enough/2beBfO?src=5
54  if (ft.response < minimum_KLT_response)
55  { // Nope!
56  ft.track_status = status_LOST;
57  }
58  }
59  else
60  { // Out of bounds
61  ft.response = 0;
62  ft.track_status = status_OOB;
63  }
64  }
65 } // end of trackFeatures_checkResponses<>
66 
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_)
72 {
73  if (featureList.empty()) return;
74 
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_);
79 
80  for (int N = featureList.size() - 1; N >= 0; --N)
81  {
82  typename FEAT_LIST::feature_t& ft = featureList[N];
83  if (ft.track_status != status_TRACKED)
84  continue; // Skip if it's not correctly tracked.
85 
86  if (ft.pt.x > half_win && ft.pt.y > half_win && ft.pt.x < max_x &&
87  ft.pt.y < max_y)
88  { // Update response:
89  ft.response =
90  cur_gray.KLT_response(ft.pt.x, ft.pt.y, KLT_response_half_win);
91 
92  // Is it good enough?
93  // http://grooveshark.com/s/Goonies+Are+Good+Enough/2beBfO?src=5
94  if (ft.response < minimum_KLT_response)
95  { // Nope!
96  ft.track_status = status_LOST;
97  }
98  }
99  else
100  { // Out of bounds
101  ft.response = 0;
102  ft.track_status = status_OOB;
103  }
104  }
105 } // end of trackFeatures_checkResponses<>
106 
107 template <>
109  TKeyPointList& featureList, const CImage& cur_gray,
110  const float minimum_KLT_response, const unsigned int KLT_response_half_win,
111  const unsigned int max_x, const unsigned int max_y)
112 {
113  trackFeatures_checkResponses_impl_simple<TKeyPointList>(
114  featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
115  max_x, max_y);
116 }
117 template <>
119  TKeyPointfList& featureList, const CImage& cur_gray,
120  const float minimum_KLT_response, const unsigned int KLT_response_half_win,
121  const unsigned int max_x, const unsigned int max_y)
122 {
123  trackFeatures_checkResponses_impl_simple<TKeyPointfList>(
124  featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
125  max_x, max_y);
126 }
127 
128 template <typename FEATLIST>
129 inline void trackFeatures_updatePatch(
130  FEATLIST& featureList, const CImage& cur_gray);
131 
132 template <>
134  CFeatureList& featureList, const CImage& cur_gray)
135 {
136  for (auto& ft : featureList)
137  {
138  if (ft.track_status != status_TRACKED)
139  continue; // Skip if it's not correctly tracked.
140 
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)
144  {
145  try
146  {
147  const int offset = (int)patch_width / 2; // + 1;
148  ft.patch.emplace();
149  cur_gray.extract_patch(
150  *ft.patch, round(ft.keypoint.pt.x) - offset,
151  round(ft.keypoint.pt.y) - offset, patch_width,
152  patch_height);
153  }
154  catch (std::exception&)
155  {
156  ft.track_status = status_OOB; // Out of bounds!
157  }
158  }
159  }
160 } // end of trackFeatures_updatePatch<>
161 template <>
163  [[maybe_unused]] TKeyPointList& featureList,
164  [[maybe_unused]] const CImage& cur_gray)
165 {
166  // This list type does not have patch stored explicitly
167 } // end of trackFeatures_updatePatch<>
168 template <>
170  [[maybe_unused]] TKeyPointfList& featureList,
171  [[maybe_unused]] const CImage& cur_gray)
172 {
173  // This list type does not have patch stored explicitly
174 } // end of trackFeatures_updatePatch<>
175 
176 template <typename FEATLIST>
177 inline void trackFeatures_addNewFeats(
178  FEATLIST& featureList, const TKeyPointList& new_feats,
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,
182  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input);
183 
184 template <>
186  CFeatureList& featureList, const TKeyPointList& new_feats,
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,
190  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
191 {
192  const TImageSize imgSize = cur_gray.getSize();
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);
196 
197  for (size_t i = 0; i < nNewToCheck && featureList.size() < maxNumFeatures;
198  i++)
199  {
200  const TKeyPoint& feat = new_feats[sorted_indices[i]];
201 
202  if (feat.response < minimum_KLT_response_to_add) continue;
203 
204  double min_dist_sqr = square(10000);
205 
206  if (!featureList.empty())
207  {
208  min_dist_sqr =
209  featureList.kdTreeClosestPoint2DsqrError(feat.pt.x, feat.pt.y);
210  }
211 
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 &&
214  feat.pt.y < h_off)
215  {
216  // Add new feature:
217  CFeature ft;
218  ft.type = featFAST;
219  ft.keypoint.ID = ++max_feat_ID_at_input;
220  ft.keypoint.pt.x = feat.pt.x;
221  ft.keypoint.pt.y = feat.pt.y;
222  ft.response = feat.response;
223  ft.orientation = 0;
224  ft.keypoint.octave = 1;
225  ft.patchSize = patchSize; // The size of the feature patch
226 
227  // Image patch surronding the feature
228  if (patchSize > 0)
229  {
230  ft.patch.emplace();
231  cur_gray.extract_patch(
232  *ft.patch, round(feat.pt.x) - offset,
233  round(feat.pt.y) - offset, patchSize, patchSize);
234  }
235 
236  featureList.emplace_back(std::move(ft));
237  }
238  }
239 } // end of trackFeatures_addNewFeats<>
240 
241 template <class FEAT_LIST>
243  FEAT_LIST& featureList, const TKeyPointList& new_feats,
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,
248  [[maybe_unused]] const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
249 {
250  // Version with KD-tree
252  featureList.getVector());
253 
254  for (size_t i = 0; i < nNewToCheck && featureList.size() < maxNumFeatures;
255  i++)
256  {
257  const TKeyPoint& feat = new_feats[sorted_indices[i]];
258  if (feat.response < minimum_KLT_response_to_add) break; // continue;
259 
260  // Check the min-distance:
261  double min_dist_sqr = std::numeric_limits<double>::max();
262 
263  if (!featureList.empty())
264  {
265  min_dist_sqr =
266  kdtree.kdTreeClosestPoint2DsqrError(feat.pt.x, feat.pt.y);
267  }
268 
269  if (min_dist_sqr > threshold_sqr_dist_to_add_new)
270  {
271  // OK: accept it
272  featureList.emplace_back(feat.pt.x, feat.pt.y);
273  kdtree.mark_as_outdated();
274 
275  // Fill out the rest of data:
276  typename FEAT_LIST::feature_t& newFeat = featureList.back();
277 
278  newFeat.ID = ++max_feat_ID_at_input;
279  newFeat.response = feat.response;
280  newFeat.octave = 0;
281  /** Inactive: right after detection, and before being tried to track
282  */
283  newFeat.track_status = status_IDLE;
284  }
285  }
286 } // end of trackFeatures_addNewFeats<>
287 
288 template <>
290  TKeyPointList& featureList, const TKeyPointList& new_feats,
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,
294  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
295 {
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);
300 }
301 template <>
303  TKeyPointfList& featureList, const TKeyPointList& new_feats,
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,
307  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
308 {
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);
313 }
314 
315 // Return the number of removed features
316 template <typename FEATLIST>
317 inline size_t trackFeatures_deleteOOB(
318  FEATLIST& trackedFeats, const size_t img_width, const size_t img_height,
319  const int MIN_DIST_MARGIN_TO_STOP_TRACKING);
320 
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)
325 {
326  if (trackedFeats.empty()) return 0;
327 
328  std::vector<size_t> survival_idxs;
329  const size_t N = trackedFeats.size();
330 
331  // 1st: Build list of survival indexes:
332  survival_idxs.reserve(N);
333  for (size_t i = 0; i < N; i++)
334  {
335  const typename FEATLIST::feature_t& ft = trackedFeats[i];
336  const TFeatureTrackStatus status = ft.track_status;
337  bool eras = (status_TRACKED != status && status_IDLE != status);
338  if (!eras)
339  {
340  // Also, check if it's too close to the image border:
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))
349  {
350  eras = true;
351  }
352  }
353  if (!eras) survival_idxs.push_back(i);
354  }
355 
356  // 2nd: Build updated list:
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++)
360  {
361  if (survival_idxs[i] != i)
362  trackedFeats[i] = trackedFeats[survival_idxs[i]];
363  }
364  trackedFeats.resize(N2);
365  return n_removed;
366 } // end of trackFeatures_deleteOOB
367 
368 template <>
370  TKeyPointList& trackedFeats, const size_t img_width,
371  const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
372 {
373  return trackFeatures_deleteOOB_impl_simple_feat<TKeyPointList>(
374  trackedFeats, img_width, img_height, MIN_DIST_MARGIN_TO_STOP_TRACKING);
375 }
376 template <>
378  TKeyPointfList& trackedFeats, const size_t img_width,
379  const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
380 {
381  return trackFeatures_deleteOOB_impl_simple_feat<TKeyPointfList>(
382  trackedFeats, img_width, img_height, MIN_DIST_MARGIN_TO_STOP_TRACKING);
383 }
384 
385 template <>
387  CFeatureList& trackedFeats, const size_t img_width, const size_t img_height,
388  const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
389 {
390  auto itFeat = trackedFeats.begin();
391  size_t n_removed = 0;
392  while (itFeat != trackedFeats.end())
393  {
394  const TFeatureTrackStatus status = itFeat->track_status;
395  bool eras = (status_TRACKED != status && status_IDLE != status);
396  if (!eras)
397  {
398  // Also, check if it's too close to the image border:
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))
405  {
406  eras = true;
407  }
408  }
409  if (eras) // Erase or keep?
410  {
411  itFeat = trackedFeats.erase(itFeat);
412  n_removed++;
413  }
414  else
415  ++itFeat;
416  }
417  return n_removed;
418 } // end of trackFeatures_deleteOOB
419 } // namespace mrpt::vision::detail
420 // ---------------------------- end of internal helper templates
421 // -------------------------------
422 
424  [[maybe_unused]] const CImage& old_img,
425  [[maybe_unused]] const CImage& new_img,
426  [[maybe_unused]] TKeyPointfList& inout_featureList)
427 {
428  THROW_EXCEPTION("Method not implemented by derived class!");
429 }
430 
431 /** Perform feature tracking from "old_img" to "new_img", with a (possibly
432  *empty) list of previously tracked features "featureList".
433  * This is a list of parameters (in "extraParams") accepted by ALL
434  *implementations of feature tracker (see each derived class for more specific
435  *parameters).
436  * - "add_new_features" (Default=0). If set to "1", new features will be
437  *also
438  *added to the existing ones in areas of the image poor of features.
439  * This method actually first call the pure virtual "trackFeatures_impl"
440  *method, then implements the optional detection of new features if
441  *"add_new_features"!=0.
442  */
443 template <typename FEATLIST>
445  const CImage& old_img, const CImage& new_img, FEATLIST& featureList)
446 {
447  mrpt::system::CTimeLoggerEntry tleg(m_timlog, "CGenericFeatureTracker");
448 
449  const size_t img_width = new_img.getWidth();
450  const size_t img_height = new_img.getHeight();
451 
452  // Take the maximum ID of "old" features so new feats (if
453  // "add_new_features==true") will be id+1, id+2, ...
454  TFeatureID max_feat_ID_at_input = 0;
455  if (!featureList.empty()) max_feat_ID_at_input = featureList.getMaxID();
456 
457  // Grayscale images
458  // =========================================
459  m_timlog.enter("CGenericFeatureTracker.to_grayscale");
460 
461  const CImage prev_gray(old_img, FAST_REF_OR_CONVERT_TO_GRAY);
462  const CImage cur_gray(new_img, FAST_REF_OR_CONVERT_TO_GRAY);
463 
464  m_timlog.leave("CGenericFeatureTracker.to_grayscale");
465 
466  // =================================
467  // (1st STEP) Do the actual tracking
468  // =================================
469  m_newly_detected_feats.clear();
470 
471  m_timlog.enter("CGenericFeatureTracker.trackFeatures_impl");
472 
473  trackFeatures_impl(prev_gray, cur_gray, featureList);
474 
475  m_timlog.leave("CGenericFeatureTracker.trackFeatures_impl");
476 
477  // ========================================================
478  // (2nd STEP) For successfully followed features, check their KLT response??
479  // ========================================================
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);
486 
487  if (check_KLT_response_every > 0 &&
488  ++m_check_KLT_counter >= size_t(check_KLT_response_every))
489  {
490  m_timlog.enter("CGenericFeatureTracker.check_KLT_responses");
491  m_check_KLT_counter = 0;
492 
493  const unsigned int max_x = img_width - KLT_response_half_win;
494  const unsigned int max_y = img_height - KLT_response_half_win;
495 
497  featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
498  max_x, max_y);
499 
500  m_timlog.leave("CGenericFeatureTracker.check_KLT_responses");
501 
502  } // end check_KLT_response_every
503 
504  // ============================================================
505  // (3rd STEP) Remove Out-of-bounds or badly tracked features
506  // or those marked as "bad" by their low KLT response
507  // ============================================================
508  const bool remove_lost_features =
509  extra_params.getWithDefaultVal("remove_lost_features", 0) != 0;
510 
511  if (remove_lost_features)
512  {
513  m_timlog.enter("CGenericFeatureTracker.OOB_remove");
514 
515  static const int MIN_DIST_MARGIN_TO_STOP_TRACKING = 10;
516 
517  const size_t nRemoved = detail::trackFeatures_deleteOOB(
518  featureList, img_width, img_height,
519  MIN_DIST_MARGIN_TO_STOP_TRACKING);
520 
521  m_timlog.leave("CGenericFeatureTracker.OOB_remove");
522 
523  last_execution_extra_info.num_deleted_feats = nRemoved;
524  }
525  else
526  {
527  last_execution_extra_info.num_deleted_feats = 0;
528  }
529 
530  // ========================================================
531  // (4th STEP) For successfully followed features, update its patch:
532  // ========================================================
533  const int update_patches_every =
534  extra_params.getWithDefaultVal("update_patches_every", 0);
535 
536  if (update_patches_every > 0 &&
537  ++m_update_patches_counter >= size_t(update_patches_every))
538  {
540  m_timlog, "CGenericFeatureTracker.update_patches");
541 
542  m_update_patches_counter = 0;
543 
544  // Update the patch for each valid feature:
545  detail::trackFeatures_updatePatch(featureList, cur_gray);
546 
547  } // end if update_patches_every
548 
549  // ========================================================
550  // (5th STEP) Do detection of new features??
551  // ========================================================
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);
556 
557  // Additional operation: if "add_new_features==true", find new features and
558  // add them in areas spare of valid features:
559  if (add_new_features)
560  {
562  m_timlog, "CGenericFeatureTracker.add_new_features");
563 
564  // Look for new features and save in "m_newly_detected_feats", if
565  // they're not already computed:
566  if (m_newly_detected_feats.empty())
567  {
568  // Do the detection
571  fe.options.FASTOptions.threshold = m_detector_adaptive_thres;
572 
573  CFeatureList new_feats;
574 
575  fe.detectFeatures(cur_gray, new_feats);
576 
577  m_newly_detected_feats.clear();
578  m_newly_detected_feats.reserve(new_feats.size());
579 
580  for (const CFeature& f : new_feats)
581  {
582  m_newly_detected_feats.push_back(TKeyPoint(f.keypoint));
583  }
584  }
585 
586  // Extra out info.
587  const size_t N = m_newly_detected_feats.size();
588  last_execution_extra_info.raw_FAST_feats_detected = N;
589 
590  // Update the adaptive threshold.
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);
595 
596  // Use KLT response instead of the OpenCV's original "response" field:
597  {
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++)
601  {
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 =
607  cur_gray.KLT_response(x, y, KLT_response_half_win);
608  else
609  m_newly_detected_feats[i].response = 0; // Out of bounds
610  }
611  }
612 
613  // Sort them by "response": It's ~100 times faster to sort a list of
614  // indices "sorted_indices" than sorting directly the actual list
615  // of features "m_newly_detected_feats"
616  std::vector<size_t> sorted_indices(N);
617  for (size_t i = 0; i < N; i++) sorted_indices[i] = i;
618 
619  std::sort(
620  sorted_indices.begin(), sorted_indices.end(),
621  KeypointResponseSorter<TKeyPointList>(m_newly_detected_feats));
622 
623  // For each new good feature, add it to the list of tracked ones only if
624  // it's pretty
625  // isolated:
626 
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);
636 
637  // Do it:
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);
643  }
644 
645 } // end of CGenericFeatureTracker::trackFeatures
646 
648  const CImage& old_img, const CImage& new_img, TKeyPointList& featureList)
649 {
650  internal_trackFeatures<TKeyPointList>(old_img, new_img, featureList);
651 }
652 
654  const CImage& old_img, const CImage& new_img, TKeyPointfList& featureList)
655 {
656  internal_trackFeatures<TKeyPointfList>(old_img, new_img, featureList);
657 }
658 
660  const size_t nNewlyDetectedFeats, const size_t desired_num_features)
661 {
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;
664 
665  if (nNewlyDetectedFeats < hysteresis_min_num_feats)
666  m_detector_adaptive_thres = std::max(
667  2.0, std::min(
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);
673 }
void trackFeatures_updatePatch< TKeyPointList >([[maybe_unused]] TKeyPointList &featureList, [[maybe_unused]] const CImage &cur_gray)
Definition: tracking.cpp:162
void trackFeatures_addNewFeats< CFeatureList >(CFeatureList &featureList, const TKeyPointList &new_feats, const std::vector< size_t > &sorted_indices, const size_t nNewToCheck, const size_t maxNumFeatures, const float minimum_KLT_response_to_add, const double threshold_sqr_dist_to_add_new, const size_t patchSize, const CImage &cur_gray, TFeatureID &max_feat_ID_at_input)
Definition: tracking.cpp:185
struct mrpt::vision::CFeatureExtraction::TOptions::TFASTOptions FASTOptions
Helper class: KD-tree search class for vector<KeyPoint>: Call mark_as_outdated() to force rebuilding ...
Definition: TKeyPoint.h:299
uint64_t TFeatureID
Definition of a feature ID.
void trackFeatures(const mrpt::img::CImage &old_img, const mrpt::img::CImage &new_img, TKeyPointList &inout_featureList)
Perform feature tracking from "old_img" to "new_img", with a (possibly empty) list of previously trac...
Definition: tracking.cpp:647
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
size_t trackFeatures_deleteOOB_impl_simple_feat(FEATLIST &trackedFeats, const size_t img_width, const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
Definition: tracking.cpp:322
TKeyPointMethod type
Keypoint method used to detect this feature.
Definition: CFeature.h:73
TKeyPoint_templ< mrpt::img::TPixelCoord > TKeyPoint
Simple structure for image key points.
Definition: TKeyPoint.h:78
void detectFeatures(const mrpt::img::CImage &img, CFeatureList &feats, const unsigned int init_ID=0, const unsigned int nDesiredFeatures=0, const TImageROI &ROI=TImageROI())
Extract features from the image based on the method defined in TOptions.
void internal_trackFeatures(const mrpt::img::CImage &old_img, const mrpt::img::CImage &new_img, FEATLIST &inout_featureList)
Perform feature tracking from "old_img" to "new_img", with a (possibly empty) list of previously trac...
Definition: tracking.cpp:444
size_t size() const
Definition: CFeature.h:352
Unable to track this feature (mismatch is too high for the given tracking window: lack of texture...
TOptions options
Set all the parameters of the desired method here before calling detectFeatures() ...
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:855
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
STL namespace.
void trackFeatures_checkResponses< TKeyPointfList >(TKeyPointfList &featureList, const CImage &cur_gray, const float minimum_KLT_response, const unsigned int KLT_response_half_win, const unsigned int max_x, const unsigned int max_y)
Definition: tracking.cpp:118
TFeatureID ID
ID of the feature.
Definition: TKeyPoint.h:39
void trackFeatures_addNewFeats_simple_list(FEAT_LIST &featureList, const TKeyPointList &new_feats, const std::vector< size_t > &sorted_indices, const size_t nNewToCheck, const size_t maxNumFeatures, const float minimum_KLT_response_to_add, const double threshold_sqr_dist_to_add_new, [[maybe_unused]] const size_t patchSize, [[maybe_unused]] const CImage &cur_gray, TFeatureID &max_feat_ID_at_input)
Definition: tracking.cpp:242
Simple structure for image key points.
Definition: TKeyPoint.h:28
void trackFeatures_checkResponses_impl_simple(FEAT_LIST &featureList, const CImage &cur_gray, const float minimum_KLT_response, const unsigned int KLT_response_half_win, const unsigned int max_x_, const unsigned int max_y_)
Definition: tracking.cpp:68
A helper struct to sort keypoints by their response: It can be used with these types: ...
Definition: TKeyPoint.h:283
virtual void trackFeatures_impl(const mrpt::img::CImage &old_img, const mrpt::img::CImage &new_img, TKeyPointfList &inout_featureList)
The tracking method implementation, to be implemented in children classes.
Inactive (right after detection, and before being tried to track)
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:818
float KLT_response(const unsigned int x, const unsigned int y, const unsigned int half_window_size) const
Compute the KLT response at a given pixel (x,y) - Only for grayscale images (for efficiency it avoids...
Definition: CImage.cpp:1967
TKeyPointf keypoint
Definition: CFeature.h:64
std::optional< mrpt::img::CImage > patch
A patch of the image surrounding the feature.
Definition: CFeature.h:67
Feature fell Out Of Bounds (out of the image limits, too close to image borders)
void updateAdaptiveNewFeatsThreshold(const size_t nNewlyDetectedFeats, const size_t desired_num_features)
Adapts the threshold m_detector_adaptive_thres according to the real and desired number of features j...
Definition: tracking.cpp:659
A pair (x,y) of pixel coordinates (integer resolution).
Definition: TPixelCoord.h:40
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
A generic 2D feature from an image, extracted with CFeatureExtraction Each feature may have one or mo...
Definition: CFeature.h:53
iterator erase(const iterator &it)
Definition: CFeature.h:345
TKeyPointMethod featsType
Type of the extracted features.
FAST feature detector, OpenCV&#39;s implementation ("Faster and better: A machine learning approach to...
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:275
void trackFeatures_addNewFeats< TKeyPointList >(TKeyPointList &featureList, const TKeyPointList &new_feats, const std::vector< size_t > &sorted_indices, const size_t nNewToCheck, const size_t maxNumFeatures, const float minimum_KLT_response_to_add, const double threshold_sqr_dist_to_add_new, const size_t patchSize, const CImage &cur_gray, TFeatureID &max_feat_ID_at_input)
Definition: tracking.cpp:289
return_t square(const num_t x)
Inline function for the square of a number.
float response
A measure of the "goodness" of the feature.
Definition: CFeature.h:79
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void trackFeatures_checkResponses(FEATLIST &featureList, const CImage &cur_gray, const float minimum_KLT_response, const unsigned int KLT_response_half_win, const unsigned int max_x, const unsigned int max_y)
size_t trackFeatures_deleteOOB(FEATLIST &trackedFeats, const size_t img_width, const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
uint8_t octave
The image octave the image was found in: 0=original image, 1=1/2 image, 2=1/4 image, etc.
Definition: TKeyPoint.h:49
void trackFeatures_updatePatch< CFeatureList >(CFeatureList &featureList, const CImage &cur_gray)
Definition: tracking.cpp:133
void trackFeatures_addNewFeats(FEATLIST &featureList, const TKeyPointList &new_feats, const std::vector< size_t > &sorted_indices, const size_t nNewToCheck, const size_t maxNumFeatures, const float minimum_KLT_response_to_add, const double threshold_sqr_dist_to_add_new, const size_t patchSize, const CImage &cur_gray, TFeatureID &max_feat_ID_at_input)
void trackFeatures_checkResponses< CFeatureList >(CFeatureList &featureList, const CImage &cur_gray, const float minimum_KLT_response, const unsigned int KLT_response_half_win, const unsigned int max_x, const unsigned int max_y)
Definition: tracking.cpp:35
float orientation
Main orientation of the feature.
Definition: CFeature.h:81
Functions for estimating the optimal transformation between two frames of references given measuremen...
float response
A measure of the "goodness" of the feature (typically, the KLT_response value)
Definition: TKeyPoint.h:46
void trackFeatures_updatePatch< TKeyPointfList >([[maybe_unused]] TKeyPointfList &featureList, [[maybe_unused]] const CImage &cur_gray)
Definition: tracking.cpp:169
uint16_t patchSize
Size of the patch (patchSize x patchSize) (it must be an odd number)
Definition: CFeature.h:70
void trackFeatures_updatePatch(FEATLIST &featureList, const CImage &cur_gray)
size_t trackFeatures_deleteOOB(CFeatureList &trackedFeats, const size_t img_width, const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
Definition: tracking.cpp:386
void trackFeatures_addNewFeats< TKeyPointfList >(TKeyPointfList &featureList, const TKeyPointList &new_feats, const std::vector< size_t > &sorted_indices, const size_t nNewToCheck, const size_t maxNumFeatures, const float minimum_KLT_response_to_add, const double threshold_sqr_dist_to_add_new, const size_t patchSize, const CImage &cur_gray, TFeatureID &max_feat_ID_at_input)
Definition: tracking.cpp:302
pixel_coords_t pt
Coordinates in the image.
Definition: TKeyPoint.h:36
void trackFeatures_checkResponses< TKeyPointList >(TKeyPointList &featureList, const CImage &cur_gray, const float minimum_KLT_response, const unsigned int KLT_response_half_win, const unsigned int max_x, const unsigned int max_y)
Definition: tracking.cpp:108
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
The central class from which images can be analyzed in search of different kinds of interest points a...
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020