Main MRPT website > C++ reference for MRPT 1.5.7
multiDesc_utils.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 #include "vision-precomp.h" // Precompiled headers
10 
12 #include <mrpt/vision/utils.h>
13 #include <mrpt/vision/pinhole.h>
15 #include <mrpt/vision/CFeature.h>
16 
17 #include <mrpt/poses/CPoint3D.h>
18 //#include <mrpt/maps/CLandmarksMap.h>
22 #include <mrpt/system/filesystem.h>
23 #include <mrpt/system/os.h>
24 #include <mrpt/utils/CTicTac.h>
25 #include <mrpt/utils/CTimeLogger.h>
26 #include <mrpt/math/utils.h>
27 #include <mrpt/math/ops_vectors.h>
29 #include <mrpt/math/geometry.h>
30 
31 // Universal include for all versions of OpenCV
32 #include <mrpt/otherlibs/do_opencv_includes.h>
33 
34 using namespace mrpt;
35 using namespace mrpt::vision;
36 using namespace mrpt::utils;
37 //using namespace mrpt::maps;
38 using namespace mrpt::math;
39 using namespace mrpt::system;
40 using namespace std;
41 
42 #ifdef MRPT_OS_WINDOWS
43  #include <process.h>
44  #include <windows.h> // TODO: This is temporary!!!
45 #endif
46 
47 const int FEAT_FREE = -1;
48 //const int NOT_ASIG = 0; // JL: Not used anymore?? (FAMD)
49 //const int ASG_FEAT = 1;
50 //const int AMB_FEAT = 2;
51 
52 /*-------------------------------------------------------------
53  insertHashCoeffs
54 -------------------------------------------------------------*/
56  const CFeaturePtr & feat,
57  TQuantizationTable & qTable )
58 {
60  for( int k = 0; k < int(feat->multiScales.size()); ++k )
61  {
62  for( int m = 0; m < int(feat->multiOrientations[k].size()); ++m )
63  {
64  int key1 = feat->multiHashCoeffs[k][m][0];
65  int key2 = feat->multiHashCoeffs[k][m][1];
66  int key3 = feat->multiHashCoeffs[k][m][2];
67 
68  bool found = false;
69  if( qTable.find(key1) != qTable.end() &&
70  qTable[key1].find(key2) != qTable[key1].end() &&
71  qTable[key1][key2].find(key3) != qTable[key1][key2].end() )
72  {
73  // The entry for these keys already exists
74  // Check if the qTable already contain this ID and multiScale!
75  for( int n = 0; n < int(qTable[key1][key2][key3].size()); ++n )
76  {
77  TFeatureID thisID = qTable[key1][key2][key3][n].first;
78  double thisScale = qTable[key1][key2][key3][n].second;
79  if( thisID == feat->ID && thisScale == feat->multiScales[k] )
80  found = true;
81  // cout << "Inserting in: " << key1 << "," << key2 << "," << key3 << endl;
82  } // end-for
83  } // end-if
84  if( !found ) // Insert the new coefficients if they haven't been inserted before
85  qTable[key1][key2][key3].push_back( make_pair(feat->ID,feat->multiScales[k]) );
86  } // end for multiOrientations
87  } // end for multiScales
88  MRPT_END
89 } // end-insertHashCoeffs
90 
91 /*-------------------------------------------------------------
92  saveQTableToFile
93 -------------------------------------------------------------*/
94 void vision::saveQTableToFile( const TQuantizationTable &qTable, const string &filename )
95 {
96  FILE *f = mrpt::system::os::fopen( filename, "wt" );
97 
98  typedef map<int,map<int,map<int,deque<pair<TFeatureID, double> > > > > TQuantizationTable;
99 
101  map<int,map<int,deque<pair<TFeatureID, double> > > >::const_iterator it2;
102  map<int,deque<pair<TFeatureID, double> > >::const_iterator it3;
103 
104  for( it1 = qTable.begin(); it1 != qTable.end(); ++it1 )
105  for( it2 = it1->second.begin(); it2 != it1->second.end(); ++it2 )
106  for( it3 = it2->second.begin(); it3 != it2->second.end(); ++it3 )
107  {
108  mrpt::system::os::fprintf( f, "%d\t%d\t%d\t", it1->first, it2->first, it3->first );
109  for( int k = 0; k < int(it3->second.size()); ++k )
110  mrpt::system::os::fprintf( f, "%lu\t%.2f\t",static_cast<long unsigned int>(it3->second[k].first), it3->second[k].second );
111  mrpt::system::os::fprintf( f, "\n" );
112  } // end-for
114 } // end-saveQTableToFile
115 
116 /*-------------------------------------------------------------
117  relocalizeMultiDesc
118 -------------------------------------------------------------*/
119 // Return the number of features which have been properly re-matched
121  const CImage & image,
122  CFeatureList & baseList,
123  CFeatureList & currentList,
124  TQuantizationTable & qTable,
125  const TMultiResDescOptions & desc_opts,
126  const TMultiResDescMatchOptions & match_opts )
127 {
128  MRPT_START
129  const bool PARAR = false;
130 
131  int TH = 30; // The threshold for searching in the quantization table
133  output.firstListCorrespondences.resize( baseList.size(), -1 );
134  output.firstListDistance.resize( baseList.size() );
135  output.firstListFoundScales.resize( baseList.size() );
136  output.secondListCorrespondences.resize( currentList.size(), -1 );
137  output.nMatches = 0;
138 
140  map<int,map<int,deque<pair<TFeatureID, double> > > >::iterator it2;
141  map<int,deque<pair<TFeatureID, double> > >::iterator it3;
142 
143  vector<TFeatureID> vID;
144 
145  int hpSize = desc_opts.basePSize/2; // Half of the patch size
146  int w = image.getWidth();
147  int h = image.getHeight();
148 
149  map<TFeatureID, vector<double> > featsToCompareMap;
150 
151  int curCounter = 0;
153  for( it = currentList.begin(); it != currentList.end(); ++it, ++curCounter )
154  {
155  if( PARAR && (*it)->ID == 193 )
156  {
157  for( int i = 0; i < (int)(*it)->multiScales.size(); ++i )
158  cout << (*it)->multiScales[i] << endl;
159  }
160  ASSERT_( (*it)->multiHashCoeffs.size() == (*it)->multiScales.size() );
161 
162 // (*it)->dumpToConsole();
163 // (*it)->patch.saveToFile(format("imgs/patch_ini_%d.jpg", int((*it)->ID)));
164 // cout << "done" << endl;
165 
166  featsToCompareMap.clear();
167  if( !(*it)->descriptors.hasDescriptorMultiSIFT() )
168  {
169  cout << "[relocalizeMultiDesc] Feature " << (*it)->ID << " in currentList hasn't got any descriptor." << endl;
170 
171  // Compute the new descriptors at scale 1.0
172  TMultiResDescOptions nopts = desc_opts;
173  nopts.scales.resize(1);
174  nopts.scales[0] = 1.0;
175  if( (*it)->x+hpSize > (w-1) || (*it)->y+hpSize > (h-1) ||
176  (*it)->x-hpSize < 0 || (*it)->y-hpSize < 0 )
177  {
178  cout << "[relocalizeMultiDesc] WARNING: Feature too close to the border. MultiDescriptor computation skipped." << endl;
179  continue;
180  }
182  } // end-if
183 
184  for( int k = 0; k < int((*it)->multiOrientations[0].size()); ++k )
185  {
186  /* Where to look for within the quantization table?*/
187  /* this is done for each feature and for each main orientation*/
188  int c1mn = (*it)->multiHashCoeffs[0][k][0] - TH;
189  int c1mx = (*it)->multiHashCoeffs[0][k][0] + TH;
190 
191  int c2mn = (*it)->multiHashCoeffs[0][k][1] - TH;
192  int c2mx = (*it)->multiHashCoeffs[0][k][1] + TH;
193 
194  int c3mn = (*it)->multiHashCoeffs[0][k][2] - TH;
195  int c3mx = (*it)->multiHashCoeffs[0][k][2] + TH;
196 
197  for( int m1 = c1mn; m1 < c1mx; ++m1 )
198  {
199  it1 = qTable.find(m1);
200  if( it1 != qTable.end() )
201  {
202  for( int m2 = c2mn; m2 < c2mx; ++m2 )
203  {
204  it2 = it1->second.find(m2);
205  if( it2 != it1->second.end() )
206  {
207  for( int m3 = c3mn; m3 < c3mx; ++m3 )
208  {
209  it3 = it2->second.find(m3);
210  if( it3 != it2->second.end() )
211  {
212  for( int n = 0; n < int(it3->second.size()); ++n )
213  {
214  featsToCompareMap[qTable[m1][m2][m3][n].first].push_back( qTable[m1][m2][m3][n].second );
215  } // endfor n
216  } // endif it3
217  } // endfor m3
218  } // endif it2
219  } // endfor m2
220  } // endif it1
221  } //endfor m1
222 
223  // Erase duplicates
224  vID.resize( featsToCompareMap.size() ); // To store only the IDs
225  int counter = 0;
226  for( map< TFeatureID, vector<double> >::iterator nit = featsToCompareMap.begin(); nit != featsToCompareMap.end(); ++nit, ++counter )
227  {
228  // Remove duplicates
229  std::sort( nit->second.begin(), nit->second.end() );
230 
232  vit = std::unique( nit->second.begin(), nit->second.end() );
233 
234  nit->second.resize( vit - nit->second.begin() );
235 
236  // Add to the ID vector
237  vID[counter] = nit->first;
238  }
239 
240  // Find the scales where to look
241  // Use all orientations for each scale (we don't know a priori what's the change in orientation)
242  counter = 0;
243  // for each feature to compare
244  int minDist = 1e6;
245  int minBaseScl = 0;
246  int minBaseFeat = 0;
247  for( map< TFeatureID, vector<double> >::iterator nit = featsToCompareMap.begin(); nit != featsToCompareMap.end(); ++nit, ++counter )
248  {
249  int baseIdx;
250  CFeaturePtr baseFeat = baseList.getByID( nit->first, baseIdx );
251 
252 // cout << int((*it)->ID) << " (" << (*it)->x << "," << (*it)->y << ")";
253 // cout << " -------------------------------------------------------------" << endl;
254 
255  // for each scale within the base feature
256  for( int k1 = 0; k1 < int(baseFeat->multiScales.size()); ++k1 )
257  {
258  // for each scale from the qTable
259  for( int k2 = 0; k2 < int(nit->second.size()); ++k2 )
260  {
261  if( baseFeat->multiScales[k1] == nit->second[k2] ) // if this scale has been selected
262  {
263  // for each orientation of the feature 1
264  for( int k3 = 0; k3 < int(baseFeat->multiOrientations[k1].size()); ++k3 )
265  {
266  // for each orientation of the feature 2
267  for( int k4 = 0; k4 < int((*it)->multiOrientations[0].size()); ++k4 )
268  {
269  // check orientation
270  if( fabs(baseFeat->multiOrientations[k1][k3]-(*it)->multiOrientations[0][k4]) > DEG2RAD(10) )
271  continue;
272 // cout << "Orientation check passed" << endl;
273 // cout << "HASH: " << (*it)->multiHashCoeffs[0][k4] << endl;
274 
275  // Compute the distance
276  int dist = 0;
277  for( int d = 0; d < int(baseFeat->descriptors.multiSIFTDescriptors[k1][k3].size()); ++d )
278  dist += fabs( float(baseFeat->descriptors.multiSIFTDescriptors[k1][k3][d]) - float((*it)->descriptors.multiSIFTDescriptors[0][k4][d]) );
279 
280 // cout << nit->first << "[" << baseFeat->multiScales[k1] << "] - ori: " << baseFeat->multiOrientations[k1][k3] << " - HASH: " << baseFeat->multiHashCoeffs[k1][k3] << " - dist: " << dist << endl;
281  if( dist < minDist )
282  {
283  minDist = dist; // minimun distance of the features
284  minBaseFeat = baseIdx; // index of the base feature
285  minBaseScl = k1; // scale of the base feature
286  } // end-if
287  } // end-for
288  } // end-for
289  } // end-if
290  } // end-for-k2
291  } // end-for-k1
292 // if( baseFeat->ID == 32 && (*it)->ID == 9 )
293 // mrpt::system::pause();
294  } // end-for-nit
295  if( minDist < match_opts.matchingThreshold )
296  {
297  // We've found a match
298  // Store the index of the current feature
299  output.firstListCorrespondences[minBaseFeat] = curCounter;
300  output.firstListFoundScales[minBaseFeat] = minBaseScl;
301  output.firstListDistance[minBaseFeat] = minDist;
302  output.secondListCorrespondences[curCounter] = minBaseFeat;
303  output.nMatches++;
304  }
305  } // end-for orientations
306 // if( (*it)->ID == 9 )
307 // {
308 // baseList.getByID(32)->dumpToConsole();
309 // (*it)->dumpToConsole();
310 // mrpt::system::pause();
311 // }
312 
313  } // end-for-it
314 
315  return output;
316 
317  MRPT_END
318 } // end-relocalizeMultiDesc
319 
320 /*-------------------------------------------------------------
321  updateBaseList
322 -------------------------------------------------------------*/
323 // Updates the following parameters for each feature in Base List:
324 // Coordinates, the number of frames it has been seen in, the number of frames since the last time it was seen and
325 // the number of frames it has not been seen. This is done according to the information in vector "idx" and the
326 // positions of the features in current list.
328  CFeatureList & baseList,
329  const CFeatureList & currentList,
330  const vector<int> & idx )
331 {
332  MRPT_START
333  ASSERT_( idx.size() == baseList.size() );
334 
335  size_t sz = idx.size();
336 
337  CVectorDouble dp(sz), my(sz);
338  int counter = 0;
339  for( int k = 0; k < (int)sz; ++k )
340  {
341  if( idx[k] == FEAT_FREE )
342  {
343  baseList[k]->nTimesNotSeen++;
344  baseList[k]->nTimesLastSeen++;
345  }
346  else
347  {
348  baseList[k]->nTimesSeen++;
349  baseList[k]->nTimesLastSeen = 0;
350 
351  // Update position!
352  dp[k] = fabs(baseList[k]->depth-currentList[idx[k]]->depth);
353 // cout << "dp:" << dp[k] << endl;
354 
355  counter++;
356  } // end-else
357  } // end-for
358 
359  double m_dp, std_dp;
360  mrpt::math::meanAndStd( dp, m_dp, std_dp );
361  cout << "mean&std: " << m_dp << "," << std_dp << endl;
362 
363  for( int k = 0; k < (int)idx.size(); ++k )
364  {
365  if( idx[k] != FEAT_FREE )
366  {
367  // Update position!
368  if( dp[k] < (m_dp+std_dp) )
369  {
370  baseList[k]->x = currentList[idx[k]]->x;
371  baseList[k]->y = currentList[idx[k]]->y;
372  baseList[k]->p3D = currentList[idx[k]]->p3D;
373  }
374  } // end-if
375  } // end-for
376 
377  MRPT_END
378 } // updateBaseList
379 
380 /*-------------------------------------------------------------
381  checkScalesAndFindMore
382 -------------------------------------------------------------*/
383 // Checks the scales where the matched were found and find more descriptors if they are located at the boundaries of
384 // the scale range o a certain feature within base list.
385 // It also deletes the features in base list which are have not been seen for a long time and it wasn't seen enough.
387  CMatchedFeatureList & baseList,
388  const CFeatureList & currentList,
389  const CImage & currentImage,
390  const TMultiResMatchingOutput & output,
391  const TMultiResDescOptions & computeOpts,
392  const TMultiResDescMatchOptions & matchOpts )
393 {
395  int m = 0;
396 // cout << "Base: " << baseList.size() << " and output: " << output.firstListCorrespondences.size() << endl;
397  for( itBase = baseList.begin(); itBase!= baseList.end(); ++m )
398  {
399 
400 // cout << m << " was last seen " << itBase->->first->nTimesLastSeen << " frames ago and has been seen " << itBase->first->nTimesSeen << " times so far." << endl;
401  if( itBase->first->nTimesLastSeen > matchOpts.lastSeenThreshold &&
402  itBase->first->nTimesSeen < matchOpts.timesSeenThreshold )
403  {
404 // cnt++;
405 // cout << "Deleted feature #" << m << endl;
406  itBase = baseList.erase( itBase );
407  //++itBase;
408  }
409  else
410  {
411  // We've found a tracked match
412  // We have found the match in both frames! Check out the scales!
413 // if( !(m < (int)output.firstListCorrespondences.size() ))
414 // {
415 // cout << m << " vs " << (int)output.firstListCorrespondences.size() << endl;
416 // ASSERT_( m < (int)output.firstListCorrespondences.size() );
417 // }
418 
419  int tidx = output.firstListCorrespondences[m];
420 // ASSERT_( tidx < (int)currentList.size() );
421 
422  if( tidx != FEAT_FREE )
423  {
424  if( output.firstListFoundScales[m] == 0 )
425  {
426  cout << "Left feature " << m << " found in scale 0!" << endl;
427 // currentImage.saveToFile("imgs/current.jpg");
428 // cout << "px: "<< currentList[tidx]->x << ","<< currentList[tidx]->y << endl;
429  int res = computeMoreDescriptors( currentImage, currentList[tidx], itBase->first, true, computeOpts );
430  ASSERT_( itBase->first->descriptors.hasDescriptorMultiSIFT() );
431  if( res == 0) cout << "LF LOWSCALES Out of bounds!!" << endl;
432  //mrpt::system::pause();
433  }
434  else
435  {
436  size_t nscales = itBase->first->multiScales.size()-1;
437  //itBase->first->dumpToConsole();
438  if( output.firstListFoundScales[m] == (int)nscales )
439  {
440  cout << "Left feature " << m << " found in last scale!" << endl;
441  int res = computeMoreDescriptors( currentImage, currentList[tidx], itBase->first, false, computeOpts );
442  if( res == 0) cout << "LF HIGHSCALES Out of bounds!!" << endl;
443  // mrpt::system::pause();
444  }
445  }
446  } // end-if
447  ++itBase;
448  } // end-else
449  } // end-for
450 } // checkScalesAndFindMore
451 
452 /*-------------------------------------------------------------
453  computeGradient
454 -------------------------------------------------------------*/
455 // computeGradient.- Computes both the (approximated) gradient magnitude and orientation for a certain point within the image
456 // return: true = OK; false = if the keypoint is located at the image border (where the gradient cannot be computed).
457 // x = col, y = row
459  const CImage & image,
460  const unsigned int x,
461  const unsigned int y,
462  double & mag,
463  double & ori )
464 {
465  if( x > 0 && x < image.getWidth()-1 && y > 0 && y < image.getHeight()-1 )
466  {
467  float d1, d2;
468  //----------
469  //| 0|-1| 0|
470  //----------
471  //|-1| 0| 1|
472  //----------
473  //| 0| 1| 0|
474  //----------
475 
476  // According to Hess's implementation (Lowe does: d2 = image.getAsFloat(x,y+1) - px4 = image.getAsFloat(x,y-1); )
477  d1 = image.getAsFloat(x+1,y) - image.getAsFloat(x-1,y);
478  d2 = image.getAsFloat(x,y-1) - image.getAsFloat(x,y+1);
479 
480  mag = sqrt( d1*d1 + d2*d2 );
481  ori = atan2( d2, d1 ); // From -pi to pi
482  return true;
483  }
484  else
485  {
486  cout << "[computeGradient]: Out of bounds in " << x << "," << y << " with image: " << image.getWidth() << "x" << image.getHeight() << endl;
487  return false;
488  }
489 } // end-computeGradient
490 
491 /*-------------------------------------------------------------
492  computeMainOrientations
493 -------------------------------------------------------------*/
494 // Compute a set of main orientations (if there are more than one) of a certain keypoint within the image.
495 // return: true = OK; false = keypoint is too close the image margin (there is no space for the whole patch)
497  const unsigned int x,
498  const unsigned int y,
499  const unsigned int patchSize,
500  std::vector<double> & orientations,
501  const double & sigma )
502 {
503  MRPT_START
504 
505  // Pre operations
506  orientations.clear();
507 
508  // Local variables:
509  const unsigned int NBINS = 36;
510  const int hPatchSize = patchSize/2;
511 
512  vector<double> oris( NBINS, 0.0 );
513  int mx = (int)x, my = (int)y;
514 
515  // Check if there's no margin problems when computing the orientation
516  if( mx-(hPatchSize+1) < 0 || my-(hPatchSize+1) < 0 || mx+(hPatchSize+1) > (int)(image.getWidth()-1) || my+(hPatchSize+1) > (int)(image.getHeight()-1) )
517  return false; // Feature is too close to the image's border
518 
519  // For each pixel within the patch (patchSize should be 29x29):
520  // Compute local gradients (magnitude and orientation)
521  // The orientation histogram is weighted with a Gaussian with sigma = 7.5 (Cheklov's Thesis)
522 // cout << "IM: " << image.getWidth() << " and PS: " << patchSize << endl;
523  double exp_denom = 2.0 * sigma *sigma;
524  double bin_size = M_2PI/NBINS;
525  double mag, ori;
526  for( int ii = -hPatchSize; ii <= hPatchSize; ++ii )
527  for( int jj = -hPatchSize; jj <= hPatchSize; ++jj )
528  {
529  if( computeGradient( image, mx+ii, my+jj, mag, ori ) )
530  {
531  ori += M_PI; // ori: from 0 to 2*pi
532  double w = mag*exp( -( ii*ii + jj*jj ) / exp_denom );
533 
534  int bin = ((int)(floor(ori/bin_size))) % NBINS; // from 0 to 35
535  double bin_center = bin*bin_size;
536  double dif = ori-bin_center;
537  int nxbin;
538  if( dif > 0 )
539  nxbin = bin == NBINS-1 ? 0 : bin+1; // the other involved bin is the next one
540  else
541  nxbin = bin == 0 ? NBINS-1 : bin-1; // the other involved bin is the previous one
542 
543  double nbin_center = nxbin*bin_size;
544  double dif2 = ori-nbin_center;
545 
546  oris[bin] += w*fabs(dif2)/bin_size; // dif2 == 0 means that "ori" is equal to "bin_center"
547  oris[nxbin] += w*fabs(dif)/bin_size; // dif == 0 means that "ori" is equal to "nbin_center"
548  } // end-if
549  else
550  return false;
551  } // end
552  // Search for the maximum
553  double mxori = 0.0;
554  for( unsigned int k = 0; k < oris.size(); ++k )
555  {
556  if( oris[k] > mxori )
557  {
558  mxori = oris[k];
559  }
560  } // end-for
561 
562  // Compute the peaks of the histogram of orientations
563  double hist_mag_th = 0.8*mxori;
564  for( unsigned int k = 0; k < oris.size(); ++k )
565  {
566  double pv = k == 0 ? oris[oris.size()-1] : oris[k-1]; // Previous peak of the histogram
567  double nv = k == oris.size()-1 ? 0 : oris[k+1]; // Next peak of the histogram
568 
569  if( oris[k] > pv && oris[k] > nv && oris[k] > hist_mag_th ) // It this peak is maximum and is over 0.8 of the maximum peak
570  {
571  // Check this formulae:
572  // double A = (pv-nv)/(4*oris[k]+2*nv-2*pv-4*oris[k]);
573  // double peak = A/(1+2*A);
574 
575  // Interpolate the position of the peak
576  double int_bin = k + 0.5 * (pv-nv)/(pv-2.0*oris[k]+nv); // Hess formulae
577  int_bin = ( int_bin < 0 ) ? NBINS + int_bin : ( int_bin >= NBINS ) ? int_bin - NBINS : int_bin;
578  double int_ori = ( ( M_2PI * int_bin ) / NBINS ) - M_PI; // Back to -pi:pi
579  orientations.push_back( int_ori );
580 
581 // cout << "Ori found: " << int_ori << endl;
582  }
583  }
584  return true;
585  MRPT_END
586 } // end vision::computeMainOrientation
587 
588 /*-------------------------------------------------------------
589  interpolateHistEntry
590 -------------------------------------------------------------*/
592  vector<double> & oris,
593  const double & cbin,
594  const double & rbin,
595  const double & obin,
596  const double & mag,
597  const int d,
598  const int n )
599 {
600  // Insert the sample into the orientation histogram taking into account that there is an overlapping
601  // of half a bin between consecutive bins.
602  // [And the first one overlaps with the last one??? --> Apparently not]
603 
604  CTimeLogger logger;
605  logger.disable();
606 
607  double ncbin = cbin + d/2. - 0.5;
608  double nrbin = rbin + d/2. - 0.5;
609 
610  int ncbin_i = floor( ncbin );
611  int nrbin_i = floor( nrbin );
612  int nobin_i = floor( obin );
613 
614  double d_c = ncbin_i - ncbin;
615  double d_r = nrbin_i - nrbin;
616  double d_o = nobin_i - obin;
617 
618  for( int k = 0; k < 2; ++k )
619  for( int m = 0; m < 2; ++m )
620  for( int l = 0; l < 2; ++l )
621  if( ncbin_i+k >= 0 && ncbin_i+k < d && nrbin_i+m >= 0 && nrbin_i+m < d )
622  {
623  int idx = ((nobin_i+l)%n) + n*(ncbin_i+k) + n*d*(nrbin_i+m);
624  oris[idx] += mag*(1-fabs(d_c+k))*(1-fabs(d_r+m))*(1-fabs(d_o+l));
625  }
626 
627 // // Spatial bin
628 // std::vector<int> colIndex, rowIndex;
629 // std::vector<double> colBinDistance, rowBinDistance;
630 //
631 // logger.enter("Main loop");
632 // colIndex.reserve(2);
633 // colBinDistance.reserve(2);
634 // rowIndex.reserve(2);
635 // rowBinDistance.reserve(2);
636 // for( int bin = 0; bin < d; bin++ )
637 // {
638 // double binCenter = bin-1.5; // Center of the bin
639 // double colDistance = fabs( cbin - binCenter ); // Distance to the center of the bin
640 // if( colDistance < 1.0 ) // If it is close enough
641 // {
642 // colIndex.push_back( bin );
643 // colBinDistance.push_back( 1-colDistance );
644 // }
645 //
646 // double rowDistance = fabs( rbin - binCenter );
647 // if( rowDistance < 1.0 )
648 // {
649 // rowIndex.push_back( bin );
650 // rowBinDistance.push_back( 1-rowDistance );
651 // }
652 //
653 // if( colIndex.size() == 2 && rowIndex.size() == 2 ) // We have found all we need (stop looping)
654 // break;
655 // } // end for
656 //
657 // logger.leave("Main loop");
658 // ASSERT_( colIndex.size() <= 2 && rowIndex.size() <= 2 );
659 //
660 // // Orientation bin
661 // std::vector<int> oriIndex(2);
662 // std::vector<double> oriBinDistance(2);
663 // oriIndex[0] = floor( obin );
664 // oriIndex[1] = oriIndex[0] == n-1 ? 0 : oriIndex[0]+1;
665 // oriBinDistance[0] = 1 - (obin-oriIndex[0]);
666 // oriBinDistance[1] = 1 - oriBinDistance[0];
667 //
668 // // The entry can affect to 2 (1x1x2), 4 (2x1x2 or 1x2x2) or 8 (2x2x2) bins
669 // // Insert into the histogram
670 // logger.enter("Insertion in histogram");
671 // for( unsigned int k = 0; k < colIndex.size(); ++k )
672 // for( unsigned int l = 0; l < rowIndex.size(); ++l )
673 // for( unsigned int m = 0; m < 2; ++m )
674 // {
675 // if( colIndex[k] >= 0 && rowIndex[l] >= 0 )
676 // {
677 // unsigned int idx = (unsigned int)(oriIndex[m] + n*colIndex[k] + n*d*rowIndex[l] );
678 // oris[idx] += mag*colBinDistance[k]*rowBinDistance[l]*oriBinDistance[m];
679 // } // end if
680 // } // end for
681 // logger.leave("Insertion in histogram");
682 } // end of interpolateHistEntry
683 
684 /*-------------------------------------------------------------
685  computeHistogramOfOrientations
686 -------------------------------------------------------------*/
687 // x = col, y = row
689  const CImage & image,
690  const unsigned int x,
691  const unsigned int y,
692  const unsigned int patchSize,
693  const double & orientation,
694  vector<int32_t> & descriptor,
695  const TMultiResDescOptions & opts,
696  vector<int32_t> & hashCoeffs )
697 {
698  MRPT_START
699  CTimeLogger tlogger;
700  tlogger.disable();
701 
702  // The patch is 29x29
703  int w = 16; // width of the used patch
704  int Bp = 4; // Number of spatial bins
705  int n = 8; // Number of frequential bins (per spatial bin) --> Descriptor size: 4 x 4 x 8 = 128
706  double cos_t = cos( orientation );
707  double sin_t = sin( orientation );
708  double bins_per_rad = n / M_2PI;
709  double exp_denom = opts.sg3*opts.sg3*2;
710  int radius = 0.5*patchSize;
711  vector<double> oris( Bp*Bp*n, 0.0 ); // Typically 128-D
712 
713  // Normalize patch
714  CImage nimage, thePatch;
715  if( opts.computeHashCoeffs )
716  {
717  image.extract_patch( thePatch, x-radius, y-radius, patchSize, patchSize );
718  normalizeImage( thePatch, nimage ); // Normalize to zero-mean and unit standard deviation
719  }
720 
721 // For each pixel in the 23x23 patch:
722 // a. find its rotated position
723 // b. determine its proper spatial bin (and the other affected one)
724 // c. rotate its orientation with respect the main orientation
725 // d. determine its frequential bin
726 // e. compute the weight
727 // const double kp = ((double)(Bp+1))/((double)w); // [0.31250] Constant for mapping between -8,8 -> -2.5,2.5
728  const double kp = double(Bp)/double(w); // [0.25] Constant for mapping between -8,8 -> -2,2
729  double mag, ori;
730  double cbin, rbin, obin;
731  double c1 = 0.0, c2 = 0.0, c3 = 0.0;
732 
733 // FILE *f1 = mrpt::system::os::fopen( "imgs/c1p.txt", "wt" );
734 // FILE *f2 = mrpt::system::os::fopen( "imgs/c1n.txt", "wt" );
735 // FILE *f3 = mrpt::system::os::fopen( "imgs/c2p.txt", "wt" );
736 // FILE *f4 = mrpt::system::os::fopen( "imgs/c2n.txt", "wt" );
737 // FILE *f5 = mrpt::system::os::fopen( "imgs/c3p.txt", "wt" );
738 // FILE *f6 = mrpt::system::os::fopen( "imgs/c3n.txt", "wt" );
739 // FILE *f7 = mrpt::system::os::fopen( "imgs/c0.txt", "wt" );
740 // FILE *f8 = mrpt::system::os::fopen( "imgs/c3.txt", "wt" );
741 // FILE *f9 = mrpt::system::os::fopen( "imgs/r0.txt", "wt" );
742 // FILE *f10 = mrpt::system::os::fopen( "imgs/r3.txt", "wt" );
743 // FILE *f11 = mrpt::system::os::fopen( "imgs/out.txt", "wt" );
744 
745  for( int c = -radius; c <= radius; ++c )
746  for( int r = -radius; r <= radius; ++r )
747  {
748  cbin = kp*c*cos_t - kp*r*sin_t;
749  rbin = kp*c*sin_t + kp*r*cos_t;
750 
751  if( cbin > -2.5 && cbin < 2.5 && rbin > -2.5 && rbin < 2.5 )
752  {
753  tlogger.enter("computeGradient");
754  bool res = vision::computeGradient( image, x+c, y+r, mag, ori );
755  tlogger.leave("computeGradient");
756  if( res )
757  {
758  ori -= orientation;
759  while( ori < 0.0 )
760  ori += M_2PI;
761  while( ori >= M_2PI )
762  ori -= M_2PI;
763 
764  obin = ori*bins_per_rad;
765  w = exp( -(c*c + r*r) / exp_denom );
766  tlogger.enter("interpolate");
767  vision::interpolateHistEntry( oris, cbin, rbin, obin, mag, Bp, n );
768  tlogger.leave("interpolate");
769  } // end if
770 
771 // if( cbin < -0.5 )
772 // mrpt::system::os::fprintf( f7, "%d %d\n", y+r, x+c );
773 // if( cbin > 0.5 )
774 // mrpt::system::os::fprintf( f8, "%d %d\n", y+r, x+c );
775 // if( rbin < -0.5 )
776 // mrpt::system::os::fprintf( f9, "%d %d\n", y+r, x+c );
777 // if( rbin > 0.5 )
778 // mrpt::system::os::fprintf( f10, "%d %d\n", y+r, x+c );
779 
780  // Compute the hashing coefficients:
781  if( opts.computeHashCoeffs )
782  {
783  if( cbin < 0 )
784  {
785  c1 += nimage.getAsFloat( c+radius, r+radius );
786 // mrpt::system::os::fprintf( f1, "%d %d\n", y+r, x+c );
787  }
788  else
789  {
790  c1 -= nimage.getAsFloat( c+radius, r+radius );
791 // mrpt::system::os::fprintf( f2, "%d %d\n", y+r, x+c );
792  }
793  if( rbin < 0 )
794  {
795  c2 += nimage.getAsFloat( c+radius, r+radius );
796 // mrpt::system::os::fprintf( f3, "%d %d\n", y+r, x+c );
797  }
798  else
799  {
800  c2 -= nimage.getAsFloat( c+radius, r+radius );
801 // mrpt::system::os::fprintf( f4, "%d %d\n", y+r, x+c );
802  }
803  if( (cbin < 0 && rbin < 0) || (cbin > 0 && rbin > 0) )
804  {
805  c3 += nimage.getAsFloat( c+radius, r+radius );
806 // mrpt::system::os::fprintf( f5, "%d %d\n", y+r, x+c );
807  }
808  else
809  {
810  c3 -= nimage.getAsFloat( c+radius, r+radius );
811 // mrpt::system::os::fprintf( f6, "%d %d\n", y+r, x+c );
812  }
813  } // end-if
814  } // end
815 // else
816 // mrpt::system::os::fprintf( f11, "%d %d\n", y+r, x+c );
817  } // end-for
818 // mrpt::system::os::fclose(f1);
819 // mrpt::system::os::fclose(f2);
820 // mrpt::system::os::fclose(f3);
821 // mrpt::system::os::fclose(f4);
822 // mrpt::system::os::fclose(f5);
823 // mrpt::system::os::fclose(f6);
824 // mrpt::system::os::fclose(f7);
825 // mrpt::system::os::fclose(f8);
826 // mrpt::system::os::fclose(f9);
827 // mrpt::system::os::fclose(f10);
828 // mrpt::system::os::fclose(f11);
829 
830 // mrpt::system::pause();
831 
832  if( opts.computeHashCoeffs )
833  {
834  hashCoeffs.resize(3);
835  hashCoeffs[0] = round(c1);
836  hashCoeffs[1] = round(c2);
837  hashCoeffs[2] = round(c3);
838 
839 // cout << "Hash Coeffs: " << hashCoeffs << endl;
840  }
841 
842  // Normalize
843  tlogger.enter("normalize");
844  double sum = 0.0;
845  for( unsigned int ii = 0; ii < oris.size(); ++ii )
846  sum += oris[ii]*oris[ii];
847  sum = 1/sqrt(sum);
848  for( unsigned int ii = 0; ii < oris.size(); ++ii )
849  oris[ii] *= sum;
850 
851  // Crop to "crop_value"
852  for( unsigned int ii = 0; ii < oris.size(); ++ii )
853  oris[ii] = min(opts.cropValue,oris[ii]);
854 
855  // Normalize again -> we have the descriptor!
856  for( unsigned int ii = 0; ii < oris.size(); ++ii )
857  sum += oris[ii]*oris[ii];
858  sum = 1/sqrt(sum);
859  for( unsigned int ii = 0; ii < oris.size(); ++ii )
860  oris[ii] *= sum;
861 
862  // Convert it to std::vector<int>
863  descriptor.resize( oris.size() );
864  for( unsigned int ii = 0; ii < descriptor.size(); ++ii )
865  descriptor[ii] = (int)(255.0f*oris[ii]);
866 
867  tlogger.leave("normalize");
868 
869  MRPT_END
870 } // end vision::computeHistogramOfOrientations
871 
872 /*-------------------------------------------------------------
873  matchMultiResolutionFeatures
874 -------------------------------------------------------------*/
876  const CFeatureList & list1,
877  CFeatureList & list2,
878  const CImage & rightImage,
879  const TMultiResDescMatchOptions & matchOpts,
880  const TMultiResDescOptions & computeOpts )
881 {
882  MRPT_START
883  // "List1" has a set of features with their depths computed
884  // "List2" has a set of FAST features detected in the next frame (with their depths)
885  // We perform a prediction of the "List1" features and try to find its matches within List2
886  // --------------------------------------------------------
887  // Algortihm summary:
888  // --------------------------------------------------------
889  // For each feature in List1 we find a search region: by now a fixed window e.g. 20x20 pixels
890  // TO DO: Non-maximal supression according to the feature score
891  // From the remaining set, we compute the main orientation and check its consistency with the List1 feature
892  // NEW: compute the corresponding feature in right image and compute its depth
893  // From the remaining set, we compute the descriptor at scale 1.0 and determine the set of scales where to look for
894  // If the distance between descriptors is below a certain threshold, we've found a match.
895  // --------------------------------------------------------
896 
897  // General variables
898  CTimeLogger logger;
899  logger.disable();
900 
902 
903  // Preliminary tasks
904  output.firstListCorrespondences.resize(list1.size(),FEAT_FREE);
905  output.secondListCorrespondences.resize(list2.size(),FEAT_FREE);
906  output.firstListFoundScales.resize(list1.size(),-1);
907  output.firstListDistance.resize(list1.size(),1.0);
908 
909  // Local variables
910  int hWindow = matchOpts.searchAreaSize/2; // Half of the search window width
911  int mxsize = hWindow+2*matchOpts.lastSeenThreshold;
912  int imageW = rightImage.getWidth(); // Image width
913  int imageH = rightImage.getHeight(); // Image height
914  int leftFeatCounter = 0;
915  int rightFeatCounter = 0; // Counter for features
916  int patchSize = computeOpts.basePSize;
917  int minScale;
918  double maxResponse;
919  output.nMatches = 0;
920  int ridx = 0;
921 
924 // cout << "Starting the loop" << endl;
925  cout << endl;
926  for( leftFeatCounter = 0, it1 = list1.begin(); it1 != list1.end(); ++it1, ++leftFeatCounter )
927  {
928  if( !(*it1)->descriptors.hasDescriptorMultiSIFT() )
929  continue;
930 // cout << (*it1)->ID << " with " << endl;
931  double sRegionX0 = max( (*it1)->x-min((hWindow+2*(*it1)->nTimesLastSeen), mxsize), 0.0f);
932  double sRegionX1 = min( (*it1)->x+min((hWindow+2*(*it1)->nTimesLastSeen), mxsize), (float)imageW );
933  double sRegionY0 = max( (*it1)->y-min((hWindow+2*(*it1)->nTimesLastSeen), mxsize), 0.0f);
934  double sRegionY1 = min( (*it1)->y+min((hWindow+2*(*it1)->nTimesLastSeen), mxsize), (float)imageH );
935 
936  int minDist = 1e6;
937  //int minIdx = -1;
938  minScale = -1;
939  maxResponse = 0;
940  ridx = 0;
941 
942  for( rightFeatCounter = 0, it2 = list2.begin(); it2 != list2.end(); ++it2, ++rightFeatCounter )
943  {
944 // if( (*it2)->ID == 193 )
945 // {
946 // cout << (*it1)->ID << " with " << (*it2)->ID << endl;
947 // mrpt::system::pause();
948 // }
949 
950 
951  // FILTER 1: Search region
952  if( (*it2)->x < sRegionX0 || (*it2)->x > sRegionX1 || (*it2)->y < sRegionY0 || (*it2)->y > sRegionY1 )
953  {
954 // if( (*it2)->ID == 193 )
955 // {
956 // cout << (*it2)->ID << " OOB" << endl;
957 // }
958  continue;
959  }
960 
961 // cout << " " << (*it2)->ID << endl;
962 
963  // Compute main orientations
964 // logger.enter("cmorientation");
965 // (*it2)->multiScales.push_back( 1.0 );
966 // (*it2)->multiOrientations.resize( 1 );
967 // (*it2)->descriptors.multiSIFTDescriptors.resize( 1 );
968 // (*it2)->multiHashCoeffs.resize( 1 );
969  vector<double> thisOris;
970  if( !vision::computeMainOrientations( rightImage, (*it2)->x, (*it2)->y, patchSize, thisOris, computeOpts.sg2 ) )
971  {
972 // if( (*it2)->ID == 193 )
973 // {
974 // cout << (*it2)->ID << " bad orientation computed" << endl;
975 // mrpt::system::pause();
976 // }
977  continue;
978  }
979 // logger.leave("cmorientation");
980 
981  // Compute the proper scales where to look for
982  int firstScale, lastScale;
983  if( matchOpts.useDepthFilter )
984  vision::setProperScales( (*it1), (*it2), firstScale, lastScale );
985  else
986  {
987  firstScale = max( 0, (int)matchOpts.lowScl1 );
988  lastScale = min( (int)(*it1)->multiScales.size()-1, (int)matchOpts.highScl1 );
989  }
990 
991 // if( (*it2)->ID == 193 )
992 // {
993 // cout << "Searching in scales: " << firstScale << " to " << lastScale << endl;
994 // mrpt::system::pause();
995 // }
996 
997  // Search for consistency of the orientation
998  for( int k1 = firstScale; k1 <= lastScale; ++k1 )
999  for( int k2 = 0; k2 < (int)(*it1)->multiOrientations[k1].size(); ++k2 )
1000  for( int k3 = 0; k3 < (int)thisOris.size(); ++k3 ) // FILTER 2: Orientation
1001  if( fabs( (*it1)->multiOrientations[k1][k2] - thisOris[k3] ) < matchOpts.oriThreshold ||
1002  fabs( M_2PI - fabs( (*it1)->multiOrientations[k1][k2] - thisOris[k3] ) ) < matchOpts.oriThreshold )
1003  {
1004  // Orientation check passed
1005  // FILTER 3: Feature response
1006  // has it a better score than its 8 neighbours?
1007 // logger.enter("non-max");
1008  // TO DO: search the maximum number of neighbours up to 8
1009  bool isMax = true;
1010  if( list2.size() >= 8 )
1011  {
1012  std::vector< size_t > out_idx;
1013  std::vector< float > out_dist_sqr;
1014  maxResponse = (*it2)->response;
1015 
1016  list2.kdTreeNClosestPoint2DIdx( (*it2)->x, (*it2)->y, 8, out_idx, out_dist_sqr );
1017  for( size_t kdcounter = 0; kdcounter < out_idx.size(); ++kdcounter )
1018  {
1019  if( out_dist_sqr[kdcounter] > 1.4142 )
1020  continue;
1021 
1022  if( list2[out_idx[kdcounter]]->response > maxResponse )
1023  {
1024  maxResponse = list2[out_idx[kdcounter]]->response;
1025  isMax = false;
1026  break;
1027  }
1028  }
1029  } // end-if
1030 
1031  if( !isMax )
1032  {
1033  cout << "NO ES MAX" << endl;
1035  continue;
1036  }
1037 
1038  // Candidate found at scale "k1" and orientation "k2" (list1) and orientation "k3" (list2)
1039  // Compute descriptor for these conditions!
1040 
1041 // vector<int> desc, hashC;
1042 // TMultiResDescOptions auxOpts = computeOpts;
1043 // auxOpts.computeHashCoeffs = false;
1044 
1045  // All the filters have been passed -> we can add all the information into the feature
1046  // If it has a previous computed descriptor it is substituted by this one
1047  if( (*it2)->multiScales.size() == 0 )
1048  {
1049  (*it2)->multiScales.resize(1);
1050  (*it2)->multiScales[0] = 1.0;
1051  (*it2)->multiOrientations.resize(1);
1052  (*it2)->descriptors.multiSIFTDescriptors.resize(1);
1053  (*it2)->multiHashCoeffs.resize(1);
1054  }
1055 
1056  /* ORIENTATIONS */
1057  bool oriFound = false;
1058  int wh = 0;
1059  for( int co = 0; co < (int)(*it2)->multiOrientations[0].size(); ++co )
1060  if( (*it2)->multiOrientations[0][co] == thisOris[k3] )
1061  {
1062  wh = co;
1063  oriFound = true;
1064  }
1065 
1066 
1067  int dist = 0;
1068  if( !oriFound ) // The feature hasn't got this orientation already -> compute the descriptor & coeffs
1069  {
1070 // if( (*it2)->ID == 193 )
1071 // cout << "Orientation not found -> compute the descriptor." << endl;
1072 
1073  (*it2)->multiOrientations[0].push_back( thisOris[k3] );
1074 
1075  vector<int32_t> thisDesc, thisHash;
1077  rightImage,
1078  (*it2)->x, (*it2)->y,
1079  patchSize,
1080  thisOris[k3],
1081  thisDesc,
1082  computeOpts,
1083  thisHash );
1084 
1085  /* DESCRIPTORS */
1086  (*it2)->descriptors.multiSIFTDescriptors[0].push_back( thisDesc );
1087  /* HASH COEFFICIENTS */
1088  (*it2)->multiHashCoeffs[0].push_back( thisHash );
1089 
1090  for( int n = 0; n < int(thisDesc.size()); n++ )
1091  dist += square( (*it1)->descriptors.multiSIFTDescriptors[k1][k2][n] - thisDesc[n] );
1092  } // end if
1093  else
1094  {
1095 // if( (*it2)->ID == 193 )
1096 // cout << "Skipping descriptor computation." << endl;
1097 
1098  for( int n = 0; n < (int)(*it2)->descriptors.multiSIFTDescriptors[0][wh].size(); n++ )
1099  dist += square( (*it1)->descriptors.multiSIFTDescriptors[k1][k2][n] - (*it2)->descriptors.multiSIFTDescriptors[0][wh][n] );
1100  } // end else
1101 
1102 // if( PARAR && (*it2)->ID == 193 )
1103 // {
1104 // (*it2)->dumpToConsole();
1105 // mrpt::system::pause();
1106 // }
1107 
1108  // FILTER 4 for the matching: Descriptor distance
1109  if( dist < minDist )
1110  {
1111  minDist = dist;
1112  ridx = rightFeatCounter;
1113  maxResponse = (*it2)->response;
1114  minScale = k1;
1115 
1116 // minauxfscale = auxfscale;
1117 // minauxlscale = auxlscale;
1118 
1119 // mindepth1 = (*it1)->depth;
1120 // mindepth2 = (*it2)->depth;
1121  }
1122 
1123 // if( (*it2)->ID == 193 )
1124 // {
1125 // cout << "Matching entre 193 y "<< (*it2)->ID << " entre escalas: " << firstScale << " y " << lastScale << endl;
1126 // cout << "OR1: " << (*it1)->multiOrientations[k1][k2] << " OR2: " << (*it2)->multiOrientations[0][k3];
1127 // cout << "SCL: " << (*it1)->multiScales[k1] << " DIST: " << dist << endl;
1128 // }
1129 
1130  } // end if
1131  } // end for it2
1132  if( minDist < matchOpts.matchingThreshold )
1133  {
1134  // AVOID COLLISIONS IN A GOOD MANNER
1135  int auxIdx = output.secondListCorrespondences[ ridx ];
1136  if( auxIdx != FEAT_FREE && minDist < output.firstListDistance[ auxIdx ] )
1137  {
1138  // We've found a better match
1139  output.firstListDistance[ leftFeatCounter ] = minDist;
1140  output.firstListCorrespondences[ leftFeatCounter ] = ridx;
1141  output.secondListCorrespondences[ ridx ] = leftFeatCounter;
1142  output.firstListFoundScales[ leftFeatCounter ] = minScale;
1143 
1144  // Undo the previous one
1145  output.firstListDistance[ auxIdx ] = 1.0;
1146  output.firstListCorrespondences[ auxIdx ] = FEAT_FREE;
1147  output.firstListFoundScales[ auxIdx ] = -1;
1148 
1149 // cout << "Better match at scale: " << minScale << endl;
1150  } // end-if
1151  else
1152  {
1153 //// cout << "New match found: [R] " << minRightIdx << " with [L] " << minLeftIdx << "(" << minVal << ")" << endl;
1154  output.secondListCorrespondences[ ridx ] = leftFeatCounter;
1155  output.firstListCorrespondences[ leftFeatCounter ] = ridx;
1156  output.firstListDistance[ leftFeatCounter ] = minDist;
1157  output.firstListFoundScales[ leftFeatCounter ] = minScale;
1158  output.nMatches++;
1159 // cout << "Match found at scale: " << minScale << endl;
1160  }
1161  // Match found
1162 // leftMatchingIdx.push_back( leftFeatCounter );
1163 // rightMatchingIdx.push_back( minIdx );
1164 // outScales.push_back( minScale );
1165 //
1166 // cout << "Match found: [" << leftFeatCounter << "," << minIdx;
1167 // cout << "] at scale " << minScale << " [" << minauxfscale << "," << minauxlscale << "]";
1168 // cout << " with depths: [" << mindepth1 << "," << mindepth2 << "]" << endl;
1169  } // end if
1170  } // end for it1
1171 
1172  return output;
1173  MRPT_END
1174 } // end matchMultiResolutionFeatures
1175 
1176 /*-------------------------------------------------------------
1177  matchMultiResolutionFeatures
1178 -------------------------------------------------------------*/
1180  CMatchedFeatureList & mList1,
1181  CMatchedFeatureList & mList2,
1182  const CImage & leftImage,
1183  const CImage & rightImage,
1184  const TMultiResDescMatchOptions & matchOpts,
1185  const TMultiResDescOptions & computeOpts )
1186 {
1187  MRPT_START
1188  // "mList1" has a set of matched features with their depths computed
1189  // "mList2" has a set of matched FAST features detected in the next frame at scale 1.0 (with their depths)
1190  // We perform a prediction of the "mList1" features and try to find its matches within mList2
1191  // --------------------------------------------------------
1192  // Algortihm summary:
1193  // --------------------------------------------------------
1194  // For each feature in List1 we find a search region: by now a fixed window e.g. 20x20 pixels
1195  // TO DO: Non-maximal supression according to the feature score
1196  // From the remaining set, we compute the main orientation and check its consistency with the List1 feature
1197  // NEW: compute the corresponding feature in right image and compute its depth
1198  // From the remaining set, we compute the descriptor at scale 1.0 and determine the set of scales where to look for
1199  // If the distance between descriptors is below a certain threshold, we've found a match.
1200  // --------------------------------------------------------
1201 
1202  // General variables
1203  CTimeLogger logger;
1204  //logger.disable();
1205 
1206  ASSERT_( mList1.size() > 0 && mList2.size() > 0 );
1207 
1208  CFeatureList baseList1, baseList2;
1209  mList1.getBothFeatureLists( baseList1, baseList2 );
1210 
1211  CFeatureList auxList1, auxList2;
1212  mList2.getBothFeatureLists( auxList1, auxList2 );
1213 
1214  vector<int> scales1, scales2;
1215 
1216  TMultiResMatchingOutput output1, output2;
1217 
1218  // Left image
1219  output1 = matchMultiResolutionFeatures( baseList1, auxList1, leftImage, matchOpts, computeOpts );
1220  updateBaseList( baseList1, auxList1, output1.firstListCorrespondences ); //Update counters
1221  //updateBaseList(leftIdx2,auxList1);
1222 
1223 // CImage auxImg1, auxImg2;
1224 // auxImg1 = leftImage;
1225 // int hwsize = matchOpts.searchAreaSize/2;
1226 // for( int k = 0; k < leftIdx1.size(); ++k )
1227 // if( leftIdx1[k] != FEAT_FREE )
1228 // {
1229 // auxImg1.cross( baseList1[k]->x, baseList1[k]->y, TColor::red, '+' );
1230 // auxImg1.textOut( baseList1[k]->x, baseList1[k]->y, format("%d", scales1[k]), TColor::red );
1231 // auxImg1.rectangle( baseList1[k]->x-hwsize, baseList1[k]->y-hwsize, baseList1[k]->x+hwsize, baseList1[k]->y+hwsize, TColor::red );
1232 // }
1233 // win1.showImage( auxImg1 );
1234 
1235  // Right image
1236  output2 = matchMultiResolutionFeatures( baseList2, auxList2, rightImage, matchOpts, computeOpts );
1237  updateBaseList(baseList2, auxList2, output2.firstListCorrespondences); //Update counters
1238  //updateBaseList(rightIdx2,auxList2);
1239 
1240 // auxImg2 = rightImage;
1241 // for( int k = 0; k < rightIdx1.size(); ++k )
1242 // if( rightIdx1[k] != FEAT_FREE )
1243 // {
1244 // auxImg2.cross( baseList2[k]->x, baseList2[k]->y, TColor::red, '+' );
1245 // auxImg2.textOut( baseList2[k]->x, baseList2[k]->y, format("%d", scales2[k]), TColor::red );
1246 // auxImg2.rectangle( baseList2[k]->x-hwsize, baseList2[k]->y-hwsize, baseList2[k]->x+hwsize, baseList2[k]->y+hwsize, TColor::red );
1247 // }
1248  //win2.showImage( auxImg2 );
1249  //mrpt::system::pause();
1250 
1251  cout << "Left matches: " << output1.nMatches << " out of " << baseList1.size() << "," << auxList1.size() << endl;
1252  cout << "Right matches: " << output2.nMatches << " out of " << baseList2.size() << "," << auxList2.size() << endl;
1253  cout << "Matched list: " << mList1.size() << endl;
1254 
1255 // for(int k = 0; k < auxList2.size(); ++k)
1256 // {
1257 // cout << k;
1258 // auxList2[k]->dumpToConsole();
1259 // }
1260 // mrpt::system::pause();
1261 
1263  int m;
1264  //int cnt = 0;
1265  for( m = 0, itMatch = mList1.begin(); itMatch != mList1.end(); ++m )
1266  {
1267 
1268  //cout << m << " " << endl;
1269  if( itMatch->first->nTimesLastSeen > matchOpts.lastSeenThreshold || itMatch->second->nTimesLastSeen > matchOpts.lastSeenThreshold )
1270  {
1271 // cnt++;
1272 // cout << "feature " << m << " must be deleted" << endl;
1273  itMatch = mList1.erase( itMatch );
1274  }
1275  else
1276  {
1277  // We've found a tracked match
1278  // We have found the match in both frames! Check out the scales!
1279  int tidx = output1.firstListCorrespondences[m];
1280  if( tidx != FEAT_FREE )
1281  {
1282  if( scales1[m] == 0 )
1283  {
1284  cout << "Left feature " << m << " found in scale 0!" << endl;
1285  int res = computeMoreDescriptors( leftImage, auxList1[tidx], itMatch->first, true, computeOpts );
1286  if( res == 0) cout << "LF LOWSCALES Out of bounds!!" << endl;
1287  //mrpt::system::pause();
1288  }
1289  else
1290  {
1291  int nScales = (int)itMatch->first->multiScales.size();
1292  if( scales1[m] == nScales-1 )
1293  {
1294  cout << "Left feature " << m << " found in last scale!" << endl; //computeMoreDescriptors( auxList1[k1], leftImage, false, itMatch->first );
1295  cout << "tidx=" << tidx << endl;
1296  int res = computeMoreDescriptors( leftImage, auxList1[tidx], itMatch->first, false, computeOpts );
1297  if( res == 0) cout << "LF HIGHSCALES Out of bounds!!" << endl;
1298  // mrpt::system::pause();
1299  }
1300  }
1301  } // end-if
1302 
1303 // cout << "Left: " << m << " with " << << " at scale: " << output1.firstListFoundScales[m] << endl;
1304 // cout << "Right: " << m << " with " << output2.firstListCorrespondences[m] << " at scale: " << output2.firstListFoundScales[m] << endl;
1305  tidx = output2.firstListCorrespondences[m];
1306  if( tidx != FEAT_FREE )
1307  {
1308  if( scales2[m] == 0 )
1309  {
1310  cout << "Right feature " << m << " found in scale 0!" << endl; //computeMoreDescriptors( auxList2[k2], rightImage, true, itMatch->second );
1311  int res = computeMoreDescriptors( rightImage, auxList2[tidx], itMatch->second, true, computeOpts );
1312  if( res == 0) cout << "RF LOWSCALES Out of bounds!!" << endl;
1313  //mrpt::system::pause();
1314  }
1315  else
1316  {
1317  int nScales = (int)itMatch->second->multiScales.size();
1318  if( scales2[m] == nScales-1 )
1319  {
1320  cout << "Right feature " << m << " found in scale!" << endl; //computeMoreDescriptors( auxList2[k2], rightImage, false, itMatch->second );
1321  int res = computeMoreDescriptors( rightImage, auxList2[tidx], itMatch->second, false, computeOpts );
1322  if( res == 0) cout << "RF HIGHSCALES Out of bounds!!" << endl;
1323  // mrpt::system::pause();
1324  }
1325  }
1326  } // end-else
1327  itMatch++;
1328  } // end-else
1329  } // end-for
1330 // cout << cnt << " features would be deleted." << endl;
1331  return mList1.size();
1332  MRPT_END
1333 } // end matchMultiResolutionFeatures
1334 
1335 /*-------------------------------------------------------------
1336  computeMoreDescriptors
1337 -------------------------------------------------------------*/
1339  const CImage & image,
1340  const CFeaturePtr & inputFeat,
1341  CFeaturePtr & outputFeat,
1342  const bool & lowerScales,
1343  const TMultiResDescOptions & opts )
1344 {
1345 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM>=0x211
1346 
1347  MRPT_START
1348  //**************************************************************************
1349  // Pre-smooth the image with sigma = sg1 (typically 0.5)
1350  //**************************************************************************
1351  cv::Mat tempImg1;
1352  IplImage aux1;
1353 
1354  const cv::Mat inImg1 = cv::cvarrToMat(image.getAs<IplImage>(),false /*dont copy data*/);
1355 
1356  cv::GaussianBlur( inImg1, tempImg1, cvSize(0,0), opts.sg1 /*sigmaX*/, opts.sg1 /*sigmaY*/ );
1357  aux1 = tempImg1;
1358  CImage smLeftImg( &aux1 );
1359  //--------------------------------------------------------------------------
1360 
1361  unsigned int a = opts.basePSize;
1362 
1363  int largestSize = lowerScales ? round(a*0.8) : round(a*2.0);
1364  largestSize = largestSize%2 ? largestSize : largestSize+1; // round to the closest odd number
1365  int hLargestSize = largestSize/2;
1366 
1367  unsigned int npSize, hpSize;
1368 
1369  // We don't take into account the matching if it is too close to the image border (the largest patch cannot be extracted)
1370  if( inputFeat->x+hLargestSize > smLeftImg.getWidth()-1 || inputFeat->x-hLargestSize < 0 ||
1371  inputFeat->y+hLargestSize > smLeftImg.getHeight()-1 || inputFeat->y-hLargestSize < 0 )
1372  return 0;
1373 
1374  //int iniScale = 0, endScale = 0;
1375  if( lowerScales )
1376  {
1377  vector<double> thisScales(2);
1378  thisScales[0] = 0.5;
1379  thisScales[1] = 0.8;
1380 
1381  double baseScale = outputFeat->multiScales[0];
1382 
1383 // cout << " :: Lower scales" << endl;
1384  outputFeat->multiScales.push_front( thisScales[1]*baseScale );
1385  outputFeat->multiScales.push_front( thisScales[0]*baseScale );
1386 
1387 // iniScale = 0;
1388 // endScale = 2;
1389 
1390  for( int k = 1; k >= 0; --k )
1391  {
1392  npSize = round( a*thisScales[k] );
1393  npSize = npSize%2 ? npSize : npSize+1; // round to the closest odd number
1394  hpSize = npSize/2; // half size of the patch
1395 
1396  CImage tPatch;
1397  // LEFT IMAGE:
1398  if( npSize )
1399  smLeftImg.extract_patch( tPatch, inputFeat->x-hpSize, inputFeat->y-hpSize, npSize, npSize );
1400 
1401  cv::Mat out_mat_patch;
1402  // The size is a+2xa+2 because we have to compute the gradient (magnitude and orientation) in every pixel within the axa patch so we need
1403  // one more row and column. For instance, for a 23x23 patch we need a 25x25 patch.
1404  cv::resize( cv::cvarrToMat(tPatch.getAs<IplImage>(),false ), out_mat_patch, cv::Size(a+2,a+2) );
1405  IplImage aux_img = IplImage(out_mat_patch);
1406  CImage rsPatch( &aux_img );
1407 
1408 // cout << " ::: Patch extracted and resized" << endl;
1409  vector<double> auxOriVector;
1410  if( !vision::computeMainOrientations( rsPatch, a/2+1, a/2+1, a, auxOriVector, opts.sg2 ))
1411  {
1412  cout << "computeMainOrientations returned false" << endl;
1414  }
1415 
1416 // cout << " ::: Orientation computed" << endl;
1417  vector< vector<int32_t> > auxDescVector;
1418  vector< vector<int32_t> > auxHashCoeffs;
1419  auxDescVector.resize( auxOriVector.size() );
1420  auxHashCoeffs.resize( auxOriVector.size() );
1421  for( unsigned int m = 0; m < auxOriVector.size(); ++m )
1422  {
1423 // cout << " :::: Descriptor for orientation " << auxOriVector[m];
1425  rsPatch,
1426  a/2+1, a/2+1, a,
1427  auxOriVector[m],
1428  auxDescVector[m], opts,
1429  auxHashCoeffs[m] );
1430 // cout << " ...done" << endl;
1431  } // end-for
1432  outputFeat->multiOrientations.push_front( auxOriVector );
1433  outputFeat->descriptors.multiSIFTDescriptors.push_front( auxDescVector );
1434  outputFeat->multiHashCoeffs.push_front( auxHashCoeffs );
1435  } // end-for
1436  }
1437  else
1438  {
1439 // cout << " :: Higher scales" << endl;
1440  vector<double> thisScales(4);
1441  thisScales[0] = 1.2;
1442  thisScales[1] = 1.5;
1443  thisScales[2] = 1.8;
1444  thisScales[3] = 2.0;
1445 
1446  size_t nCurrScales = outputFeat->multiScales.size();
1447  outputFeat->multiScales.push_back( thisScales[0]*outputFeat->multiScales[nCurrScales-1] );
1448  outputFeat->multiScales.push_back( thisScales[1]*outputFeat->multiScales[nCurrScales-1] );
1449  outputFeat->multiScales.push_back( thisScales[2]*outputFeat->multiScales[nCurrScales-1] );
1450  outputFeat->multiScales.push_back( thisScales[3]*outputFeat->multiScales[nCurrScales-1] );
1451 
1452 // for( int k = nCurrScales; k < (int)outputFeat->multiScales.size(); ++k )
1453  for( int k = 0; k < (int)thisScales.size(); ++k )
1454  {
1455 // cout << " ::: For scale " << k+nCurrScales << endl;
1456 
1457  npSize = round( a*thisScales[k] );
1458  npSize = npSize%2 ? npSize : npSize+1; // round to the closest odd number
1459  hpSize = npSize/2; // half size of the patch
1460 
1461  CImage tPatch;
1462 
1463  // LEFT IMAGE:
1464  smLeftImg.extract_patch( tPatch, inputFeat->x-hpSize, inputFeat->y-hpSize, npSize, npSize );
1465 
1466  cv::Mat out_mat_patch;
1467  // The size is a+2xa+2 because we have to compute the gradient (magnitude and orientation) in every pixel within the axa patch so we need
1468  // one more row and column. For instance, for a 23x23 patch we need a 25x25 patch.
1469  cv::resize( cv::cvarrToMat( tPatch.getAs<IplImage>(),false), out_mat_patch, cv::Size(a+2,a+2) );
1470  IplImage aux_img = IplImage(out_mat_patch);
1471  CImage rsPatch( &aux_img );
1472 
1473 // cout << " ::: Patch extracted and resized" << endl;
1474 
1475  vector<double> auxOriVector;
1477  rsPatch, a/2+1, a/2+1, a,
1478  auxOriVector, opts.sg2 );
1479 
1480 // cout << " ::: Orientation computed" << endl;
1481 
1482  vector< vector<int32_t> > auxDescVector;
1483  vector< vector<int32_t> > auxCoefVector;
1484  auxDescVector.resize( auxOriVector.size() );
1485  auxCoefVector.resize( auxOriVector.size() );
1486  for( unsigned int m = 0; m < auxOriVector.size(); ++m )
1487  {
1488 // cout << " :::: Descriptor for orientation " << auxOriVector[m];
1490  rsPatch,
1491  a/2+1, a/2+1, a,
1492  auxOriVector[m],
1493  auxDescVector[m], opts,
1494  auxCoefVector[m] );
1495 // cout << " ...done" << endl;
1496  } // end-for
1497  outputFeat->multiOrientations.push_back( auxOriVector );
1498  outputFeat->descriptors.multiSIFTDescriptors.push_back( auxDescVector );
1499  outputFeat->multiHashCoeffs.push_back( auxCoefVector );
1500  } // end-for
1501  } // end else
1502  ASSERT_( outputFeat->descriptors.hasDescriptorMultiSIFT() );
1503  return 1;
1504  MRPT_END
1505 #else
1506  THROW_EXCEPTION("This function needs OpenCV 2.1+")
1507  return 0;
1508 #endif
1509 } // end-computeMoreDescriptors
1510 
1511 /*-------------------------------------------------------------
1512  setProperScales
1513 -------------------------------------------------------------*/
1515  const CFeaturePtr & feat1,
1516  const CFeaturePtr & feat2,
1517  int & firstScale,
1518  int & lastScale )
1519 {
1520  // Find the range of scales (of those within feat1) where the descriptor should be matched
1521  // For the feat1 we use "initialDepth" since all the scales are relative to this depth while for the
1522  // feat2 it is used "depth" which is the actual scale of the feature.
1523  MRPT_START
1524  int numScales = int(feat1->multiScales.size());
1525 // if( numScales <= 1 )
1526 // {
1527 // cout << "BAD NUMBER OF SCALES: " << endl;
1528 // feat1->dumpToConsole();
1529 // feat2->dumpToConsole();
1530  ASSERT_( numScales > 1 );
1531 // }
1532 
1533  firstScale = 0;
1534  lastScale = numScales-1;
1535 
1536  // Determine the range of scale where to look for in the list1
1537  double smin = (feat2->depth-0.15*feat1->initialDepth)/feat1->initialDepth;
1538  double smax = (feat2->depth+0.15*feat1->initialDepth)/feat1->initialDepth;
1539 
1540  if( smin <= feat1->multiScales[1] )
1541  firstScale = 0;
1542  else
1543  {
1544  if( smin > feat1->multiScales[numScales-2] )
1545  firstScale = numScales-2;
1546  else // it is in between the limits
1547  {
1548  for( int k = 1; k <= numScales-3; ++k )
1549  if( smin > feat1->multiScales[k] )
1550  firstScale = k;
1551  } // end else
1552  } // end else
1553 
1554  if( smax <= feat1->multiScales[1] )
1555  lastScale = 1;
1556  else
1557  {
1558  if( smax > feat1->multiScales[numScales-2] )
1559  lastScale = numScales-1;
1560  else
1561  {
1562  for( int k = 1; k <= numScales-3; ++k )
1563  if( smax <= feat1->multiScales[k] )
1564  {
1565  lastScale = k;
1566  break;
1567  } // end if
1568  } // end else
1569  } // end else
1570 
1571  ASSERT_( firstScale >= 0 && lastScale < numScales && firstScale < lastScale );
1572  MRPT_END
1573 } // end setProperScales
1574 
1575 /*-------------------------------------------------------------
1576  computeMultiResolutionDescriptors
1577 -------------------------------------------------------------*/
1579  const CImage & imageLeft,
1580  const CImage & imageRight,
1581  CMatchedFeatureList & matchedFeats,
1582  const TMultiResDescOptions & opts )
1583 {
1584 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM>=0x211
1585 
1586  MRPT_START
1587  CTimeLogger tlogger;
1588  tlogger.disable();
1589  ASSERT_( matchedFeats.size() > 0 );
1590 
1591  //**************************************************************************
1592  // Pre-smooth the image with sigma = sg1 (typically 0.5)
1593  //**************************************************************************
1594  tlogger.enter("smooth");
1595  cv::Mat tempImg1, tempImg2;
1596  IplImage aux1, aux2;
1597 
1598  const cv::Mat inImg1 = cv::cvarrToMat(imageLeft.getAs<IplImage>());
1599  const cv::Mat inImg2 = cv::cvarrToMat(imageRight.getAs<IplImage>());
1600 
1601  cv::GaussianBlur( inImg1, tempImg1, cvSize(0,0), opts.sg1 /*sigmaX*/, opts.sg1 /*sigmaY*/ );
1602  aux1 = tempImg1;
1603  CImage smLeftImg( &aux1 );
1604 
1605  cv::GaussianBlur( inImg2, tempImg2, cvSize(0,0), opts.sg1 /*sigmaX*/, opts.sg1 /*sigmaY*/ );
1606  aux2 = tempImg2;
1607  CImage smRightImg( &aux2 );
1608  tlogger.leave("smooth");
1609  //--------------------------------------------------------------------------
1610 
1611  unsigned int a = opts.basePSize;
1612  unsigned int feat_counter = 0;
1613  unsigned int good_matches = 0;
1614 
1615  int largestSize = round(a*opts.scales[opts.scales.size()-1]);
1616  largestSize = largestSize%2 ? largestSize : largestSize+1; // round to the closest odd number
1617  int hLargestSize = largestSize/2;
1618 
1619  unsigned int npSize;
1620  unsigned int hpSize;
1621 
1622  for( CMatchedFeatureList::iterator itMatch = matchedFeats.begin(); itMatch != matchedFeats.end(); ++itMatch, ++feat_counter )
1623  {
1624  // We don't take into account the matching if it is too close to the image border (the largest patch cannot be extracted)
1625  if( itMatch->first->x+hLargestSize > smLeftImg.getWidth()-1 || itMatch->first->x-hLargestSize < 0 ||
1626  itMatch->first->y+hLargestSize > smLeftImg.getHeight()-1 || itMatch->first->y-hLargestSize < 0 ||
1627  itMatch->second->x+hLargestSize > smRightImg.getWidth()-1 || itMatch->second->x-hLargestSize < 0 ||
1628  itMatch->second->y+hLargestSize > smRightImg.getHeight()-1 || itMatch->second->y-hLargestSize < 0 )
1629  continue;
1630 
1631  // We have found a proper match to obtain the multi-descriptor
1632  // Compute the depth and store the coords:
1633  tlogger.enter("compute depth");
1634  if( opts.computeDepth )
1635  {
1636  double disp = itMatch->first->x - itMatch->second->x;
1637  double aux = opts.baseline/disp;
1638  double x3D = (itMatch->first->x-opts.cx)*aux;
1639  double y3D = (itMatch->first->y-opts.cy)*aux;
1640  double z3D = opts.fx*aux;
1641 
1642  itMatch->first->depth = sqrt( x3D*x3D + y3D*y3D + z3D*z3D );
1643  itMatch->second->depth = sqrt( (x3D-opts.baseline)*(x3D-opts.baseline) + y3D*y3D + z3D*z3D );
1644  }
1645  tlogger.leave("compute depth");
1646 
1647  tlogger.enter("cp scales");
1648  itMatch->first->multiScales.resize( opts.scales.size() );
1649  itMatch->first->multiOrientations.resize( opts.scales.size() );
1650  itMatch->first->descriptors.multiSIFTDescriptors.resize( opts.scales.size() );
1651  itMatch->first->multiHashCoeffs.resize( opts.scales.size() );
1652 
1653  itMatch->second->multiScales.resize( opts.scales.size() );
1654  itMatch->second->multiOrientations.resize( opts.scales.size() );
1655  itMatch->second->descriptors.multiSIFTDescriptors.resize( opts.scales.size() );
1656  itMatch->second->multiHashCoeffs.resize( opts.scales.size() );
1657 
1658  // Copy the scale values within the feature.
1659  memcpy( &itMatch->first->multiScales[0], &opts.scales[0], opts.scales.size()*sizeof(double) );
1660  memcpy( &itMatch->second->multiScales[0], &opts.scales[0], opts.scales.size()*sizeof(double) );
1661  tlogger.leave("cp scales");
1662 
1663  // For each of the involved scales
1664  for( unsigned int k = 0; k < opts.scales.size(); ++k )
1665  {
1666  npSize = round(a*opts.scales[k]);
1667  npSize = npSize%2 ? npSize : npSize+1; // round to the closest odd number
1668  hpSize = npSize/2; // half size of the patch
1669 
1670  CImage tPatch;
1671 
1672  // LEFT IMAGE:
1673  tlogger.enter("extract & resize");
1674  smLeftImg.extract_patch( tPatch, itMatch->first->x-hpSize, itMatch->first->y-hpSize, npSize, npSize );
1675 
1676  cv::Mat out_mat_patch;
1677  // The size is a+2xa+2 because we have to compute the gradient (magnitude and orientation) in every pixel within the axa patch so we need
1678  // one more row and column. For instance, for a 23x23 patch we need a 25x25 patch.
1679  cv::resize( cv::cvarrToMat( tPatch.getAs<IplImage>(),false ), out_mat_patch, cv::Size(a+2,a+2) );
1680  IplImage aux_img = IplImage(out_mat_patch);
1681  CImage rsPatch( &aux_img );
1682  tlogger.leave("extract & resize");
1683 
1684  tlogger.enter("main orientations");
1685  // Compute the main orientations for the axa patch, taking into account that the actual patch has a size of a+2xa+2
1686  // (being the center point the one with coordinates a/2+1,a/2+1).
1687  // A sigma = opts.sg2 is used to smooth the entries in the orientations histograms
1688  // Orientation vector will be resize inside the function, so don't reserve space for it
1690  rsPatch, a/2+1, a/2+1, a,
1691  itMatch->first->multiOrientations[k], opts.sg2 );
1692  tlogger.leave("main orientations");
1693 
1694  size_t nMainOris = itMatch->first->multiOrientations[k].size();
1695  itMatch->first->descriptors.multiSIFTDescriptors[k].resize( nMainOris );
1696  for( unsigned int m = 0; m < nMainOris; ++m )
1697  {
1698  tlogger.enter("compute histogram");
1700  rsPatch,
1701  a/2+1, a/2+1, a,
1702  itMatch->first->multiOrientations[k][m],
1703  itMatch->first->descriptors.multiSIFTDescriptors[k][m], opts,
1704  itMatch->first->multiHashCoeffs[k][m] );
1705  tlogger.leave("compute histogram");
1706  } // end for
1707 
1708  // RIGHT IMAGE:
1709  tlogger.enter("extract & resize");
1710  imageRight.extract_patch( tPatch, itMatch->second->x-hpSize, itMatch->second->y-hpSize, npSize, npSize );
1711 
1712  cv::resize( cv::cvarrToMat( tPatch.getAs<IplImage>(),false), out_mat_patch, cv::Size(a+2,a+2) );
1713  IplImage aux_img2 = IplImage(out_mat_patch);
1714  CImage rsPatch2( &aux_img2 );
1715  tlogger.leave("extract & resize");
1716 
1717  tlogger.enter("main orientations");
1719  rsPatch2, a/2+1, a/2+1, a,
1720  itMatch->second->multiOrientations[k], opts.sg2 );
1721  tlogger.leave("main orientations");
1722 
1723  nMainOris = itMatch->second->multiOrientations[k].size();
1724  itMatch->second->descriptors.multiSIFTDescriptors[k].resize( nMainOris );
1725 
1726  for( unsigned int m = 0; m < nMainOris; ++m )
1727  {
1728  tlogger.enter("compute histogram");
1730  rsPatch2,
1731  a/2+1, a/2+1, a,
1732  itMatch->second->multiOrientations[k][m],
1733  itMatch->second->descriptors.multiSIFTDescriptors[k][m], opts,
1734  itMatch->second->multiHashCoeffs[k][m] );
1735  tlogger.leave("compute histogram");
1736  } // end for
1737  } // end scales for
1738  good_matches++;
1739  } // end matches for
1740  MRPT_END
1741 #else
1742  THROW_EXCEPTION("This function needs OpenCV 2.1+")
1743 #endif
1744 } // end-vision::computeMultiResolutionDescriptors
1745 
1746 /*-------------------------------------------------------------
1747  computeMultiResolutionDescriptors
1748 -------------------------------------------------------------*/
1750  const CImage & image,
1751  CFeaturePtr & feat,
1752  const TMultiResDescOptions & opts )
1753 {
1754 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM>=0x211
1755 
1756  MRPT_START
1757  int a = opts.basePSize;
1758  int maxScale = opts.scales.size();
1759  int h = image.getHeight();
1760  int w = image.getWidth();
1761  int npSize, hpSize;
1762 
1763  int largestSize = round(a*opts.scales[maxScale-1]);
1764  largestSize = largestSize%2 ? largestSize : largestSize+1; // round to the closest odd number
1765  int hLargestSize = largestSize/2;
1766 
1767  if( feat->x+hLargestSize > (w-1) || feat->x-hLargestSize < 0 ||
1768  feat->y+hLargestSize > (h-1) || feat->y-hLargestSize < 0 )
1769  {
1770  cout << endl << "[computeMultiResolutionDescriptors] WARNING: Feature is too close to the border. MultiDescriptor computation skipped." << endl;
1771  return false;
1772  }
1773 
1774  feat->multiScales.resize( maxScale );
1775  feat->multiOrientations.resize( maxScale );
1776  feat->descriptors.multiSIFTDescriptors.resize( maxScale );
1777  // If the hash coeffs have to be computed, resize the vector of hash coeffs.
1778  if( opts.computeHashCoeffs )
1779  feat->multiHashCoeffs.resize( maxScale );
1780 
1781  // Copy the scale values within the feature.
1782  for( int k = 0; k < maxScale; ++k )
1783  feat->multiScales[k] = opts.scales[k];
1784 
1785  // For each of the involved scales
1786  for( int k = 0; k < maxScale; ++k )
1787  {
1788  npSize = round( a*opts.scales[k] );
1789  npSize = npSize%2 ? npSize : npSize+1; // round to the closest odd number
1790  hpSize = npSize/2; // half size of the patch
1791 
1792  CImage tPatch(npSize,npSize);
1793 
1794  // LEFT IMAGE:
1795 // if( feat->x+hpSize > image.getWidth()-1 || feat->y+hpSize > image.getHeight()-1 || feat->x-hpSize < 0 || feat->y-hpSize < 0 )
1796 // cout << "(" << feat->x << "," << feat->y << ") and hpSize: " << hpSize << " with scales ";
1797 // cout << opts.scales << " imSize: " << image.getWidth() << "x" << image.getHeight() << endl;
1798  image.extract_patch( tPatch, feat->x-hpSize, feat->y-hpSize, npSize, npSize );
1799 
1800  cv::Mat out_mat_patch;
1801  // The size is a+2xa+2 because we have to compute the gradient (magnitude and orientation) in every pixel within the axa patch so we need
1802  // one more row and column. For instance, for a 23x23 patch we need a 25x25 patch.
1803  cv::resize( cv::cvarrToMat(tPatch.getAs<IplImage>(),false), out_mat_patch, cv::Size(a+2,a+2) );
1804  IplImage aux_img = IplImage(out_mat_patch);
1805  CImage rsPatch( &aux_img );
1806 
1807  // Compute the main orientations for the axa patch, taking into account that the actual patch has a size of a+2xa+2
1808  // (being the center point the one with coordinates a/2+1,a/2+1).
1809  // A sigma = opts.sg2 is used to smooth the entries in the orientations histograms
1810  // Orientation vector will be resized inside the function, so don't reserve space for it
1812  rsPatch, a/2+1, a/2+1, a,
1813  feat->multiOrientations[k], opts.sg2 );
1814 
1815  size_t nMainOris = feat->multiOrientations[k].size();
1816  feat->descriptors.multiSIFTDescriptors[k].resize( nMainOris );
1817  if( opts.computeHashCoeffs )
1818  feat->multiHashCoeffs[k].resize( nMainOris );
1819 
1820  for( unsigned int m = 0; m < nMainOris; ++m )
1821  {
1822  if( opts.computeHashCoeffs )
1823  {
1825  rsPatch,
1826  a/2+1, a/2+1, a,
1827  feat->multiOrientations[k][m],
1828  feat->descriptors.multiSIFTDescriptors[k][m],
1829  opts,
1830  feat->multiHashCoeffs[k][m] );
1831  } // end-if
1832  else
1833  {
1834  vector<int32_t> vec;
1836  rsPatch,
1837  a/2+1, a/2+1, a,
1838  feat->multiOrientations[k][m],
1839  feat->descriptors.multiSIFTDescriptors[k][m],
1840  opts,
1841  vec );
1842  } // end-else
1843  } // end orientations for (m)
1844  } // end scales for (k)
1845  return true;
1846  MRPT_END
1847 #else
1848  THROW_EXCEPTION("This function needs OpenCV 2.1+")
1849 #endif
1850 } // end-computeMultiResolutionDescriptors
1851 
1852 /*-------------------------------------------------------------
1853  computeMultiResolutionDescriptors
1854 -------------------------------------------------------------*/
1856  const CImage & image,
1857  CFeatureList & list,
1858  const TMultiResDescOptions & opts )
1859 {
1860 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM>=0x211
1861  MRPT_START
1862  CTimeLogger tlogger;
1863  tlogger.disable();
1864  ASSERT_( list.size() > 0 );
1865  CImage smLeftImg;
1866  if( opts.blurImage )
1867  {
1868  //**************************************************************************
1869  // Pre-smooth the image with sigma = sg1 (typically 0.5)
1870  //**************************************************************************
1871  cv::Mat tempImg;
1872  IplImage aux;
1873 
1874  const cv::Mat inImg = cv::cvarrToMat(image.getAs<IplImage>());
1875 
1876  cv::GaussianBlur( inImg, tempImg, cvSize(0,0), opts.sg1 /*sigmaX*/, opts.sg1 /*sigmaY*/ );
1877  aux = tempImg;
1878  smLeftImg.loadFromIplImage( &aux );
1879  //--------------------------------------------------------------------------
1880  }
1881  else
1882  smLeftImg = image;
1883 
1884  TMultiResDescOptions auxOpts = opts;
1885  auxOpts.blurImage = false;
1886  vector<bool> st( list.size() );
1887  int k = 0;
1888  for( CFeatureList::iterator it = list.begin(); it != list.end(); ++it, ++k )
1889  st[k] = computeMultiResolutionDescriptors( smLeftImg, (*it), auxOpts );
1890  return st;
1891  MRPT_END
1892 #else
1893  THROW_EXCEPTION("This function needs OpenCV 2.1+")
1894 #endif
1895 
1896 } // end computeMultiResolutionDescriptors
1897 
1898 /*-------------------------------------------------------------
1899  computeMultiResolutionDescriptors
1900 -------------------------------------------------------------*/
1902  const CImage & image,
1903  CFeatureList & list,
1904  const TMultiResDescOptions & opts )
1905 {
1906  #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM>=0x211
1907  MRPT_START
1908  CTimeLogger tlogger;
1909  tlogger.disable();
1910  ASSERT_( list.size() > 0 );
1911 
1912  //**************************************************************************
1913  // Pre-smooth the image with sigma = sg1 (typically 0.5)
1914  //**************************************************************************
1915  tlogger.enter("smooth");
1916  cv::Mat tempImg1;
1917  IplImage aux1;
1918 
1919  const cv::Mat inImg1 = cv::cvarrToMat(image.getAs<IplImage>(),false);
1920 
1921  cv::GaussianBlur( inImg1, tempImg1, cvSize(0,0), opts.sg1 /*sigmaX*/, opts.sg1 /*sigmaY*/ );
1922  aux1 = tempImg1;
1923  CImage smLeftImg( &aux1 );
1924  //--------------------------------------------------------------------------
1925 
1926  unsigned int a = opts.basePSize;
1927 
1928  int largestSize = round(a*opts.scales[opts.scales.size()-1]);
1929  largestSize = largestSize%2 ? largestSize : largestSize+1; // round to the closest odd number
1930  int hLargestSize = largestSize/2;
1931 
1932  unsigned int npSize;
1933  unsigned int hpSize;
1934 
1935  for( CFeatureList::iterator it = list.begin(); it != list.end(); ++it )
1936  {
1937  // We don't take into account the matching if it is too close to the image border (the largest patch cannot be extracted)
1938  if( (*it)->x+hLargestSize > smLeftImg.getWidth()-1 || (*it)->x-hLargestSize < 0 ||
1939  (*it)->y+hLargestSize > smLeftImg.getHeight()-1 || (*it)->y-hLargestSize < 0 )
1940  continue;
1941 
1942  // We have found a proper match to obtain the multi-descriptor
1943  tlogger.enter("cp scales");
1944  (*it)->multiScales.resize( opts.scales.size() );
1945  (*it)->multiOrientations.resize( opts.scales.size() );
1946 
1947  // Copy the scale values within the feature.
1948  memcpy( &(*it)->multiScales[0], &opts.scales[0], opts.scales.size()*sizeof(double) );
1949  tlogger.leave("cp scales");
1950 
1951  // For each of the involved scales
1952  for( unsigned int k = 0; k < opts.scales.size(); ++k )
1953  {
1954  npSize = round(a*opts.scales[k]);
1955  npSize = npSize%2 ? npSize : npSize+1; // round to the closest odd number
1956  hpSize = npSize/2; // half size of the patch
1957 
1958  CImage tPatch;
1959 
1960  // LEFT IMAGE:
1961  tlogger.enter("extract & resize");
1962  smLeftImg.extract_patch( tPatch, (*it)->x-hpSize, (*it)->y-hpSize, npSize, npSize );
1963 
1964  cv::Mat out_mat_patch;
1965  // The size is a+2xa+2 because we have to compute the gradient (magnitude and orientation) in every pixel within the axa patch so we need
1966  // one more row and column. For instance, for a 23x23 patch we need a 25x25 patch.
1967  cv::resize( cv::cvarrToMat(tPatch.getAs<IplImage>(),false), out_mat_patch, cv::Size(a+2,a+2) );
1968  IplImage aux_img = IplImage(out_mat_patch);
1969  CImage rsPatch( &aux_img );
1970  tlogger.leave("extract & resize");
1971 
1972  tlogger.enter("main orientations");
1973  // Compute the main orientations for the axa patch, taking into account that the actual patch has a size of a+2xa+2
1974  // (being the center point the one with coordinates a/2+1,a/2+1).
1975  // A sigma = opts.sg2 is used to smooth the entries in the orientations histograms
1976  // Orientation vector will be resize inside the function, so don't reserve space for it
1978  rsPatch, a/2+1, a/2+1, a,
1979  (*it)->multiOrientations[k], opts.sg2 );
1980  tlogger.leave("main orientations");
1981 
1982  } // end scales for
1983  } // end matches for
1984  MRPT_END
1985 #else
1986  THROW_EXCEPTION("This function needs OpenCV 2.1+")
1987 #endif
1988 } // end computeMultiOrientations
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
float getAsFloat(unsigned int col, unsigned int row, unsigned int channel) const
Returns the contents of a given pixel at the desired channel, in float format: [0,255]->[0,1] The coordinate origin is pixel(0,0)=top-left corner of the image.
Definition: CImage.cpp:942
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
void VISION_IMPEXP setProperScales(const CFeaturePtr &feat1, const CFeaturePtr &feat2, int &firstScale, int &lastScale)
Computes the initial and final scales where to look when finding a match between multi-resolution fea...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define min(a, b)
bool VISION_IMPEXP computeMainOrientations(const mrpt::utils::CImage &image, const unsigned int x, const unsigned int y, const unsigned int patchSize, std::vector< double > &orientations, const double &sigma)
Computes the main orientations (within 80% of the peak value of orientation histogram) of a certain p...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
void VISION_IMPEXP computeMultiResolutionDescriptors(const mrpt::utils::CImage &imageLeft, const mrpt::utils::CImage &imageRight, CMatchedFeatureList &matchedFeats, const TMultiResDescOptions &opts)
Computes the multi-resolution SIFT-like descriptor of a set of matched features.
void BASE_IMPEXP pause(const std::string &msg=std::string("Press any key to continue...")) MRPT_NO_THROWS
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:435
size_t size() const
Definition: CFeature.h:280
#define THROW_EXCEPTION(msg)
GLenum GLsizei n
Definition: glext.h:4618
Scalar * iterator
Definition: eigen_plugins.h:23
Struct containing the output after matching multi-resolution SIFT-like descriptors.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
bool VISION_IMPEXP computeGradient(const mrpt::utils::CImage &image, const unsigned int x, const unsigned int y, double &mag, double &ori)
Computes the gradient of certain pixel within the image.
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
GLenum GLsizei GLenum GLenum const GLvoid * image
Definition: glext.h:3522
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:262
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glext.h:3545
std::vector< int > secondListCorrespondences
Contains the indexes within the first list corresponding to the second one.
STL namespace.
#define M_PI
Definition: bits.h:78
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void kdTreeNClosestPoint2DIdx(float x0, float y0, size_t knn, std::vector< size_t > &out_idx, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes...
std::vector< double > firstListDistance
Contains the distances between the descriptors.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
const T * getAs() const
Returns a pointer to a const T* containing the image - the idea is to call like "img.getAs<IplImage>()" so we can avoid here including OpenCV&#39;s headers.
Definition: CImage.h:517
void VISION_IMPEXP computeHistogramOfOrientations(const mrpt::utils::CImage &image, const unsigned int x, const unsigned int y, const unsigned int patchSize, const double &orientation, std::vector< int32_t > &descriptor, const TMultiResDescOptions &opts, std::vector< int32_t > &hashCoeffs)
Computes the SIFT-like descriptor of a certain point within an image at the base scale, i.e.
#define M_2PI
Definition: mrpt_macros.h:380
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
double baseline
Intrinsic stereo pair parameters for computing the depth of the feature.
bool computeHashCoeffs
Whether or not compute the coefficients for mantaining a HASH table of descriptors (for relocalizatio...
std::vector< int > firstListFoundScales
Contains the scales of the first list where the correspondence was found.
void getBothFeatureLists(CFeatureList &list1, CFeatureList &list2)
Returns the matching features as two separate CFeatureLists.
Definition: CFeature.cpp:1182
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
uint32_t basePSize
The size of the base patch.
#define MRPT_END
uint32_t timesSeenThreshold
The minimum number of frames for a certain feature to be considered stable.
const GLubyte * c
Definition: glext.h:5590
uint32_t lastSeenThreshold
The allowed number of frames since a certain feature was seen for the last time.
bool useDepthFilter
Whether or not use the filter based on the depth test.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
TMultiResMatchingOutput VISION_IMPEXP relocalizeMultiDesc(const mrpt::utils::CImage &image, CFeatureList &baseList, CFeatureList &currentList, TQuantizationTable &qTable, const TMultiResDescOptions &desc_opts, const TMultiResDescMatchOptions &match_opts)
double sg3
The sigmas for the Gaussian kernels.
void VISION_IMPEXP interpolateHistEntry(std::vector< double > &hist, const double &cbin, const double &rbin, const double &obin, const double &mag, const int d, const int n)
Inserts the orientation value of a certain pixel within the keypoint neighbourhood into the histogram...
Classes for computer vision, detectors, features, etc.
std::vector< int > firstListCorrespondences
Contains the indexes within the second list corresponding to the first one.
#define DEG2RAD
uint64_t TFeatureID
Definition of a feature ID.
double cropValue
The SIFT-like descriptor is cropped at this value during normalization.
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:211
int VISION_IMPEXP computeMoreDescriptors(const mrpt::utils::CImage &image, const CFeaturePtr &inputFeat, CFeaturePtr &outputFeat, const bool &lowerScales, const TMultiResDescOptions &opts)
Computes more multi-resolution SIFT-like descriptors for a feature using its position in a new image...
uint32_t searchAreaSize
Size of the squared area where to search for a match.
#define MRPT_START
std::vector< double > scales
The set of scales relatives to the base patch.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool computeDepth
Whether or not to compute the depth of the feature.
void VISION_IMPEXP insertHashCoeffs(const CFeaturePtr &feat, TQuantizationTable &qTable)
double oriThreshold
The threshold for the orientation test.
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
void meanAndStd(const VECTORLIKE &v, double &out_mean, double &out_std, bool unbiased=true)
Computes the standard deviation of a vector.
Struct containing the options when computing the multi-resolution SIFT-like descriptors.
void VISION_IMPEXP updateBaseList(CFeatureList &baseList, const CFeatureList &currentList, const std::vector< int > &idx)
void VISION_IMPEXP saveQTableToFile(const TQuantizationTable &qTable, const std::string &filename)
std::map< int, std::map< int, std::map< int, std::deque< std::pair< TFeatureID, double > > > > > TQuantizationTable
void extract_patch(CImage &patch, const unsigned int col=0, const unsigned int row=0, const unsigned int width=1, const unsigned int height=1) const
Extract a patch from this image, saveing it into "patch" (its previous contents will be overwritten)...
Definition: CImage.cpp:1368
#define ASSERT_(f)
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
Struct containing the options when matching multi-resolution SIFT-like descriptors.
TInternalFeatList::iterator iterator
Definition: CFeature.h:261
void VISION_IMPEXP checkScalesAndFindMore(CMatchedFeatureList &baseList, const CFeatureList &currentList, const mrpt::utils::CImage &currentImage, const TMultiResMatchingOutput &output, const TMultiResDescOptions &computeOpts, const TMultiResDescMatchOptions &matchOpts)
GLenum GLint GLint y
Definition: glext.h:3516
double matchingThreshold
The absolute threshold in descriptor distance for considering a match.
GLsizeiptr size
Definition: glext.h:3779
void VISION_IMPEXP computeMultiOrientations(const mrpt::utils::CImage &image, CFeatureList &list, const TMultiResDescOptions &opts)
Computes the multi-resolution SIFT-like descriptor of a list of features.
CFeaturePtr getByID(const TFeatureID &ID) const
Get a reference to a Feature from its ID.
Definition: CFeature.cpp:1006
GLuint res
Definition: glext.h:6298
GLenum GLint x
Definition: glext.h:3516
const int FEAT_FREE
bool blurImage
Whether or not to blur the image previously to compute the descriptors.
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
void VISION_IMPEXP normalizeImage(const mrpt::utils::CImage &image, mrpt::utils::CImage &nimage)
Normalizes the brigthness and contrast of an image by setting its mean value to zero and its standard...
size_t getWidth() const MRPT_OVERRIDE
Returns the width of the image in pixels.
Definition: CImage.cpp:855
size_t getHeight() const MRPT_OVERRIDE
Returns the height of the image in pixels.
Definition: CImage.cpp:884
TMultiResMatchingOutput VISION_IMPEXP matchMultiResolutionFeatures(const CFeatureList &list1, CFeatureList &list2, const mrpt::utils::CImage &rightImage, const TMultiResDescMatchOptions &matchOpts, const TMultiResDescOptions &computeOpts)
Matches two CFeatureList containing mulit-resolution descriptors.
A list of features.
Definition: CFeature.h:361



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