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