Main MRPT website > C++ reference for MRPT 1.9.9
CFeature.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+
9  */
10 
11 #include "vision-precomp.h" // Precompiled headers
12 
17 #include <mrpt/vision/CFeature.h>
18 #include <mrpt/vision/types.h>
20 #include <mrpt/math/data_utils.h>
21 #include <mrpt/system/os.h>
22 #include <iostream>
23 
24 using namespace mrpt;
25 using namespace mrpt::vision;
26 using namespace mrpt::img;
27 using namespace mrpt::math;
28 using namespace mrpt::system;
29 using namespace mrpt::io;
30 using namespace std;
31 
33 
34 // --------------------------------------------------
35 // loadFromConfigFile
36 // --------------------------------------------------
37 /** Load all the params from a config source, in the format described in
38  * saveToConfigFile()
39  */
40 void TMultiResDescMatchOptions::loadFromConfigFile(
41  const mrpt::config::CConfigFileBase& cfg, const std::string& section)
42 {
43  useOriFilter = cfg.read_bool(section, "useOriFilter", true, false);
44  oriThreshold = cfg.read_double(section, "oriThreshold", 0.2, false);
45  lastSeenThreshold = cfg.read_int(section, "lastSeenThreshold", 10, false);
46  timesSeenThreshold = cfg.read_int(section, "timesSeenThreshold", 5, false);
47  minFeaturesToFind = cfg.read_int(section, "minFeaturesToFind", 5, false);
48  minFeaturesToBeLost =
49  cfg.read_int(section, "minFeaturesToBeLost", 5, false);
50 
51  useDepthFilter = cfg.read_bool(section, "useDepthFilter", true, false);
52 
53  matchingThreshold =
54  cfg.read_double(section, "matchingThreshold", 1e4, false);
55  matchingRatioThreshold =
56  cfg.read_double(section, "matchingRatioThreshold", 0.5, false);
57 
58  lowScl1 = cfg.read_int(section, "lowScl1", 0, false);
59  lowScl2 = cfg.read_int(section, "lowScl1", 0, false);
60  highScl1 = cfg.read_int(section, "highScl1", 6, false);
61  highScl2 = cfg.read_int(section, "highScl2", 6, false);
62 
63  searchAreaSize = cfg.read_double(section, "searchAreaSize", 20, false);
64 }
65 
66 // --------------------------------------------------
67 // saveToConfigFile
68 // --------------------------------------------------
70  mrpt::config::CConfigFileBase& cfg, const std::string& section) const
71 {
72  if (useOriFilter)
73  {
74  cfg.write(section, "useOriFilter", "true");
75  cfg.write(section, "oriThreshold", oriThreshold);
76  }
77  else
78  cfg.write(section, "useOriFilter", "false");
79 
80  if (useDepthFilter)
81  cfg.write(section, "useDepthFilter", "true");
82  else
83  cfg.write(section, "useDepthFilter", "false");
84 
85  cfg.write(section, "matchingThreshold", matchingThreshold);
86  cfg.write(section, "matchingRatioThreshold", matchingRatioThreshold);
87  cfg.write(section, "lowScl1", lowScl1);
88  cfg.write(section, "lowScl2", lowScl2);
89  cfg.write(section, "highScl1", highScl1);
90  cfg.write(section, "highScl2", highScl2);
91 
92  cfg.write(section, "searchAreaSize", searchAreaSize);
93  cfg.write(section, "lastSeenThreshold", lastSeenThreshold);
94  cfg.write(section, "timesSeenThreshold", timesSeenThreshold);
95  cfg.write(section, "minFeaturesToFind", minFeaturesToFind);
96  cfg.write(section, "minFeaturesToBeLost", minFeaturesToBeLost);
97 } // end-saveToConfigFile
98 
99 // --------------------------------------------------
100 // dumpToTextStream
101 // --------------------------------------------------
102 void TMultiResDescMatchOptions::dumpToTextStream(std::ostream& out) const
103 {
104  out << mrpt::format(
105  "\n----------- [vision::TMultiResDescMatchOptions] ------------ \n");
106  out << mrpt::format("Use orientation filter?: ");
107  if (useOriFilter)
108  {
109  out << mrpt::format("Yes\n");
110  out << mrpt::format(
111  "· Orientation threshold: %.1f deg\n",
112  RAD2DEG(oriThreshold));
113  }
114  else
115  out << mrpt::format("No\n");
116  out << mrpt::format("Use depth filter?: ");
117  if (useDepthFilter)
118  out << mrpt::format("Yes\n");
119  else
120  {
121  out << mrpt::format("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 << mrpt::format(
142  "-------------------------------------------------------- \n");
143 } // end-dumpToTextStream
144 
145 // --------------------------------------------------
146 // loadFromConfigFile
147 // --------------------------------------------------
148 /** Load all the params from a config source, in the format described in
149  * saveToConfigFile()
150  */
152  const mrpt::config::CConfigFileBase& cfg, const std::string& section)
153 {
154  basePSize = cfg.read_double(section, "basePSize", 23, false);
155  comLScl = cfg.read_int(section, "comLScl", 0, false);
156  comHScl = cfg.read_int(section, "comHScl", 6, false);
157  sg1 = cfg.read_double(section, "sg1", 0.5, false);
158  sg2 = cfg.read_double(section, "sg2", 7.5, false);
159  sg3 = cfg.read_double(section, "sg3", 8.0, false);
160  computeDepth = cfg.read_bool(section, "computeDepth", true, false);
161  blurImage = cfg.read_bool(section, "blurImage", true, false);
162  fx = cfg.read_double(section, "fx", 0.0, false);
163  cx = cfg.read_double(section, "cx", 0.0, false);
164  cy = cfg.read_double(section, "cy", 0.0, false);
165  baseline = cfg.read_double(section, "baseline", 0.0, false);
166  computeHashCoeffs =
167  cfg.read_bool(section, "computeHashCoeffs", false, false);
168 
169  cfg.read_vector(section, "scales", vector<double>(), scales, false);
170  if (scales.size() < 1)
171  {
172  scales.resize(7);
173  scales[0] = 0.5;
174  scales[1] = 0.8;
175  scales[2] = 1.0;
176  scales[3] = 1.2;
177  scales[4] = 1.5;
178  scales[5] = 1.8;
179  scales[6] = 2.0;
180  } // end-if
181 }
182 
183 // --------------------------------------------------
184 // saveToConfigFile
185 // --------------------------------------------------
187  mrpt::config::CConfigFileBase& cfg, const std::string& section) const
188 {
189  cfg.write(section, "basePSize", basePSize);
190  cfg.write(section, "comLScl", comLScl);
191  cfg.write(section, "comHScl", comHScl);
192  cfg.write(section, "sg1", sg1);
193  cfg.write(section, "sg2", sg2);
194  cfg.write(section, "sg3", sg3);
195 
196  cfg.write(section, "computeDepth", computeDepth ? "true" : "false");
197  cfg.write(section, "blurImage", blurImage ? "true" : "false");
198  cfg.write(section, "fx", fx);
199  cfg.write(section, "cx", cx);
200  cfg.write(section, "cy", cy);
201  cfg.write(section, "baseline", baseline);
202  cfg.write(
203  section, "computeHashCoeffs", computeHashCoeffs ? "true" : "false");
204 
205  char buf[300];
206  for (unsigned int k = 0; k < scales.size(); ++k)
207  mrpt::system::os::sprintf(buf, 300, "%.2f ", scales[k]);
208  cfg.write(section, "scales", buf);
209 } // end-saveToConfigFile
210 
211 // --------------------------------------------------
212 // dumpToTextStream
213 // --------------------------------------------------
214 void TMultiResDescOptions::dumpToTextStream(std::ostream& out) const
215 {
216  out << mrpt::format(
217  "\n----------- [vision::TMultiResDescOptions] ------------ \n");
218  out << mrpt::format("Base patch size: %d px\n", basePSize);
219  out << mrpt::format("Lowest scale to compute: %d\n", comLScl);
220  out << mrpt::format("Highest scale to compute: %d\n", comHScl);
221  out << mrpt::format("Image smoothing sigma: %.2f px\n", sg1);
222  out << mrpt::format("Orientation histogram sigma: %.2f\n", sg2);
223  out << mrpt::format("Descriptor histogram sigma: %.2f\n", sg3);
224  out << mrpt::format("Compute depth: ");
225  if (computeDepth)
226  {
227  out << mrpt::format("Yes\n");
228  out << mrpt::format("Focal length: %.2f px\n", fx);
229  out << mrpt::format("Principal point (cx): %.2f px\n", cx);
230  out << mrpt::format("Principal point (cy): %.2f px\n", cy);
231  out << mrpt::format(
232  "Baseline: %.2f m\n", baseline);
233  }
234  else
235  out << mrpt::format("No\n");
236 
237  out << mrpt::format("Compute Hash Coeffs: ");
238  if (computeHashCoeffs)
239  out << mrpt::format("Yes\n");
240  else
241  out << mrpt::format("No\n");
242 
243  out << mrpt::format("Blur image previously: ");
244  if (blurImage)
245  out << mrpt::format("Yes\n");
246  else
247  out << mrpt::format("No\n");
248 
249  out << mrpt::format("Scales: ");
250  for (unsigned int k = 0; k < scales.size(); ++k)
251  out << mrpt::format("%.2f ", scales[k]);
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("Feature ID: %d\n", (int)ID);
261  out << mrpt::format(
262  "Coordinates: (%.2f,%.2f) px\n", x, y);
263  out << mrpt::format("PatchSize: %d\n", patchSize);
264  out << mrpt::format("Type: ");
265  switch (type)
266  {
267  case -1:
268  out << mrpt::format("Not defined\n");
269  break;
270  case 0:
271  out << mrpt::format("KLT\n");
272  break;
273  case 1:
274  out << mrpt::format("Harris\n");
275  break;
276  case 2:
277  out << mrpt::format("BCD\n");
278  break;
279  case 3:
280  out << mrpt::format("SIFT\n");
281  break;
282  case 4:
283  out << mrpt::format("SURF\n");
284  break;
285  case 5:
286  out << mrpt::format("Beacon\n");
287  break;
288  case 6:
289  out << mrpt::format("FAST\n");
290  break;
291  case 7:
292  out << mrpt::format("FASTER-9\n");
293  break;
294  case 8:
295  out << mrpt::format("FASTER-10\n");
296  break;
297  case 9:
298  out << mrpt::format("FASTER-12\n");
299  break;
300  case 10:
301  out << mrpt::format("ORB\n");
302  break;
303  case 11:
304  out << mrpt::format("AKAZE\n");
305  break;
306  case 12:
307  out << mrpt::format("LSD");
308  break;
309  }
310  out << mrpt::format("Status: ");
311  switch (track_status)
312  {
313  case 0:
314  out << mrpt::format("Idle\n");
315  break;
316  case 1:
317  out << mrpt::format("[KLT] Out of bounds [KLT]\n");
318  break;
319  case 5:
320  out << mrpt::format("[KLT] Tracked\n");
321  break;
322  case 10:
323  out << mrpt::format("[KLT] Lost\n");
324  break;
325  }
326 
327  out << mrpt::format("Response: %.2f\n", response);
328  out << mrpt::format("Main orientation: %.2f\n", orientation);
329  out << mrpt::format("Main scale: %.2f\n", scale);
330  out << mrpt::format("# frames seen: %d\n", nTimesSeen);
331  out << mrpt::format("# frames not seen: %d\n", nTimesNotSeen);
332  out << mrpt::format("# frames since last seen: %d\n", nTimesLastSeen);
333  out << mrpt::format(
334  "Initial Depth: %.2f m\n", initialDepth);
335  out << mrpt::format("Depth: %.2f m\n", depth);
336  out << mrpt::format(
337  "3D point: (%.2f,%.2f,%.2f) m\n", p3D.x, p3D.y,
338  p3D.z);
339  out << mrpt::format("Is point feature?: ");
340  isPointFeature() ? out << mrpt::format("Yes\n")
341  : out << mrpt::format("No\n");
342 
343  out << mrpt::format("Has SIFT descriptor?: ");
344  descriptors.hasDescriptorSIFT() ? out << mrpt::format("Yes\n")
345  : out << mrpt::format("No\n");
346  out << mrpt::format("Has SURF descriptor?: ");
347  descriptors.hasDescriptorSURF() ? out << mrpt::format("Yes\n")
348  : out << mrpt::format("No\n");
349  out << mrpt::format("Has Spin image descriptor?: ");
350  descriptors.hasDescriptorSpinImg() ? out << mrpt::format("Yes\n")
351  : out << mrpt::format("No\n");
352  out << mrpt::format("Has Polar descriptor?: ");
353  descriptors.hasDescriptorPolarImg() ? out << mrpt::format("Yes\n")
354  : out << mrpt::format("No\n");
355  out << mrpt::format("Has Log Polar descriptor?: ");
356  descriptors.hasDescriptorLogPolarImg() ? out << mrpt::format("Yes\n")
357  : out << mrpt::format("No\n");
358  out << mrpt::format("Has ORB descriptor?: ");
359  descriptors.hasDescriptorORB() ? out << mrpt::format("Yes\n")
360  : out << mrpt::format("No\n");
361  //# added by Raghavender Sahdev
362  out << mrpt::format("Has BLD descriptor?: ");
363  descriptors.hasDescriptorBLD() ? out << mrpt::format("Yes\n")
364  : out << mrpt::format("No\n");
365  out << mrpt::format("Has LATCH descriptor?: ");
366  descriptors.hasDescriptorLATCH() ? out << mrpt::format("Yes\n")
367  : out << mrpt::format("No\n");
368 
369  out << mrpt::format("Has multiscale?: ");
370  if (!descriptors.hasDescriptorMultiSIFT())
371  out << mrpt::format("No\n");
372  else
373  {
374  out << mrpt::format("Yes [%d]\n", (int)multiScales.size());
375  for (int k = 0; k < (int)multiScales.size(); ++k)
376  {
377  out << mrpt::format(" · Scale %d: %.2f\n", k, multiScales[k]);
378  for (int m = 0; m < (int)multiOrientations[k].size(); ++m)
379  {
380  out << mrpt::format(
381  " ·· Orientation %d: %.2f\n", m, multiOrientations[k][m]);
382  out << mrpt::format(" ·· [D] ");
383  for (int n = 0;
384  n < (int)descriptors.multiSIFTDescriptors[k][m].size();
385  ++n)
386  out << mrpt::format(
387  "%d ", descriptors.multiSIFTDescriptors[k][m][n]);
388  out << mrpt::format("\n");
389  if (multiHashCoeffs.size() > 0)
390  out << mrpt::format(
391  " ·· HASH coefficients %d,%d,%d\n",
392  multiHashCoeffs[k][m][0], multiHashCoeffs[k][m][1],
393  multiHashCoeffs[k][m][2]);
394  } // end-for-m
395  } // end-for-k
396  } // end else
397 } // end dumpToTextStream
398 
399 void CFeature::dumpToConsole() const { dumpToTextStream(std::cout); }
402 {
403  // The coordinates:
404  out << x << y << ID << patch << patchSize << (uint32_t)type
405  << (uint32_t)track_status << response << orientation << scale
406  << user_flags << nTimesSeen << nTimesNotSeen << nTimesLastSeen << depth
407  << initialDepth << p3D << multiScales << multiOrientations
408  << multiHashCoeffs << descriptors.SIFT << descriptors.SURF
409  << descriptors.SpinImg << descriptors.SpinImg_range_rows
410  << descriptors.PolarImg << descriptors.LogPolarImg
411  << descriptors.polarImgsNoRotation << descriptors.multiSIFTDescriptors
412  << descriptors.ORB
413  //# ADDED by Raghavender Sahdev
414  << descriptors.BLD << descriptors.LATCH;
415 }
416 
418 {
419  switch (version)
420  {
421  case 0:
422  case 1:
423  case 2:
424  {
425  // The coordinates:
426  uint32_t aux_type, aux_KLTS;
427  in >> x >> y >> ID >> patch >> patchSize >> aux_type >> aux_KLTS >>
428  response >> orientation >> scale >> user_flags;
429  if (version > 0)
430  {
431  in >> nTimesSeen >> nTimesNotSeen >> nTimesLastSeen >> depth >>
432  initialDepth >> p3D >> multiScales >> multiOrientations >>
433  multiHashCoeffs;
434  }
435  in >> descriptors.SIFT >> descriptors.SURF >> descriptors.SpinImg >>
436  descriptors.SpinImg_range_rows >> descriptors.PolarImg >>
437  descriptors.LogPolarImg >> descriptors.polarImgsNoRotation
438  // # added by Raghavender Sahdev
439  >> descriptors.BLD >> descriptors.LATCH;
440  if (version > 0) in >> descriptors.multiSIFTDescriptors;
441  if (version > 1) in >> descriptors.ORB;
442 
443  type = (TFeatureType)aux_type;
444  track_status = (TFeatureTrackStatus)aux_KLTS;
445  }
446  break;
447  default:
449  };
450 }
451 
452 /****************************************************
453  Class CFEATURE
454 *****************************************************/
455 // CONSTRUCTOR
457  : x(0.0f),
458  y(0.0f),
459  ID(0),
460  patchSize(21),
462  track_status(status_IDLE),
463  response(0.0),
464  orientation(0.0),
465  scale(0.0),
466  user_flags(0),
467  nTimesSeen(1),
468  nTimesNotSeen(0),
469  nTimesLastSeen(0),
470  depth(0),
471  initialDepth(0),
472  p3D(),
473  multiScales(),
474  multiOrientations(),
475  multiHashCoeffs(),
476  descriptors()
477 {
478 }
479 
480 // Ctor
482  : SIFT(),
483  SURF(),
484  SpinImg(),
485  SpinImg_range_rows(0),
486  PolarImg(0, 0),
487  LogPolarImg(0, 0),
488  polarImgsNoRotation(false),
489  ORB(),
490  BLD(),
491  LATCH()
492 {
493 }
494 
495 // Return false only for Blob detectors (SIFT, SURF)
497 {
498  return type == featSIFT || type == featSURF;
499 }
500 
501 // --------------------------------------------------
502 // patchCorrelationTo
503 // --------------------------------------------------
504 float CFeature::patchCorrelationTo(const CFeature& oFeature) const
505 {
506  MRPT_START
507  ASSERT_(patch.getWidth() == oFeature.patch.getWidth());
508  ASSERT_(patch.getHeight() == oFeature.patch.getHeight());
509  ASSERT_(patch.getHeight() > 0 && patch.getWidth() > 0);
510  size_t x_max, y_max;
511  double max_val;
512  patch.cross_correlation(oFeature.patch, x_max, y_max, max_val);
513 
514  return 0.5 -
515  0.5 * max_val; // Value as "distance" in the range [0,1], best = 0
516 
517  MRPT_END
518 }
519 
520 // --------------------------------------------------
521 // descriptorDistanceTo
522 // --------------------------------------------------
524  const CFeature& oFeature, TDescriptorType descriptorToUse,
525  bool normalize_distances) const
526 {
527  MRPT_START
528 
529  // If we are not ask for a specific descriptor, select the first one found:
530  if (descriptorToUse == descAny)
531  {
533  descriptorToUse = descSIFT;
534  else if (descriptors.hasDescriptorSURF())
535  descriptorToUse = descSURF;
537  descriptorToUse = descSpinImages;
539  descriptorToUse = descPolarImages;
541  descriptorToUse = descLogPolarImages;
542  else if (descriptors.hasDescriptorORB())
543  descriptorToUse = descORB;
544  // # added by Raghavender Sahdev - BLD/LATCH descriptors
545  else if (descriptors.hasDescriptorBLD())
546  descriptorToUse = descBLD;
547  else if (descriptors.hasDescriptorLATCH())
548  descriptorToUse = descLATCH;
549  else
551  "Feature has no descriptors and descriptorToUse=descAny")
552  }
553 
554  switch (descriptorToUse)
555  {
556  case descSIFT:
557  return descriptorSIFTDistanceTo(oFeature, normalize_distances);
558  case descSURF:
559  return descriptorSURFDistanceTo(oFeature, normalize_distances);
560  case descSpinImages:
561  return descriptorSpinImgDistanceTo(oFeature, normalize_distances);
562  case descPolarImages:
563  {
564  float minAng;
566  oFeature, minAng, normalize_distances);
567  }
568  case descLogPolarImages:
569  {
570  float minAng;
572  oFeature, minAng, normalize_distances);
573  }
574  case descORB:
575  return float(descriptorORBDistanceTo(oFeature));
576  // # added by Raghavender Sahdev
577  case descBLD:
578  return (descriptorBLDDistanceTo(oFeature));
579  case descLATCH:
580  return (descriptorLATCHDistanceTo(oFeature));
581  default:
583  "Unknown value for 'descriptorToUse'=%u",
584  (unsigned)descriptorToUse);
585  }
586 
587  MRPT_END
588 }
589 
590 // --------------------------------------------------
591 // descriptorSIFTDistanceTo
592 // --------------------------------------------------
594  const CFeature& oFeature, bool normalize_distances) const
595 {
596  ASSERT_(this->descriptors.SIFT.size() == oFeature.descriptors.SIFT.size());
597  ASSERT_(
598  this->descriptors.hasDescriptorSIFT() &&
599  oFeature.descriptors.hasDescriptorSIFT());
600 
601  float dist = 0.0f;
603  for (itDesc1 = this->descriptors.SIFT.begin(),
604  itDesc2 = oFeature.descriptors.SIFT.begin();
605  itDesc1 != this->descriptors.SIFT.end(); itDesc1++, itDesc2++)
606  {
607  dist += square(*itDesc1 - *itDesc2);
608  }
609  if (normalize_distances) dist /= this->descriptors.SIFT.size();
610  dist = sqrt(dist);
611  if (normalize_distances) dist /= 64.0f;
612  return dist;
613 } // end descriptorSIFTDistanceTo
614 
615 // --------------------------------------------------
616 // descriptorSURFDistanceTo
617 // --------------------------------------------------
619  const CFeature& oFeature, bool normalize_distances) const
620 {
621  ASSERT_(this->descriptors.SURF.size() == oFeature.descriptors.SURF.size());
622  ASSERT_(
623  this->descriptors.hasDescriptorSURF() &&
624  oFeature.descriptors.hasDescriptorSURF());
625 
626  float dist = 0.0f;
627  std::vector<float>::const_iterator itDesc1, itDesc2;
628  for (itDesc1 = this->descriptors.SURF.begin(),
629  itDesc2 = oFeature.descriptors.SURF.begin();
630  itDesc1 != this->descriptors.SURF.end(); itDesc1++, itDesc2++)
631  {
632  dist += square(*itDesc1 - *itDesc2);
633  }
634  if (normalize_distances) dist /= this->descriptors.SURF.size();
635  dist = sqrt(dist);
636  if (normalize_distances)
637  dist /= 0.20f; // JL: Ad-hoc value! Investigate where does this come
638  // from...
639  return dist;
640 } // end descriptorSURFDistanceTo
641 
642 // --------------------------------------------------
643 // descriptorSpinImgDistanceTo
644 // --------------------------------------------------
646  const CFeature& oFeature, bool normalize_by_vector_length) const
647 {
648  ASSERT_(
649  this->descriptors.SpinImg.size() ==
650  oFeature.descriptors.SpinImg.size());
651  ASSERT_(
653  oFeature.descriptors.hasDescriptorSpinImg());
654  ASSERT_(!this->descriptors.SpinImg.empty());
655  float dist = 0.0f;
656  std::vector<float>::const_iterator itDesc1, itDesc2;
657  for (itDesc1 = this->descriptors.SpinImg.begin(),
658  itDesc2 = oFeature.descriptors.SpinImg.begin();
659  itDesc1 != this->descriptors.SpinImg.end(); itDesc1++, itDesc2++)
660  {
661  dist += square(*itDesc1 - *itDesc2);
662  }
663 
664  if (normalize_by_vector_length) dist /= 0.25 * descriptors.SpinImg.size();
665 
666  return sqrt(dist);
667 } // end descriptorSpinImgDistanceTo
668 
669 // --------------------------------------------------
670 // descriptorPolarImgDistanceTo
671 // --------------------------------------------------
673  const CMatrix& desc1, const CMatrix& desc2, float& minDistAngle,
674  bool normalize_distances, bool dont_shift_angle)
675 {
676  MRPT_START
677 
678  // Find the smallest distance:
679  unsigned int delta, i, j, ii, height = desc1.rows(), width = desc1.cols();
680  float dist, minDist = 0;
681 
682 //#define LM_CORR_BIAS_MEAN
683 
684 #define LM_CORR_METHOD_EUCLID
685 //#define LM_CORR_METHOD_MANHATTAN
686 //#define LM_CORR_METHOD_CORRELATION
687 
688 #if defined(LM_CORR_BIAS_MEAN) || defined(LM_CORR_METHOD_CORRELATION)
689  const float desc1_mean = desc1.sum() / static_cast<float>(width * height);
690  const float desc2_mean = desc2.sum() / static_cast<float>(width * height);
691 #endif
692 
693  CVectorFloat distances(height, 0); // Distances for each shift
694 
695  for (delta = 0; delta < height; delta++)
696  {
697 #if defined(LM_CORR_METHOD_CORRELATION)
698  float s11 = 0;
699  float s22 = 0;
700  float s12 = 0;
701 #endif
702  // Compute the mean distance between desc1[t] and desc2[t-delta]:
703  dist = 0;
704  for (i = 0; i < height; i++)
705  {
706  ii = (i + delta) % height; // Shifted index
707  for (j = 0; j < width; j++)
708  {
709 #ifdef LM_CORR_METHOD_EUCLID
710 #ifdef LM_CORR_BIAS_MEAN
711  dist += square(
712  desc1.get_unsafe(i, j) - desc1_mean -
713  desc2.get_unsafe(ii, j) + desc2_mean);
714 #else
715  dist +=
716  square(desc1.get_unsafe(i, j) - desc2.get_unsafe(ii, j));
717 #endif
718 #elif defined(LM_CORR_METHOD_MANHATTAN)
719 #ifdef LM_CORR_BIAS_MEAN
720  dist +=
721  abs(desc1.get_unsafe(i, j) - desc1_mean -
722  desc2.get_unsafe(ii, j) + desc2_mean);
723 #else
724  dist += abs(desc1.get_unsafe(i, j) - desc2.get_unsafe(ii, j));
725 #endif
726 #elif defined(LM_CORR_METHOD_CORRELATION)
727  float d1 = desc1.get_unsafe(i, j) - desc1_mean;
728  float d2 = desc2.get_unsafe(ii, j) - desc2_mean;
729  s11 += square(d1);
730  s22 += square(d2);
731  s12 += d1 * d2;
732 #else
733 #error A LM_CORR_METHOD_XXX method must be selected!
734 #endif
735  }
736  }
737 
738  // Average:
739  if (normalize_distances) dist /= static_cast<float>(width * height);
740 
741 #ifdef LM_CORR_METHOD_EUCLID
742  dist = sqrt(dist);
743 #endif
744 
745 #if defined(LM_CORR_METHOD_CORRELATION)
746  dist = 1 - (s12 / sqrt(s11 * s22));
747 #endif
748 
749  distances[delta] = dist;
750  if (!delta && dont_shift_angle)
751  {
752  distances.resize(1);
753  break;
754  }
755  } // end for delta
756 
757  size_t minDistIdx;
758  minDist = distances.minimum(&minDistIdx);
759 
760  double dist_mean, dist_std;
761  mrpt::math::meanAndStd(distances, dist_mean, dist_std);
762 
763 #if 0
764  {
765  cout << "min dist: " << minDist << endl;
766 
767  static mrpt::gui::CDisplayWindowPlots win("distances");
768  win.plot(distances,"b.4");
769  CImage img1(desc1);
770  win.image(img1,0,-0.5,0.4*width,0.5,"img1");
771 
772  CImage img2(desc2);
773  win.image(img2,0.6*width,-0.5,0.4*width,0.5,"img2");
774 
775  //win.axis_fit();
776  win.waitForKey();
777  }
778 #endif
779 
780  // Output:
781  minDistAngle = minDistIdx * M_2PI / static_cast<float>(width);
782  return minDist;
783 
784  MRPT_END
785 }
786 
787 // --------------------------------------------------
788 // descriptorPolarImgDistanceTo
789 // --------------------------------------------------
791  const CFeature& oFeature, float& minDistAngle,
792  bool normalize_distances) const
793 {
794  MRPT_START
795 
796  ASSERT_(
797  descriptors.PolarImg.rows() == oFeature.descriptors.PolarImg.rows());
798  ASSERT_(
799  descriptors.PolarImg.cols() == oFeature.descriptors.PolarImg.cols());
800  ASSERT_(
803  ASSERT_(descriptors.PolarImg.rows() > 1 && descriptors.PolarImg.cols() > 1);
804 
805  // Call the common method for computing these distances:
807  descriptors.PolarImg, oFeature.descriptors.PolarImg, minDistAngle,
808  normalize_distances, descriptors.polarImgsNoRotation);
809 
810  MRPT_END
811 } // end descriptorPolarImgDistanceTo
812 
813 // --------------------------------------------------
814 // descriptorLogPolarImgDistanceTo
815 // --------------------------------------------------
817  const CFeature& oFeature, float& minDistAngle,
818  bool normalize_distances) const
819 {
820  MRPT_START
821 
822  ASSERT_(
823  descriptors.LogPolarImg.rows() ==
824  oFeature.descriptors.LogPolarImg.rows());
825  ASSERT_(
826  descriptors.LogPolarImg.cols() ==
827  oFeature.descriptors.LogPolarImg.cols());
828  ASSERT_(
831  ASSERT_(
832  descriptors.LogPolarImg.rows() > 1 &&
833  descriptors.LogPolarImg.cols() > 1);
834 
835  // Call the common method for computing these distances:
837  descriptors.LogPolarImg, oFeature.descriptors.LogPolarImg, minDistAngle,
838  normalize_distances, descriptors.polarImgsNoRotation);
839 
840  MRPT_END
841 } // end descriptorPolarImgDistanceTo
842 
843 // --------------------------------------------------
844 // descriptorORBDistanceTo
845 // --------------------------------------------------
847 {
848  ASSERT_(
849  this->descriptors.hasDescriptorORB() &&
850  oFeature.descriptors.hasDescriptorORB());
851  ASSERT_(this->descriptors.ORB.size() == oFeature.descriptors.ORB.size());
852  const std::vector<uint8_t>& t_desc = this->descriptors.ORB;
853  const std::vector<uint8_t>& o_desc = oFeature.descriptors.ORB;
854 
855  // Descriptors XOR + Hamming weight
856  uint8_t distance = 0;
857  for (uint8_t k = 0; k < t_desc.size(); ++k)
858  {
859  uint8_t x_or = t_desc[k] ^ o_desc[k];
860  uint8_t count; // from : Wegner, Peter (1960), "A technique for
861  // counting ones in a binary computer", Communications
862  // of the ACM 3 (5): 322, doi:10.1145/367236.367286
863  for (count = 0; x_or; count++) // ...
864  x_or &= x_or - 1; // ...
865  distance += count;
866  }
867 
868  return float(distance);
869 } // end-descriptorORBDistanceTo
870 
871 // # added by Raghavender Sahdev
872 // --------------------------------------------------
873 // descriptorBLDDistanceTo
874 // --------------------------------------------------
876  const CFeature& oFeature, bool normalize_distances) const
877 {
878  ASSERT_(this->descriptors.BLD.size() == oFeature.descriptors.BLD.size());
879  ASSERT_(
880  this->descriptors.hasDescriptorBLD() &&
881  oFeature.descriptors.hasDescriptorBLD());
882 
883  float dist = 0.0f;
885  for (itDesc1 = this->descriptors.BLD.begin(),
886  itDesc2 = oFeature.descriptors.BLD.begin();
887  itDesc1 != this->descriptors.BLD.end(); itDesc1++, itDesc2++)
888  {
889  dist += square(*itDesc1 - *itDesc2);
890  }
891  if (normalize_distances) dist /= this->descriptors.BLD.size();
892  dist = sqrt(dist);
893  if (normalize_distances) dist /= 64.0f;
894  return dist;
895 } // end descriptorBLDDistanceTo
896 
897 // --------------------------------------------------
898 // descriptorLATCHDistanceTo
899 // --------------------------------------------------
901  const CFeature& oFeature, bool normalize_distances) const
902 {
903  ASSERT_(
904  this->descriptors.LATCH.size() == oFeature.descriptors.LATCH.size());
905  ASSERT_(
907  oFeature.descriptors.hasDescriptorLATCH());
908 
909  float dist = 0.0f;
911  for (itDesc1 = this->descriptors.LATCH.begin(),
912  itDesc2 = oFeature.descriptors.LATCH.begin();
913  itDesc1 != this->descriptors.LATCH.end(); itDesc1++, itDesc2++)
914  {
915  dist += square(*itDesc1 - *itDesc2);
916  }
917  if (normalize_distances) dist /= this->descriptors.LATCH.size();
918  dist = sqrt(dist);
919  if (normalize_distances) dist /= 64.0f;
920  return dist;
921 } // end descriptorLATCHDistanceTo
922 
923 // --------------------------------------------------
924 // saveToTextFile
925 // --------------------------------------------------
926 void CFeature::saveToTextFile(const std::string& filename, bool APPEND)
927 {
928  MRPT_START
929  // "%% Dump of mrpt::vision::CFeatureList. Each line format is:\n"
930  // "%% ID TYPE X Y ORIENTATION SCALE TRACK_STATUS RESPONSE HAS_SIFT
931  // [SIFT] HAS_SURF [SURF] HAS_MULTI [MULTI_i] HAS_ORB [ORB]"
932  // "%% \\---------------------- feature ------------------/ \\---------
933  // descriptors -------/\n"
934  // "%% with:\n"
935  // "%% TYPE : The used detector: 0:KLT, 1: Harris, 2: BCD, 3: SIFT, 4:
936  // SURF, 5: Beacon, 6: FAST\n"
937  // "%% HAS_* : 1 if a descriptor of that type is associated to the
938  // feature. \n"
939  // "%% SIFT : Present if HAS_SIFT=1: N DESC_0 ... DESC_N-1 \n"
940  // "%% SURF : Present if HAS_SURF=1: N DESC_0 ... DESC_N-1 \n"
941  // "%% MULTI : Present if HAS_MULTI=1: SCALE ORI N DESC_0 ... DESC_N-1"
942  // "%% ORB : Present if HAS_ORB=1: VALUE
943  // "%%-------------------------------------------------------------------------------------------\n");
945 
946  if (!f.open(filename, APPEND))
948  "[CFeature::saveToTextFile] ERROR: File could not be open for "
949  "writing");
950 
951  f.printf(
952  "%5u %2d %7.3f %7.3f %6.2f %6.2f %2d %6.3f ", (unsigned int)this->ID,
953  (int)this->get_type(), this->x, this->y, this->orientation, this->scale,
954  (int)this->track_status, this->response);
955 
956  f.printf("%2d ", int(this->descriptors.hasDescriptorSIFT() ? 1 : 0));
957  if (this->descriptors.hasDescriptorSIFT())
958  {
959  f.printf("%4d ", int(this->descriptors.SIFT.size()));
960  for (unsigned int k = 0; k < this->descriptors.SIFT.size(); k++)
961  f.printf("%4d ", this->descriptors.SIFT[k]);
962  }
963 
964  f.printf("%2d ", int(this->descriptors.hasDescriptorSURF() ? 1 : 0));
965  if (this->descriptors.hasDescriptorSURF())
966  {
967  f.printf("%4d ", int(this->descriptors.SURF.size()));
968  for (unsigned int k = 0; k < this->descriptors.SURF.size(); k++)
969  f.printf("%8.5f ", this->descriptors.SURF[k]);
970  }
971 
972  f.printf("%2d ", int(this->descriptors.hasDescriptorMultiSIFT() ? 1 : 0));
974  {
975  for (int k = 0; k < int(this->multiScales.size()); ++k)
976  {
977  for (int m = 0; m < int(this->multiOrientations[k].size()); ++m)
978  {
979  f.printf(
980  "%.2f %6.2f ", this->multiScales[k],
981  this->multiOrientations[k][m]);
982  f.printf(
983  "%4d ",
984  int(this->descriptors.multiSIFTDescriptors[k][m].size()));
985  for (unsigned int n = 0;
986  n < this->descriptors.multiSIFTDescriptors[k][m].size();
987  ++n)
988  f.printf(
989  "%4d ",
990  this->descriptors.multiSIFTDescriptors[k][m][n]);
991  }
992  } // end-for
993  } // end-if
994 
995  f.printf("%2d ", int(this->descriptors.hasDescriptorORB() ? 1 : 0));
996  if (this->descriptors.hasDescriptorORB())
997  for (size_t k = 0; k < this->descriptors.ORB.size(); ++k)
998  f.printf("%d ", this->descriptors.ORB[k]);
999 
1000  // # ADDED by Raghavender Sahdev
1001  f.printf("%2d ", int(this->descriptors.hasDescriptorBLD() ? 1 : 0));
1002  if (this->descriptors.hasDescriptorBLD())
1003  {
1004  f.printf("%4d ", int(this->descriptors.BLD.size()));
1005  for (unsigned int k = 0; k < this->descriptors.BLD.size(); k++)
1006  f.printf("%4d ", this->descriptors.BLD[k]);
1007  }
1008 
1009  f.printf("%2d ", int(this->descriptors.hasDescriptorLATCH() ? 1 : 0));
1010  if (this->descriptors.hasDescriptorLATCH())
1011  {
1012  f.printf("%4d ", int(this->descriptors.LATCH.size()));
1013  for (unsigned int k = 0; k < this->descriptors.LATCH.size(); k++)
1014  f.printf("%4d ", this->descriptors.LATCH[k]);
1015  }
1016 
1017  f.printf("\n");
1018  f.close();
1019 
1020  MRPT_END
1021 } // end saveToTextFile
1022 
1023 /****************************************************
1024  Class CFEATURELIST
1025 *****************************************************/
1026 // --------------------------------------------------
1027 // CONSTRUCTOR
1028 // --------------------------------------------------
1029 CFeatureList::CFeatureList() {} // end constructor
1030 // --------------------------------------------------
1031 // DESTRUCTOR
1032 // --------------------------------------------------
1033 CFeatureList::~CFeatureList() {} // end destructor
1034 // --------------------------------------------------
1035 // saveToTextFile
1036 // --------------------------------------------------
1037 // FORMAT: ID type x y orientation scale [descriptorSIFT] [descriptorSURF]
1038 // track_status response
1039 void CFeatureList::saveToTextFile(const std::string& filename, bool APPEND)
1040 {
1041  MRPT_START
1042 
1044 
1045  if (!f.open(filename, APPEND))
1047  "[CFeatureList::saveToTextFile] ERROR: File could not be open for "
1048  "writing");
1049 
1050  f.printf(
1051  "%% Dump of mrpt::vision::CFeatureList. Each line format is:\n"
1052  "%% ID TYPE X Y ORIENTATION SCALE TRACK_STATUS RESPONSE HAS_SIFT "
1053  "[SIFT] HAS_SURF [SURF]\n"
1054  "%% \\---------------------- feature ------------------/ \\--------- "
1055  "descriptors -------/\n"
1056  "%% with:\n"
1057  "%% TYPE : The used detector: 0:KLT, 1: Harris, 2: BCD, 3: SIFT, 4: "
1058  "SURF, 5: Beacon, 6: FAST\n"
1059  "%% HAS_* : 1 if a descriptor of that type is associated to the "
1060  "feature. \n"
1061  "%% SIFT : Present if HAS_SIFT=1: N DESC_0 ... DESC_N-1 \n"
1062  "%% SURF : Present if HAS_SURF=1: N DESC_0 ... DESC_N-1 \n"
1063  "%%--------------------------------------------------------------------"
1064  "-----------------------\n");
1065 
1066  for (CFeatureList::iterator it = this->begin(); it != this->end(); ++it)
1067  {
1068  f.printf(
1069  "%5u %2d %7.3f %7.3f %6.2f %6.2f %2d %6.3f ",
1070  (unsigned int)(*it)->ID, (int)(*it)->get_type(), (*it)->x, (*it)->y,
1071  (*it)->orientation, (*it)->scale, (int)(*it)->track_status,
1072  (*it)->response);
1073 
1074  f.printf("%2d ", int((*it)->descriptors.hasDescriptorSIFT() ? 1 : 0));
1075  if ((*it)->descriptors.hasDescriptorSIFT())
1076  {
1077  f.printf("%4d ", int((*it)->descriptors.SIFT.size()));
1078  for (unsigned int k = 0; k < (*it)->descriptors.SIFT.size(); k++)
1079  f.printf("%4d ", (*it)->descriptors.SIFT[k]);
1080  }
1081 
1082  f.printf("%2d ", int((*it)->descriptors.hasDescriptorSURF() ? 1 : 0));
1083  if ((*it)->descriptors.hasDescriptorSURF())
1084  {
1085  f.printf("%4d ", int((*it)->descriptors.SURF.size()));
1086  for (unsigned int k = 0; k < (*it)->descriptors.SURF.size(); k++)
1087  f.printf("%8.5f ", (*it)->descriptors.SURF[k]);
1088  }
1089  // # added by Raghavender Sahdev
1090  f.printf("%2d ", int((*it)->descriptors.hasDescriptorBLD() ? 1 : 0));
1091  if ((*it)->descriptors.hasDescriptorBLD())
1092  {
1093  f.printf("%4d ", int((*it)->descriptors.BLD.size()));
1094  for (unsigned int k = 0; k < (*it)->descriptors.BLD.size(); k++)
1095  f.printf("%4d ", (*it)->descriptors.BLD[k]);
1096  }
1097 
1098  f.printf("%2d ", int((*it)->descriptors.hasDescriptorLATCH() ? 1 : 0));
1099  if ((*it)->descriptors.hasDescriptorLATCH())
1100  {
1101  f.printf("%4d ", int((*it)->descriptors.LATCH.size()));
1102  for (unsigned int k = 0; k < (*it)->descriptors.LATCH.size(); k++)
1103  f.printf("%4d ", (*it)->descriptors.LATCH[k]);
1104  }
1105 
1106  f.printf("\n");
1107  } // end for
1108 
1109  f.close();
1110 
1111  MRPT_END
1112 } // end saveToTextFile
1113 
1114 // --------------------------------------------------
1115 // loadFromTextFile
1116 // --------------------------------------------------
1118 {
1119  MRPT_START
1120 
1121  mrpt::io::CTextFileLinesParser parser(filename);
1122  std::istringstream line;
1123 
1124  while (parser.getNextLine(line))
1125  {
1126  try
1127  {
1128  CFeature::Ptr feat_ptr = std::make_shared<CFeature>();
1129  CFeature* feat = feat_ptr.get(); // for faster access
1130 
1131  int _ID;
1132  if (!(line >> _ID)) throw std::string("ID");
1133  feat->ID = TFeatureID(_ID);
1134 
1135  int _type;
1136  if (!(line >> _type)) throw std::string("type");
1137  feat->type = TFeatureType(_type);
1138 
1139  if (!(line >> feat->x >> feat->y)) throw std::string("x,y");
1140  if (!(line >> feat->orientation)) throw std::string("orientation");
1141  if (!(line >> feat->scale)) throw std::string("scale");
1142 
1143  int _track_st;
1144  if (!(line >> _track_st)) throw std::string("track_status");
1145  feat->track_status = TFeatureTrackStatus(_track_st);
1146 
1147  if (!(line >> feat->response)) throw std::string("response");
1148 
1149  int hasSIFT;
1150  if (!(line >> hasSIFT)) throw std::string("hasSIFT");
1151  if (hasSIFT)
1152  {
1153  size_t N;
1154  if (!(line >> N)) throw std::string("SIFT-len");
1155  feat->descriptors.SIFT.resize(N);
1156  for (size_t i = 0; i < N; i++)
1157  {
1158  int val;
1159  line >> val;
1160  feat->descriptors.SIFT[i] =
1161  val; // DON'T read directly SIFT[i] since it's a
1162  // uint8_t, interpreted as a cha
1163  }
1164 
1165  if (!line) throw std::string("SIFT-data");
1166  }
1167 
1168  //# ADDED by Raghavender Sahdev
1169  int hasBLD;
1170  if (!(line >> hasBLD)) throw std::string("hasBLD");
1171  if (hasBLD)
1172  {
1173  size_t N;
1174  if (!(line >> N)) throw std::string("BLD-len");
1175  feat->descriptors.BLD.resize(N);
1176  for (size_t i = 0; i < N; i++)
1177  {
1178  int val;
1179  line >> val;
1180  feat->descriptors.BLD[i] =
1181  val; // comment copied from SIFT, DON'T read directly
1182  // SIFT[i] since it's a uint8_t, interpreted as a
1183  // cha
1184  }
1185 
1186  if (!line) throw std::string("BLD-data");
1187  }
1188 
1189  int hasLATCH;
1190  if (!(line >> hasLATCH)) throw std::string("hasLATCH");
1191  if (hasBLD)
1192  {
1193  size_t N;
1194  if (!(line >> N)) throw std::string("LATCH-len");
1195  feat->descriptors.LATCH.resize(N);
1196  for (size_t i = 0; i < N; i++)
1197  {
1198  int val;
1199  line >> val;
1200  feat->descriptors.LATCH[i] =
1201  val; // comment copied from SIFT, DON'T read directly
1202  // SIFT[i] since it's a uint8_t, interpreted as a
1203  // cha
1204  }
1205 
1206  if (!line) throw std::string("LATCH-data");
1207  }
1208 
1209  int hasSURF;
1210  if (!(line >> hasSURF)) throw std::string("hasSURF");
1211  if (hasSURF)
1212  {
1213  size_t N;
1214  if (!(line >> N)) throw std::string("SURF-len");
1215  feat->descriptors.SURF.resize(N);
1216  for (size_t i = 0; i < N; i++)
1217  line >> feat->descriptors.SURF[i];
1218  if (!line) throw std::string("SURF-data");
1219  }
1220 
1221  push_back(feat_ptr);
1222  }
1223  catch (std::string& msg)
1224  {
1226  format(
1227  "%s:%d: Error parsing features text file (%s).",
1228  filename.c_str(), (int)parser.getCurrentLineNumber(),
1229  msg.c_str()))
1230  }
1231  }
1232 
1233  MRPT_END
1234 } // end loadFromTextFile
1235 
1236 // --------------------------------------------------
1237 // copyListFrom()
1238 // --------------------------------------------------
1240 {
1241  this->resize(otherList.size());
1244  for (it1 = otherList.begin(), it2 = this->begin(); it1 != otherList.end();
1245  ++it1, ++it2)
1246  {
1247  *it2 = *it1;
1248  (*it2).reset(dynamic_cast<CFeature*>((*it2)->clone()));
1249  }
1250 } // end-copyListFrom
1251 
1252 // --------------------------------------------------
1253 // getByID()
1254 // --------------------------------------------------
1256 {
1257  for (CFeatureList::const_iterator it = begin(); it != end(); ++it)
1258  if ((*it)->ID == ID) return (*it);
1259 
1260  return CFeature::Ptr();
1261 } // end getByID
1262 
1263 // --------------------------------------------------
1264 // getByID()
1265 // --------------------------------------------------
1267 {
1268  int k = 0;
1269  for (CFeatureList::const_iterator it = begin(); it != end(); ++it, ++k)
1270  if ((*it)->ID == ID)
1271  {
1272  out_idx = k;
1273  return (*it);
1274  }
1275  out_idx = -1;
1276  return CFeature::Ptr();
1277 } // end getByID
1278 
1279 // --------------------------------------------------
1280 // getByID()
1281 // --------------------------------------------------
1283  const vector<TFeatureID>& IDs, vector<CFeature::Ptr>& out,
1284  vector<int>& outIndex) const
1285 {
1286  out.clear();
1287  outIndex.clear();
1288  out.reserve(IDs.size());
1289  outIndex.reserve(IDs.size());
1290 
1291  for (int k = 0; k < int(IDs.size()); ++k)
1292  {
1293  int idx;
1294  CFeature::Ptr f = getByID(IDs[k], idx);
1295  out.push_back(f);
1296  outIndex.push_back(idx);
1297  }
1298 } // end getByID
1299 
1300 // --------------------------------------------------
1301 // nearest(x,y)
1302 // --------------------------------------------------
1304  const float x, const float y, double& dist_prev) const
1305 {
1306  if (this->empty()) return CFeature::Ptr();
1307 
1308  float closest_x, closest_y;
1309  float closest_sqDist;
1310 
1311  // Look for the closest feature using KD-tree look up:
1312  const size_t closest_idx =
1313  this->kdTreeClosestPoint2D(x, y, closest_x, closest_y, closest_sqDist);
1314  float closest_dist = std::sqrt(closest_sqDist);
1315 
1316  if (closest_dist <= dist_prev)
1317  {
1318  dist_prev = closest_dist;
1319  return m_feats[closest_idx];
1320  }
1321  else
1322  return CFeature::Ptr();
1323 } // end nearest
1324 
1325 // --------------------------------------------------
1326 // getMaxID()
1327 // --------------------------------------------------
1329 {
1330  MRPT_START
1331  ASSERT_(!empty());
1332  vision::TFeatureID maxID = (*begin())->ID;
1333  for (CFeatureList::const_iterator itList = begin(); itList != end();
1334  itList++)
1335  mrpt::keep_max(maxID, (*itList)->ID);
1336  return maxID;
1337  MRPT_END
1338 
1339 } // end getMaxID()
1340 
1341 /****************************************************
1342  Class CMATCHEDFEATUREKLT
1343 *****************************************************/
1344 // --------------------------------------------------
1345 // CONSTRUCTOR
1346 // --------------------------------------------------
1347 CMatchedFeatureList::CMatchedFeatureList() : m_leftMaxID(0), m_rightMaxID(0) {}
1348 // --------------------------------------------------
1349 // DESTRUCTOR
1350 // --------------------------------------------------
1352 // --------------------------------------------------
1353 // saveToTextFile
1354 // --------------------------------------------------
1356 {
1357  // OUTPUT FORMAT: ID_1 x_1 y_1 ID_2 x_2 y_2
1358 
1359  FILE* f = os::fopen(filename.c_str(), "wt");
1360  if (!f) return;
1361 
1363  for (it = this->begin(); it != this->end(); it++)
1364  {
1365  os::fprintf(
1366  f, "%d %.3f %.3f %d %.3f %.3f\n", (unsigned int)(*it->first).ID,
1367  (*it->first).x, (*it->first).y, (unsigned int)(*it->second).ID,
1368  (*it->second).x, (*it->second).y);
1369 
1370  } // end for
1371  os::fclose(f);
1372 }
1373 
1374 // --------------------------------------------------
1375 // getBothFeatureLists
1376 // --------------------------------------------------
1378  const TFeatureID& ID, const TListIdx& idx)
1379 {
1381  for (it = begin(); it != end(); ++it)
1382  {
1383  CFeature::Ptr feat = (idx == firstList) ? it->first : it->second;
1384  if (feat->ID == ID) return feat;
1385  }
1386  return CFeature::Ptr();
1387 }
1388 
1389 // --------------------------------------------------
1390 // updateMaxID()
1391 // --------------------------------------------------
1393 {
1394  MRPT_START
1395  TFeatureID maxID1 = begin()->first->ID;
1396  TFeatureID maxID2 = begin()->second->ID;
1397  for (CMatchedFeatureList::const_iterator itList = begin(); itList != end();
1398  itList++)
1399  {
1400  if (idx == firstList || idx == bothLists)
1401  mrpt::keep_max(maxID1, itList->first->ID);
1402  if (idx == secondList || idx == bothLists)
1403  mrpt::keep_max(maxID2, itList->second->ID);
1404  }
1405  if (idx == firstList || idx == bothLists) m_leftMaxID = maxID1;
1406  if (idx == secondList || idx == bothLists) m_rightMaxID = maxID2;
1407  MRPT_END
1408 }
1409 
1410 // --------------------------------------------------
1411 // getMaxID()
1412 // --------------------------------------------------
1414  const TListIdx& idx, TFeatureID& firstListID, TFeatureID& secondListID)
1415 {
1416  MRPT_START
1417  ASSERT_(!empty());
1418  if (idx == firstList || idx == bothLists)
1419  if (m_leftMaxID == 0) updateMaxID(firstList);
1420  if (idx == secondList || idx == bothLists)
1421  if (m_rightMaxID == 0) updateMaxID(secondList);
1422  firstListID = m_leftMaxID;
1423  secondListID = m_rightMaxID;
1424  MRPT_END
1425 } // end getMaxID()
1426 // --------------------------------------------------
1427 // getBothFeatureLists
1428 // --------------------------------------------------
1430  CFeatureList& list1, CFeatureList& list2)
1431 {
1432  MRPT_START
1433  list1.resize(this->size());
1434  list2.resize(this->size());
1435 
1436  unsigned int k = 0;
1437  for (CMatchedFeatureList::iterator it = this->begin(); it != this->end();
1438  ++it, ++k)
1439  {
1440  list1[k] = it->first;
1441  list2[k] = it->second;
1442  } // end for
1443  MRPT_END
1444 }
1445 
1446 // --------------------------------------------------
1447 // getFirstDescriptorAsMatrix
1448 // --------------------------------------------------
1450 {
1452  {
1453  desc.setSize(1, descriptors.SIFT.size());
1454  for (size_t i = 0; i < descriptors.SIFT.size(); i++)
1455  desc(0, i) = descriptors.SIFT[i];
1456  return true;
1457  }
1458  else if (descriptors.hasDescriptorBLD())
1459  {
1460  desc.setSize(1, descriptors.BLD.size());
1461  for (size_t i = 0; i < descriptors.BLD.size(); i++)
1462  desc(0, i) = descriptors.BLD[i];
1463  return true;
1464  }
1465  else if (descriptors.hasDescriptorLATCH())
1466  {
1467  desc.setSize(1, descriptors.LATCH.size());
1468  for (size_t i = 0; i < descriptors.LATCH.size(); i++)
1469  desc(0, i) = descriptors.LATCH[i];
1470  return true;
1471  }
1472  else if (descriptors.hasDescriptorSURF())
1473  {
1474  desc.setSize(1, descriptors.SURF.size());
1475  for (size_t i = 0; i < descriptors.SURF.size(); i++)
1476  desc(0, i) = descriptors.SURF[i];
1477  return true;
1478  }
1479  else if (descriptors.hasDescriptorSpinImg())
1480  {
1481  const size_t nR = descriptors.SpinImg_range_rows;
1482  const size_t nC =
1484  desc.resize(nR, nC);
1486  for (size_t r = 0; r < nR; r++)
1487  for (size_t c = 0; c < nC; c++) desc.coeffRef(r, c) = *itD++;
1488  return true;
1489  }
1491  {
1492  desc = descriptors.PolarImg;
1493  return true;
1494  }
1496  {
1497  desc = descriptors.LogPolarImg;
1498  return true;
1499  }
1500  else
1501  return false;
1502 }
void updateMaxID(const TListIdx &idx)
Updates the value of the maximum ID of the features in the matched list, i.e.
Definition: CFeature.cpp:1392
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:523
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CFeature.cpp:417
std::shared_ptr< CFeature > Ptr
Definition: CFeature.h:58
Scalar * iterator
Definition: eigen_plugins.h:26
GLuint GLuint GLsizei count
Definition: glext.h:3528
Non-defined feature (also used for Occupancy features)
TFeatureID ID
ID of the feature.
Definition: CFeature.h:62
EIGEN_STRONG_INLINE bool empty() const
#define MRPT_START
Definition: exceptions.h:262
uint8_t descriptorORBDistanceTo(const CFeature &oFeature) const
Computes the Hamming distance "this" and the "other" descriptor ORB descriptor.
Definition: CFeature.cpp:846
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:645
bool hasDescriptorBLD() const
Whether this feature has this kind of descriptor.
Definition: CFeature.h:169
void cross_correlation(const CImage &patch_img, size_t &u_max, size_t &v_max, double &max_val, int u_search_ini=-1, int v_search_ini=-1, int u_search_size=-1, int v_search_size=-1, CImage *out_corr_image=nullptr) const
Computes the correlation between this image and another one, encapsulating the openCV function cvMatc...
Definition: CImage.cpp:1454
double RAD2DEG(const double x)
Radians to degrees.
TInternalFeatList::iterator iterator
Definition: CFeature.h:365
Used in some methods to mean "any of the present descriptors".
std::vector< uint8_t > BLD
BLD feature descriptor.
Definition: CFeature.h:134
void loadFromTextFile(const std::string &fileName)
Save feature list to a text file.
Definition: CFeature.cpp:1117
#define M_2PI
Definition: common.h:58
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
Create a GUI window and display plots with MATLAB-like interfaces and commands.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
bool hasDescriptorLogPolarImg() const
Whether this feature has this kind of descriptor.
Definition: CFeature.h:153
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:6502
TFeatureTrackStatus track_status
featKLT, featHarris, featSURF, featBeacon
Definition: CFeature.h:68
size_t size() const
Definition: CFeature.h:386
void dumpToConsole() const
Definition: CFeature.cpp:399
float descriptorSIFTDistanceTo(const CFeature &oFeature, bool normalize_distances=true) const
Computes the Euclidean Distance between "this" and the "other" descriptors.
Definition: CFeature.cpp:593
GLenum GLsizei n
Definition: glext.h:5074
float descriptorSURFDistanceTo(const CFeature &oFeature, bool normalize_distances=true) const
Computes the Euclidean Distance between "this" and the "other" descriptors.
Definition: CFeature.cpp:618
A class for parsing text files, returning each non-empty and non-comment line, along its line number...
void getByMultiIDs(const std::vector< TFeatureID > &IDs, std::vector< CFeature::Ptr > &out, std::vector< int > &outIndex) const
Get a vector of references to a subset of features from their IDs.
Definition: CFeature.cpp:1282
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:29
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
EIGEN_STRONG_INLINE void push_back(Scalar val)
Insert an element at the end of the container (for 1D vectors/arrays)
std::vector< float > SURF
SURF feature descriptor.
Definition: CFeature.h:112
std::deque< double > multiScales
A set of scales where the.
Definition: CFeature.h:92
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glext.h:3600
STL namespace.
TFeatureType get_type() const
Get the type of the feature.
Definition: CFeature.h:276
mrpt::math::CMatrix LogPolarImg
A log-polar image centered at the interest point.
Definition: CFeature.h:121
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:69
bool hasDescriptorMultiSIFT() const
Whether this feature has this kind of descriptor.
Definition: CFeature.h:157
GLenum GLsizei width
Definition: glext.h:3531
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CFeature.cpp:401
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
unsigned char uint8_t
Definition: rptypes.h:41
Inactive (right after detection, and before being tried to track)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void dumpToTextStream(std::ostream &out) const
Dump feature information into a text stream.
Definition: CFeature.cpp:257
bool polarImgsNoRotation
If set to true (manually, default=false)
Definition: CFeature.h:122
void getBothFeatureLists(CFeatureList &list1, CFeatureList &list2)
Returns the matching features as two separate CFeatureLists.
Definition: CFeature.cpp:1429
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:926
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.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:864
void resize(size_t N)
Definition: CFeature.h:392
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...
void getMaxID(const TListIdx &idx, TFeatureID &firstListID, TFeatureID &secondListID)
Returns the maximum ID of the features in the list.
Definition: CFeature.cpp:1413
static float internal_distanceBetweenPolarImages(const mrpt::math::CMatrix &desc1, const mrpt::math::CMatrix &desc2, float &minDistAngle, bool normalize_distances, bool dont_shift_angle)
Internal function used by "descriptorLogPolarImgDistanceTo" and "descriptorPolarImgDistanceTo".
Definition: CFeature.cpp:672
const GLubyte * c
Definition: glext.h:6313
Scale Invariant Feature Transform [LOWE&#39;04].
GLuint GLuint end
Definition: glext.h:3528
mrpt::config::CConfigFileBase CConfigFileBase
std::vector< uint8_t > SIFT
SIFT feature descriptor.
Definition: CFeature.h:111
int val
Definition: mrpt_jpeglib.h:955
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
std::deque< std::vector< double > > multiOrientations
multi-resolution descriptor has been computed
Definition: CFeature.h:95
CFeature()
Constructor.
Definition: CFeature.cpp:456
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:20
uint16_t SpinImg_range_rows
The number of rows (corresponding to.
Definition: CFeature.h:114
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:900
Bit-based feature descriptor.
This CStream derived class allow using a file as a write-only, binary stream.
bool hasDescriptorORB() const
Whether this feature has this kind of descriptor.
Definition: CFeature.h:164
mrpt::gui::CDisplayWindow3D::Ptr win
void saveToTextFile(const std::string &fileName)
Save list of matched features to a text file.
Definition: CFeature.cpp:1355
GLsizei const GLchar ** string
Definition: glext.h:4101
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:304
TDescriptorType
The bitwise OR combination of values of TDescriptorType are used in CFeatureExtraction::computeDescri...
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())
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:366
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:406
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:214
CFeature::Ptr 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:1377
TFeatureType
Types of features - This means that the point has been detected with this algorithm, which is independent of additional descriptors a feature may also have.
bool hasDescriptorLATCH() const
Whether this feature has this kind of descriptor.
Definition: CFeature.h:173
CFeature::Ptr 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:1303
struct mrpt::vision::CFeature::TDescriptors descriptors
void saveToTextFile(const std::string &fileName, bool APPEND=false)
Save feature list to a text file.
Definition: CFeature.cpp:1039
float response
process (old name: KLT_status)
Definition: CFeature.h:70
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CFeature.cpp:400
bool getFirstDescriptorAsMatrix(mrpt::math::CMatrixFloat &desc) const
Return the first found descriptor, as a matrix.
Definition: CFeature.cpp:1449
virtual ~CFeatureList()
Virtual destructor.
Definition: CFeature.cpp:1033
CFeatureList()
Constructor.
Definition: CFeature.cpp:1029
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:186
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
void meanAndStd(const VECTORLIKE &v, double &out_mean, double &out_std, bool unbiased=true)
Computes the standard deviation of a vector.
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:102
std::vector< float > SpinImg
The 2D histogram as a single row.
Definition: CFeature.h:113
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:151
Speeded Up Robust Feature [BAY&#39;06].
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
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:790
#define MRPT_END
Definition: exceptions.h:266
std::vector< uint8_t > LATCH
LATCH feature descriptor.
Definition: CFeature.h:135
GLuint in
Definition: glext.h:7274
float patchCorrelationTo(const CFeature &oFeature) const
Computes the normalized cross-correlation between the patches of this and another feature (normalized...
Definition: CFeature.cpp:504
virtual ~CMatchedFeatureList()
Virtual destructor.
Definition: CFeature.cpp:1351
TFeatureID getMaxID() const
Get the maximum ID into the list.
Definition: CFeature.cpp:1328
Struct containing the options when matching multi-resolution SIFT-like descriptors.
GLenum GLint GLint y
Definition: glext.h:3538
Intensity-domain spin image descriptors.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:255
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
bool hasDescriptorPolarImg() const
Whether this feature has this kind of descriptor.
Definition: CFeature.h:149
GLsizeiptr size
Definition: glext.h:3923
float orientation
KLT_val)
Definition: CFeature.h:72
GLenum GLint x
Definition: glext.h:3538
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:816
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:24
GLenum GLsizei GLsizei height
Definition: glext.h:3554
mrpt::img::CImage patch
A patch of the image surrounding the feature.
Definition: CFeature.h:63
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
unsigned __int32 uint32_t
Definition: rptypes.h:47
float descriptorBLDDistanceTo(const CFeature &oFeature, bool normalize_distances=true) const
Computes the Euclidean Distance between "this" and the "other" descriptors.
Definition: CFeature.cpp:875
const Scalar * const_iterator
Definition: eigen_plugins.h:27
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3528
std::deque< std::vector< std::vector< int32_t > > > multiSIFTDescriptors
the call to "descriptorDistanceTo" will not consider all the rotations between polar image descriptor...
Definition: CFeature.h:128
CFeature::Ptr getByID(const TFeatureID &ID) const
Get a reference to a Feature from its ID.
Definition: CFeature.cpp:1255
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
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:189
mrpt::math::CMatrix PolarImg
range bins in the 2D histogram) of the original matrix from which SpinImg was extracted as a vector...
Definition: CFeature.h:119
void copyListFrom(const CFeatureList &otherList)
Copies the content of another CFeatureList inside this one.
Definition: CFeature.cpp:1239
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:130
bool isPointFeature() const
a HASH table of descriptors
Definition: CFeature.cpp:496
bool hasDescriptorSURF() const
Whether this feature has this kind of descriptor.
Definition: CFeature.h:141
std::vector< uint8_t > ORB
orientation and scale of the multiResolution feature (there is a vector of descriptors for each scale...
Definition: CFeature.h:132



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019