MRPT  1.9.9
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-2019, 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/math/ops_matrices.h>
13 #include <mrpt/otherlibs/do_opencv_includes.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  TKeyPointList& featureList, const CImage& cur_gray)
164 {
165  MRPT_UNUSED_PARAM(featureList);
166  MRPT_UNUSED_PARAM(cur_gray);
167  // This list type does not have patch stored explicitly
168 } // end of trackFeatures_updatePatch<>
169 template <>
171  TKeyPointfList& featureList, const CImage& cur_gray)
172 {
173  MRPT_UNUSED_PARAM(featureList);
174  MRPT_UNUSED_PARAM(cur_gray);
175  // This list type does not have patch stored explicitly
176 } // end of trackFeatures_updatePatch<>
177 
178 template <typename FEATLIST>
179 inline void trackFeatures_addNewFeats(
180  FEATLIST& featureList, const TKeyPointList& new_feats,
181  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
182  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
183  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
184  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input);
185 
186 template <>
188  CFeatureList& featureList, const TKeyPointList& new_feats,
189  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
190  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
191  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
192  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
193 {
194  const TImageSize imgSize = cur_gray.getSize();
195  const int offset = (int)patchSize / 2 + 1;
196  const int w_off = int(imgSize.x - offset);
197  const int h_off = int(imgSize.y - offset);
198 
199  for (size_t i = 0; i < nNewToCheck && featureList.size() < maxNumFeatures;
200  i++)
201  {
202  const TKeyPoint& feat = new_feats[sorted_indices[i]];
203 
204  if (feat.response < minimum_KLT_response_to_add) continue;
205 
206  double min_dist_sqr = square(10000);
207 
208  if (!featureList.empty())
209  {
210  min_dist_sqr =
211  featureList.kdTreeClosestPoint2DsqrError(feat.pt.x, feat.pt.y);
212  }
213 
214  if (min_dist_sqr > threshold_sqr_dist_to_add_new &&
215  feat.pt.x > offset && feat.pt.y > offset && feat.pt.x < w_off &&
216  feat.pt.y < h_off)
217  {
218  // Add new feature:
219  CFeature ft;
220  ft.type = featFAST;
221  ft.keypoint.ID = ++max_feat_ID_at_input;
222  ft.keypoint.pt.x = feat.pt.x;
223  ft.keypoint.pt.y = feat.pt.y;
224  ft.response = feat.response;
225  ft.orientation = 0;
226  ft.keypoint.octave = 1;
227  ft.patchSize = patchSize; // The size of the feature patch
228 
229  // Image patch surronding the feature
230  if (patchSize > 0)
231  {
232  ft.patch.emplace();
233  cur_gray.extract_patch(
234  *ft.patch, round(feat.pt.x) - offset,
235  round(feat.pt.y) - offset, patchSize, patchSize);
236  }
237 
238  featureList.emplace_back(std::move(ft));
239  }
240  }
241 } // end of trackFeatures_addNewFeats<>
242 
243 template <class FEAT_LIST>
245  FEAT_LIST& featureList, const TKeyPointList& new_feats,
246  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
247  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
248  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
249  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
250 {
251  MRPT_UNUSED_PARAM(patchSize);
252  MRPT_UNUSED_PARAM(cur_gray);
253  // Version with KD-tree
255  featureList.getVector());
256 
257  for (size_t i = 0; i < nNewToCheck && featureList.size() < maxNumFeatures;
258  i++)
259  {
260  const TKeyPoint& feat = new_feats[sorted_indices[i]];
261  if (feat.response < minimum_KLT_response_to_add) break; // continue;
262 
263  // Check the min-distance:
264  double min_dist_sqr = std::numeric_limits<double>::max();
265 
266  if (!featureList.empty())
267  {
268  min_dist_sqr =
269  kdtree.kdTreeClosestPoint2DsqrError(feat.pt.x, feat.pt.y);
270  }
271 
272  if (min_dist_sqr > threshold_sqr_dist_to_add_new)
273  {
274  // OK: accept it
275  featureList.emplace_back(feat.pt.x, feat.pt.y);
276  kdtree.mark_as_outdated();
277 
278  // Fill out the rest of data:
279  typename FEAT_LIST::feature_t& newFeat = featureList.back();
280 
281  newFeat.ID = ++max_feat_ID_at_input;
282  newFeat.response = feat.response;
283  newFeat.octave = 0;
284  /** Inactive: right after detection, and before being tried to track
285  */
286  newFeat.track_status = status_IDLE;
287  }
288  }
289 } // end of trackFeatures_addNewFeats<>
290 
291 template <>
293  TKeyPointList& featureList, const TKeyPointList& new_feats,
294  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
295  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
296  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
297  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
298 {
299  trackFeatures_addNewFeats_simple_list<TKeyPointList>(
300  featureList, new_feats, sorted_indices, nNewToCheck, maxNumFeatures,
301  minimum_KLT_response_to_add, threshold_sqr_dist_to_add_new, patchSize,
302  cur_gray, max_feat_ID_at_input);
303 }
304 template <>
306  TKeyPointfList& featureList, const TKeyPointList& new_feats,
307  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
308  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
309  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
310  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
311 {
312  trackFeatures_addNewFeats_simple_list<TKeyPointfList>(
313  featureList, new_feats, sorted_indices, nNewToCheck, maxNumFeatures,
314  minimum_KLT_response_to_add, threshold_sqr_dist_to_add_new, patchSize,
315  cur_gray, max_feat_ID_at_input);
316 }
317 
318 // Return the number of removed features
319 template <typename FEATLIST>
320 inline size_t trackFeatures_deleteOOB(
321  FEATLIST& trackedFeats, const size_t img_width, const size_t img_height,
322  const int MIN_DIST_MARGIN_TO_STOP_TRACKING);
323 
324 template <typename FEATLIST>
326  FEATLIST& trackedFeats, const size_t img_width, const size_t img_height,
327  const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
328 {
329  if (trackedFeats.empty()) return 0;
330 
331  std::vector<size_t> survival_idxs;
332  const size_t N = trackedFeats.size();
333 
334  // 1st: Build list of survival indexes:
335  survival_idxs.reserve(N);
336  for (size_t i = 0; i < N; i++)
337  {
338  const typename FEATLIST::feature_t& ft = trackedFeats[i];
339  const TFeatureTrackStatus status = ft.track_status;
340  bool eras = (status_TRACKED != status && status_IDLE != status);
341  if (!eras)
342  {
343  // Also, check if it's too close to the image border:
344  const int x = ft.pt.x;
345  const int y = ft.pt.y;
346  if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
347  y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
348  x > static_cast<int>(
349  img_width - MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
350  y > static_cast<int>(
351  img_height - MIN_DIST_MARGIN_TO_STOP_TRACKING))
352  {
353  eras = true;
354  }
355  }
356  if (!eras) survival_idxs.push_back(i);
357  }
358 
359  // 2nd: Build updated list:
360  const size_t N2 = survival_idxs.size();
361  const size_t n_removed = N - N2;
362  for (size_t i = 0; i < N2; i++)
363  {
364  if (survival_idxs[i] != i)
365  trackedFeats[i] = trackedFeats[survival_idxs[i]];
366  }
367  trackedFeats.resize(N2);
368  return n_removed;
369 } // end of trackFeatures_deleteOOB
370 
371 template <>
373  TKeyPointList& trackedFeats, const size_t img_width,
374  const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
375 {
376  return trackFeatures_deleteOOB_impl_simple_feat<TKeyPointList>(
377  trackedFeats, img_width, img_height, MIN_DIST_MARGIN_TO_STOP_TRACKING);
378 }
379 template <>
381  TKeyPointfList& trackedFeats, const size_t img_width,
382  const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
383 {
384  return trackFeatures_deleteOOB_impl_simple_feat<TKeyPointfList>(
385  trackedFeats, img_width, img_height, MIN_DIST_MARGIN_TO_STOP_TRACKING);
386 }
387 
388 template <>
390  CFeatureList& trackedFeats, const size_t img_width, const size_t img_height,
391  const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
392 {
393  auto itFeat = trackedFeats.begin();
394  size_t n_removed = 0;
395  while (itFeat != trackedFeats.end())
396  {
397  const TFeatureTrackStatus status = itFeat->track_status;
398  bool eras = (status_TRACKED != status && status_IDLE != status);
399  if (!eras)
400  {
401  // Also, check if it's too close to the image border:
402  const float x = itFeat->keypoint.pt.x;
403  const float y = itFeat->keypoint.pt.y;
404  if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
405  y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
406  x > (img_width - MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
407  y > (img_height - MIN_DIST_MARGIN_TO_STOP_TRACKING))
408  {
409  eras = true;
410  }
411  }
412  if (eras) // Erase or keep?
413  {
414  itFeat = trackedFeats.erase(itFeat);
415  n_removed++;
416  }
417  else
418  ++itFeat;
419  }
420  return n_removed;
421 } // end of trackFeatures_deleteOOB
422 } // namespace mrpt::vision::detail
423 // ---------------------------- end of internal helper templates
424 // -------------------------------
425 
427  const CImage& old_img, const CImage& new_img,
428  TKeyPointfList& inout_featureList)
429 {
430  MRPT_UNUSED_PARAM(old_img);
431  MRPT_UNUSED_PARAM(new_img);
432  MRPT_UNUSED_PARAM(inout_featureList);
433  THROW_EXCEPTION("Method not implemented by derived class!");
434 }
435 
436 /** Perform feature tracking from "old_img" to "new_img", with a (possibly
437  *empty) list of previously tracked features "featureList".
438  * This is a list of parameters (in "extraParams") accepted by ALL
439  *implementations of feature tracker (see each derived class for more specific
440  *parameters).
441  * - "add_new_features" (Default=0). If set to "1", new features will be
442  *also
443  *added to the existing ones in areas of the image poor of features.
444  * This method actually first call the pure virtual "trackFeatures_impl"
445  *method, then implements the optional detection of new features if
446  *"add_new_features"!=0.
447  */
448 template <typename FEATLIST>
450  const CImage& old_img, const CImage& new_img, FEATLIST& featureList)
451 {
452  mrpt::system::CTimeLoggerEntry tleg(m_timlog, "CGenericFeatureTracker");
453 
454  const size_t img_width = new_img.getWidth();
455  const size_t img_height = new_img.getHeight();
456 
457  // Take the maximum ID of "old" features so new feats (if
458  // "add_new_features==true") will be id+1, id+2, ...
459  TFeatureID max_feat_ID_at_input = 0;
460  if (!featureList.empty()) max_feat_ID_at_input = featureList.getMaxID();
461 
462  // Grayscale images
463  // =========================================
464  m_timlog.enter("CGenericFeatureTracker.to_grayscale");
465 
466  const CImage prev_gray(old_img, FAST_REF_OR_CONVERT_TO_GRAY);
467  const CImage cur_gray(new_img, FAST_REF_OR_CONVERT_TO_GRAY);
468 
469  m_timlog.leave("CGenericFeatureTracker.to_grayscale");
470 
471  // =================================
472  // (1st STEP) Do the actual tracking
473  // =================================
474  m_newly_detected_feats.clear();
475 
476  m_timlog.enter("CGenericFeatureTracker.trackFeatures_impl");
477 
478  trackFeatures_impl(prev_gray, cur_gray, featureList);
479 
480  m_timlog.leave("CGenericFeatureTracker.trackFeatures_impl");
481 
482  // ========================================================
483  // (2nd STEP) For successfully followed features, check their KLT response??
484  // ========================================================
485  const int check_KLT_response_every =
486  extra_params.getWithDefaultVal("check_KLT_response_every", 1);
487  const float minimum_KLT_response =
488  extra_params.getWithDefaultVal("minimum_KLT_response", 30.f);
489  const unsigned int KLT_response_half_win =
490  extra_params.getWithDefaultVal("KLT_response_half_win", 8U);
491 
492  if (check_KLT_response_every > 0 &&
493  ++m_check_KLT_counter >= size_t(check_KLT_response_every))
494  {
495  m_timlog.enter("CGenericFeatureTracker.check_KLT_responses");
496  m_check_KLT_counter = 0;
497 
498  const unsigned int max_x = img_width - KLT_response_half_win;
499  const unsigned int max_y = img_height - KLT_response_half_win;
500 
502  featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
503  max_x, max_y);
504 
505  m_timlog.leave("CGenericFeatureTracker.check_KLT_responses");
506 
507  } // end check_KLT_response_every
508 
509  // ============================================================
510  // (3rd STEP) Remove Out-of-bounds or badly tracked features
511  // or those marked as "bad" by their low KLT response
512  // ============================================================
513  const bool remove_lost_features =
514  extra_params.getWithDefaultVal("remove_lost_features", 0) != 0;
515 
516  if (remove_lost_features)
517  {
518  m_timlog.enter("CGenericFeatureTracker.OOB_remove");
519 
520  static const int MIN_DIST_MARGIN_TO_STOP_TRACKING = 10;
521 
522  const size_t nRemoved = detail::trackFeatures_deleteOOB(
523  featureList, img_width, img_height,
524  MIN_DIST_MARGIN_TO_STOP_TRACKING);
525 
526  m_timlog.leave("CGenericFeatureTracker.OOB_remove");
527 
528  last_execution_extra_info.num_deleted_feats = nRemoved;
529  }
530  else
531  {
532  last_execution_extra_info.num_deleted_feats = 0;
533  }
534 
535  // ========================================================
536  // (4th STEP) For successfully followed features, update its patch:
537  // ========================================================
538  const int update_patches_every =
539  extra_params.getWithDefaultVal("update_patches_every", 0);
540 
541  if (update_patches_every > 0 &&
542  ++m_update_patches_counter >= size_t(update_patches_every))
543  {
545  m_timlog, "CGenericFeatureTracker.update_patches");
546 
547  m_update_patches_counter = 0;
548 
549  // Update the patch for each valid feature:
550  detail::trackFeatures_updatePatch(featureList, cur_gray);
551 
552  } // end if update_patches_every
553 
554  // ========================================================
555  // (5th STEP) Do detection of new features??
556  // ========================================================
557  const bool add_new_features =
558  extra_params.getWithDefaultVal("add_new_features", 1) != 0;
559  const double threshold_dist_to_add_new =
560  extra_params.getWithDefaultVal("add_new_feat_min_separation", 15);
561 
562  // Additional operation: if "add_new_features==true", find new features and
563  // add them in areas spare of valid features:
564  if (add_new_features)
565  {
567  m_timlog, "CGenericFeatureTracker.add_new_features");
568 
569  // Look for new features and save in "m_newly_detected_feats", if
570  // they're not already computed:
571  if (m_newly_detected_feats.empty())
572  {
573  // Do the detection
574  const int fast_v = extra_params.getWithDefaultVal(
575  "add_new_features_FAST_version", 10);
576 
577  switch (fast_v)
578  {
579  case 9:
581  cur_gray, m_newly_detected_feats,
582  m_detector_adaptive_thres);
583  break;
584  case 10:
586  cur_gray, m_newly_detected_feats,
587  m_detector_adaptive_thres);
588  break;
589  case 12:
591  cur_gray, m_newly_detected_feats,
592  m_detector_adaptive_thres);
593  break;
594  default:
596  "Invalid value for `add_new_features_FAST_version`: "
597  "valid are 9,10,12");
598  }
599  }
600 
601  // Extra out info.
602  const size_t N = m_newly_detected_feats.size();
603  last_execution_extra_info.raw_FAST_feats_detected = N;
604 
605  // Update the adaptive threshold.
606  const size_t desired_num_features = extra_params.getWithDefaultVal(
607  "desired_num_features_adapt",
608  size_t((img_width * img_height) >> 9));
609  updateAdaptiveNewFeatsThreshold(N, desired_num_features);
610 
611  // Use KLT response instead of the OpenCV's original "response" field:
612  {
613  const unsigned int max_x = img_width - KLT_response_half_win;
614  const unsigned int max_y = img_height - KLT_response_half_win;
615  for (size_t i = 0; i < N; i++)
616  {
617  const unsigned int x = m_newly_detected_feats[i].pt.x;
618  const unsigned int y = m_newly_detected_feats[i].pt.y;
619  if (x > KLT_response_half_win && y > KLT_response_half_win &&
620  x < max_x && y < max_y)
621  m_newly_detected_feats[i].response =
622  cur_gray.KLT_response(x, y, KLT_response_half_win);
623  else
624  m_newly_detected_feats[i].response = 0; // Out of bounds
625  }
626  }
627 
628  // Sort them by "response": It's ~100 times faster to sort a list of
629  // indices "sorted_indices" than sorting directly the actual list
630  // of features "m_newly_detected_feats"
631  std::vector<size_t> sorted_indices(N);
632  for (size_t i = 0; i < N; i++) sorted_indices[i] = i;
633 
634  std::sort(
635  sorted_indices.begin(), sorted_indices.end(),
636  KeypointResponseSorter<TKeyPointList>(m_newly_detected_feats));
637 
638  // For each new good feature, add it to the list of tracked ones only if
639  // it's pretty
640  // isolated:
641 
642  const size_t nNewToCheck = std::min(size_t(1500), N);
643  const double threshold_sqr_dist_to_add_new =
644  square(threshold_dist_to_add_new);
645  const size_t maxNumFeatures =
646  extra_params.getWithDefaultVal("add_new_feat_max_features", 100U);
647  const size_t patchSize =
648  extra_params.getWithDefaultVal("add_new_feat_patch_size", 0U);
649  const float minimum_KLT_response_to_add =
650  extra_params.getWithDefaultVal("minimum_KLT_response_to_add", 70.f);
651 
652  // Do it:
654  featureList, m_newly_detected_feats, sorted_indices, nNewToCheck,
655  maxNumFeatures, minimum_KLT_response_to_add,
656  threshold_sqr_dist_to_add_new, patchSize, cur_gray,
657  max_feat_ID_at_input);
658  }
659 
660 } // end of CGenericFeatureTracker::trackFeatures
661 
663  const CImage& old_img, const CImage& new_img, TKeyPointList& featureList)
664 {
665  internal_trackFeatures<TKeyPointList>(old_img, new_img, featureList);
666 }
667 
669  const CImage& old_img, const CImage& new_img, TKeyPointfList& featureList)
670 {
671  internal_trackFeatures<TKeyPointfList>(old_img, new_img, featureList);
672 }
673 
675  const size_t nNewlyDetectedFeats, const size_t desired_num_features)
676 {
677  const size_t hysteresis_min_num_feats = desired_num_features * 0.9;
678  const size_t hysteresis_max_num_feats = desired_num_features * 1.1;
679 
680  if (nNewlyDetectedFeats < hysteresis_min_num_feats)
681  m_detector_adaptive_thres = std::max(
682  2.0, std::min(
683  m_detector_adaptive_thres - 1.0,
684  m_detector_adaptive_thres * 0.8));
685  else if (nNewlyDetectedFeats > hysteresis_max_num_feats)
686  m_detector_adaptive_thres = std::max(
687  m_detector_adaptive_thres + 1.0, m_detector_adaptive_thres * 1.2);
688 }
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:187
Helper class: KD-tree search class for vector<KeyPoint>: Call mark_as_outdated() to force rebuilding ...
Definition: TKeyPoint.h:304
uint64_t TFeatureID
Definition of a feature ID.
#define min(a, b)
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:662
#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:325
TKeyPointMethod type
Keypoint method used to detect this feature.
Definition: CFeature.h:73
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:449
Unable to track this feature (mismatch is too high for the given tracking window: lack of texture...
GLintptr offset
Definition: glext.h:3936
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:878
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
Simple structure for image key points.
Definition: TKeyPoint.h:28
void trackFeatures_updatePatch< TKeyPointfList >(TKeyPointfList &featureList, const CImage &cur_gray)
Definition: tracking.cpp:170
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
static void detectFeatures_SSE2_FASTER10(const mrpt::img::CImage &img, TKeyPointList &corners, const int threshold=20, bool append_to_list=false, uint8_t octave=0, std::vector< size_t > *out_feats_index_by_row=nullptr)
Just like detectFeatures_SSE2_FASTER9() for another version of the detector.
A helper struct to sort keypoints by their response: It can be used with these types: ...
Definition: TKeyPoint.h:288
T square(const T x)
Inline function for the square of a number.
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.
Definition: tracking.cpp:426
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:847
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:1905
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:674
A pair (x,y) of pixel coordinates (integer resolution).
Definition: TPixelCoord.h:39
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
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:292
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
_u8 status
Definition: rplidar_cmd.h:19
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)
static void detectFeatures_SSE2_FASTER9(const mrpt::img::CImage &img, TKeyPointList &corners, const int threshold=20, bool append_to_list=false, uint8_t octave=0, std::vector< size_t > *out_feats_index_by_row=nullptr)
A SSE2-optimized implementation of FASTER-9 (requires img to be grayscale).
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, const size_t patchSize, const CImage &cur_gray, TFeatureID &max_feat_ID_at_input)
Definition: tracking.cpp:244
GLenum GLint GLint y
Definition: glext.h:3542
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
GLenum GLint x
Definition: glext.h:3542
static void detectFeatures_SSE2_FASTER12(const mrpt::img::CImage &img, TKeyPointList &corners, const int threshold=20, bool append_to_list=false, uint8_t octave=0, std::vector< size_t > *out_feats_index_by_row=nullptr)
Just like detectFeatures_SSE2_FASTER9() for another version of the detector.
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
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:389
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:305
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
void trackFeatures_updatePatch< TKeyPointList >(TKeyPointList &featureList, const CImage &cur_gray)
Definition: tracking.cpp:162
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019