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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019