MRPT  1.9.9
CFeature.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in https://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+
9  */
10 
11 #include "vision-precomp.h" // Precompiled headers
12 
16 #include <mrpt/math/data_utils.h>
19 #include <mrpt/system/os.h>
20 #include <mrpt/vision/CFeature.h>
21 #include <mrpt/vision/types.h>
22 #include <mrpt/vision/utils.h>
23 #include <iostream>
24 
25 using namespace mrpt;
26 using namespace mrpt::vision;
27 using namespace mrpt::img;
28 using namespace mrpt::math;
29 using namespace mrpt::system;
30 using namespace mrpt::io;
31 using namespace std;
32 
34 
35 // --------------------------------------------------
36 // loadFromConfigFile
37 // --------------------------------------------------
38 /** Load all the params from a config source, in the format described in
39  * saveToConfigFile()
40  */
41 void TMultiResDescMatchOptions::loadFromConfigFile(
42  const mrpt::config::CConfigFileBase& cfg, const std::string& section)
43 {
44  useOriFilter = cfg.read_bool(section, "useOriFilter", true, false);
45  oriThreshold = cfg.read_double(section, "oriThreshold", 0.2, false);
46  lastSeenThreshold = cfg.read_int(section, "lastSeenThreshold", 10, false);
47  timesSeenThreshold = cfg.read_int(section, "timesSeenThreshold", 5, false);
48  minFeaturesToFind = cfg.read_int(section, "minFeaturesToFind", 5, false);
49  minFeaturesToBeLost =
50  cfg.read_int(section, "minFeaturesToBeLost", 5, false);
51 
52  useDepthFilter = cfg.read_bool(section, "useDepthFilter", true, false);
53 
54  matchingThreshold =
55  cfg.read_double(section, "matchingThreshold", 1e4, false);
56  matchingRatioThreshold =
57  cfg.read_double(section, "matchingRatioThreshold", 0.5, false);
58 
59  lowScl1 = cfg.read_int(section, "lowScl1", 0, false);
60  lowScl2 = cfg.read_int(section, "lowScl1", 0, false);
61  highScl1 = cfg.read_int(section, "highScl1", 6, false);
62  highScl2 = cfg.read_int(section, "highScl2", 6, false);
63 
64  searchAreaSize = cfg.read_double(section, "searchAreaSize", 20, false);
65 }
66 
67 // --------------------------------------------------
68 // saveToConfigFile
69 // --------------------------------------------------
71  mrpt::config::CConfigFileBase& cfg, const std::string& section) const
72 {
73  if (useOriFilter)
74  {
75  cfg.write(section, "useOriFilter", "true");
76  cfg.write(section, "oriThreshold", oriThreshold);
77  }
78  else
79  cfg.write(section, "useOriFilter", "false");
80 
81  if (useDepthFilter)
82  cfg.write(section, "useDepthFilter", "true");
83  else
84  cfg.write(section, "useDepthFilter", "false");
85 
86  cfg.write(section, "matchingThreshold", matchingThreshold);
87  cfg.write(section, "matchingRatioThreshold", matchingRatioThreshold);
88  cfg.write(section, "lowScl1", lowScl1);
89  cfg.write(section, "lowScl2", lowScl2);
90  cfg.write(section, "highScl1", highScl1);
91  cfg.write(section, "highScl2", highScl2);
92 
93  cfg.write(section, "searchAreaSize", searchAreaSize);
94  cfg.write(section, "lastSeenThreshold", lastSeenThreshold);
95  cfg.write(section, "timesSeenThreshold", timesSeenThreshold);
96  cfg.write(section, "minFeaturesToFind", minFeaturesToFind);
97  cfg.write(section, "minFeaturesToBeLost", minFeaturesToBeLost);
98 } // end-saveToConfigFile
99 
100 // --------------------------------------------------
101 // dumpToTextStream
102 // --------------------------------------------------
104 {
105  out << "\n----------- [vision::TMultiResDescMatchOptions] ------------ \n";
106  out << "Use orientation filter?: ";
107  if (useOriFilter)
108  {
109  out << "Yes\n";
110  out << mrpt::format(
111  "ยท Orientation threshold: %.1f deg\n",
112  RAD2DEG(oriThreshold));
113  }
114  else
115  out << "No\n";
116  out << "Use depth filter?: ";
117  if (useDepthFilter)
118  out << "Yes\n";
119  else
120  {
121  out << "No\n";
122  out << mrpt::format("Lowest scale in list1: %d\n", lowScl1);
123  out << mrpt::format("Highest scale in list1: %d\n", highScl1);
124  out << mrpt::format("Lowest scale in list2: %d\n", lowScl2);
125  out << mrpt::format("Highest scale in list2: %d\n", highScl2);
126  }
127  out << mrpt::format(
128  "#frames last seen threshold: %d\n", lastSeenThreshold);
129  out << mrpt::format(
130  "#frames to be stable threshold: %d\n", timesSeenThreshold);
131  out << mrpt::format(
132  "min. # features in system: %d\n", minFeaturesToFind);
133  out << mrpt::format(
134  "min. # features to be lost: %d\n", minFeaturesToBeLost);
135  out << mrpt::format(
136  "Matching threshold: %.2f\n", matchingThreshold);
137  out << mrpt::format(
138  "Matching ratio threshold: %.2f\n", matchingRatioThreshold);
139  out << mrpt::format(
140  "Size of the search window: %d px\n", searchAreaSize);
141  out << "-------------------------------------------------------- \n";
142 } // end-dumpToTextStream
143 
144 // --------------------------------------------------
145 // loadFromConfigFile
146 // --------------------------------------------------
147 /** Load all the params from a config source, in the format described in
148  * saveToConfigFile()
149  */
151  const mrpt::config::CConfigFileBase& cfg, const std::string& section)
152 {
153  basePSize = cfg.read_double(section, "basePSize", 23, false);
154  comLScl = cfg.read_int(section, "comLScl", 0, false);
155  comHScl = cfg.read_int(section, "comHScl", 6, false);
156  sg1 = cfg.read_double(section, "sg1", 0.5, false);
157  sg2 = cfg.read_double(section, "sg2", 7.5, false);
158  sg3 = cfg.read_double(section, "sg3", 8.0, false);
159  computeDepth = cfg.read_bool(section, "computeDepth", true, false);
160  blurImage = cfg.read_bool(section, "blurImage", true, false);
161  fx = cfg.read_double(section, "fx", 0.0, false);
162  cx = cfg.read_double(section, "cx", 0.0, false);
163  cy = cfg.read_double(section, "cy", 0.0, false);
164  baseline = cfg.read_double(section, "baseline", 0.0, false);
165  computeHashCoeffs =
166  cfg.read_bool(section, "computeHashCoeffs", false, false);
167 
168  cfg.read_vector(section, "scales", vector<double>(), scales, false);
169  if (scales.size() < 1)
170  {
171  scales.resize(7);
172  scales[0] = 0.5;
173  scales[1] = 0.8;
174  scales[2] = 1.0;
175  scales[3] = 1.2;
176  scales[4] = 1.5;
177  scales[5] = 1.8;
178  scales[6] = 2.0;
179  } // end-if
180 }
181 
182 // --------------------------------------------------
183 // saveToConfigFile
184 // --------------------------------------------------
186  mrpt::config::CConfigFileBase& cfg, const std::string& section) const
187 {
188  cfg.write(section, "basePSize", basePSize);
189  cfg.write(section, "comLScl", comLScl);
190  cfg.write(section, "comHScl", comHScl);
191  cfg.write(section, "sg1", sg1);
192  cfg.write(section, "sg2", sg2);
193  cfg.write(section, "sg3", sg3);
194 
195  cfg.write(section, "computeDepth", computeDepth ? "true" : "false");
196  cfg.write(section, "blurImage", blurImage ? "true" : "false");
197  cfg.write(section, "fx", fx);
198  cfg.write(section, "cx", cx);
199  cfg.write(section, "cy", cy);
200  cfg.write(section, "baseline", baseline);
201  cfg.write(
202  section, "computeHashCoeffs", computeHashCoeffs ? "true" : "false");
203 
204  char buf[300];
205  for (double scale : scales)
206  mrpt::system::os::sprintf(buf, 300, "%.2f ", scale);
207  cfg.write(section, "scales", buf);
208 } // end-saveToConfigFile
209 
210 // --------------------------------------------------
211 // dumpToTextStream
212 // --------------------------------------------------
214 {
215  out << "\n----------- [vision::TMultiResDescOptions] ------------ \n";
216  out << mrpt::format("Base patch size: %d px\n", basePSize);
217  out << mrpt::format("Lowest scale to compute: %d\n", comLScl);
218  out << mrpt::format("Highest scale to compute: %d\n", comHScl);
219  out << mrpt::format("Image smoothing sigma: %.2f px\n", sg1);
220  out << mrpt::format("Orientation histogram sigma: %.2f\n", sg2);
221  out << mrpt::format("Descriptor histogram sigma: %.2f\n", sg3);
222  out << "Compute depth: ";
223  if (computeDepth)
224  {
225  out << "Yes\n";
226  out << mrpt::format("Focal length: %.2f px\n", fx);
227  out << mrpt::format("Principal point (cx): %.2f px\n", cx);
228  out << mrpt::format("Principal point (cy): %.2f px\n", cy);
229  out << mrpt::format(
230  "Baseline: %.2f m\n", baseline);
231  }
232  else
233  out << "No\n";
234 
235  out << "Compute Hash Coeffs: ";
236  if (computeHashCoeffs)
237  out << "Yes\n";
238  else
239  out << "No\n";
240 
241  out << "Blur image previously: ";
242  if (blurImage)
243  out << "Yes\n";
244  else
245  out << "No\n";
246 
247  out << "Scales: ";
248  for (double scale : scales) out << mrpt::format("%.2f ", scale);
249  out << "\n";
250  out << "-------------------------------------------------------- \n";
251 } // end-dumpToTextStream
252 
253 void CFeature::dumpToTextStream(std::ostream& out) const
254 {
255  out << "\n----------- [vision::CFeature] ------------ \n";
256  out << mrpt::format(
257  "Feature ID: %d\n", (int)keypoint.ID);
258  out << mrpt::format(
259  "Coordinates: (%.2f,%.2f) px\n", keypoint.pt.x,
260  keypoint.pt.y);
261  out << mrpt::format("PatchSize: %d\n", patchSize);
262  out << "Type: ";
263  out << mrpt::typemeta::TEnumType<decltype(type)>::value2name(type) << "\n";
264  out << "Status: ";
265  switch (track_status)
266  {
267  case 0:
268  out << "Idle\n";
269  break;
270  case 1:
271  out << "[KLT] Out of bounds [KLT]\n";
272  break;
273  case 5:
274  out << "[KLT] Tracked\n";
275  break;
276  case 10:
277  out << "[KLT] Lost\n";
278  break;
279  }
280 
281  out << mrpt::format("Response: %.2f\n", response);
282  out << mrpt::format("Main orientation: %.2f\n", orientation);
283  out << mrpt::format(
284  "Initial Depth: %.2f m\n", initialDepth);
285  out << mrpt::format("Depth: %.2f m\n", depth);
286  out << mrpt::format(
287  "3D point: (%.2f,%.2f,%.2f) m\n", p3D.x, p3D.y,
288  p3D.z);
289  out << "Is point feature?: ";
290  isPointFeature() ? out << "Yes\n" : out << "No\n";
291 
292  out << "Has SIFT descriptor?: ";
293  descriptors.hasDescriptorSIFT() ? out << "Yes\n" : out << "No\n";
294  out << "Has SURF descriptor?: ";
295  descriptors.hasDescriptorSURF() ? out << "Yes\n" : out << "No\n";
296  out << "Has Spin image descriptor?: ";
297  descriptors.hasDescriptorSpinImg() ? out << "Yes\n" : out << "No\n";
298  out << "Has Polar descriptor?: ";
299  descriptors.hasDescriptorPolarImg() ? out << "Yes\n" : out << "No\n";
300  out << "Has Log Polar descriptor?: ";
301  descriptors.hasDescriptorLogPolarImg() ? out << "Yes\n" : out << "No\n";
302  out << "Has ORB descriptor?: ";
303  descriptors.hasDescriptorORB() ? out << "Yes\n" : out << "No\n";
304  //# added by Raghavender Sahdev
305  out << "Has BLD descriptor?: ";
306  descriptors.hasDescriptorBLD() ? out << "Yes\n" : out << "No\n";
307  out << "Has LATCH descriptor?: ";
308  descriptors.hasDescriptorLATCH() ? out << "Yes\n" : out << "No\n";
309 
310 } // end dumpToTextStream
311 
312 void CFeature::dumpToConsole() const { dumpToTextStream(std::cout); }
313 uint8_t CFeature::serializeGetVersion() const { return 2; }
315 {
316  // The coordinates:
317  out << keypoint.pt.x << keypoint.pt.y << keypoint.ID << patch << patchSize
318  << (uint32_t)type << (uint32_t)keypoint.track_status
319  << keypoint.response << orientation << keypoint.octave << user_flags
320  << depth << initialDepth << p3D << descriptors.SIFT << descriptors.SURF
321  << descriptors.SpinImg << descriptors.SpinImg_range_rows
322  << descriptors.PolarImg << descriptors.LogPolarImg
323  << descriptors.polarImgsNoRotation << descriptors.ORB << descriptors.BLD
324  << descriptors.LATCH;
325 }
326 
328 {
329  switch (version)
330  {
331  case 0:
332  case 1:
333  case 2:
334  {
335  // The coordinates:
336  uint32_t aux_type, aux_KLTS;
337  in >> keypoint.pt.x >> keypoint.pt.y >> keypoint.ID >> patch >>
338  patchSize >> aux_type >> aux_KLTS >> keypoint.response >>
339  orientation >> keypoint.octave >> user_flags;
340  if (version > 0)
341  {
342  in >> depth >> initialDepth >> p3D;
343  }
344  in >> descriptors.SIFT >> descriptors.SURF >> descriptors.SpinImg >>
345  descriptors.SpinImg_range_rows >> descriptors.PolarImg >>
346  descriptors.LogPolarImg >> descriptors.polarImgsNoRotation >>
347  descriptors.BLD >> descriptors.LATCH;
348  if (version > 1) in >> descriptors.ORB;
349 
350  type = (TKeyPointMethod)aux_type;
351  track_status = (TFeatureTrackStatus)aux_KLTS;
352  }
353  break;
354  default:
356  };
357 }
358 
359 /****************************************************
360  Class CFEATURE
361 *****************************************************/
362 
363 // Return false only for Blob detectors (SIFT, SURF)
365 {
366  return type == featSIFT || type == featSURF;
367 }
368 
369 // --------------------------------------------------
370 // patchCorrelationTo
371 // --------------------------------------------------
372 float CFeature::patchCorrelationTo(const CFeature& oFeature) const
373 {
374  MRPT_START
375  ASSERT_(patch);
376 
377  ASSERT_(patch->getWidth() == oFeature.patch->getWidth());
378  ASSERT_(patch->getHeight() == oFeature.patch->getHeight());
379  ASSERT_(patch->getHeight() > 0 && patch->getWidth() > 0);
380  size_t x_max, y_max;
381  double max_val;
383  *patch, *oFeature.patch, x_max, y_max, max_val);
384 
385  return 0.5 -
386  0.5 * max_val; // Value as "distance" in the range [0,1], best = 0
387 
388  MRPT_END
389 }
390 
391 // --------------------------------------------------
392 // descriptorDistanceTo
393 // --------------------------------------------------
395  const CFeature& oFeature, TDescriptorType descriptorToUse,
396  bool normalize_distances) const
397 {
398  MRPT_START
399 
400  // If we are not ask for a specific descriptor, select the first one found:
401  if (descriptorToUse == descAny)
402  {
403  if (descriptors.hasDescriptorSIFT())
404  descriptorToUse = descSIFT;
405  else if (descriptors.hasDescriptorSURF())
406  descriptorToUse = descSURF;
407  else if (descriptors.hasDescriptorSpinImg())
408  descriptorToUse = descSpinImages;
409  else if (descriptors.hasDescriptorPolarImg())
410  descriptorToUse = descPolarImages;
411  else if (descriptors.hasDescriptorLogPolarImg())
412  descriptorToUse = descLogPolarImages;
413  else if (descriptors.hasDescriptorORB())
414  descriptorToUse = descORB;
415  // # added by Raghavender Sahdev - BLD/LATCH descriptors
416  else if (descriptors.hasDescriptorBLD())
417  descriptorToUse = descBLD;
418  else if (descriptors.hasDescriptorLATCH())
419  descriptorToUse = descLATCH;
420  else
422  "Feature has no descriptors and descriptorToUse=descAny");
423  }
424 
425  switch (descriptorToUse)
426  {
427  case descSIFT:
428  return descriptorSIFTDistanceTo(oFeature, normalize_distances);
429  case descSURF:
430  return descriptorSURFDistanceTo(oFeature, normalize_distances);
431  case descSpinImages:
432  return descriptorSpinImgDistanceTo(oFeature, normalize_distances);
433  case descPolarImages:
434  {
435  float minAng;
436  return descriptorPolarImgDistanceTo(
437  oFeature, minAng, normalize_distances);
438  }
439  case descLogPolarImages:
440  {
441  float minAng;
442  return descriptorLogPolarImgDistanceTo(
443  oFeature, minAng, normalize_distances);
444  }
445  case descORB:
446  return float(descriptorORBDistanceTo(oFeature));
447  // # added by Raghavender Sahdev
448  case descBLD:
449  return (descriptorBLDDistanceTo(oFeature));
450  case descLATCH:
451  return (descriptorLATCHDistanceTo(oFeature));
452  default:
454  "Unknown value for 'descriptorToUse'=%u",
455  (unsigned)descriptorToUse);
456  }
457 
458  MRPT_END
459 }
460 
461 // --------------------------------------------------
462 // descriptorSIFTDistanceTo
463 // --------------------------------------------------
465  const CFeature& oFeature, bool normalize_distances) const
466 {
467  ASSERT_(descriptors.SIFT);
468  ASSERT_(oFeature.descriptors.SIFT);
469  ASSERT_(descriptors.SIFT->size() == oFeature.descriptors.SIFT->size());
470  ASSERT_(
471  descriptors.hasDescriptorSIFT() &&
472  oFeature.descriptors.hasDescriptorSIFT());
473 
474  float dist = 0.0f;
475  std::vector<unsigned char>::const_iterator itDesc1, itDesc2;
476  for (itDesc1 = descriptors.SIFT->begin(),
477  itDesc2 = oFeature.descriptors.SIFT->begin();
478  itDesc1 != descriptors.SIFT->end(); itDesc1++, itDesc2++)
479  {
480  dist += square(*itDesc1 - *itDesc2);
481  }
482  if (normalize_distances) dist /= descriptors.SIFT->size();
483  dist = sqrt(dist);
484  if (normalize_distances) dist /= 64.0f;
485  return dist;
486 } // end descriptorSIFTDistanceTo
487 
488 // --------------------------------------------------
489 // descriptorSURFDistanceTo
490 // --------------------------------------------------
492  const CFeature& oFeature, bool normalize_distances) const
493 {
494  ASSERT_(descriptors.SURF);
495  ASSERT_(descriptors.SURF->size() == oFeature.descriptors.SURF->size());
496  ASSERT_(
497  descriptors.hasDescriptorSURF() &&
498  oFeature.descriptors.hasDescriptorSURF());
499 
500  float dist = 0.0f;
501  std::vector<float>::const_iterator itDesc1, itDesc2;
502  for (itDesc1 = descriptors.SURF->begin(),
503  itDesc2 = oFeature.descriptors.SURF->begin();
504  itDesc1 != descriptors.SURF->end(); itDesc1++, itDesc2++)
505  {
506  dist += square(*itDesc1 - *itDesc2);
507  }
508  if (normalize_distances) dist /= descriptors.SURF->size();
509  dist = sqrt(dist);
510  if (normalize_distances)
511  dist /= 0.20f; // JL: Ad-hoc value! Investigate where does this come
512  // from...
513  return dist;
514 } // end descriptorSURFDistanceTo
515 
516 // --------------------------------------------------
517 // descriptorSpinImgDistanceTo
518 // --------------------------------------------------
520  const CFeature& oFeature, bool normalize_by_vector_length) const
521 {
522  ASSERT_(descriptors.SpinImg);
523 
524  ASSERT_(
525  descriptors.SpinImg->size() == oFeature.descriptors.SpinImg->size());
526  ASSERT_(
527  descriptors.hasDescriptorSpinImg() &&
528  oFeature.descriptors.hasDescriptorSpinImg());
529  ASSERT_(!descriptors.SpinImg->empty());
530  float dist = 0.0f;
531  std::vector<float>::const_iterator itDesc1, itDesc2;
532  for (itDesc1 = descriptors.SpinImg->begin(),
533  itDesc2 = oFeature.descriptors.SpinImg->begin();
534  itDesc1 != descriptors.SpinImg->end(); itDesc1++, itDesc2++)
535  {
536  dist += square(*itDesc1 - *itDesc2);
537  }
538 
539  if (normalize_by_vector_length) dist /= 0.25 * descriptors.SpinImg->size();
540 
541  return sqrt(dist);
542 } // end descriptorSpinImgDistanceTo
543 
544 // --------------------------------------------------
545 // descriptorPolarImgDistanceTo
546 // --------------------------------------------------
548  const CMatrixF& desc1, const CMatrixF& desc2, float& minDistAngle,
549  bool normalize_distances, bool dont_shift_angle)
550 {
551  MRPT_START
552 
553  // Find the smallest distance:
554  unsigned int delta, i, j, ii, height = desc1.rows(), width = desc1.cols();
555  float dist, minDist = 0;
556 
557  //#define LM_CORR_BIAS_MEAN
558 
559 #define LM_CORR_METHOD_EUCLID
560  //#define LM_CORR_METHOD_MANHATTAN
561  //#define LM_CORR_METHOD_CORRELATION
562 
563 #if defined(LM_CORR_BIAS_MEAN) || defined(LM_CORR_METHOD_CORRELATION)
564  const float desc1_mean = desc1.sum() / static_cast<float>(width * height);
565  const float desc2_mean = desc2.sum() / static_cast<float>(width * height);
566 #endif
567 
568  CVectorFloat distances(height, 0); // Distances for each shift
569 
570  for (delta = 0; delta < height; delta++)
571  {
572 #if defined(LM_CORR_METHOD_CORRELATION)
573  float s11 = 0;
574  float s22 = 0;
575  float s12 = 0;
576 #endif
577  // Compute the mean distance between desc1[t] and desc2[t-delta]:
578  dist = 0;
579  for (i = 0; i < height; i++)
580  {
581  ii = (i + delta) % height; // Shifted index
582  for (j = 0; j < width; j++)
583  {
584 #ifdef LM_CORR_METHOD_EUCLID
585 #ifdef LM_CORR_BIAS_MEAN
586  dist += square(
587  desc1(i, j) - desc1_mean - desc2(ii, j) + desc2_mean);
588 #else
589  dist += square(desc1(i, j) - desc2(ii, j));
590 #endif
591 #elif defined(LM_CORR_METHOD_MANHATTAN)
592 #ifdef LM_CORR_BIAS_MEAN
593  dist +=
594  abs(desc1(i, j) - desc1_mean - desc2(ii, j) + desc2_mean);
595 #else
596  dist += abs(desc1(i, j) - desc2(ii, j));
597 #endif
598 #elif defined(LM_CORR_METHOD_CORRELATION)
599  float d1 = desc1(i, j) - desc1_mean;
600  float d2 = desc2(ii, j) - desc2_mean;
601  s11 += square(d1);
602  s22 += square(d2);
603  s12 += d1 * d2;
604 #else
605 #error A LM_CORR_METHOD_XXX method must be selected!
606 #endif
607  }
608  }
609 
610  // Average:
611  if (normalize_distances) dist /= static_cast<float>(width * height);
612 
613 #ifdef LM_CORR_METHOD_EUCLID
614  dist = sqrt(dist);
615 #endif
616 
617 #if defined(LM_CORR_METHOD_CORRELATION)
618  dist = 1 - (s12 / sqrt(s11 * s22));
619 #endif
620 
621  distances[delta] = dist;
622  if (!delta && dont_shift_angle)
623  {
624  distances.resize(1);
625  break;
626  }
627  } // end for delta
628 
629  size_t minDistIdx;
630  minDist = distances.minCoeff(minDistIdx);
631 
632  double dist_mean, dist_std;
633  mrpt::math::meanAndStd(distances, dist_mean, dist_std);
634 
635 #if 0
636  {
637  cout << "min dist: " << minDist << endl;
638 
639  static mrpt::gui::CDisplayWindowPlots win("distances");
640  win.plot(distances,"b.4");
641  CImage img1(desc1);
642  win.image(img1,0,-0.5,0.4*width,0.5,"img1");
643 
644  CImage img2(desc2);
645  win.image(img2,0.6*width,-0.5,0.4*width,0.5,"img2");
646 
647  //win.axis_fit();
648  win.waitForKey();
649  }
650 #endif
651 
652  // Output:
653  minDistAngle = minDistIdx * M_2PI / static_cast<float>(width);
654  return minDist;
655 
656  MRPT_END
657 }
658 
659 // --------------------------------------------------
660 // descriptorPolarImgDistanceTo
661 // --------------------------------------------------
663  const CFeature& oFeature, float& minDistAngle,
664  bool normalize_distances) const
665 {
666  MRPT_START
667 
668  ASSERT_(descriptors.PolarImg);
670  descriptors.PolarImg->rows(), oFeature.descriptors.PolarImg->rows());
672  descriptors.PolarImg->cols(), oFeature.descriptors.PolarImg->cols());
673  ASSERT_(
674  descriptors.hasDescriptorPolarImg() &&
676  ASSERT_ABOVE_(descriptors.PolarImg->rows(), 1);
677  ASSERT_ABOVE_(descriptors.PolarImg->cols(), 1);
678 
679  // Call the common method for computing these distances:
680  return internal_distanceBetweenPolarImages(
681  *descriptors.PolarImg, *oFeature.descriptors.PolarImg, minDistAngle,
682  normalize_distances, descriptors.polarImgsNoRotation);
683 
684  MRPT_END
685 } // end descriptorPolarImgDistanceTo
686 
687 // --------------------------------------------------
688 // descriptorLogPolarImgDistanceTo
689 // --------------------------------------------------
691  const CFeature& oFeature, float& minDistAngle,
692  bool normalize_distances) const
693 {
694  MRPT_START
695 
696  ASSERT_(descriptors.LogPolarImg);
697  ASSERT_(
698  descriptors.LogPolarImg->rows() ==
699  oFeature.descriptors.LogPolarImg->rows());
700  ASSERT_(
701  descriptors.LogPolarImg->cols() ==
702  oFeature.descriptors.LogPolarImg->cols());
703  ASSERT_(
704  descriptors.hasDescriptorLogPolarImg() &&
706  ASSERT_(
707  descriptors.LogPolarImg->rows() > 1 &&
708  descriptors.LogPolarImg->cols() > 1);
709 
710  // Call the common method for computing these distances:
711  return internal_distanceBetweenPolarImages(
712  *descriptors.LogPolarImg, *oFeature.descriptors.LogPolarImg,
713  minDistAngle, normalize_distances, descriptors.polarImgsNoRotation);
714 
715  MRPT_END
716 } // end descriptorPolarImgDistanceTo
717 
718 // --------------------------------------------------
719 // descriptorORBDistanceTo
720 // --------------------------------------------------
721 uint8_t CFeature::descriptorORBDistanceTo(const CFeature& oFeature) const
722 {
723  ASSERT_(
724  descriptors.hasDescriptorORB() &&
725  oFeature.descriptors.hasDescriptorORB());
726  ASSERT_(descriptors.ORB->size() == oFeature.descriptors.ORB->size());
727  const std::vector<uint8_t>& t_desc = *descriptors.ORB;
728  const std::vector<uint8_t>& o_desc = *oFeature.descriptors.ORB;
729 
730  // Descriptors XOR + Hamming weight
731  uint8_t distance = 0;
732  for (uint8_t k = 0; k < t_desc.size(); ++k)
733  {
734  // from : Wegner, Peter (1960), "A technique for counting ones in a
735  // binary computer", Communications of the ACM 3 (5): 322,
736  // doi:10.1145/367236.367286
737  uint8_t x_or = t_desc[k] ^ o_desc[k];
738  uint8_t count;
739  for (count = 0; x_or; count++) // ...
740  x_or &= x_or - 1; // ...
741  distance += count;
742  }
743 
744  return float(distance);
745 } // end-descriptorORBDistanceTo
746 
747 // # added by Raghavender Sahdev
748 // --------------------------------------------------
749 // descriptorBLDDistanceTo
750 // --------------------------------------------------
752  const CFeature& oFeature, bool normalize_distances) const
753 {
754  ASSERT_(descriptors.BLD->size() == oFeature.descriptors.BLD->size());
755  ASSERT_(
756  descriptors.hasDescriptorBLD() &&
757  oFeature.descriptors.hasDescriptorBLD());
758 
759  float dist = 0.0f;
760  std::vector<unsigned char>::const_iterator itDesc1, itDesc2;
761  for (itDesc1 = descriptors.BLD->begin(),
762  itDesc2 = oFeature.descriptors.BLD->begin();
763  itDesc1 != descriptors.BLD->end(); itDesc1++, itDesc2++)
764  {
765  dist += square(*itDesc1 - *itDesc2);
766  }
767  if (normalize_distances) dist /= descriptors.BLD->size();
768  dist = sqrt(dist);
769  if (normalize_distances) dist /= 64.0f;
770  return dist;
771 } // end descriptorBLDDistanceTo
772 
773 // --------------------------------------------------
774 // descriptorLATCHDistanceTo
775 // --------------------------------------------------
777  const CFeature& oFeature, bool normalize_distances) const
778 {
779  ASSERT_(descriptors.LATCH->size() == oFeature.descriptors.LATCH->size());
780  ASSERT_(
781  descriptors.hasDescriptorLATCH() &&
782  oFeature.descriptors.hasDescriptorLATCH());
783 
784  float dist = 0.0f;
785  std::vector<unsigned char>::const_iterator itDesc1, itDesc2;
786  for (itDesc1 = descriptors.LATCH->begin(),
787  itDesc2 = oFeature.descriptors.LATCH->begin();
788  itDesc1 != descriptors.LATCH->end(); itDesc1++, itDesc2++)
789  {
790  dist += square(*itDesc1 - *itDesc2);
791  }
792  if (normalize_distances) dist /= descriptors.LATCH->size();
793  dist = sqrt(dist);
794  if (normalize_distances) dist /= 64.0f;
795  return dist;
796 } // end descriptorLATCHDistanceTo
797 
798 // --------------------------------------------------
799 // saveToTextFile
800 // --------------------------------------------------
801 void CFeature::saveToTextFile(const std::string& filename, bool APPEND)
802 {
803  MRPT_START
804  // "%% Dump of mrpt::vision::CFeatureList. Each line format is:\n"
805  // "%% ID TYPE X Y ORIENTATION SCALE TRACK_STATUS RESPONSE HAS_SIFT
806  // [SIFT] HAS_SURF [SURF] HAS_MULTI [MULTI_i] HAS_ORB [ORB]"
807  // "%% \\---------------------- feature ------------------/ \\---------
808  // descriptors -------/\n"
809  // "%% with:\n"
810  // "%% TYPE : The used detector: 0:KLT, 1: Harris, 2: BCD, 3: SIFT, 4:
811  // SURF, 5: Beacon, 6: FAST\n"
812  // "%% HAS_* : 1 if a descriptor of that type is associated to the
813  // feature. \n"
814  // "%% SIFT : Present if HAS_SIFT=1: N DESC_0 ... DESC_N-1 \n"
815  // "%% SURF : Present if HAS_SURF=1: N DESC_0 ... DESC_N-1 \n"
816  // "%% MULTI : Present if HAS_MULTI=1: SCALE ORI N DESC_0 ... DESC_N-1"
817  // "%% ORB : Present if HAS_ORB=1: VALUE
818  // "%%-------------------------------------------------------------------------------------------\n");
820 
821  if (!f.open(filename, APPEND))
823  "[CFeature::saveToTextFile] ERROR: File could not be open for "
824  "writing");
825 
826  f.printf(
827  "%5u %2d %7.3f %7.3f %6.2f %2d %2d %6.3f ", (unsigned int)keypoint.ID,
828  (int)get_type(), keypoint.pt.x, keypoint.pt.y, orientation,
829  keypoint.octave, (int)keypoint.track_status, keypoint.response);
830 
831  f.printf("%2d ", int(descriptors.hasDescriptorSIFT() ? 1 : 0));
832  if (descriptors.hasDescriptorSIFT())
833  {
834  f.printf("%4d ", int(descriptors.SIFT->size()));
835  for (unsigned char k : *descriptors.SIFT) f.printf("%4d ", k);
836  }
837 
838  f.printf("%2d ", int(descriptors.hasDescriptorSURF() ? 1 : 0));
839  if (descriptors.hasDescriptorSURF())
840  {
841  f.printf("%4d ", int(descriptors.SURF->size()));
842  for (float k : *descriptors.SURF) f.printf("%8.5f ", k);
843  }
844 
845  f.printf("%2d ", int(descriptors.hasDescriptorORB() ? 1 : 0));
846  if (descriptors.hasDescriptorORB())
847  for (unsigned char k : *descriptors.ORB) f.printf("%d ", k);
848 
849  // # ADDED by Raghavender Sahdev
850  f.printf("%2d ", int(descriptors.hasDescriptorBLD() ? 1 : 0));
851  if (descriptors.hasDescriptorBLD())
852  {
853  f.printf("%4d ", int(descriptors.BLD->size()));
854  for (unsigned char k : *descriptors.BLD) f.printf("%4d ", k);
855  }
856 
857  f.printf("%2d ", int(descriptors.hasDescriptorLATCH() ? 1 : 0));
858  if (descriptors.hasDescriptorLATCH())
859  {
860  f.printf("%4d ", int(descriptors.LATCH->size()));
861  for (unsigned char k : *descriptors.LATCH) f.printf("%4d ", k);
862  }
863 
864  f.printf("\n");
865  f.close();
866 
867  MRPT_END
868 } // end saveToTextFile
869 
870 /****************************************************
871  Class CFEATURELIST
872 *****************************************************/
873 
874 CFeatureList::~CFeatureList() = default;
875 
876 // --------------------------------------------------
877 // saveToTextFile
878 // --------------------------------------------------
879 // FORMAT: ID type x y orientation scale [descriptorSIFT] [descriptorSURF]
880 // track_status response
881 void CFeatureList::saveToTextFile(const std::string& filename, bool APPEND)
882 {
883  MRPT_START
884 
886 
887  if (!f.open(filename, APPEND))
889  "[CFeatureList::saveToTextFile] ERROR: File could not be open for "
890  "writing");
891 
892  f.printf(
893  "%% Dump of mrpt::vision::CFeatureList. Each line format is:\n"
894  "%% ID TYPE X Y ORIENTATION SCALE TRACK_STATUS RESPONSE HAS_SIFT "
895  "[SIFT] HAS_SURF [SURF]\n"
896  "%% \\---------------------- feature ------------------/ \\--------- "
897  "descriptors -------/\n"
898  "%% with:\n"
899  "%% TYPE : The used detector: 0:KLT, 1: Harris, 2: BCD, 3: SIFT, 4: "
900  "SURF, 5: Beacon, 6: FAST\n"
901  "%% HAS_* : 1 if a descriptor of that type is associated to the "
902  "feature. \n"
903  "%% SIFT : Present if HAS_SIFT=1: N DESC_0 ... DESC_N-1 \n"
904  "%% SURF : Present if HAS_SURF=1: N DESC_0 ... DESC_N-1 \n"
905  "%%--------------------------------------------------------------------"
906  "-----------------------\n");
907 
908  for (auto& feat : *this)
909  {
910  f.printf(
911  "%5u %2d %7.3f %7.3f %6.2f %2d %2d %6.3f ",
912  (unsigned int)feat.keypoint.ID, (int)feat.get_type(),
913  feat.keypoint.pt.x, feat.keypoint.pt.y, feat.orientation,
914  feat.keypoint.octave, (int)feat.track_status, feat.response);
915 
916  f.printf("%2d ", int(feat.descriptors.hasDescriptorSIFT() ? 1 : 0));
917  if (feat.descriptors.hasDescriptorSIFT())
918  {
919  f.printf("%4d ", int(feat.descriptors.SIFT->size()));
920  for (unsigned int k = 0; k < feat.descriptors.SIFT->size(); k++)
921  f.printf("%4d ", (*feat.descriptors.SIFT)[k]);
922  }
923 
924  f.printf("%2d ", int(feat.descriptors.hasDescriptorSURF() ? 1 : 0));
925  if (feat.descriptors.hasDescriptorSURF())
926  {
927  f.printf("%4d ", int(feat.descriptors.SURF->size()));
928  for (unsigned int k = 0; k < feat.descriptors.SURF->size(); k++)
929  f.printf("%8.5f ", (*feat.descriptors.SURF)[k]);
930  }
931  // # added by Raghavender Sahdev
932  f.printf("%2d ", int(feat.descriptors.hasDescriptorBLD() ? 1 : 0));
933  if (feat.descriptors.hasDescriptorBLD())
934  {
935  f.printf("%4d ", int(feat.descriptors.BLD->size()));
936  for (unsigned int k = 0; k < feat.descriptors.BLD->size(); k++)
937  f.printf("%4d ", (*feat.descriptors.BLD)[k]);
938  }
939 
940  f.printf("%2d ", int(feat.descriptors.hasDescriptorLATCH() ? 1 : 0));
941  if (feat.descriptors.hasDescriptorLATCH())
942  {
943  f.printf("%4d ", int(feat.descriptors.LATCH->size()));
944  for (unsigned int k = 0; k < feat.descriptors.LATCH->size(); k++)
945  f.printf("%4d ", (*feat.descriptors.LATCH)[k]);
946  }
947 
948  f.printf("\n");
949  } // end for
950 
951  f.close();
952 
953  MRPT_END
954 } // end saveToTextFile
955 
956 // --------------------------------------------------
957 // loadFromTextFile
958 // --------------------------------------------------
960 {
961  MRPT_START
962 
963  mrpt::io::CTextFileLinesParser parser(filename);
964  std::istringstream line;
965 
966  while (parser.getNextLine(line))
967  {
968  try
969  {
970  CFeature feat;
971 
972  int _ID;
973  if (!(line >> _ID)) throw std::string("ID");
974  feat.keypoint.ID = TFeatureID(_ID);
975 
976  int _type;
977  if (!(line >> _type)) throw std::string("type");
978  feat.type = TKeyPointMethod(_type);
979 
980  if (!(line >> feat.keypoint.pt.x >> feat.keypoint.pt.y))
981  throw std::string("x,y");
982  if (!(line >> feat.orientation)) throw std::string("orientation");
983  if (!(line >> feat.keypoint.octave)) throw std::string("scale");
984 
985  int _track_st;
986  if (!(line >> _track_st)) throw std::string("track_status");
987  feat.track_status = TFeatureTrackStatus(_track_st);
988 
989  if (!(line >> feat.response)) throw std::string("response");
990 
991  int hasSIFT;
992  if (!(line >> hasSIFT)) throw std::string("hasSIFT");
993  if (hasSIFT)
994  {
995  size_t N;
996  if (!(line >> N)) throw std::string("SIFT-len");
997  feat.descriptors.SIFT->resize(N);
998  for (size_t i = 0; i < N; i++)
999  {
1000  int val;
1001  line >> val;
1002  // DON'T read directly SIFT[i] since it's a
1003  // uint8_t, interpreted as a cha
1004  (*feat.descriptors.SIFT)[i] = val;
1005  }
1006 
1007  if (!line) throw std::string("SIFT-data");
1008  }
1009 
1010  //# ADDED by Raghavender Sahdev
1011  int hasBLD;
1012  if (!(line >> hasBLD)) throw std::string("hasBLD");
1013  if (hasBLD)
1014  {
1015  size_t N;
1016  if (!(line >> N)) throw std::string("BLD-len");
1017  feat.descriptors.BLD->resize(N);
1018  for (size_t i = 0; i < N; i++)
1019  {
1020  int val;
1021  line >> val;
1022  (*feat.descriptors.BLD)[i] = val;
1023  }
1024 
1025  if (!line) throw std::string("BLD-data");
1026  }
1027 
1028  int hasLATCH;
1029  if (!(line >> hasLATCH)) throw std::string("hasLATCH");
1030  if (hasBLD)
1031  {
1032  size_t N;
1033  if (!(line >> N)) throw std::string("LATCH-len");
1034  feat.descriptors.LATCH->resize(N);
1035  for (size_t i = 0; i < N; i++)
1036  {
1037  int val;
1038  line >> val;
1039  (*feat.descriptors.LATCH)[i] = val;
1040  }
1041 
1042  if (!line) throw std::string("LATCH-data");
1043  }
1044 
1045  int hasSURF;
1046  if (!(line >> hasSURF)) throw std::string("hasSURF");
1047  if (hasSURF)
1048  {
1049  size_t N;
1050  if (!(line >> N)) throw std::string("SURF-len");
1051  feat.descriptors.SURF->resize(N);
1052  for (size_t i = 0; i < N; i++)
1053  line >> (*feat.descriptors.SURF)[i];
1054  if (!line) throw std::string("SURF-data");
1055  }
1056 
1057  emplace_back(std::move(feat));
1058  }
1059  catch (std::string& msg)
1060  {
1062  "%s:%d: Error parsing features text file (%s).",
1063  filename.c_str(), (int)parser.getCurrentLineNumber(),
1064  msg.c_str()));
1065  }
1066  }
1067 
1068  MRPT_END
1069 } // end loadFromTextFile
1070 
1071 // --------------------------------------------------
1072 // copyListFrom()
1073 // --------------------------------------------------
1075 {
1076  resize(otherList.size());
1079  for (it1 = otherList.begin(), it2 = begin(); it1 != otherList.end();
1080  ++it1, ++it2)
1081  {
1082  *it2 = *it1;
1083  }
1084 } // end-copyListFrom
1085 
1087 {
1088  for (const auto& f : *this)
1089  if (f.keypoint.ID == ID) return &f;
1090 
1091  return nullptr;
1092 }
1093 
1094 const CFeature* CFeatureList::getByID(const TFeatureID& ID, int& out_idx) const
1095 {
1096  int k = 0;
1097  for (auto it = begin(); it != end(); ++it, ++k)
1098  if (it->keypoint.ID == ID)
1099  {
1100  out_idx = k;
1101  return &(*it);
1102  }
1103  out_idx = -1;
1104  return nullptr;
1105 }
1106 
1107 // --------------------------------------------------
1108 // nearest(x,y)
1109 // --------------------------------------------------
1111  const float x, const float y, double& dist_prev) const
1112 {
1113  if (empty()) return nullptr;
1114 
1115  float closest_x, closest_y;
1116  float closest_sqDist;
1117 
1118  // Look for the closest feature using KD-tree look up:
1119  const size_t closest_idx =
1120  kdTreeClosestPoint2D(x, y, closest_x, closest_y, closest_sqDist);
1121  float closest_dist = std::sqrt(closest_sqDist);
1122 
1123  if (closest_dist <= dist_prev)
1124  {
1125  dist_prev = closest_dist;
1126  return &m_feats[closest_idx];
1127  }
1128  else
1129  return nullptr;
1130 } // end nearest
1131 
1132 // --------------------------------------------------
1133 // getMaxID()
1134 // --------------------------------------------------
1136 {
1137  MRPT_START
1138  ASSERT_(!empty());
1139  vision::TFeatureID maxID = begin()->keypoint.ID;
1140  for (const auto& f : *this) mrpt::keep_max(maxID, f.keypoint.ID);
1141  return maxID;
1142  MRPT_END
1143 }
1144 
1145 /****************************************************
1146  Class CMATCHEDFEATUREKLT
1147 *****************************************************/
1148 
1150 {
1151  // OUTPUT FORMAT: ID_1 x_1 y_1 ID_2 x_2 y_2
1152 
1153  FILE* f = os::fopen(filename.c_str(), "wt");
1154  if (!f) return;
1155 
1156  for (const auto& p : *this)
1157  {
1158  os::fprintf(
1159  f, "%d %.3f %.3f %d %.3f %.3f\n",
1160  (unsigned int)(p.first.keypoint.ID), p.first.keypoint.pt.x,
1161  p.first.keypoint.pt.y, (unsigned int)(p.second.keypoint.ID),
1162  p.second.keypoint.pt.x, p.second.keypoint.pt.y);
1163 
1164  } // end for
1165  os::fclose(f);
1166 }
1167 
1168 // --------------------------------------------------
1169 // getBothFeatureLists
1170 // --------------------------------------------------
1172  const TFeatureID& ID, const TListIdx& idx)
1173 {
1174  CMatchedFeatureList::iterator it;
1175  for (it = begin(); it != end(); ++it)
1176  {
1177  const auto& feat = (idx == firstList) ? it->first : it->second;
1178  if (feat.keypoint.ID == ID) return &feat;
1179  }
1180  return nullptr;
1181 }
1182 
1183 // --------------------------------------------------
1184 // updateMaxID()
1185 // --------------------------------------------------
1187 {
1188  MRPT_START
1189  TFeatureID maxID1 = begin()->first.keypoint.ID;
1190  TFeatureID maxID2 = begin()->second.keypoint.ID;
1191  for (auto itList = begin(); itList != end(); itList++)
1192  {
1193  if (idx == firstList || idx == bothLists)
1194  mrpt::keep_max(maxID1, itList->first.keypoint.ID);
1195  if (idx == secondList || idx == bothLists)
1196  mrpt::keep_max(maxID2, itList->second.keypoint.ID);
1197  }
1198  if (idx == firstList || idx == bothLists) m_leftMaxID = maxID1;
1199  if (idx == secondList || idx == bothLists) m_rightMaxID = maxID2;
1200  MRPT_END
1201 }
1202 
1203 // --------------------------------------------------
1204 // getMaxID()
1205 // --------------------------------------------------
1207  const TListIdx& idx, TFeatureID& firstListID, TFeatureID& secondListID)
1208 {
1209  MRPT_START
1210  ASSERT_(!empty());
1211  if (idx == firstList || idx == bothLists)
1212  if (m_leftMaxID == 0) updateMaxID(firstList);
1213  if (idx == secondList || idx == bothLists)
1214  if (m_rightMaxID == 0) updateMaxID(secondList);
1215  firstListID = m_leftMaxID;
1216  secondListID = m_rightMaxID;
1217  MRPT_END
1218 } // end getMaxID()
1219 // --------------------------------------------------
1220 // getBothFeatureLists
1221 // --------------------------------------------------
1223  CFeatureList& list1, CFeatureList& list2)
1224 {
1225  MRPT_START
1226  list1.resize(size());
1227  list2.resize(size());
1228 
1229  unsigned int k = 0;
1230  for (auto it = begin(); it != end(); ++it, ++k)
1231  {
1232  list1[k] = it->first;
1233  list2[k] = it->second;
1234  } // end for
1235  MRPT_END
1236 }
1237 
1238 // --------------------------------------------------
1239 // getFirstDescriptorAsMatrix
1240 // --------------------------------------------------
1242 {
1243  if (descriptors.hasDescriptorSIFT())
1244  {
1245  desc.setSize(1, descriptors.SIFT->size());
1246  for (size_t i = 0; i < descriptors.SIFT->size(); i++)
1247  desc(0, i) = (*descriptors.SIFT)[i];
1248  return true;
1249  }
1250  else if (descriptors.hasDescriptorBLD())
1251  {
1252  desc.setSize(1, descriptors.BLD->size());
1253  for (size_t i = 0; i < descriptors.BLD->size(); i++)
1254  desc(0, i) = (*descriptors.BLD)[i];
1255  return true;
1256  }
1257  else if (descriptors.hasDescriptorLATCH())
1258  {
1259  desc.setSize(1, descriptors.LATCH->size());
1260  for (size_t i = 0; i < descriptors.LATCH->size(); i++)
1261  desc(0, i) = (*descriptors.LATCH)[i];
1262  return true;
1263  }
1264  else if (descriptors.hasDescriptorSURF())
1265  {
1266  desc.setSize(1, descriptors.SURF->size());
1267  for (size_t i = 0; i < descriptors.SURF->size(); i++)
1268  desc(0, i) = (*descriptors.SURF)[i];
1269  return true;
1270  }
1271  else if (descriptors.hasDescriptorSpinImg())
1272  {
1273  const size_t nR = descriptors.SpinImg_range_rows;
1274  const size_t nC =
1275  descriptors.SpinImg->size() / descriptors.SpinImg_range_rows;
1276  desc.resize(nR, nC);
1277  auto itD = descriptors.SpinImg->begin();
1278  for (size_t r = 0; r < nR; r++)
1279  for (size_t c = 0; c < nC; c++) desc.coeffRef(r, c) = *itD++;
1280  return true;
1281  }
1282  else if (descriptors.hasDescriptorPolarImg())
1283  {
1284  desc = *descriptors.PolarImg;
1285  return true;
1286  }
1287  else if (descriptors.hasDescriptorLogPolarImg())
1288  {
1289  desc = *descriptors.LogPolarImg;
1290  return true;
1291  }
1292  else
1293  return false;
1294 }
void updateMaxID(const TListIdx &idx)
Updates the value of the maximum ID of the features in the matched list, i.e.
Definition: CFeature.cpp:1186
float descriptorDistanceTo(const CFeature &oFeature, TDescriptorType descriptorToUse=descAny, bool normalize_distances=true) const
Computes the Euclidean Distance between this feature&#39;s and other feature&#39;s descriptors, using the given descriptor or the first present one.
Definition: CFeature.cpp:394
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CFeature.cpp:327
GLuint GLuint GLsizei count
Definition: glext.h:3532
#define MRPT_START
Definition: exceptions.h:241
uint8_t descriptorORBDistanceTo(const CFeature &oFeature) const
Computes the Hamming distance "this" and the "other" descriptor ORB descriptor.
Definition: CFeature.cpp:721
uint64_t TFeatureID
Definition of a feature ID.
float descriptorSpinImgDistanceTo(const CFeature &oFeature, bool normalize_distances=true) const
Computes the Euclidean Distance between "this" and the "other" descriptors.
Definition: CFeature.cpp:519
void resize(size_t row, size_t col)
TInternalFeatList::iterator iterator
Definition: CFeature.h:331
std::optional< std::vector< uint8_t > > ORB
ORB feature descriptor.
Definition: CFeature.h:135
void loadFromTextFile(const std::string &fileName)
Save feature list to a text file.
Definition: CFeature.cpp:959
#define M_2PI
Definition: common.h:58
Template for column vectors of dynamic size, compatible with Eigen.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
Create a GUI window and display plots with MATLAB-like interfaces and commands.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
std::optional< std::vector< uint8_t > > BLD
BLD feature descriptor.
Definition: CFeature.h:139
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:6604
TKeyPointMethod type
Keypoint method used to detect this feature.
Definition: CFeature.h:73
TFeatureTrackStatus track_status
Status of the feature tracking process.
Definition: CFeature.h:76
Intensity-domain spin image descriptors.
size_t size() const
Definition: CFeature.h:352
void dumpToConsole() const
Definition: CFeature.cpp:312
float descriptorSIFTDistanceTo(const CFeature &oFeature, bool normalize_distances=true) const
Computes the Euclidean Distance between "this" and the "other" descriptors.
Definition: CFeature.cpp:464
float descriptorSURFDistanceTo(const CFeature &oFeature, bool normalize_distances=true) const
Computes the Euclidean Distance between "this" and the "other" descriptors.
Definition: CFeature.cpp:491
Bit-based feature descriptor.
A class for parsing text files, returning each non-empty and non-comment line, along its line number...
Scale Invariant Feature Transform [LOWE&#39;04].
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glext.h:3606
STL namespace.
TFeatureID ID
ID of the feature.
Definition: TKeyPoint.h:39
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
Definition: CFeature.cpp:70
GLenum GLsizei width
Definition: glext.h:3535
Used in some methods to mean "any of the present descriptors".
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CFeature.cpp:314
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void dumpToTextStream(std::ostream &out) const
Dump feature information into a text stream.
Definition: CFeature.cpp:253
void getBothFeatureLists(CFeatureList &list1, CFeatureList &list2)
Returns the matching features as two separate CFeatureLists.
Definition: CFeature.cpp:1222
void saveToTextFile(const std::string &filename, bool APPEND=false)
Save the feature to a text file in this format: "%% Dump of mrpt::vision::CFeatureList. Each line format is:\n" "%% ID TYPE X Y ORIENTATION SCALE TRACK_STATUS RESPONSE HAS_SIFT [SIFT] HAS_SURF [SURF] HAS_MULTI [MULTI_i] HAS_ORB [ORB]" "%% |---------------------- feature ------------------| |---------------------- descriptors ------------------------|" "%% with:\n" "%% TYPE : The used detector: 0:KLT, 1: Harris, 2: BCD, 3: SIFT, 4: SURF, 5: Beacon, 6: FAST, 7: ORB\n" "%% HAS_* : 1 if a descriptor of that type is associated to the feature.
Definition: CFeature.cpp:801
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void resize(size_t N)
Definition: CFeature.h:358
TKeyPointf keypoint
Definition: CFeature.h:64
std::optional< mrpt::img::CImage > patch
A patch of the image surrounding the feature.
Definition: CFeature.h:67
void getMaxID(const TListIdx &idx, TFeatureID &firstListID, TFeatureID &secondListID)
Returns the maximum ID of the features in the list.
Definition: CFeature.cpp:1206
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
T square(const T x)
Inline function for the square of a number.
const GLubyte * c
Definition: glext.h:6406
GLuint GLuint end
Definition: glext.h:3532
mrpt::config::CConfigFileBase CConfigFileBase
TKeyPointMethod
Types of key point detectors.
TDescriptorType
The bitwise OR combination of values of TDescriptorType are used in CFeatureExtraction::computeDescri...
int val
Definition: mrpt_jpeglib.h:957
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
A generic 2D feature from an image, extracted with CFeatureExtraction Each feature may have one or mo...
Definition: CFeature.h:53
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
float descriptorLATCHDistanceTo(const CFeature &oFeature, bool normalize_distances=true) const
Computes the Euclidean Distance between "this" and the "other" descriptors.
Definition: CFeature.cpp:776
static float internal_distanceBetweenPolarImages(const mrpt::math::CMatrixF &desc1, const mrpt::math::CMatrixF &desc2, float &minDistAngle, bool normalize_distances, bool dont_shift_angle)
Internal function used by "descriptorLogPolarImgDistanceTo" and "descriptorPolarImgDistanceTo".
Definition: CFeature.cpp:547
Speeded Up Robust Feature [BAY&#39;06].
This CStream derived class allow using a file as a write-only, binary stream.
mrpt::gui::CDisplayWindow3D::Ptr win
void saveToTextFile(const std::string &fileName)
Save list of matched features to a text file.
Definition: CFeature.cpp:1149
GLsizei const GLchar ** string
Definition: glext.h:4116
size_type rows() const
Number of rows in the matrix.
const CFeature * getByID(const TFeatureID &ID) const
Get a reference to a Feature from its ID.
Definition: CFeature.cpp:1086
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:275
size_type cols() const
Number of columns in the matrix.
const CFeature * nearest(const float x, const float y, double &max_dist) const
Get a reference to the nearest feature to the a given 2D point (version returning distance to closest...
Definition: CFeature.cpp:1110
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
bool empty() const
Definition: ts_hash_map.h:191
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:332
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CFeature.cpp:213
std::optional< mrpt::math::CMatrixF > PolarImg
A polar image centered at the interest point.
Definition: CFeature.h:124
void saveToTextFile(const std::string &fileName, bool APPEND=false)
Save feature list to a text file.
Definition: CFeature.cpp:881
float response
A measure of the "goodness" of the feature.
Definition: CFeature.h:79
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CFeature.cpp:313
bool getFirstDescriptorAsMatrix(mrpt::math::CMatrixFloat &desc) const
Return the first found descriptor, as a matrix.
Definition: CFeature.cpp:1241
virtual ~CFeatureList()
Virtual destructor.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
Definition: CFeature.cpp:185
const_iterator begin() const
Definition: ts_hash_map.h:240
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
void meanAndStd(const VECTORLIKE &v, double &out_mean, double &out_std, bool unbiased=true)
Computes the standard deviation of a vector (or all elements of a matrix)
std::optional< std::vector< uint8_t > > SIFT
SIFT feature descriptor.
Definition: CFeature.h:110
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CFeature.cpp:103
Scalar sum() const
Sum of all elements in matrix/vector.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
Load all the params from a config source, in the format described in saveToConfigFile() ...
Definition: CFeature.cpp:150
mrpt::vision::TStereoCalibResults out
TDescriptors descriptors
Definition: CFeature.h:157
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
const CFeature * getByID(const TFeatureID &ID, const TListIdx &idx)
Returns a smart pointer to the feature with the provided ID or a empty one if not found...
Definition: CFeature.cpp:1171
float descriptorPolarImgDistanceTo(const CFeature &oFeature, float &minDistAngle, bool normalize_distances=true) const
Returns the minimum Euclidean Distance between "this" and the "other" polar image descriptor...
Definition: CFeature.cpp:662
matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
std::optional< std::vector< float > > SURF
SURF feature descriptor.
Definition: CFeature.h:113
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
uint8_t octave
The image octave the image was found in: 0=original image, 1=1/2 image, 2=1/4 image, etc.
Definition: TKeyPoint.h:49
float patchCorrelationTo(const CFeature &oFeature) const
Computes the normalized cross-correlation between the patches of this and another feature (normalized...
Definition: CFeature.cpp:372
TFeatureID getMaxID() const
Get the maximum ID into the list.
Definition: CFeature.cpp:1135
Struct containing the options when matching multi-resolution SIFT-like descriptors.
GLenum GLint GLint y
Definition: glext.h:3542
Scalar minCoeff() const
Minimum value in the matrix/vector.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
GLsizeiptr size
Definition: glext.h:3934
float orientation
Main orientation of the feature.
Definition: CFeature.h:81
void resize(std::size_t N, bool zeroNewElements=false)
GLenum GLint x
Definition: glext.h:3542
void openCV_cross_correlation(const mrpt::img::CImage &img, const mrpt::img::CImage &patch_img, size_t &x_max, size_t &y_max, double &max_val, int x_search_ini=-1, int y_search_ini=-1, int x_search_size=-1, int y_search_size=-1)
Computes the correlation between this image and another one, encapsulating the openCV function cvMatc...
float descriptorLogPolarImgDistanceTo(const CFeature &oFeature, float &minDistAngle, bool normalize_distances=true) const
Returns the minimum Euclidean Distance between "this" and the "other" log-polar image descriptor...
Definition: CFeature.cpp:690
GLenum GLsizei GLsizei height
Definition: glext.h:3558
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
This template class provides the basic functionality for a general 2D any-size, resizable container o...
images resize(NUM_IMGS)
float descriptorBLDDistanceTo(const CFeature &oFeature, bool normalize_distances=true) const
Computes the Euclidean Distance between "this" and the "other" descriptors.
Definition: CFeature.cpp:751
GLfloat GLfloat p
Definition: glext.h:6398
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3532
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1887
std::optional< std::vector< float > > SpinImg
The 2D histogram as a single row.
Definition: CFeature.h:116
std::optional< mrpt::math::CMatrixF > LogPolarImg
A log-polar image centered at the interest point.
Definition: CFeature.h:127
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:191
pixel_coords_t pt
Coordinates in the image.
Definition: TKeyPoint.h:36
void copyListFrom(const CFeatureList &otherList)
Copies the content of another CFeatureList inside this one.
Definition: CFeature.cpp:1074
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
bool isPointFeature() const
Return false only for Blob detectors (SIFT, SURF)
Definition: CFeature.cpp:364
Scalar & coeffRef(int r, int c)
std::optional< std::vector< uint8_t > > LATCH
LATCH feature descriptor.
Definition: CFeature.h:141



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019