Main MRPT website > C++ reference for MRPT 1.9.9
tracking.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/vision/tracking.h>
14 
15 // Universal include for all versions of OpenCV
16 #include <mrpt/otherlibs/do_opencv_includes.h>
17 
18 using namespace mrpt;
19 using namespace mrpt::vision;
20 using namespace mrpt::img;
21 using namespace mrpt::tfest;
22 using namespace mrpt::math;
23 using namespace std;
24 
25 // ------------------------------- internal helper templates
26 // ---------------------------------
27 namespace mrpt
28 {
29 namespace vision
30 {
31 namespace detail
32 {
33 template <typename FEATLIST>
35  FEATLIST& featureList, const CImage& cur_gray,
36  const float minimum_KLT_response, const unsigned int KLT_response_half_win,
37  const unsigned int max_x, const unsigned int max_y);
38 
39 template <>
41  CFeatureList& featureList, const CImage& cur_gray,
42  const float minimum_KLT_response, const unsigned int KLT_response_half_win,
43  const unsigned int max_x, const unsigned int max_y)
44 {
45  const CFeatureList::iterator itFeatEnd = featureList.end();
46  for (CFeatureList::iterator itFeat = featureList.begin();
47  itFeat != itFeatEnd; ++itFeat)
48  {
49  CFeature* ft = itFeat->get();
50  if (ft->track_status != status_TRACKED)
51  continue; // Skip if it's not correctly tracked.
52 
53  const unsigned int x = ft->x;
54  const unsigned int y = ft->y;
55  if (x > KLT_response_half_win && y > KLT_response_half_win &&
56  x < max_x && y < max_y)
57  { // Update response:
58  ft->response = cur_gray.KLT_response(x, y, KLT_response_half_win);
59 
60  // Is it good enough?
61  // http://grooveshark.com/s/Goonies+Are+Good+Enough/2beBfO?src=5
62  if (ft->response < minimum_KLT_response)
63  { // Nope!
65  }
66  }
67  else
68  { // Out of bounds
69  ft->response = 0;
71  }
72  }
73 } // end of trackFeatures_checkResponses<>
74 
75 template <class FEAT_LIST>
77  FEAT_LIST& featureList, const CImage& cur_gray,
78  const float minimum_KLT_response, const unsigned int KLT_response_half_win,
79  const unsigned int max_x_, const unsigned int max_y_)
80 {
81  if (featureList.empty()) return;
82 
83  using pixel_coord_t = typename FEAT_LIST::feature_t::pixel_coord_t;
84  const pixel_coord_t half_win =
85  static_cast<pixel_coord_t>(KLT_response_half_win);
86  const pixel_coord_t max_x = static_cast<pixel_coord_t>(max_x_);
87  const pixel_coord_t max_y = static_cast<pixel_coord_t>(max_y_);
88 
89  for (int N = featureList.size() - 1; N >= 0; --N)
90  {
91  typename FEAT_LIST::feature_t& ft = featureList[N];
92  if (ft.track_status != status_TRACKED)
93  continue; // Skip if it's not correctly tracked.
94 
95  if (ft.pt.x > half_win && ft.pt.y > half_win && ft.pt.x < max_x &&
96  ft.pt.y < max_y)
97  { // Update response:
98  ft.response =
99  cur_gray.KLT_response(ft.pt.x, ft.pt.y, KLT_response_half_win);
100 
101  // Is it good enough?
102  // http://grooveshark.com/s/Goonies+Are+Good+Enough/2beBfO?src=5
103  if (ft.response < minimum_KLT_response)
104  { // Nope!
105  ft.track_status = status_LOST;
106  }
107  }
108  else
109  { // Out of bounds
110  ft.response = 0;
111  ft.track_status = status_OOB;
112  }
113  }
114 } // end of trackFeatures_checkResponses<>
115 
116 template <>
118  TSimpleFeatureList& featureList, const CImage& cur_gray,
119  const float minimum_KLT_response, const unsigned int KLT_response_half_win,
120  const unsigned int max_x, const unsigned int max_y)
121 {
122  trackFeatures_checkResponses_impl_simple<TSimpleFeatureList>(
123  featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
124  max_x, max_y);
125 }
126 template <>
128  TSimpleFeaturefList& featureList, const CImage& cur_gray,
129  const float minimum_KLT_response, const unsigned int KLT_response_half_win,
130  const unsigned int max_x, const unsigned int max_y)
131 {
132  trackFeatures_checkResponses_impl_simple<TSimpleFeaturefList>(
133  featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
134  max_x, max_y);
135 }
136 
137 template <typename FEATLIST>
138 inline void trackFeatures_updatePatch(
139  FEATLIST& featureList, const CImage& cur_gray);
140 
141 template <>
143  CFeatureList& featureList, const CImage& cur_gray)
144 {
145  for (CFeatureList::iterator itFeat = featureList.begin();
146  itFeat != featureList.end(); ++itFeat)
147  {
148  CFeature* ft = itFeat->get();
149  if (ft->track_status != status_TRACKED)
150  continue; // Skip if it's not correctly tracked.
151 
152  const size_t patch_width = ft->patch.getWidth();
153  const size_t patch_height = ft->patch.getHeight();
154  if (patch_width > 0 && patch_height > 0)
155  {
156  try
157  {
158  const int offset = (int)patch_width / 2; // + 1;
159  cur_gray.extract_patch(
160  ft->patch, round(ft->x) - offset, round(ft->y) - offset,
161  patch_width, patch_height);
162  }
163  catch (std::exception&)
164  {
165  ft->track_status = status_OOB; // Out of bounds!
166  }
167  }
168  }
169 } // end of trackFeatures_updatePatch<>
170 template <>
172  TSimpleFeatureList& featureList, const CImage& cur_gray)
173 {
174  MRPT_UNUSED_PARAM(featureList);
175  MRPT_UNUSED_PARAM(cur_gray);
176  // This list type does not have patch stored explicitly
177 } // end of trackFeatures_updatePatch<>
178 template <>
180  TSimpleFeaturefList& featureList, const CImage& cur_gray)
181 {
182  MRPT_UNUSED_PARAM(featureList);
183  MRPT_UNUSED_PARAM(cur_gray);
184  // This list type does not have patch stored explicitly
185 } // end of trackFeatures_updatePatch<>
186 
187 template <typename FEATLIST>
188 inline void trackFeatures_addNewFeats(
189  FEATLIST& featureList, const TSimpleFeatureList& new_feats,
190  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
191  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
192  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
193  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input);
194 
195 template <>
197  CFeatureList& featureList, const TSimpleFeatureList& new_feats,
198  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
199  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
200  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
201  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
202 {
203  const TImageSize imgSize = cur_gray.getSize();
204  const int offset = (int)patchSize / 2 + 1;
205  const int w_off = int(imgSize.x - offset);
206  const int h_off = int(imgSize.y - offset);
207 
208  for (size_t i = 0; i < nNewToCheck && featureList.size() < maxNumFeatures;
209  i++)
210  {
211  const TSimpleFeature& feat = new_feats[sorted_indices[i]];
212 
213  if (feat.response < minimum_KLT_response_to_add) continue;
214 
215  double min_dist_sqr = square(10000);
216 
217  if (!featureList.empty())
218  {
219  // m_timlog.enter("[CGenericFeatureTracker] add new
220  // features.kdtree");
221  min_dist_sqr =
222  featureList.kdTreeClosestPoint2DsqrError(feat.pt.x, feat.pt.y);
223  // m_timlog.leave("[CGenericFeatureTracker] add new
224  // features.kdtree");
225  }
226 
227  if (min_dist_sqr > threshold_sqr_dist_to_add_new &&
228  feat.pt.x > offset && feat.pt.y > offset && feat.pt.x < w_off &&
229  feat.pt.y < h_off)
230  {
231  // Add new feature:
232  CFeature::Ptr ft = mrpt::make_aligned_shared<CFeature>();
233  ft->type = featFAST;
234  ft->ID = ++max_feat_ID_at_input;
235  ft->x = feat.pt.x;
236  ft->y = feat.pt.y;
237  ft->response = feat.response;
238  ft->orientation = 0;
239  ft->scale = 1;
240  ft->patchSize = patchSize; // The size of the feature patch
241 
242  if (patchSize > 0)
243  cur_gray.extract_patch(
244  ft->patch, round(ft->x) - offset, round(ft->y) - offset,
245  patchSize,
246  patchSize); // Image patch surronding the feature
247 
248  featureList.push_back(ft);
249  }
250  }
251 } // end of trackFeatures_addNewFeats<>
252 
253 template <class FEAT_LIST>
255  FEAT_LIST& featureList, const TSimpleFeatureList& new_feats,
256  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
257  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
258  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
259  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
260 {
261 #if 0
262  // Brute-force version:
263  const int max_manhatan_dist = std::sqrt(2*threshold_sqr_dist_to_add_new);
264 
265  for (size_t i=0;i<nNewToCheck && featureList.size()<maxNumFeatures;i++)
266  {
267  const TSimpleFeature &feat = new_feats[ sorted_indices[i] ];
268  if (feat.response<minimum_KLT_response_to_add) break; // continue;
269 
270  // Check the min-distance:
271  int manh_dist = std::numeric_limits<int>::max();
272  for (size_t j=0;j<featureList.size();j++)
273  {
274  const TSimpleFeature &existing = featureList[j];
275  const int d = std::abs(existing.pt.x-feat.pt.x)+std::abs(existing.pt.y-feat.pt.y);
276  mrpt::keep_min(manh_dist, d);
277  }
278 
279  if (manh_dist<max_manhatan_dist)
280  continue; // Already occupied! skip.
281 
282  // OK: accept it
283  featureList.push_back_fast(feat.pt.x,feat.pt.y); // (x,y)
284  //featureList.mark_kdtree_as_outdated();
285 
286  // Fill out the rest of data:
287  TSimpleFeature &newFeat = featureList.back();
288 
289  newFeat.ID = ++max_feat_ID_at_input;
290  newFeat.response = feat.response;
291  newFeat.octave = 0;
292  /** Inactive: right after detection, and before being tried to track */
293  newFeat.track_status = status_IDLE;
294  }
295 #elif 0
296  // Version with an occupancy grid:
297  const int grid_cell_log2 = round(
298  std::log(std::sqrt(threshold_sqr_dist_to_add_new) * 0.5) /
299  std::log(2.0));
300 
301  int grid_lx = 1 + (cur_gray.getWidth() >> grid_cell_log2);
302  int grid_ly = 1 + (cur_gray.getHeight() >> grid_cell_log2);
303 
304  mrpt::math::CMatrixBool& occupied_sections =
305  featureList.getOccupiedSectionsMatrix();
306 
307  occupied_sections.setSize(
308  grid_lx, grid_ly); // See the comments above for an explanation.
309  occupied_sections.fillAll(false);
310 
311  for (size_t i = 0; i < featureList.size(); i++)
312  {
313  const TSimpleFeature& feat = featureList[i];
314  const int section_idx_x = feat.pt.x >> grid_cell_log2;
315  const int section_idx_y = feat.pt.y >> grid_cell_log2;
316 
317  if (!section_idx_x || !section_idx_y || section_idx_x >= grid_lx - 1 ||
318  section_idx_y >= grid_ly - 1)
319  continue; // This may be too radical, but speeds up the logic
320  // below...
321 
322  // Mark sections as occupied
323  bool* ptr1 =
324  &occupied_sections.get_unsafe(section_idx_x - 1, section_idx_y - 1);
325  bool* ptr2 =
326  &occupied_sections.get_unsafe(section_idx_x - 1, section_idx_y);
327  bool* ptr3 =
328  &occupied_sections.get_unsafe(section_idx_x - 1, section_idx_y + 1);
329  ptr1[0] = ptr1[1] = ptr1[2] = true;
330  ptr2[0] = ptr2[1] = ptr2[2] = true;
331  ptr3[0] = ptr3[1] = ptr3[2] = true;
332  }
333 
334  for (size_t i = 0; i < nNewToCheck && featureList.size() < maxNumFeatures;
335  i++)
336  {
337  const TSimpleFeature& feat = new_feats[sorted_indices[i]];
338  if (feat.response < minimum_KLT_response_to_add) break; // continue;
339 
340  // Check the min-distance:
341  const int section_idx_x = feat.pt.x >> grid_cell_log2;
342  const int section_idx_y = feat.pt.y >> grid_cell_log2;
343 
344  if (!section_idx_x || !section_idx_y || section_idx_x >= grid_lx - 2 ||
345  section_idx_y >= grid_ly - 2)
346  continue; // This may be too radical, but speeds up the logic
347  // below...
348 
349  if (occupied_sections(section_idx_x, section_idx_y))
350  continue; // Already occupied! skip.
351 
352  // Mark section as occupied
353  bool* ptr1 =
354  &occupied_sections.get_unsafe(section_idx_x - 1, section_idx_y - 1);
355  bool* ptr2 =
356  &occupied_sections.get_unsafe(section_idx_x - 1, section_idx_y);
357  bool* ptr3 =
358  &occupied_sections.get_unsafe(section_idx_x - 1, section_idx_y + 1);
359 
360  ptr1[0] = ptr1[1] = ptr1[2] = true;
361  ptr2[0] = ptr2[1] = ptr2[2] = true;
362  ptr3[0] = ptr3[1] = ptr3[2] = true;
363 
364  // OK: accept it
365  featureList.push_back_fast(feat.pt.x, feat.pt.y); // (x,y)
366  // featureList.mark_kdtree_as_outdated();
367 
368  // Fill out the rest of data:
369  TSimpleFeature& newFeat = featureList.back();
370 
371  newFeat.ID = ++max_feat_ID_at_input;
372  newFeat.response = feat.response;
373  newFeat.octave = 0;
374  /** Inactive: right after detection, and before being tried to track */
375  newFeat.track_status = status_IDLE;
376  }
377 #else
378  MRPT_UNUSED_PARAM(patchSize);
379  MRPT_UNUSED_PARAM(cur_gray);
380  // Version with KD-tree
382  featureList.getVector());
383 
384  for (size_t i = 0; i < nNewToCheck && featureList.size() < maxNumFeatures;
385  i++)
386  {
387  const TSimpleFeature& feat = new_feats[sorted_indices[i]];
388  if (feat.response < minimum_KLT_response_to_add) break; // continue;
389 
390  // Check the min-distance:
391  double min_dist_sqr = std::numeric_limits<double>::max();
392 
393  if (!featureList.empty())
394  {
395  // m_timlog.enter("[CGenericFeatureTracker] add new
396  // features.kdtree");
397  min_dist_sqr =
398  kdtree.kdTreeClosestPoint2DsqrError(feat.pt.x, feat.pt.y);
399  // m_timlog.leave("[CGenericFeatureTracker] add new
400  // features.kdtree");
401  }
402 
403  if (min_dist_sqr > threshold_sqr_dist_to_add_new)
404  {
405  // OK: accept it
406  featureList.push_back_fast(feat.pt.x, feat.pt.y); // (x,y)
407  kdtree.mark_as_outdated();
408 
409  // Fill out the rest of data:
410  typename FEAT_LIST::feature_t& newFeat = featureList.back();
411 
412  newFeat.ID = ++max_feat_ID_at_input;
413  newFeat.response = feat.response;
414  newFeat.octave = 0;
415  /** Inactive: right after detection, and before being tried to track
416  */
417  newFeat.track_status = status_IDLE;
418  }
419  }
420 
421 #endif
422 } // end of trackFeatures_addNewFeats<>
423 
424 template <>
426  TSimpleFeatureList& featureList, const TSimpleFeatureList& new_feats,
427  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
428  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
429  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
430  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
431 {
432  trackFeatures_addNewFeats_simple_list<TSimpleFeatureList>(
433  featureList, new_feats, sorted_indices, nNewToCheck, maxNumFeatures,
434  minimum_KLT_response_to_add, threshold_sqr_dist_to_add_new, patchSize,
435  cur_gray, max_feat_ID_at_input);
436 }
437 template <>
439  TSimpleFeaturefList& featureList, const TSimpleFeatureList& new_feats,
440  const std::vector<size_t>& sorted_indices, const size_t nNewToCheck,
441  const size_t maxNumFeatures, const float minimum_KLT_response_to_add,
442  const double threshold_sqr_dist_to_add_new, const size_t patchSize,
443  const CImage& cur_gray, TFeatureID& max_feat_ID_at_input)
444 {
445  trackFeatures_addNewFeats_simple_list<TSimpleFeaturefList>(
446  featureList, new_feats, sorted_indices, nNewToCheck, maxNumFeatures,
447  minimum_KLT_response_to_add, threshold_sqr_dist_to_add_new, patchSize,
448  cur_gray, max_feat_ID_at_input);
449 }
450 
451 // Return the number of removed features
452 template <typename FEATLIST>
453 inline size_t trackFeatures_deleteOOB(
454  FEATLIST& trackedFeats, const size_t img_width, const size_t img_height,
455  const int MIN_DIST_MARGIN_TO_STOP_TRACKING);
456 
457 template <typename FEATLIST>
459  FEATLIST& trackedFeats, const size_t img_width, const size_t img_height,
460  const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
461 {
462  if (trackedFeats.empty()) return 0;
463 
464  std::vector<size_t> survival_idxs;
465  const size_t N = trackedFeats.size();
466 
467  // 1st: Build list of survival indexes:
468  survival_idxs.reserve(N);
469  for (size_t i = 0; i < N; i++)
470  {
471  const typename FEATLIST::feature_t& ft = trackedFeats[i];
472  const TFeatureTrackStatus status = ft.track_status;
473  bool eras = (status_TRACKED != status && status_IDLE != status);
474  if (!eras)
475  {
476  // Also, check if it's too close to the image border:
477  const int x = ft.pt.x;
478  const int y = ft.pt.y;
479  if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
480  y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
481  x > static_cast<int>(
482  img_width - MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
483  y > static_cast<int>(
484  img_height - MIN_DIST_MARGIN_TO_STOP_TRACKING))
485  {
486  eras = true;
487  }
488  }
489  if (!eras) survival_idxs.push_back(i);
490  }
491 
492  // 2nd: Build updated list:
493  const size_t N2 = survival_idxs.size();
494  const size_t n_removed = N - N2;
495  for (size_t i = 0; i < N2; i++)
496  {
497  if (survival_idxs[i] != i)
498  trackedFeats[i] = trackedFeats[survival_idxs[i]];
499  }
500  trackedFeats.resize(N2);
501  return n_removed;
502 } // end of trackFeatures_deleteOOB
503 
504 template <>
506  TSimpleFeatureList& trackedFeats, const size_t img_width,
507  const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
508 {
509  return trackFeatures_deleteOOB_impl_simple_feat<TSimpleFeatureList>(
510  trackedFeats, img_width, img_height, MIN_DIST_MARGIN_TO_STOP_TRACKING);
511 }
512 template <>
514  TSimpleFeaturefList& trackedFeats, const size_t img_width,
515  const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
516 {
517  return trackFeatures_deleteOOB_impl_simple_feat<TSimpleFeaturefList>(
518  trackedFeats, img_width, img_height, MIN_DIST_MARGIN_TO_STOP_TRACKING);
519 }
520 
521 template <>
523  CFeatureList& trackedFeats, const size_t img_width, const size_t img_height,
524  const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
525 {
526  CFeatureList::iterator itFeat = trackedFeats.begin();
527  size_t n_removed = 0;
528  while (itFeat != trackedFeats.end())
529  {
530  const TFeatureTrackStatus status = (*itFeat)->track_status;
531  bool eras = (status_TRACKED != status && status_IDLE != status);
532  if (!eras)
533  {
534  // Also, check if it's too close to the image border:
535  const float x = (*itFeat)->x;
536  const float y = (*itFeat)->y;
537  if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
538  y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
539  x > (img_width - MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
540  y > (img_height - MIN_DIST_MARGIN_TO_STOP_TRACKING))
541  {
542  eras = true;
543  }
544  }
545  if (eras) // Erase or keep?
546  {
547  itFeat = trackedFeats.erase(itFeat);
548  n_removed++;
549  }
550  else
551  ++itFeat;
552  }
553  return n_removed;
554 } // end of trackFeatures_deleteOOB
555 }
556 }
557 } // end NS's
558 // ---------------------------- end of internal helper templates
559 // -------------------------------
560 
562  const CImage& old_img, const CImage& new_img,
563  TSimpleFeaturefList& inout_featureList)
564 {
565  MRPT_UNUSED_PARAM(old_img);
566  MRPT_UNUSED_PARAM(new_img);
567  MRPT_UNUSED_PARAM(inout_featureList);
568  THROW_EXCEPTION("Method not implemented by derived class!");
569 }
570 
571 /** Perform feature tracking from "old_img" to "new_img", with a (possibly
572  *empty) list of previously tracked features "featureList".
573  * This is a list of parameters (in "extraParams") accepted by ALL
574  *implementations of feature tracker (see each derived class for more specific
575  *parameters).
576  * - "add_new_features" (Default=0). If set to "1", new features will be
577  *also
578  *added to the existing ones in areas of the image poor of features.
579  * This method actually first call the pure virtual "trackFeatures_impl"
580  *method, then implements the optional detection of new features if
581  *"add_new_features"!=0.
582  */
583 template <typename FEATLIST>
585  const CImage& old_img, const CImage& new_img, FEATLIST& featureList)
586 {
587  m_timlog.enter(
588  "[CGenericFeatureTracker::trackFeatures] Complete iteration");
589 
590  const size_t img_width = new_img.getWidth();
591  const size_t img_height = new_img.getHeight();
592 
593  // Take the maximum ID of "old" features so new feats (if
594  // "add_new_features==true") will be id+1, id+2, ...
595  TFeatureID max_feat_ID_at_input = 0;
596  if (!featureList.empty()) max_feat_ID_at_input = featureList.getMaxID();
597 
598  // Grayscale images
599  // =========================================
600  m_timlog.enter("[CGenericFeatureTracker] Convert grayscale");
601 
602  const CImage prev_gray(old_img, FAST_REF_OR_CONVERT_TO_GRAY);
603  const CImage cur_gray(new_img, FAST_REF_OR_CONVERT_TO_GRAY);
604 
605  m_timlog.leave("[CGenericFeatureTracker] Convert grayscale");
606 
607  // =================================
608  // (1st STEP) Do the actual tracking
609  // =================================
610  m_newly_detected_feats.clear();
611 
612  m_timlog.enter("[CGenericFeatureTracker] trackFeatures_impl");
613 
614  trackFeatures_impl(prev_gray, cur_gray, featureList);
615 
616  m_timlog.leave("[CGenericFeatureTracker] trackFeatures_impl");
617 
618  // ========================================================
619  // (2nd STEP) For successfully followed features, check their KLT response??
620  // ========================================================
621  const int check_KLT_response_every =
622  extra_params.getWithDefaultVal("check_KLT_response_every", 0);
623  const float minimum_KLT_response =
624  extra_params.getWithDefaultVal("minimum_KLT_response", 5);
625  const unsigned int KLT_response_half_win =
626  extra_params.getWithDefaultVal("KLT_response_half_win", 4);
627 
628  if (check_KLT_response_every > 0 &&
629  ++m_check_KLT_counter >= size_t(check_KLT_response_every))
630  {
631  m_timlog.enter("[CGenericFeatureTracker] check KLT responses");
632  m_check_KLT_counter = 0;
633 
634  const unsigned int max_x = img_width - KLT_response_half_win;
635  const unsigned int max_y = img_height - KLT_response_half_win;
636 
638  featureList, cur_gray, minimum_KLT_response, KLT_response_half_win,
639  max_x, max_y);
640 
641  m_timlog.leave("[CGenericFeatureTracker] check KLT responses");
642 
643  } // end check_KLT_response_every
644 
645  // ============================================================
646  // (3rd STEP) Remove Out-of-bounds or badly tracked features
647  // or those marked as "bad" by their low KLT response
648  // ============================================================
649  const bool remove_lost_features =
650  extra_params.getWithDefaultVal("remove_lost_features", 0) != 0;
651 
652  if (remove_lost_features)
653  {
654  m_timlog.enter("[CGenericFeatureTracker] removal of OOB");
655 
656  static const int MIN_DIST_MARGIN_TO_STOP_TRACKING = 10;
657 
658  const size_t nRemoved = detail::trackFeatures_deleteOOB(
659  featureList, img_width, img_height,
660  MIN_DIST_MARGIN_TO_STOP_TRACKING);
661 
662  m_timlog.leave("[CGenericFeatureTracker] removal of OOB");
663 
664  last_execution_extra_info.num_deleted_feats = nRemoved;
665  }
666  else
667  {
668  last_execution_extra_info.num_deleted_feats = 0;
669  }
670 
671  // ========================================================
672  // (4th STEP) For successfully followed features, update its patch:
673  // ========================================================
674  const int update_patches_every =
675  extra_params.getWithDefaultVal("update_patches_every", 0);
676 
677  if (update_patches_every > 0 &&
678  ++m_update_patches_counter >= size_t(update_patches_every))
679  {
680  m_timlog.enter("[CGenericFeatureTracker] update patches");
681  m_update_patches_counter = 0;
682 
683  // Update the patch for each valid feature:
684  detail::trackFeatures_updatePatch(featureList, cur_gray);
685 
686  m_timlog.leave("[CGenericFeatureTracker] update patches");
687  } // end if update_patches_every
688 
689  // ========================================================
690  // (5th STEP) Do detection of new features??
691  // ========================================================
692  const bool add_new_features =
693  extra_params.getWithDefaultVal("add_new_features", 0) != 0;
694  const double threshold_dist_to_add_new =
695  extra_params.getWithDefaultVal("add_new_feat_min_separation", 15);
696 
697  // Additional operation: if "add_new_features==true", find new features and
698  // add them in
699  // areas spare of valid features:
700  if (add_new_features)
701  {
702  m_timlog.enter("[CGenericFeatureTracker] add new features");
703 
704  // Look for new features and save in "m_newly_detected_feats", if
705  // they're not already computed:
706  if (m_newly_detected_feats.empty())
707  {
708  // Do the detection
710  cur_gray, m_newly_detected_feats, m_detector_adaptive_thres);
711  }
712 
713  const size_t N = m_newly_detected_feats.size();
714 
715  last_execution_extra_info.raw_FAST_feats_detected =
716  N; // Extra out info.
717 
718  // Update the adaptive threshold.
719  const size_t desired_num_features = extra_params.getWithDefaultVal(
720  "desired_num_features_adapt",
721  size_t((img_width * img_height) >> 9));
722  updateAdaptiveNewFeatsThreshold(N, desired_num_features);
723 
724  // Use KLT response instead of the OpenCV's original "response" field:
725  {
726  const unsigned int max_x = img_width - KLT_response_half_win;
727  const unsigned int max_y = img_height - KLT_response_half_win;
728  for (size_t i = 0; i < N; i++)
729  {
730  const unsigned int x = m_newly_detected_feats[i].pt.x;
731  const unsigned int y = m_newly_detected_feats[i].pt.y;
732  if (x > KLT_response_half_win && y > KLT_response_half_win &&
733  x < max_x && y < max_y)
734  m_newly_detected_feats[i].response =
735  cur_gray.KLT_response(x, y, KLT_response_half_win);
736  else
737  m_newly_detected_feats[i].response = 0; // Out of bounds
738  }
739  }
740 
741  // Sort them by "response": It's ~100 times faster to sort a list of
742  // indices "sorted_indices" than sorting directly the actual list
743  // of features "m_newly_detected_feats"
744  std::vector<size_t> sorted_indices(N);
745  for (size_t i = 0; i < N; i++) sorted_indices[i] = i;
746 
747  std::sort(
748  sorted_indices.begin(), sorted_indices.end(),
749  KeypointResponseSorter<TSimpleFeatureList>(m_newly_detected_feats));
750 
751  // For each new good feature, add it to the list of tracked ones only if
752  // it's pretty
753  // isolated:
754 
755  const size_t nNewToCheck = std::min(size_t(1500), N);
756  const double threshold_sqr_dist_to_add_new =
757  square(threshold_dist_to_add_new);
758  const size_t maxNumFeatures =
759  extra_params.getWithDefaultVal("add_new_feat_max_features", 100);
760  const size_t patchSize =
761  extra_params.getWithDefaultVal("add_new_feat_patch_size", 11);
762 
763  const float minimum_KLT_response_to_add =
764  extra_params.getWithDefaultVal("minimum_KLT_response_to_add", 10);
765 
766  // Do it:
768  featureList, m_newly_detected_feats, sorted_indices, nNewToCheck,
769  maxNumFeatures, minimum_KLT_response_to_add,
770  threshold_sqr_dist_to_add_new, patchSize, cur_gray,
771  max_feat_ID_at_input);
772 
773  m_timlog.leave("[CGenericFeatureTracker] add new features");
774  }
775 
776  m_timlog.leave(
777  "[CGenericFeatureTracker::trackFeatures] Complete iteration");
778 
779 } // end of CGenericFeatureTracker::trackFeatures
780 
782  const CImage& old_img, const CImage& new_img, CFeatureList& featureList)
783 {
784  internal_trackFeatures<CFeatureList>(old_img, new_img, featureList);
785 }
786 
788  const CImage& old_img, const CImage& new_img,
789  TSimpleFeatureList& featureList)
790 {
791  internal_trackFeatures<TSimpleFeatureList>(old_img, new_img, featureList);
792 }
793 
795  const CImage& old_img, const CImage& new_img,
796  TSimpleFeaturefList& featureList)
797 {
798  internal_trackFeatures<TSimpleFeaturefList>(old_img, new_img, featureList);
799 }
800 
802  const size_t nNewlyDetectedFeats, const size_t desired_num_features)
803 {
804  const size_t hysteresis_min_num_feats = desired_num_features * 0.9;
805  const size_t hysteresis_max_num_feats = desired_num_features * 1.1;
806 
807  if (nNewlyDetectedFeats < hysteresis_min_num_feats)
808  m_detector_adaptive_thres = std::max(
809  2.0, std::min(
810  m_detector_adaptive_thres - 1.0,
811  m_detector_adaptive_thres * 0.8));
812  else if (nNewlyDetectedFeats > hysteresis_max_num_feats)
813  m_detector_adaptive_thres = std::max(
814  m_detector_adaptive_thres + 1.0, m_detector_adaptive_thres * 1.2);
815 }
816 
817 /*------------------------------------------------------------
818  checkTrackedFeatures
819 -------------------------------------------------------------*/
821  CFeatureList& leftList, CFeatureList& rightList,
822  vision::TMatchingOptions options)
823 {
824  ASSERT_(leftList.size() == rightList.size());
825 
826  // std::cout << std::endl << "Tracked features checking ..." << std::endl;
827 
828  CFeatureList::iterator itLeft, itRight;
829  size_t u, v;
830  double res;
831 
832  for (itLeft = leftList.begin(), itRight = rightList.begin();
833  itLeft != leftList.end();)
834  {
835  bool delFeat = false;
836  if ((*itLeft)->x < 0 || (*itLeft)->y < 0 || // Out of bounds
837  (*itRight)->x < 0 || (*itRight)->y < 0 || // Out of bounds
838  fabs((*itLeft)->y - (*itRight)->y) >
839  options
840  .epipolar_TH) // Not fulfillment of the epipolar constraint
841  {
842  // Show reason
843  std::cout << "Bad tracked match:";
844  if ((*itLeft)->x < 0 || (*itLeft)->y < 0 || (*itRight)->x < 0 ||
845  (*itRight)->y < 0)
846  std::cout << " Out of bounds: (" << (*itLeft)->x << ","
847  << (*itLeft)->y << " & (" << (*itRight)->x << ","
848  << (*itRight)->y << ")" << std::endl;
849 
850  if (fabs((*itLeft)->y - (*itRight)->y) > options.epipolar_TH)
851  std::cout << " Bad row checking: "
852  << fabs((*itLeft)->y - (*itRight)->y) << std::endl;
853 
854  delFeat = true;
855  }
856  else
857  {
858  // Compute cross correlation:
860  (*itLeft)->patch, (*itRight)->patch, u, v, res);
861 
862  if (res < options.minCC_TH)
863  {
864  std::cout << "Bad tracked match (correlation failed):"
865  << " CC Value: " << res << std::endl;
866  delFeat = true;
867  }
868  } // end if
869 
870  if (delFeat) // Erase the pair of features
871  {
872  itLeft = leftList.erase(itLeft);
873  itRight = rightList.erase(itRight);
874  }
875  else
876  {
877  itLeft++;
878  itRight++;
879  }
880  } // end for
881 } // end checkTrackedFeatures
882 
883 /*-------------------------------------------------------------
884  filterBadCorrsByDistance
885 -------------------------------------------------------------*/
887  TMatchingPairList& feat_list, unsigned int numberOfSigmas)
888 {
889  ASSERT_(numberOfSigmas > 0);
890  // MRPT_UNUSED_PARAM( numberOfSigmas );
891  MRPT_START
892 
894  CMatrix dist;
895  double v_mean, v_std;
896  unsigned int count = 0;
897 
898  dist.setSize(feat_list.size(), 1);
899  // v_mean.resize(1);
900  // v_std.resize(1);
901 
902  // Compute mean and standard deviation of the distance
903  for (itPair = feat_list.begin(); itPair != feat_list.end();
904  itPair++, count++)
905  {
906  // cout << "(" << itPair->other_x << "," << itPair->other_y << "," <<
907  // itPair->this_z << ")" << "- (" << itPair->this_x << "," <<
908  // itPair->this_y << "," << itPair->other_z << "): ";
909  // cout << sqrt( square( itPair->other_x - itPair->this_x ) + square(
910  // itPair->other_y - itPair->this_y ) + square( itPair->other_z -
911  // itPair->this_z ) ) << endl;
912  dist(count, 0) = sqrt(
913  square(itPair->other_x - itPair->this_x) +
914  square(itPair->other_y - itPair->this_y) +
915  square(itPair->other_z - itPair->this_z));
916  }
917 
918  dist.meanAndStdAll(v_mean, v_std);
919 
920  cout << endl
921  << "*****************************************************" << endl;
922  cout << "Mean: " << v_mean << " - STD: " << v_std << endl;
923  cout << endl
924  << "*****************************************************" << endl;
925 
926  // Filter out bad points
927  unsigned int idx = 0;
928  // for( int idx = (int)feat_list.size()-1; idx >= 0; idx-- )
929  for (itPair = feat_list.begin(); itPair != feat_list.end(); idx++)
930  {
931  // if( dist( idx, 0 ) > 1.2 )
932  if (fabs(dist(idx, 0) - v_mean) > v_std * numberOfSigmas)
933  {
934  cout << "Outlier deleted: " << dist(idx, 0) << " vs "
935  << v_std * numberOfSigmas << endl;
936  itPair = feat_list.erase(itPair);
937  }
938  else
939  itPair++;
940  }
941 
942  MRPT_END
943 } // end filterBadCorrsByDistance
void trackFeatures_addNewFeats(FEATLIST &featureList, const TSimpleFeatureList &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)
const T & get_unsafe(size_t row, size_t col) const
Fast but unsafe method to read a value from the matrix.
Scalar * iterator
Definition: eigen_plugins.h:26
GLuint GLuint GLsizei count
Definition: glext.h:3528
Helper class: KD-tree search class for vector<KeyPoint>: Call mark_as_outdated() to force rebuilding ...
#define MRPT_START
Definition: exceptions.h:262
uint64_t TFeatureID
Definition of a feature ID.
#define min(a, b)
Declares a matrix of booleans (non serializable).
void trackFeatures_updatePatch< TSimpleFeaturefList >(TSimpleFeaturefList &featureList, const CImage &cur_gray)
Definition: tracking.cpp:179
TInternalFeatList::iterator iterator
Definition: CFeature.h:365
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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:458
TFeatureTrackStatus track_status
featKLT, featHarris, featSURF, featBeacon
Definition: CFeature.h:68
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:584
size_t size() const
Definition: CFeature.h:386
GLintptr offset
Definition: glext.h:3925
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
STL namespace.
float epipolar_TH
Epipolar constraint (rows of pixels)
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
float y
Coordinates in the image.
Definition: CFeature.h:61
TFeatureTrackStatus track_status
Status of the feature tracking process.
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:76
virtual void trackFeatures_impl(const mrpt::img::CImage &old_img, const mrpt::img::CImage &new_img, TSimpleFeaturefList &inout_featureList)
The tracking method implementation, to be implemented in children classes.
Definition: tracking.cpp:561
FAST feature detector, OpenCV&#39;s implementation ("Faster and better: A machine learning approac...
Inactive (right after detection, and before being tried to track)
A helper struct to sort keypoints by their response: It can be used with these types: ...
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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:864
void trackFeatures_addNewFeats< TSimpleFeaturefList >(TSimpleFeaturefList &featureList, const TSimpleFeatureList &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:438
void trackFeatures_updatePatch< TSimpleFeatureList >(TSimpleFeatureList &featureList, const CImage &cur_gray)
Definition: tracking.cpp:171
A list of TMatchingPair.
Definition: TMatchingPair.h:83
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:801
A pair (x,y) of pixel coordinates (integer resolution).
Definition: TPixelCoord.h:39
void trackFeatures_addNewFeats< TSimpleFeatureList >(TSimpleFeatureList &featureList, const TSimpleFeatureList &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:425
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:20
A generic 2D feature from an image, extracted with CFeatureExtraction Each feature may have one or mo...
Definition: CFeature.h:53
void trackFeatures(const mrpt::img::CImage &old_img, const mrpt::img::CImage &new_img, TSimpleFeatureList &inout_featureList)
Perform feature tracking from "old_img" to "new_img", with a (possibly empty) list of previously trac...
Definition: tracking.cpp:787
uint8_t octave
The image octave the image was found in: 0=original image, 1=1/2 image, 2=1/4 image, etc.
iterator erase(const iterator &it)
Definition: CFeature.h:379
static void detectFeatures_SSE2_FASTER12(const mrpt::img::CImage &img, TSimpleFeatureList &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 list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:304
Unable to track this feature (mismatch is too high for the given tracking window: lack of texture...
void checkTrackedFeatures(CFeatureList &leftList, CFeatureList &rightList, vision::TMatchingOptions options)
Search for correspondences which are not in the same row and deletes them ...
Definition: tracking.cpp:820
float response
process (old name: KLT_status)
Definition: CFeature.h:70
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void trackFeatures_addNewFeats< CFeatureList >(CFeatureList &featureList, const TSimpleFeatureList &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:196
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)
void trackFeatures_addNewFeats_simple_list(FEAT_LIST &featureList, const TSimpleFeatureList &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:254
TFeatureID ID
ID of the feature.
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:2593
void filterBadCorrsByDistance(mrpt::tfest::TMatchingPairList &list, unsigned int numberOfSigmas)
Filter bad correspondences by distance ...
Definition: tracking.cpp:886
size_t trackFeatures_deleteOOB(FEATLIST &trackedFeats, const size_t img_width, const size_t img_height, const int MIN_DIST_MARGIN_TO_STOP_TRACKING)
pixel_coords_t pt
Coordinates in the image.
float response
A measure of the "goodness" of the feature (typically, the KLT_response value)
#define MRPT_END
Definition: exceptions.h:266
A structure containing options for the matching.
A simple structure for representing one image feature (without descriptor nor patch) - This is the te...
void trackFeatures_updatePatch< CFeatureList >(CFeatureList &featureList, const CImage &cur_gray)
Definition: tracking.cpp:142
_u8 status
Definition: rplidar_cmd.h:19
GLenum GLint GLint y
Definition: glext.h:3538
float minCC_TH
Minimum Value of the Cross Correlation.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
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:40
void trackFeatures_checkResponses< TSimpleFeatureList >(TSimpleFeatureList &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:117
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
void openCV_cross_correlation(const mrpt::img::CImage &img, const mrpt::img::CImage &patch_img, size_t &x_max, size_t &y_max, double &max_val, int x_search_ini=-1, int y_search_ini=-1, int x_search_size=-1, int y_search_size=-1)
Computes the correlation between this image and another one, encapsulating the openCV function cvMatc...
Feature fell Out Of Bounds (out of the image limits, too close to image borders)
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:24
mrpt::img::CImage patch
A patch of the image surrounding the feature.
Definition: CFeature.h:63
Functions for estimating the optimal transformation between two frames of references given measuremen...
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:522
void trackFeatures_checkResponses< TSimpleFeaturefList >(TSimpleFeaturefList &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:127
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
void fillAll(const T &val)
#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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019