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-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #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/system/CTicTac.h>
25 #include <mrpt/system/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 #ifdef _WIN32
35 #include <process.h>
36 #include <windows.h> // TODO: This is temporary!!!
37 #endif
38 
39 using namespace mrpt;
40 using namespace mrpt::vision;
41 using namespace mrpt::img;
42 // using namespace mrpt::maps;
43 using namespace mrpt::math;
44 using namespace mrpt::system;
45 using namespace std;
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  using TQuantizationTable =
103  map<int, map<int, map<int, deque<pair<TFeatureID, double>>>>>;
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][n]
226  .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 nit =
241  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 nit =
265  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
301  ->multiOrientations[k1][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(baseFeat->descriptors
330  .multiSIFTDescriptors
331  [k1][k3][d]) -
332  float(
333  (*it)
334  ->descriptors
335  .multiSIFTDescriptors[0][k4]
336  [d]));
337 
338  // cout
339  // <<
340  // nit->first
341  // << "["
342  // <<
343  // baseFeat->multiScales[k1]
344  // << "]
345  // - ori:
346  // " <<
347  // baseFeat->multiOrientations[k1][k3]
348  // << " -
349  // HASH:
350  // " <<
351  // baseFeat->multiHashCoeffs[k1][k3]
352  // << " -
353  // dist:
354  // " <<
355  // dist
356  // <<
357  // endl;
358  if (dist < minDist)
359  {
360  minDist = dist; // minimun distance of
361  // the features
362  minBaseFeat = baseIdx; // index of the
363  // base feature
364  minBaseScl =
365  k1; // scale of the base feature
366  } // end-if
367  } // end-for
368  } // end-for
369  } // end-if
370  } // end-for-k2
371  } // end-for-k1
372  // if( baseFeat->ID == 32 && (*it)->ID == 9 )
373  // mrpt::system::pause();
374  } // end-for-nit
375  if (minDist < match_opts.matchingThreshold)
376  {
377  // We've found a match
378  // Store the index of the current feature
379  output.firstListCorrespondences[minBaseFeat] = curCounter;
380  output.firstListFoundScales[minBaseFeat] = minBaseScl;
381  output.firstListDistance[minBaseFeat] = minDist;
382  output.secondListCorrespondences[curCounter] = minBaseFeat;
383  output.nMatches++;
384  }
385  } // end-for orientations
386  // if( (*it)->ID == 9 )
387  // {
388  // baseList.getByID(32)->dumpToConsole();
389  // (*it)->dumpToConsole();
390  // mrpt::system::pause();
391  // }
392 
393  } // end-for-it
394 
395  return output;
396 
397  MRPT_END
398 } // end-relocalizeMultiDesc
399 
400 /*-------------------------------------------------------------
401  updateBaseList
402 -------------------------------------------------------------*/
403 // Updates the following parameters for each feature in Base List:
404 // Coordinates, the number of frames it has been seen in, the number of frames
405 // since the last time it was seen and
406 // the number of frames it has not been seen. This is done according to the
407 // information in vector "idx" and the
408 // positions of the features in current list.
409 void vision::updateBaseList(
410  CFeatureList& baseList, const CFeatureList& currentList,
411  const vector<int>& idx)
412 {
413  MRPT_START
414  ASSERT_(idx.size() == baseList.size());
415 
416  size_t sz = idx.size();
417 
418  CVectorDouble dp(sz), my(sz);
419  int counter = 0;
420  for (int k = 0; k < (int)sz; ++k)
421  {
422  if (idx[k] == FEAT_FREE)
423  {
424  baseList[k]->nTimesNotSeen++;
425  baseList[k]->nTimesLastSeen++;
426  }
427  else
428  {
429  baseList[k]->nTimesSeen++;
430  baseList[k]->nTimesLastSeen = 0;
431 
432  // Update position!
433  dp[k] = fabs(baseList[k]->depth - currentList[idx[k]]->depth);
434  // cout << "dp:" << dp[k] << endl;
435 
436  counter++;
437  } // end-else
438  } // end-for
439 
440  double m_dp, std_dp;
441  mrpt::math::meanAndStd(dp, m_dp, std_dp);
442  cout << "mean&std: " << m_dp << "," << std_dp << endl;
443 
444  for (int k = 0; k < (int)idx.size(); ++k)
445  {
446  if (idx[k] != FEAT_FREE)
447  {
448  // Update position!
449  if (dp[k] < (m_dp + std_dp))
450  {
451  baseList[k]->x = currentList[idx[k]]->x;
452  baseList[k]->y = currentList[idx[k]]->y;
453  baseList[k]->p3D = currentList[idx[k]]->p3D;
454  }
455  } // end-if
456  } // end-for
457 
458  MRPT_END
459 } // updateBaseList
460 
461 /*-------------------------------------------------------------
462  checkScalesAndFindMore
463 -------------------------------------------------------------*/
464 // Checks the scales where the matched were found and find more descriptors if
465 // they are located at the boundaries of
466 // the scale range o a certain feature within base list.
467 // It also deletes the features in base list which are have not been seen for a
468 // long time and it wasn't seen enough.
469 void vision::checkScalesAndFindMore(
470  CMatchedFeatureList& baseList, const CFeatureList& currentList,
471  const CImage& currentImage, const TMultiResMatchingOutput& output,
472  const TMultiResDescOptions& computeOpts,
473  const TMultiResDescMatchOptions& matchOpts)
474 {
475  CMatchedFeatureList::iterator itBase;
476  int m = 0;
477  // cout << "Base: " << baseList.size() << " and output: " <<
478  // output.firstListCorrespondences.size() << endl;
479  for (itBase = baseList.begin(); itBase != baseList.end(); ++m)
480  {
481  // cout << m << " was last seen " <<
482  // itBase->->first->nTimesLastSeen << " frames ago and has been
483  // seen " << itBase->first->nTimesSeen << " times so far." <<
484  // endl;
485  if (itBase->first->nTimesLastSeen > matchOpts.lastSeenThreshold &&
486  itBase->first->nTimesSeen < matchOpts.timesSeenThreshold)
487  {
488  // cnt++;
489  // cout << "Deleted feature #" << m << endl;
490  itBase = baseList.erase(itBase);
491  //++itBase;
492  }
493  else
494  {
495  // We've found a tracked match
496  // We have found the match in both frames! Check out the scales!
497  // if( !(m < (int)output.firstListCorrespondences.size()
498  // ))
499  // {
500  // cout << m << " vs " <<
501  // (int)output.firstListCorrespondences.size() <<
502  // endl;
503  // ASSERT_( m <
504  // (int)output.firstListCorrespondences.size() );
505  // }
506 
507  int tidx = output.firstListCorrespondences[m];
508  // ASSERT_( tidx < (int)currentList.size() );
509 
510  if (tidx != FEAT_FREE)
511  {
512  if (output.firstListFoundScales[m] == 0)
513  {
514  cout << "Left feature " << m << " found in scale 0!"
515  << endl;
516  // currentImage.saveToFile("imgs/current.jpg");
517  // cout << "px: "<< currentList[tidx]->x
518  // << ","<< currentList[tidx]->y << endl;
519  int res = computeMoreDescriptors(
520  currentImage, currentList[tidx], itBase->first, true,
521  computeOpts);
522  ASSERT_(
523  itBase->first->descriptors.hasDescriptorMultiSIFT());
524  if (res == 0)
525  cout << "LF LOWSCALES Out of bounds!!" << endl;
526  // mrpt::system::pause();
527  }
528  else
529  {
530  size_t nscales = itBase->first->multiScales.size() - 1;
531  // itBase->first->dumpToConsole();
532  if (output.firstListFoundScales[m] == (int)nscales)
533  {
534  cout << "Left feature " << m << " found in last scale!"
535  << endl;
536  int res = computeMoreDescriptors(
537  currentImage, currentList[tidx], itBase->first,
538  false, computeOpts);
539  if (res == 0)
540  cout << "LF HIGHSCALES Out of bounds!!" << endl;
541  // mrpt::system::pause();
542  }
543  }
544  } // end-if
545  ++itBase;
546  } // end-else
547  } // end-for
548 } // checkScalesAndFindMore
549 
550 /*-------------------------------------------------------------
551  computeGradient
552 -------------------------------------------------------------*/
553 // computeGradient.- Computes both the (approximated) gradient magnitude and
554 // orientation for a certain point within the image
555 // return: true = OK; false = if the keypoint is located at the image border
556 // (where the gradient cannot be computed).
557 // x = col, y = row
558 bool vision::computeGradient(
559  const CImage& image, const unsigned int x, const unsigned int y,
560  double& mag, double& ori)
561 {
562  if (x > 0 && x < image.getWidth() - 1 && y > 0 && y < image.getHeight() - 1)
563  {
564  float d1, d2;
565  //----------
566  //| 0|-1| 0|
567  //----------
568  //|-1| 0| 1|
569  //----------
570  //| 0| 1| 0|
571  //----------
572 
573  // According to Hess's implementation (Lowe does: d2 =
574  // image.getAsFloat(x,y+1) - px4 = image.getAsFloat(x,y-1); )
575  d1 = image.getAsFloat(x + 1, y) - image.getAsFloat(x - 1, y);
576  d2 = image.getAsFloat(x, y - 1) - image.getAsFloat(x, y + 1);
577 
578  mag = sqrt(d1 * d1 + d2 * d2);
579  ori = atan2(d2, d1); // From -pi to pi
580  return true;
581  }
582  else
583  {
584  cout << "[computeGradient]: Out of bounds in " << x << "," << y
585  << " with image: " << image.getWidth() << "x" << image.getHeight()
586  << endl;
587  return false;
588  }
589 } // end-computeGradient
590 
591 /*-------------------------------------------------------------
592  computeMainOrientations
593 -------------------------------------------------------------*/
594 // Compute a set of main orientations (if there are more than one) of a certain
595 // keypoint within the image.
596 // return: true = OK; false = keypoint is too close the image margin (there is
597 // no space for the whole patch)
598 bool vision::computeMainOrientations(
599  const CImage& image, const unsigned int x, const unsigned int y,
600  const unsigned int patchSize, std::vector<double>& orientations,
601  const double& sigma)
602 {
603  MRPT_START
604 
605  // Pre operations
606  orientations.clear();
607 
608  // Local variables:
609  const unsigned int NBINS = 36;
610  const int hPatchSize = patchSize / 2;
611 
612  vector<double> oris(NBINS, 0.0);
613  int mx = (int)x, my = (int)y;
614 
615  // Check if there's no margin problems when computing the orientation
616  if (mx - (hPatchSize + 1) < 0 || my - (hPatchSize + 1) < 0 ||
617  mx + (hPatchSize + 1) > (int)(image.getWidth() - 1) ||
618  my + (hPatchSize + 1) > (int)(image.getHeight() - 1))
619  return false; // Feature is too close to the image's border
620 
621  // For each pixel within the patch (patchSize should be 29x29):
622  // Compute local gradients (magnitude and orientation)
623  // The orientation histogram is weighted with a Gaussian with sigma = 7.5
624  // (Cheklov's Thesis)
625  // cout << "IM: " << image.getWidth() << " and PS: " << patchSize <<
626  // endl;
627  double exp_denom = 2.0 * sigma * sigma;
628  double bin_size = M_2PI / NBINS;
629  double mag, ori;
630  for (int ii = -hPatchSize; ii <= hPatchSize; ++ii)
631  for (int jj = -hPatchSize; jj <= hPatchSize; ++jj)
632  {
633  if (computeGradient(image, mx + ii, my + jj, mag, ori))
634  {
635  ori += M_PI; // ori: from 0 to 2*pi
636  double w = mag * exp(-(ii * ii + jj * jj) / exp_denom);
637 
638  int bin =
639  ((int)(floor(ori / bin_size))) % NBINS; // from 0 to 35
640  double bin_center = bin * bin_size;
641  double dif = ori - bin_center;
642  int nxbin;
643  if (dif > 0)
644  nxbin =
645  bin == NBINS - 1
646  ? 0
647  : bin +
648  1; // the other involved bin is the next one
649  else
650  nxbin = bin == 0 ? NBINS - 1 : bin - 1; // the other
651  // involved bin is
652  // the previous one
653 
654  double nbin_center = nxbin * bin_size;
655  double dif2 = ori - nbin_center;
656 
657  oris[bin] += w * fabs(dif2) / bin_size; // dif2 == 0 means that
658  // "ori" is equal to
659  // "bin_center"
660  oris[nxbin] += w * fabs(dif) / bin_size; // dif == 0 means that
661  // "ori" is equal to
662  // "nbin_center"
663  } // end-if
664  else
665  return false;
666  } // end
667  // Search for the maximum
668  double mxori = 0.0;
669  for (unsigned int k = 0; k < oris.size(); ++k)
670  {
671  if (oris[k] > mxori)
672  {
673  mxori = oris[k];
674  }
675  } // end-for
676 
677  // Compute the peaks of the histogram of orientations
678  double hist_mag_th = 0.8 * mxori;
679  for (unsigned int k = 0; k < oris.size(); ++k)
680  {
681  double pv = k == 0 ? oris[oris.size() - 1]
682  : oris[k - 1]; // Previous peak of the histogram
683  double nv = k == oris.size() - 1
684  ? 0
685  : oris[k + 1]; // Next peak of the histogram
686 
687  if (oris[k] > pv && oris[k] > nv &&
688  oris[k] > hist_mag_th) // It this peak is maximum and is over 0.8
689  // of the maximum peak
690  {
691  // Check this formulae:
692  // double A = (pv-nv)/(4*oris[k]+2*nv-2*pv-4*oris[k]);
693  // double peak = A/(1+2*A);
694 
695  // Interpolate the position of the peak
696  double int_bin =
697  k +
698  0.5 * (pv - nv) / (pv - 2.0 * oris[k] + nv); // Hess formulae
699  int_bin = (int_bin < 0)
700  ? NBINS + int_bin
701  : (int_bin >= NBINS) ? int_bin - NBINS : int_bin;
702  double int_ori =
703  ((M_2PI * int_bin) / NBINS) - M_PI; // Back to -pi:pi
704  orientations.push_back(int_ori);
705 
706  // cout << "Ori found: " << int_ori << endl;
707  }
708  }
709  return true;
710  MRPT_END
711 } // end vision::computeMainOrientation
712 
713 /*-------------------------------------------------------------
714  interpolateHistEntry
715 -------------------------------------------------------------*/
716 void vision::interpolateHistEntry(
717  vector<double>& oris, const double& cbin, const double& rbin,
718  const double& obin, const double& mag, const int d, const int n)
719 {
720  // Insert the sample into the orientation histogram taking into account that
721  // there is an overlapping
722  // of half a bin between consecutive bins.
723  // [And the first one overlaps with the last one??? --> Apparently not]
724 
725  CTimeLogger logger;
726  logger.disable();
727 
728  double ncbin = cbin + d / 2. - 0.5;
729  double nrbin = rbin + d / 2. - 0.5;
730 
731  int ncbin_i = floor(ncbin);
732  int nrbin_i = floor(nrbin);
733  int nobin_i = floor(obin);
734 
735  double d_c = ncbin_i - ncbin;
736  double d_r = nrbin_i - nrbin;
737  double d_o = nobin_i - obin;
738 
739  for (int k = 0; k < 2; ++k)
740  for (int m = 0; m < 2; ++m)
741  for (int l = 0; l < 2; ++l)
742  if (ncbin_i + k >= 0 && ncbin_i + k < d && nrbin_i + m >= 0 &&
743  nrbin_i + m < d)
744  {
745  int idx = ((nobin_i + l) % n) + n * (ncbin_i + k) +
746  n * d * (nrbin_i + m);
747  oris[idx] += mag * (1 - fabs(d_c + k)) *
748  (1 - fabs(d_r + m)) * (1 - fabs(d_o + l));
749  }
750 
751  // // Spatial bin
752  // std::vector<int> colIndex, rowIndex;
753  // std::vector<double> colBinDistance, rowBinDistance;
754  //
755  // logger.enter("Main loop");
756  // colIndex.reserve(2);
757  // colBinDistance.reserve(2);
758  // rowIndex.reserve(2);
759  // rowBinDistance.reserve(2);
760  // for( int bin = 0; bin < d; bin++ )
761  // {
762  // double binCenter = bin-1.5; // Center
763  // of the bin
764  // double colDistance = fabs( cbin - binCenter ); //
765  // Distance to the center of the bin
766  // if( colDistance < 1.0 ) // If it
767  // is close enough
768  // {
769  // colIndex.push_back( bin );
770  // colBinDistance.push_back( 1-colDistance );
771  // }
772  //
773  // double rowDistance = fabs( rbin - binCenter );
774  // if( rowDistance < 1.0 )
775  // {
776  // rowIndex.push_back( bin );
777  // rowBinDistance.push_back( 1-rowDistance );
778  // }
779  //
780  // if( colIndex.size() == 2 && rowIndex.size() == 2 ) // We
781  // have found all we need (stop looping)
782  // break;
783  // } // end for
784  //
785  // logger.leave("Main loop");
786  // ASSERT_( colIndex.size() <= 2 && rowIndex.size() <= 2 );
787  //
788  // // Orientation bin
789  // std::vector<int> oriIndex(2);
790  // std::vector<double> oriBinDistance(2);
791  // oriIndex[0] = floor( obin );
792  // oriIndex[1] = oriIndex[0] == n-1 ? 0 : oriIndex[0]+1;
793  // oriBinDistance[0] = 1 - (obin-oriIndex[0]);
794  // oriBinDistance[1] = 1 - oriBinDistance[0];
795  //
796  // // The entry can affect to 2 (1x1x2), 4 (2x1x2 or 1x2x2) or 8 (2x2x2)
797  // bins
798  // // Insert into the histogram
799  // logger.enter("Insertion in histogram");
800  // for( unsigned int k = 0; k < colIndex.size(); ++k )
801  // for( unsigned int l = 0; l < rowIndex.size(); ++l )
802  // for( unsigned int m = 0; m < 2; ++m )
803  // {
804  // if( colIndex[k] >= 0 && rowIndex[l] >= 0 )
805  // {
806  // unsigned int idx = (unsigned int)(oriIndex[m] +
807  // n*colIndex[k] + n*d*rowIndex[l] );
808  // oris[idx] +=
809  // mag*colBinDistance[k]*rowBinDistance[l]*oriBinDistance[m];
810  // } // end if
811  // } // end for
812  // logger.leave("Insertion in histogram");
813 } // end of interpolateHistEntry
814 
815 /*-------------------------------------------------------------
816  computeHistogramOfOrientations
817 -------------------------------------------------------------*/
818 // x = col, y = row
819 void vision::computeHistogramOfOrientations(
820  const CImage& image, const unsigned int x, const unsigned int y,
821  const unsigned int patchSize, const double& orientation,
822  vector<int32_t>& descriptor, const TMultiResDescOptions& opts,
823  vector<int32_t>& hashCoeffs)
824 {
825  MRPT_START
826  CTimeLogger tlogger;
827  tlogger.disable();
828 
829  // The patch is 29x29
830  int w = 16; // width of the used patch
831  int Bp = 4; // Number of spatial bins
832  int n = 8; // Number of frequential bins (per spatial bin) --> Descriptor
833  // size: 4 x 4 x 8 = 128
834  double cos_t = cos(orientation);
835  double sin_t = sin(orientation);
836  double bins_per_rad = n / M_2PI;
837  double exp_denom = opts.sg3 * opts.sg3 * 2;
838  int radius = 0.5 * patchSize;
839  vector<double> oris(Bp * Bp * n, 0.0); // Typically 128-D
840 
841  // Normalize patch
842  CImage nimage, thePatch;
843  if (opts.computeHashCoeffs)
844  {
845  image.extract_patch(
846  thePatch, x - radius, y - radius, patchSize, patchSize);
847  normalizeImage(
848  thePatch,
849  nimage); // Normalize to zero-mean and unit standard deviation
850  }
851 
852  // For each pixel in the 23x23 patch:
853  // a. find its rotated position
854  // b. determine its proper spatial bin (and the other affected one)
855  // c. rotate its orientation with respect the main orientation
856  // d. determine its frequential bin
857  // e. compute the weight
858  // const double kp = ((double)(Bp+1))/((double)w); // [0.31250] Constant
859  // for mapping between -8,8 -> -2.5,2.5
860  const double kp =
861  double(Bp) /
862  double(w); // [0.25] Constant for mapping between -8,8 -> -2,2
863  double mag, ori;
864  double cbin, rbin, obin;
865  double c1 = 0.0, c2 = 0.0, c3 = 0.0;
866 
867  // FILE *f1 = mrpt::system::os::fopen( "imgs/c1p.txt", "wt" );
868  // FILE *f2 = mrpt::system::os::fopen( "imgs/c1n.txt", "wt" );
869  // FILE *f3 = mrpt::system::os::fopen( "imgs/c2p.txt", "wt" );
870  // FILE *f4 = mrpt::system::os::fopen( "imgs/c2n.txt", "wt" );
871  // FILE *f5 = mrpt::system::os::fopen( "imgs/c3p.txt", "wt" );
872  // FILE *f6 = mrpt::system::os::fopen( "imgs/c3n.txt", "wt" );
873  // FILE *f7 = mrpt::system::os::fopen( "imgs/c0.txt", "wt" );
874  // FILE *f8 = mrpt::system::os::fopen( "imgs/c3.txt", "wt" );
875  // FILE *f9 = mrpt::system::os::fopen( "imgs/r0.txt", "wt" );
876  // FILE *f10 = mrpt::system::os::fopen( "imgs/r3.txt", "wt" );
877  // FILE *f11 = mrpt::system::os::fopen( "imgs/out.txt", "wt" );
878 
879  for (int c = -radius; c <= radius; ++c)
880  for (int r = -radius; r <= radius; ++r)
881  {
882  cbin = kp * c * cos_t - kp * r * sin_t;
883  rbin = kp * c * sin_t + kp * r * cos_t;
884 
885  if (cbin > -2.5 && cbin < 2.5 && rbin > -2.5 && rbin < 2.5)
886  {
887  tlogger.enter("computeGradient");
888  bool res =
889  vision::computeGradient(image, x + c, y + r, mag, ori);
890  tlogger.leave("computeGradient");
891  if (res)
892  {
893  ori -= orientation;
894  while (ori < 0.0) ori += M_2PI;
895  while (ori >= M_2PI) ori -= M_2PI;
896 
897  obin = ori * bins_per_rad;
898  w = exp(-(c * c + r * r) / exp_denom);
899  tlogger.enter("interpolate");
900  vision::interpolateHistEntry(
901  oris, cbin, rbin, obin, mag, Bp, n);
902  tlogger.leave("interpolate");
903  } // end if
904 
905  // if( cbin < -0.5 )
906  // mrpt::system::os::fprintf( f7, "%d %d\n",
907  // y+r, x+c );
908  // if( cbin > 0.5 )
909  // mrpt::system::os::fprintf( f8, "%d %d\n",
910  // y+r, x+c );
911  // if( rbin < -0.5 )
912  // mrpt::system::os::fprintf( f9, "%d %d\n",
913  // y+r, x+c );
914  // if( rbin > 0.5 )
915  // mrpt::system::os::fprintf( f10, "%d %d\n",
916  // y+r, x+c );
917 
918  // Compute the hashing coefficients:
919  if (opts.computeHashCoeffs)
920  {
921  if (cbin < 0)
922  {
923  c1 += nimage.getAsFloat(c + radius, r + radius);
924  // mrpt::system::os::fprintf( f1,
925  //"%d
926  //%d\n", y+r, x+c );
927  }
928  else
929  {
930  c1 -= nimage.getAsFloat(c + radius, r + radius);
931  // mrpt::system::os::fprintf( f2,
932  //"%d
933  //%d\n", y+r, x+c );
934  }
935  if (rbin < 0)
936  {
937  c2 += nimage.getAsFloat(c + radius, r + radius);
938  // mrpt::system::os::fprintf( f3,
939  // "%d %d\n", y+r, x+c );
940  }
941  else
942  {
943  c2 -= nimage.getAsFloat(c + radius, r + radius);
944  // mrpt::system::os::fprintf( f4,
945  // "%d %d\n", y+r, x+c );
946  }
947  if ((cbin < 0 && rbin < 0) || (cbin > 0 && rbin > 0))
948  {
949  c3 += nimage.getAsFloat(c + radius, r + radius);
950  // mrpt::system::os::fprintf( f5,
951  // "%d %d\n", y+r, x+c );
952  }
953  else
954  {
955  c3 -= nimage.getAsFloat(c + radius, r + radius);
956  // mrpt::system::os::fprintf( f6,
957  // "%d %d\n", y+r, x+c );
958  }
959  } // end-if
960  } // end
961  // else
962  // mrpt::system::os::fprintf( f11, "%d %d\n", y+r,
963  // x+c );
964  } // end-for
965  // mrpt::system::os::fclose(f1);
966  // mrpt::system::os::fclose(f2);
967  // mrpt::system::os::fclose(f3);
968  // mrpt::system::os::fclose(f4);
969  // mrpt::system::os::fclose(f5);
970  // mrpt::system::os::fclose(f6);
971  // mrpt::system::os::fclose(f7);
972  // mrpt::system::os::fclose(f8);
973  // mrpt::system::os::fclose(f9);
974  // mrpt::system::os::fclose(f10);
975  // mrpt::system::os::fclose(f11);
976 
977  // mrpt::system::pause();
978 
979  if (opts.computeHashCoeffs)
980  {
981  hashCoeffs.resize(3);
982  hashCoeffs[0] = round(c1);
983  hashCoeffs[1] = round(c2);
984  hashCoeffs[2] = round(c3);
985 
986  // cout << "Hash Coeffs: " << hashCoeffs << endl;
987  }
988 
989  // Normalize
990  tlogger.enter("normalize");
991  double sum = 0.0;
992  for (unsigned int ii = 0; ii < oris.size(); ++ii)
993  sum += oris[ii] * oris[ii];
994  sum = 1 / sqrt(sum);
995  for (unsigned int ii = 0; ii < oris.size(); ++ii) oris[ii] *= sum;
996 
997  // Crop to "crop_value"
998  for (unsigned int ii = 0; ii < oris.size(); ++ii)
999  oris[ii] = min(opts.cropValue, oris[ii]);
1000 
1001  // Normalize again -> we have the descriptor!
1002  for (unsigned int ii = 0; ii < oris.size(); ++ii)
1003  sum += oris[ii] * oris[ii];
1004  sum = 1 / sqrt(sum);
1005  for (unsigned int ii = 0; ii < oris.size(); ++ii) oris[ii] *= sum;
1006 
1007  // Convert it to std::vector<int>
1008  descriptor.resize(oris.size());
1009  for (unsigned int ii = 0; ii < descriptor.size(); ++ii)
1010  descriptor[ii] = (int)(255.0f * oris[ii]);
1011 
1012  tlogger.leave("normalize");
1013 
1014  MRPT_END
1015 } // end vision::computeHistogramOfOrientations
1016 
1017 /*-------------------------------------------------------------
1018  matchMultiResolutionFeatures
1019 -------------------------------------------------------------*/
1020 TMultiResMatchingOutput vision::matchMultiResolutionFeatures(
1021  const CFeatureList& list1, CFeatureList& list2, const CImage& rightImage,
1022  const TMultiResDescMatchOptions& matchOpts,
1023  const TMultiResDescOptions& computeOpts)
1024 {
1025  MRPT_START
1026  // "List1" has a set of features with their depths computed
1027  // "List2" has a set of FAST features detected in the next frame (with their
1028  // depths)
1029  // We perform a prediction of the "List1" features and try to find its
1030  // matches within List2
1031  // --------------------------------------------------------
1032  // Algortihm summary:
1033  // --------------------------------------------------------
1034  // For each feature in List1 we find a search region: by now a fixed window
1035  // e.g. 20x20 pixels
1036  // TO DO: Non-maximal supression according to the feature score
1037  // From the remaining set, we compute the main orientation and check its
1038  // consistency with the List1 feature
1039  // NEW: compute the corresponding feature in right image and compute its
1040  // depth
1041  // From the remaining set, we compute the descriptor at scale 1.0 and
1042  // determine the set of scales where to look for
1043  // If the distance between descriptors is below a certain threshold, we've
1044  // found a match.
1045  // --------------------------------------------------------
1046 
1047  // General variables
1048  CTimeLogger logger;
1049  logger.disable();
1050 
1051  TMultiResMatchingOutput output;
1052 
1053  // Preliminary tasks
1054  output.firstListCorrespondences.resize(list1.size(), FEAT_FREE);
1055  output.secondListCorrespondences.resize(list2.size(), FEAT_FREE);
1056  output.firstListFoundScales.resize(list1.size(), -1);
1057  output.firstListDistance.resize(list1.size(), 1.0);
1058 
1059  // Local variables
1060  int hWindow =
1061  matchOpts.searchAreaSize / 2; // Half of the search window width
1062  int mxsize = hWindow + 2 * matchOpts.lastSeenThreshold;
1063  int imageW = rightImage.getWidth(); // Image width
1064  int imageH = rightImage.getHeight(); // Image height
1065  int leftFeatCounter = 0;
1066  int rightFeatCounter = 0; // Counter for features
1067  int patchSize = computeOpts.basePSize;
1068  int minScale;
1069  double maxResponse;
1070  output.nMatches = 0;
1071  int ridx = 0;
1072 
1073  CFeatureList::const_iterator it1;
1074  CFeatureList::iterator it2;
1075  // cout << "Starting the loop" << endl;
1076  cout << endl;
1077  for (leftFeatCounter = 0, it1 = list1.begin(); it1 != list1.end();
1078  ++it1, ++leftFeatCounter)
1079  {
1080  if (!(*it1)->descriptors.hasDescriptorMultiSIFT()) continue;
1081  // cout << (*it1)->ID << " with " << endl;
1082  double sRegionX0 =
1083  max((*it1)->x - min((hWindow + 2 * (*it1)->nTimesLastSeen), mxsize),
1084  0.0f);
1085  double sRegionX1 =
1086  min((*it1)->x + min((hWindow + 2 * (*it1)->nTimesLastSeen), mxsize),
1087  (float)imageW);
1088  double sRegionY0 =
1089  max((*it1)->y - min((hWindow + 2 * (*it1)->nTimesLastSeen), mxsize),
1090  0.0f);
1091  double sRegionY1 =
1092  min((*it1)->y + min((hWindow + 2 * (*it1)->nTimesLastSeen), mxsize),
1093  (float)imageH);
1094 
1095  int minDist = 1e6;
1096  // int minIdx = -1;
1097  minScale = -1;
1098  maxResponse = 0;
1099  ridx = 0;
1100 
1101  for (rightFeatCounter = 0, it2 = list2.begin(); it2 != list2.end();
1102  ++it2, ++rightFeatCounter)
1103  {
1104  // if( (*it2)->ID == 193 )
1105  // {
1106  // cout << (*it1)->ID << " with " << (*it2)->ID <<
1107  // endl;
1108  // mrpt::system::pause();
1109  // }
1110 
1111  // FILTER 1: Search region
1112  if ((*it2)->x < sRegionX0 || (*it2)->x > sRegionX1 ||
1113  (*it2)->y < sRegionY0 || (*it2)->y > sRegionY1)
1114  {
1115  // if( (*it2)->ID == 193 )
1116  // {
1117  // cout << (*it2)->ID << " OOB" << endl;
1118  // }
1119  continue;
1120  }
1121 
1122  // cout << " " << (*it2)->ID << endl;
1123 
1124  // Compute main orientations
1125  // logger.enter("cmorientation");
1126  // (*it2)->multiScales.push_back( 1.0 );
1127  // (*it2)->multiOrientations.resize( 1 );
1128  // (*it2)->descriptors.multiSIFTDescriptors.resize( 1 );
1129  // (*it2)->multiHashCoeffs.resize( 1 );
1130  vector<double> thisOris;
1131  if (!vision::computeMainOrientations(
1132  rightImage, (*it2)->x, (*it2)->y, patchSize, thisOris,
1133  computeOpts.sg2))
1134  {
1135  // if( (*it2)->ID == 193 )
1136  // {
1137  // cout << (*it2)->ID << " bad orientation
1138  // computed" << endl;
1139  // mrpt::system::pause();
1140  // }
1141  continue;
1142  }
1143  // logger.leave("cmorientation");
1144 
1145  // Compute the proper scales where to look for
1146  int firstScale, lastScale;
1147  if (matchOpts.useDepthFilter)
1148  vision::setProperScales((*it1), (*it2), firstScale, lastScale);
1149  else
1150  {
1151  firstScale = max(0, (int)matchOpts.lowScl1);
1152  lastScale =
1153  min((int)(*it1)->multiScales.size() - 1,
1154  (int)matchOpts.highScl1);
1155  }
1156 
1157  // if( (*it2)->ID == 193 )
1158  // {
1159  // cout << "Searching in scales: " << firstScale << "
1160  // to " << lastScale << endl;
1161  // mrpt::system::pause();
1162  // }
1163 
1164  // Search for consistency of the orientation
1165  for (int k1 = firstScale; k1 <= lastScale; ++k1)
1166  for (int k2 = 0; k2 < (int)(*it1)->multiOrientations[k1].size();
1167  ++k2)
1168  for (int k3 = 0; k3 < (int)thisOris.size();
1169  ++k3) // FILTER 2: Orientation
1170  if (fabs(
1171  (*it1)->multiOrientations[k1][k2] -
1172  thisOris[k3]) < matchOpts.oriThreshold ||
1173  fabs(
1174  M_2PI - fabs(
1175  (*it1)->multiOrientations[k1][k2] -
1176  thisOris[k3])) <
1177  matchOpts.oriThreshold)
1178  {
1179  // Orientation check passed
1180  // FILTER 3: Feature response
1181  // has it a better score than its 8 neighbours?
1182  // logger.enter("non-max");
1183  // TO DO: search the maximum number of neighbours up
1184  // to 8
1185  bool isMax = true;
1186  if (list2.size() >= 8)
1187  {
1188  std::vector<size_t> out_idx;
1189  std::vector<float> out_dist_sqr;
1190  maxResponse = (*it2)->response;
1191 
1192  list2.kdTreeNClosestPoint2DIdx(
1193  (*it2)->x, (*it2)->y, 8, out_idx,
1194  out_dist_sqr);
1195  for (size_t kdcounter = 0;
1196  kdcounter < out_idx.size(); ++kdcounter)
1197  {
1198  if (out_dist_sqr[kdcounter] > 1.4142)
1199  continue;
1200 
1201  if (list2[out_idx[kdcounter]]->response >
1202  maxResponse)
1203  {
1204  maxResponse =
1205  list2[out_idx[kdcounter]]->response;
1206  isMax = false;
1207  break;
1208  }
1209  }
1210  } // end-if
1211 
1212  if (!isMax)
1213  {
1214  cout << "NO ES MAX" << endl;
1215  mrpt::system::pause();
1216  continue;
1217  }
1218 
1219  // Candidate found at scale "k1" and orientation
1220  // "k2" (list1) and orientation "k3" (list2)
1221  // Compute descriptor for these conditions!
1222 
1223  // vector<int> desc,
1224  // hashC;
1225  // TMultiResDescOptions
1226  // auxOpts = computeOpts;
1227  // auxOpts.computeHashCoeffs
1228  // = false;
1229 
1230  // All the filters have been passed -> we can add
1231  // all the information into the feature
1232  // If it has a previous computed descriptor it is
1233  // substituted by this one
1234  if ((*it2)->multiScales.size() == 0)
1235  {
1236  (*it2)->multiScales.resize(1);
1237  (*it2)->multiScales[0] = 1.0;
1238  (*it2)->multiOrientations.resize(1);
1239  (*it2)->descriptors.multiSIFTDescriptors.resize(
1240  1);
1241  (*it2)->multiHashCoeffs.resize(1);
1242  }
1243 
1244  /* ORIENTATIONS */
1245  bool oriFound = false;
1246  int wh = 0;
1247  for (int co = 0;
1248  co < (int)(*it2)->multiOrientations[0].size();
1249  ++co)
1250  if ((*it2)->multiOrientations[0][co] ==
1251  thisOris[k3])
1252  {
1253  wh = co;
1254  oriFound = true;
1255  }
1256 
1257  int dist = 0;
1258  if (!oriFound) // The feature hasn't got this
1259  // orientation already -> compute
1260  // the descriptor & coeffs
1261  {
1262  // if( (*it2)->ID
1263  // == 193 )
1264  // cout <<
1265  // "Orientation
1266  // not found
1267  // -> compute
1268  // the
1269  // descriptor."
1270  // << endl;
1271 
1272  (*it2)->multiOrientations[0].push_back(
1273  thisOris[k3]);
1274 
1275  vector<int32_t> thisDesc, thisHash;
1276  computeHistogramOfOrientations(
1277  rightImage, (*it2)->x, (*it2)->y, patchSize,
1278  thisOris[k3], thisDesc, computeOpts,
1279  thisHash);
1280 
1281  /* DESCRIPTORS */
1282  (*it2)
1283  ->descriptors.multiSIFTDescriptors[0]
1284  .push_back(thisDesc);
1285  /* HASH COEFFICIENTS */
1286  (*it2)->multiHashCoeffs[0].push_back(thisHash);
1287 
1288  for (int n = 0; n < int(thisDesc.size()); n++)
1289  dist += square(
1290  (*it1)
1291  ->descriptors
1292  .multiSIFTDescriptors[k1][k2][n] -
1293  thisDesc[n]);
1294  } // end if
1295  else
1296  {
1297  // if( (*it2)->ID
1298  // == 193 )
1299  // cout <<
1300  // "Skipping
1301  // descriptor
1302  // computation."
1303  // << endl;
1304 
1305  for (int n = 0;
1306  n < (int)(*it2)
1307  ->descriptors
1308  .multiSIFTDescriptors[0][wh]
1309  .size();
1310  n++)
1311  dist += square(
1312  (*it1)
1313  ->descriptors
1314  .multiSIFTDescriptors[k1][k2][n] -
1315  (*it2)
1316  ->descriptors
1317  .multiSIFTDescriptors[0][wh][n]);
1318  } // end else
1319 
1320  // if( PARAR &&
1321  // (*it2)->ID == 193 )
1322  // {
1323  // (*it2)->dumpToConsole();
1324  // mrpt::system::pause();
1325  // }
1326 
1327  // FILTER 4 for the matching: Descriptor distance
1328  if (dist < minDist)
1329  {
1330  minDist = dist;
1331  ridx = rightFeatCounter;
1332  maxResponse = (*it2)->response;
1333  minScale = k1;
1334 
1335  // minauxfscale =
1336  // auxfscale;
1337  // minauxlscale =
1338  // auxlscale;
1339 
1340  // mindepth1 =
1341  // (*it1)->depth;
1342  // mindepth2 =
1343  // (*it2)->depth;
1344  }
1345 
1346  // if( (*it2)->ID == 193
1347  // )
1348  // {
1349  // cout << "Matching
1350  // entre 193 y "<<
1351  // (*it2)->ID << "
1352  // entre escalas: "
1353  // << firstScale << "
1354  // y " << lastScale
1355  // << endl;
1356  // cout << "OR1: " <<
1357  // (*it1)->multiOrientations[k1][k2]
1358  // << " OR2: " <<
1359  // (*it2)->multiOrientations[0][k3];
1360  // cout << "SCL: " <<
1361  // (*it1)->multiScales[k1]
1362  // << " DIST: " <<
1363  // dist << endl;
1364  // }
1365 
1366  } // end if
1367  } // end for it2
1368  if (minDist < matchOpts.matchingThreshold)
1369  {
1370  // AVOID COLLISIONS IN A GOOD MANNER
1371  int auxIdx = output.secondListCorrespondences[ridx];
1372  if (auxIdx != FEAT_FREE &&
1373  minDist < output.firstListDistance[auxIdx])
1374  {
1375  // We've found a better match
1376  output.firstListDistance[leftFeatCounter] = minDist;
1377  output.firstListCorrespondences[leftFeatCounter] = ridx;
1378  output.secondListCorrespondences[ridx] = leftFeatCounter;
1379  output.firstListFoundScales[leftFeatCounter] = minScale;
1380 
1381  // Undo the previous one
1382  output.firstListDistance[auxIdx] = 1.0;
1383  output.firstListCorrespondences[auxIdx] = FEAT_FREE;
1384  output.firstListFoundScales[auxIdx] = -1;
1385 
1386  // cout << "Better match at scale: " << minScale
1387  // << endl;
1388  } // end-if
1389  else
1390  {
1391  //// cout << "New match found: [R] " <<
1392  /// minRightIdx
1393  ///<<
1394  ///" with [L] " << minLeftIdx << "(" << minVal << ")" << endl;
1395  output.secondListCorrespondences[ridx] = leftFeatCounter;
1396  output.firstListCorrespondences[leftFeatCounter] = ridx;
1397  output.firstListDistance[leftFeatCounter] = minDist;
1398  output.firstListFoundScales[leftFeatCounter] = minScale;
1399  output.nMatches++;
1400  // cout << "Match found at scale: " << minScale <<
1401  // endl;
1402  }
1403  // Match found
1404  // leftMatchingIdx.push_back( leftFeatCounter );
1405  // rightMatchingIdx.push_back( minIdx );
1406  // outScales.push_back( minScale );
1407  //
1408  // cout << "Match found: [" << leftFeatCounter << "," <<
1409  // minIdx;
1410  // cout << "] at scale " << minScale << " [" <<
1411  // minauxfscale << "," << minauxlscale << "]";
1412  // cout << " with depths: [" << mindepth1 << "," <<
1413  // mindepth2 << "]" << endl;
1414  } // end if
1415  } // end for it1
1416 
1417  return output;
1418  MRPT_END
1419 } // end matchMultiResolutionFeatures
1420 
1421 /*-------------------------------------------------------------
1422  matchMultiResolutionFeatures
1423 -------------------------------------------------------------*/
1424 int vision::matchMultiResolutionFeatures(
1425  CMatchedFeatureList& mList1, CMatchedFeatureList& mList2,
1426  const CImage& leftImage, const CImage& rightImage,
1427  const TMultiResDescMatchOptions& matchOpts,
1428  const TMultiResDescOptions& computeOpts)
1429 {
1430  MRPT_START
1431  // "mList1" has a set of matched features with their depths computed
1432  // "mList2" has a set of matched FAST features detected in the next frame at
1433  // scale 1.0 (with their depths)
1434  // We perform a prediction of the "mList1" features and try to find its
1435  // matches within mList2
1436  // --------------------------------------------------------
1437  // Algortihm summary:
1438  // --------------------------------------------------------
1439  // For each feature in List1 we find a search region: by now a fixed window
1440  // e.g. 20x20 pixels
1441  // TO DO: Non-maximal supression according to the feature score
1442  // From the remaining set, we compute the main orientation and check its
1443  // consistency with the List1 feature
1444  // NEW: compute the corresponding feature in right image and compute its
1445  // depth
1446  // From the remaining set, we compute the descriptor at scale 1.0 and
1447  // determine the set of scales where to look for
1448  // If the distance between descriptors is below a certain threshold, we've
1449  // found a match.
1450  // --------------------------------------------------------
1451 
1452  // General variables
1453  CTimeLogger logger;
1454  // logger.disable();
1455 
1456  ASSERT_(mList1.size() > 0 && mList2.size() > 0);
1457 
1458  CFeatureList baseList1, baseList2;
1459  mList1.getBothFeatureLists(baseList1, baseList2);
1460 
1461  CFeatureList auxList1, auxList2;
1462  mList2.getBothFeatureLists(auxList1, auxList2);
1463 
1464  vector<int> scales1, scales2;
1465 
1466  TMultiResMatchingOutput output1, output2;
1467 
1468  // Left image
1469  output1 = matchMultiResolutionFeatures(
1470  baseList1, auxList1, leftImage, matchOpts, computeOpts);
1471  updateBaseList(
1472  baseList1, auxList1,
1473  output1.firstListCorrespondences); // Update counters
1474  // updateBaseList(leftIdx2,auxList1);
1475 
1476  // CImage auxImg1, auxImg2;
1477  // auxImg1 = leftImage;
1478  // int hwsize = matchOpts.searchAreaSize/2;
1479  // for( int k = 0; k < leftIdx1.size(); ++k )
1480  // if( leftIdx1[k] != FEAT_FREE )
1481  // {
1482  // auxImg1.cross( baseList1[k]->x, baseList1[k]->y,
1483  // TColor::red(),
1484  // '+' );
1485  // auxImg1.textOut( baseList1[k]->x, baseList1[k]->y,
1486  // format("%d", scales1[k]), TColor::red );
1487  // auxImg1.rectangle( baseList1[k]->x-hwsize,
1488  // baseList1[k]->y-hwsize, baseList1[k]->x+hwsize,
1489  // baseList1[k]->y+hwsize, TColor::red );
1490  // }
1491  // win1.showImage( auxImg1 );
1492 
1493  // Right image
1494  output2 = matchMultiResolutionFeatures(
1495  baseList2, auxList2, rightImage, matchOpts, computeOpts);
1496  updateBaseList(
1497  baseList2, auxList2,
1498  output2.firstListCorrespondences); // Update counters
1499  // updateBaseList(rightIdx2,auxList2);
1500 
1501  // auxImg2 = rightImage;
1502  // for( int k = 0; k < rightIdx1.size(); ++k )
1503  // if( rightIdx1[k] != FEAT_FREE )
1504  // {
1505  // auxImg2.cross( baseList2[k]->x, baseList2[k]->y,
1506  // 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::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:131
mrpt::img::CImage::loadFromIplImage
void loadFromIplImage(void *iplImage)
Reads the image from a OpenCV IplImage object (making a COPY).
Definition: CImage.cpp:321
mrpt::img::CImage::getHeight
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
mrpt::img::CImage::getAs
const T * getAs() const
Returns a pointer to a const T* containing the image - the idea is to call like "img....
Definition: img/CImage.h:599
mrpt::img::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:1359
mrpt::img::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,...
Definition: CImage.cpp:948
mrpt::img::CImage::getWidth
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:864
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:356
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:68
mrpt::system::CTimeLogger
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
Definition: system/CTimeLogger.h:42
mrpt::vision::CFeature::Ptr
std::shared_ptr< CFeature > Ptr
Definition: CFeature.h:58
mrpt::vision::CFeatureList
A list of visual features, to be used as output by detectors, as input/output by trackers,...
Definition: CFeature.h:305
mrpt::vision::CFeatureList::size
size_t size() const
Definition: CFeature.h:386
mrpt::vision::CFeatureList::getByID
CFeature::Ptr getByID(const TFeatureID &ID) const
Get a reference to a Feature from its ID.
Definition: CFeature.cpp:1255
mrpt::vision::CFeatureList::const_iterator
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:366
mrpt::vision::CFeatureList::iterator
TInternalFeatList::iterator iterator
Definition: CFeature.h:365
mrpt::vision::CMatchedFeatureList::getBothFeatureLists
void getBothFeatureLists(CFeatureList &list1, CFeatureList &list2)
Returns the matching features as two separate CFeatureLists.
Definition: CFeature.cpp:1429
M_2PI
#define M_2PI
Definition: common.h:58
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:38
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
n
GLenum GLsizei n
Definition: glext.h:5074
depth
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glext.h:3601
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
res
GLuint res
Definition: glext.h:7268
c
const GLubyte * c
Definition: glext.h:6313
image
GLenum GLsizei GLenum GLenum const GLvoid * image
Definition: glext.h:3552
y
GLenum GLint GLint y
Definition: glext.h:3538
x
GLenum GLint x
Definition: glext.h:3538
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
size
GLsizeiptr size
Definition: glext.h:3923
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23
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:428
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
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:356
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
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:406
mrpt::vision::TFeatureID
uint64_t TFeatureID
Definition of a feature ID.
Definition: vision/include/mrpt/vision/types.h:24
mrpt::vision::normalizeImage
void normalizeImage(const mrpt::img::CImage &image, mrpt::img::CImage &nimage)
Normalizes the brigthness and contrast of an image by setting its mean value to zero and its standard...
Definition: vision_utils.cpp:432
mrpt::vision::TQuantizationTable
std::map< int, std::map< int, std::map< int, std::deque< std::pair< TFeatureID, double > >> >> TQuantizationTable
Definition: multiDesc_utils.h:34
mrpt::vision::computeMainOrientations
bool computeMainOrientations(const mrpt::img::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:598
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:716
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
mrpt::vision::computeMultiResolutionDescriptors
void computeMultiResolutionDescriptors(const mrpt::img::CImage &imageLeft, const mrpt::img::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::computeHistogramOfOrientations
void computeHistogramOfOrientations(const mrpt::img::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,...
Definition: multiDesc_utils.cpp:819
mrpt::vision::saveQTableToFile
void saveQTableToFile(const TQuantizationTable &qTable, const std::string &filename)
Definition: multiDesc_utils.cpp:97
mrpt::vision::updateBaseList
void updateBaseList(CFeatureList &baseList, const CFeatureList &currentList, const std::vector< int > &idx)
Definition: multiDesc_utils.cpp:409
mrpt::vision::computeMoreDescriptors
int computeMoreDescriptors(const mrpt::img::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
mrpt::vision::computeGradient
bool computeGradient(const mrpt::img::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:558
mrpt::vision::checkScalesAndFindMore
void checkScalesAndFindMore(CMatchedFeatureList &baseList, const CFeatureList &currentList, const mrpt::img::CImage &currentImage, const TMultiResMatchingOutput &output, const TMultiResDescOptions &computeOpts, const TMultiResDescMatchOptions &matchOpts)
Definition: multiDesc_utils.cpp:469
mrpt::vision::matchMultiResolutionFeatures
TMultiResMatchingOutput matchMultiResolutionFeatures(const CFeatureList &list1, CFeatureList &list2, const mrpt::img::CImage &rightImage, const TMultiResDescMatchOptions &matchOpts, const TMultiResDescOptions &computeOpts)
Matches two CFeatureList containing mulit-resolution descriptors.
Definition: multiDesc_utils.cpp:1020
mrpt::vision::relocalizeMultiDesc
TMultiResMatchingOutput relocalizeMultiDesc(const mrpt::img::CImage &image, CFeatureList &baseList, CFeatureList &currentList, TQuantizationTable &qTable, const TMultiResDescOptions &desc_opts, const TMultiResDescMatchOptions &match_opts)
Definition: multiDesc_utils.cpp:129
mrpt::vision::insertHashCoeffs
void insertHashCoeffs(const CFeature::Ptr &feat, TQuantizationTable &qTable)
Definition: multiDesc_utils.cpp:55
mrpt::vision::computeMultiOrientations
void computeMultiOrientations(const mrpt::img::CImage &image, CFeatureList &list, const TMultiResDescOptions &opts)
Computes the multi-resolution SIFT-like descriptor of a list of features.
Definition: multiDesc_utils.cpp:2262
lightweight_geom_data.h
FEAT_FREE
const int FEAT_FREE
Definition: multiDesc_utils.cpp:47
multiDesc_utils.h
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:12
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:209
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:304
mrpt::vision
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:19
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:31
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42
ops_vectors.h
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::vision::TMultiResDescMatchOptions
Struct containing the options when matching multi-resolution SIFT-like descriptors.
Definition: vision/include/mrpt/vision/types.h:548
mrpt::vision::TMultiResDescMatchOptions::useDepthFilter
bool useDepthFilter
Whether or not use the filter based on the depth test.
Definition: vision/include/mrpt/vision/types.h:555
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:576
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:573
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:567
mrpt::vision::TMultiResDescMatchOptions::matchingThreshold
double matchingThreshold
The absolute threshold in descriptor distance for considering a match.
Definition: vision/include/mrpt/vision/types.h:558
mrpt::vision::TMultiResDescMatchOptions::oriThreshold
double oriThreshold
The threshold for the orientation test.
Definition: vision/include/mrpt/vision/types.h:552
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:564
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:570
mrpt::vision::TMultiResDescOptions
Struct containing the options when computing the multi-resolution SIFT-like descriptors.
Definition: vision/include/mrpt/vision/types.h:643
mrpt::vision::TMultiResDescOptions::basePSize
uint32_t basePSize
The size of the base patch.
Definition: vision/include/mrpt/vision/types.h:645
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:662
mrpt::vision::TMultiResDescOptions::computeDepth
bool computeDepth
Whether or not to compute the depth of the feature.
Definition: vision/include/mrpt/vision/types.h:653
mrpt::vision::TMultiResDescOptions::cropValue
double cropValue
The SIFT-like descriptor is cropped at this value during normalization.
Definition: vision/include/mrpt/vision/types.h:666
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:656
mrpt::vision::TMultiResDescOptions::scales
std::vector< double > scales
The set of scales relatives to the base patch.
Definition: vision/include/mrpt/vision/types.h:647
mrpt::vision::TMultiResDescOptions::fx
double fx
Intrinsic stereo pair parameters for computing the depth of the feature.
Definition: vision/include/mrpt/vision/types.h:659
mrpt::vision::TMultiResDescOptions::sg1
double sg1
The sigmas for the Gaussian kernels.
Definition: vision/include/mrpt/vision/types.h:651
mrpt::vision::TMultiResMatchingOutput
Struct containing the output after matching multi-resolution SIFT-like descriptors.
Definition: vision/include/mrpt/vision/types.h:518
mrpt::vision::TMultiResMatchingOutput::firstListDistance
std::vector< double > firstListDistance
Contains the distances between the descriptors.
Definition: vision/include/mrpt/vision/types.h:531
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:523
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:529
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:526
TH
const double TH
Definition: vision_stereo_rectify/test.cpp:25
counter
int counter
Definition: visit_each_unittest.cpp:15


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 814d80880 Fri Aug 24 01:51:28 2018 +0200 at mar 26 may 2026 12:30:59 CEST