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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019