Main MRPT website > C++ reference for MRPT 1.9.9
CLandmarksMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/geometry.h>
13 #include <mrpt/utils/CFileOutputStream.h>
14 
15 #include <mrpt/maps/CLandmarksMap.h>
16 #include <mrpt/maps/CLandmark.h>
17 #include <mrpt/obs/CObservationImage.h>
18 #include <mrpt/obs/CObservationStereoImages.h>
19 #include <mrpt/obs/CObservation2DRangeScan.h>
20 #include <mrpt/obs/CObservationGPS.h>
21 #include <mrpt/poses/CPointPDFGaussian.h>
22 #include <mrpt/obs/CObservationRobotPose.h>
23 #include <mrpt/obs/CObservationBeaconRanges.h>
24 #include <mrpt/obs/CObservationVisualLandmarks.h>
25 #include <mrpt/system/os.h>
26 #include <mrpt/random.h>
27 
28 #include <mrpt/opengl/CGridPlaneXY.h>
29 #include <mrpt/opengl/CEllipsoid.h>
30 #include <mrpt/opengl/COpenGLScene.h>
31 
32 using namespace mrpt;
33 using namespace mrpt::math;
34 using namespace mrpt::maps;
35 using namespace mrpt::obs;
36 using namespace mrpt::utils;
37 using namespace mrpt::poses;
38 using namespace mrpt::random;
39 using namespace mrpt::system;
40 using namespace mrpt::vision;
41 using namespace mrpt::utils;
42 using namespace std;
43 using mrpt::maps::internal::TSequenceLandmarks;
44 
45 // =========== Begin of Map definition ============
46 MAP_DEFINITION_REGISTER("CLandmarksMap,landmarksMap", mrpt::maps::CLandmarksMap)
47 
48 CLandmarksMap::TMapDefinition::TMapDefinition() {}
49 void CLandmarksMap::TMapDefinition::loadFromConfigFile_map_specific(
50  const mrpt::utils::CConfigFileBase& source,
51  const std::string& sectionNamePrefix)
52 {
53  // [<sectionNamePrefix>+"_creationOpts"]
54  const std::string sSectCreation =
55  sectionNamePrefix + string("_creationOpts");
56  this->initialBeacons.clear();
57  const unsigned int nBeacons = source.read_int(sSectCreation, "nBeacons", 0);
58  for (unsigned int q = 1; q <= nBeacons; q++)
59  {
60  TPairIdBeacon newPair;
61  newPair.second =
62  source.read_int(sSectCreation, format("beacon_%03u_ID", q), 0);
63 
64  newPair.first.x =
65  source.read_float(sSectCreation, format("beacon_%03u_x", q), 0);
66  newPair.first.y =
67  source.read_float(sSectCreation, format("beacon_%03u_y", q), 0);
68  newPair.first.z =
69  source.read_float(sSectCreation, format("beacon_%03u_z", q), 0);
70 
71  this->initialBeacons.push_back(newPair);
72  }
73 
74  insertionOpts.loadFromConfigFile(
75  source, sectionNamePrefix + string("_insertOpts"));
76  likelihoodOpts.loadFromConfigFile(
77  source, sectionNamePrefix + string("_likelihoodOpts"));
78 }
79 
80 void CLandmarksMap::TMapDefinition::dumpToTextStream_map_specific(
81  mrpt::utils::CStream& out) const
82 {
83  out.printf(
84  "number of initial beacons = %u\n",
85  (int)initialBeacons.size());
86 
87  out.printf(" ID (X,Y,Z)\n");
88  out.printf("--------------------------------------------------------\n");
89  for (std::deque<TPairIdBeacon>::const_iterator p = initialBeacons.begin();
90  p != initialBeacons.end(); ++p)
91  out.printf(
92  " %03u (%8.03f,%8.03f,%8.03f)\n", p->second,
93  p->first.x, p->first.y, p->first.z);
94 
95  this->insertionOpts.dumpToTextStream(out);
96  this->likelihoodOpts.dumpToTextStream(out);
97 }
98 
99 mrpt::maps::CMetricMap* CLandmarksMap::internal_CreateFromMapDefinition(
100  const mrpt::maps::TMetricMapInitializer& _def)
101 {
102  const CLandmarksMap::TMapDefinition& def =
103  *dynamic_cast<const CLandmarksMap::TMapDefinition*>(&_def);
104  CLandmarksMap* obj = new CLandmarksMap();
105 
106  for (std::deque<
107  CLandmarksMap::TMapDefinition::TPairIdBeacon>::const_iterator p =
108  def.initialBeacons.begin();
109  p != def.initialBeacons.end(); ++p)
110  {
111  CLandmark lm;
112 
113  lm.createOneFeature();
114  lm.features[0]->type = featBeacon;
115 
116  lm.features[0]->ID = p->second;
117  lm.ID = p->second;
118 
119  lm.pose_mean = p->first;
120 
121  lm.pose_cov_11 = lm.pose_cov_22 = lm.pose_cov_33 = lm.pose_cov_12 =
122  lm.pose_cov_13 = lm.pose_cov_23 = square(0.01f);
123 
124  obj->landmarks.push_back(lm);
125  }
126 
127  obj->insertionOptions = def.insertionOpts;
128  obj->likelihoodOptions = def.likelihoodOpts;
129  return obj;
130 }
131 // =========== End of Map definition Block =========
132 
133 IMPLEMENTS_SERIALIZABLE(CLandmarksMap, CMetricMap, mrpt::maps)
134 
135 /*---------------------------------------------------------------
136  Static variables initialization
137  ---------------------------------------------------------------*/
138 std::map<std::pair<mrpt::maps::CLandmark::TLandmarkID,
139  mrpt::maps::CLandmark::TLandmarkID>,
140  double>
141  CLandmarksMap::_mEDD;
142 mrpt::maps::CLandmark::TLandmarkID CLandmarksMap::_mapMaxID;
143 bool CLandmarksMap::_maxIDUpdated = false;
144 /*---------------------------------------------------------------
145  Constructor
146  ---------------------------------------------------------------*/
147 CLandmarksMap::CLandmarksMap()
148  : landmarks(),
149  insertionOptions(),
150  likelihoodOptions(),
151  insertionResults(),
152  fuseOptions()
153 {
154  landmarks.clear();
155  //_mEDD.clear();
156  //_mapMaxID = 0;
157 }
158 
159 /*---------------------------------------------------------------
160  Destructor
161  ---------------------------------------------------------------*/
162 CLandmarksMap::~CLandmarksMap() { landmarks.clear(); }
163 /*---------------------------------------------------------------
164  clear
165  ---------------------------------------------------------------*/
166 void CLandmarksMap::internal_clear() { landmarks.clear(); }
167 /*---------------------------------------------------------------
168  getLandmarksCount
169  ---------------------------------------------------------------*/
170 size_t CLandmarksMap::size() const { return landmarks.size(); }
171 /*---------------------------------------------------------------
172  writeToStream
173  Implements the writing to a CStream capability of
174  CSerializable objects
175  ---------------------------------------------------------------*/
176 void CLandmarksMap::writeToStream(mrpt::utils::CStream& out, int* version) const
177 {
178  if (version)
179  *version = 0;
180  else
181  {
182  uint32_t n = landmarks.size();
183 
184  // First, write the number of landmarks:
185  out << n;
186 
187  // Write all landmarks:
188  for (TSequenceLandmarks::const_iterator it = landmarks.begin();
189  it != landmarks.end(); ++it)
190  out << (*it);
191  }
192 }
193 
194 /*---------------------------------------------------------------
195  readFromStream
196  Implements the reading from a CStream capability of
197  CSerializable objects
198  ---------------------------------------------------------------*/
199 void CLandmarksMap::readFromStream(mrpt::utils::CStream& in, int version)
200 {
201  switch (version)
202  {
203  case 0:
204  {
205  uint32_t n, i;
206  CLandmark lm;
207 
208  // Delete previous content of map:
209  // -------------------------------------
210  landmarks.clear();
211 
212  // Load from stream:
213  // -------------------------------------
214  in >> n;
215 
216  landmarks.clear(); // resize(n);
217 
218  // Read all landmarks:
219  for (i = 0; i < n; i++)
220  {
221  in >> lm;
222  landmarks.push_back(lm);
223  }
224  }
225  break;
226  default:
227  MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
228  };
229 }
230 
231 /*---------------------------------------------------------------
232  computeObservationLikelihood
233  ---------------------------------------------------------------*/
234 double CLandmarksMap::internal_computeObservationLikelihood(
235  const CObservation* obs, const CPose3D& robotPose3D)
236 {
237  MRPT_START
238 
239  if (CLASS_ID(CObservation2DRangeScan) == obs->GetRuntimeClass() &&
240  insertionOptions.insert_Landmarks_from_range_scans)
241  {
242  /********************************************************************
243  OBSERVATION TYPE: CObservation2DRangeScan
244  ********************************************************************/
245  const CObservation2DRangeScan* o =
246  static_cast<const CObservation2DRangeScan*>(obs);
247  CLandmarksMap auxMap;
248  CPose2D sensorPose2D(robotPose3D + o->sensorPose);
249 
250  auxMap.loadOccupancyFeaturesFrom2DRangeScan(
251  *o, &robotPose3D, likelihoodOptions.rangeScan2D_decimation);
252 
253  // And compute its likelihood:
254  return computeLikelihood_RSLC_2007(&auxMap, sensorPose2D);
255  } // end of likelihood of 2D range scan:
256  else if (CLASS_ID(CObservationStereoImages) == obs->GetRuntimeClass())
257  {
258  /********************************************************************
259  OBSERVATION TYPE: CObservationStereoImages
260  Lik. between "this" and "auxMap";
261  ********************************************************************/
262  const CObservationStereoImages* o =
263  static_cast<const CObservationStereoImages*>(obs);
264 
265  CLandmarksMap auxMap;
266  auxMap.insertionOptions = insertionOptions;
267  auxMap.loadSiftFeaturesFromStereoImageObservation(
268  *o, CLandmarksMap::_mapMaxID, likelihoodOptions.SIFT_feat_options);
269  auxMap.changeCoordinatesReference(robotPose3D);
270 
271  // ACCESS TO STATIC VARIABLE
272  // std::cout << "_mapMaxID, from " << CLandmarksMap::_mapMaxID << " to
273  // ";
274  if (!CLandmarksMap::_maxIDUpdated)
275  {
276  CLandmarksMap::_mapMaxID += auxMap.size();
277  CLandmarksMap::_maxIDUpdated = true;
278  } // end if
279 
280  // std::cout << CLandmarksMap::_mapMaxID <<std::endl;
281  return computeLikelihood_SIFT_LandmarkMap(&auxMap);
282 
283  } // end of likelihood of Stereo Images scan:
284  else if (CLASS_ID(CObservationBeaconRanges) == obs->GetRuntimeClass())
285  {
286  /********************************************************************
287 
288  OBSERVATION TYPE: CObservationBeaconRanges
289 
290  Lik. between "this" and "auxMap";
291 
292  ********************************************************************/
293  const CObservationBeaconRanges* o =
294  static_cast<const CObservationBeaconRanges*>(obs);
295 
296  std::deque<CObservationBeaconRanges::TMeasurement>::const_iterator it;
297  TSequenceLandmarks::iterator lm_it;
298  CPointPDFGaussian beaconPDF;
299  CPoint3D point3D, beacon3D;
300  double ret = 0; // 300;
301 
302  for (it = o->sensedData.begin(); it != o->sensedData.end(); it++)
303  {
304  // Look for the beacon in this map:
305  unsigned int sensedID = it->beaconID;
306  bool found = false;
307 
308  for (lm_it = landmarks.begin(); !found && lm_it != landmarks.end();
309  lm_it++)
310  {
311  if ((lm_it->getType() == featBeacon) &&
312  (lm_it->ID == sensedID) &&
313  (!std::isnan(it->sensedDistance)))
314  {
315  lm_it->getPose(beaconPDF);
316  beacon3D = beaconPDF.mean;
317 
318  // Compute the 3D position of the sensor:
319  point3D = robotPose3D + it->sensorLocationOnRobot;
320 
321  const float expectedRange = point3D.distanceTo(beacon3D);
322  float sensedDist = it->sensedDistance;
323  if (sensedDist < 0) sensedDist = 0;
324 
325  float sensorStd =
326  likelihoodOptions.beaconRangesUseObservationStd
327  ? o->stdError
328  : likelihoodOptions.beaconRangesStd;
329  ret +=
330  (-0.5f *
331  square((expectedRange - sensedDist) / sensorStd));
332  found = true;
333  }
334  }
335 
336  // If not found, uniform distribution:
337  if (!found)
338  {
339  if (o->maxSensorDistance != o->minSensorDistance)
340  ret += log(
341  1.0 / (o->maxSensorDistance - o->minSensorDistance));
342  }
343 
344  } // for each sensed beacon "it"
345 
346  MRPT_CHECK_NORMAL_NUMBER(ret);
347  return ret;
348 
349  } // end of likelihood of CObservationBeaconRanges
350  else if (CLASS_ID(CObservationRobotPose) == obs->GetRuntimeClass())
351  {
352  /********************************************************************
353 
354  OBSERVATION TYPE: CObservationRobotPose
355 
356  Lik. between "this" and "robotPose";
357 
358  ********************************************************************/
359  const CObservationRobotPose* o =
360  static_cast<const CObservationRobotPose*>(obs);
361 
362  // Compute the 3D position of the sensor:
363  CPose3D sensorPose3D = robotPose3D + o->sensorPose;
364 
365  // Compute the likelihood according to mahalanobis distance between
366  // poses:
367  CMatrixD dij(1, 6), Cij(6, 6), Cij_1;
368  dij(0, 0) = o->pose.mean.x() - sensorPose3D.x();
369  dij(0, 1) = o->pose.mean.y() - sensorPose3D.y();
370  dij(0, 2) = o->pose.mean.z() - sensorPose3D.z();
371  dij(0, 3) = wrapToPi(o->pose.mean.yaw() - sensorPose3D.yaw());
372  dij(0, 4) = wrapToPi(o->pose.mean.pitch() - sensorPose3D.pitch());
373  dij(0, 5) = wrapToPi(o->pose.mean.roll() - sensorPose3D.roll());
374 
375  // Equivalent covariance from "i" to "j":
376  Cij = CMatrixDouble(o->pose.cov);
377  Cij_1 = Cij.inv();
378 
379  double distMahaFlik2 = dij.multiply_HCHt_scalar(Cij_1);
380  double ret =
381  -0.5 * (distMahaFlik2 / square(likelihoodOptions.extRobotPoseStd));
382 
383  MRPT_CHECK_NORMAL_NUMBER(ret);
384  return ret;
385 
386  } // end of likelihood of CObservation
387  else if (CLASS_ID(CObservationGPS) == obs->GetRuntimeClass())
388  {
389  /********************************************************************
390 
391  OBSERVATION TYPE: CObservationGPS
392 
393  ********************************************************************/
394  const CObservationGPS* o = static_cast<const CObservationGPS*>(obs);
395  // Compute the 3D position of the sensor:
396  CPoint3D point3D = CPoint3D(robotPose3D);
397  CPoint3D GPSpose;
398  double x, y;
399  double earth_radius = 6378137;
400 
401  if ((o->has_GGA_datum) &&
402  (likelihoodOptions.GPSOrigin.min_sat <=
403  o->getMsgByClass<gnss::Message_NMEA_GGA>().fields.satellitesUsed))
404  {
405  // Compose GPS robot position
406 
407  x = DEG2RAD(
408  (o->getMsgByClass<gnss::Message_NMEA_GGA>()
409  .fields.longitude_degrees -
410  likelihoodOptions.GPSOrigin.longitude)) *
411  earth_radius * 1.03;
412  y = DEG2RAD(
413  (o->getMsgByClass<gnss::Message_NMEA_GGA>()
414  .fields.latitude_degrees -
415  likelihoodOptions.GPSOrigin.latitude)) *
416  earth_radius * 1.15;
417  GPSpose.x(
418  (x * cos(likelihoodOptions.GPSOrigin.ang) +
419  y * sin(likelihoodOptions.GPSOrigin.ang) +
420  likelihoodOptions.GPSOrigin.x_shift));
421  GPSpose.y(
422  (-x * sin(likelihoodOptions.GPSOrigin.ang) +
423  y * cos(likelihoodOptions.GPSOrigin.ang) +
424  likelihoodOptions.GPSOrigin.y_shift));
425  GPSpose.z(
426  (o->getMsgByClass<gnss::Message_NMEA_GGA>()
427  .fields.altitude_meters -
428  likelihoodOptions.GPSOrigin.altitude));
429  // std::cout<<"GPSpose calculo: "<<GPSpose.x<<","<<GPSpose.y<<"\n";
430 
431  //-------------------------------//
432  // sigmaGPS =
433  // f(o->getMsgByClass<gnss::Message_NMEA_GGA>().fields.satellitesUsed)
434  // //funcion del numero de satelites
435  //-------------------------------//
436 
437  // std::cout<<"datos de longitud y latitud:
438  // "<<o->getMsgByClass<gnss::Message_NMEA_GGA>().fields.longitude_degrees<<","<<o->getMsgByClass<gnss::Message_NMEA_GGA>().fields.latitude_degrees<<","<<"\n";
439  // std::cout<<"x,y sin rotar: "<<x<<","<<y<<","<<"\n";
440  // std::cout<<"angulo: "<<likelihoodOptions.GPSOrigin.ang<<"\n";
441  // std::cout<<"desp x,y:
442  // "<<likelihoodOptions.GPSOrigin.x_shift<<","<<likelihoodOptions.GPSOrigin.y_shift<<"\n";
443  // std::cout<<"GPS ORIGIN :
444  // "<<likelihoodOptions.GPSOrigin.longitude<<","<<likelihoodOptions.GPSOrigin.latitude<<","<<likelihoodOptions.GPSOrigin.altitude<<"\n";
445  // std::cout<<"POSE DEL ROBOT:
446  // "<<point3D.x<<","<<point3D.y<<","<<point3D.z<<"\n";
447  // std::cout<<"POSE GPS :
448  // "<<GPSpose.x<<","<<GPSpose.y<<","<<GPSpose.z<<"\n";
449  // std::cin.get();
450 
451  float distance = GPSpose.distanceTo(point3D);
452 
453  // std::cout<<"likel gps:"<<-0.5f*square( ( distance
454  // )/likelihoodOptions.GPS_sigma)<<"\n";;
455  double ret =
456  -0.5f * square((distance) / likelihoodOptions.GPS_sigma);
457  MRPT_CHECK_NORMAL_NUMBER(ret);
458  return ret;
459  }
460  else
461  return 0.5;
462  } // end of likelihood of CObservationGPS
463  else
464  {
465  /********************************************************************
466 
467  OBSERVATION TYPE: Unknown
468 
469  ********************************************************************/
470  return 0.5;
471  }
472 
473  MRPT_END
474 }
475 
476 /*---------------------------------------------------------------
477  insertObservation
478  ---------------------------------------------------------------*/
479 bool CLandmarksMap::internal_insertObservation(
480  const CObservation* obs, const CPose3D* robotPose)
481 {
482  MRPT_START
483 
484  CPose2D robotPose2D;
485  CPose3D robotPose3D;
486 
487  if (robotPose)
488  {
489  robotPose2D = CPose2D(*robotPose);
490  robotPose3D = (*robotPose);
491  }
492  else
493  {
494  // Default values are (0,0,0)
495  }
496 
497  if (CLASS_ID(CObservationImage) == obs->GetRuntimeClass() &&
498  insertionOptions.insert_SIFTs_from_monocular_images)
499  {
500  /********************************************************************
501 
502  OBSERVATION TYPE: CObservationImage
503 
504  ********************************************************************/
505  const CObservationImage* o = static_cast<const CObservationImage*>(obs);
506  CLandmarksMap tempMap;
507 
508  // 1) Load the features in a temporary 3D landmarks map:
509  tempMap.loadSiftFeaturesFromImageObservation(
510  *o, insertionOptions.SIFT_feat_options);
511 
512  // 2) This temp. map must be moved to its real position on the global
513  // reference coordinates:
514  tempMap.changeCoordinatesReference(robotPose3D);
515 
516  // 3) Fuse that map with the current contents of "this" one:
517  fuseWith(tempMap);
518 
519  // DONE!!
520 
521  // Observation was successfully inserted into the map
522  // --------------------------------------------------------
523  return true;
524  }
525  // else
526  // if ( CLASS_ID(CObservation2DRangeScan)==obs->GetRuntimeClass() &&
527  // insertionOptions.insert_Landmarks_from_range_scans)
528  // {
529  /********************************************************************
530 
531  OBSERVATION TYPE: CObservation2DRangeScan
532 
533  ********************************************************************/
534  /* CObservation2DRangeScan *o = (CObservation2DRangeScan*) obs;
535  CLandmarksMap tempMap;
536 
537  // Load into the map:
538  tempMap.loadOccupancyFeaturesFrom2DRangeScan(*o, robotPose);
539  fuseWith( tempMap );
540 
541  // Observation was successfully inserted into the map
542  // --------------------------------------------------------
543  return true;
544  } */
545  else if (
546  CLASS_ID(CObservationStereoImages) == obs->GetRuntimeClass() &&
547  insertionOptions.insert_SIFTs_from_stereo_images)
548  {
549  /********************************************************************
550  OBSERVATION TYPE: CObservationStereoImages
551  ********************************************************************/
552  const CObservationStereoImages* o =
553  static_cast<const CObservationStereoImages*>(obs);
554 
555  // Change coordinates ref:
556  CLandmarksMap auxMap;
557  auxMap.insertionOptions = insertionOptions;
558  auxMap.loadSiftFeaturesFromStereoImageObservation(
559  *o, CLandmarksMap::_mapMaxID, insertionOptions.SIFT_feat_options);
560  auxMap.changeCoordinatesReference(robotPose3D);
561 
562  fuseWith(auxMap);
563 
564  // Observation was successfully inserted into the map
565  // --------------------------------------------------------
566  return true;
567  }
568  else if (CLASS_ID(CObservationVisualLandmarks) == obs->GetRuntimeClass())
569  {
570  /********************************************************************
571 
572  OBSERVATION TYPE: CObservationVisualLandmarks
573 
574  ********************************************************************/
575  const CObservationVisualLandmarks* o =
576  static_cast<const CObservationVisualLandmarks*>(obs);
577 
578  // Change coordinates ref:
579  CLandmarksMap auxMap;
580  CPose3D acumTransform(robotPose3D + o->refCameraPose);
581  auxMap.changeCoordinatesReference(acumTransform, &o->landmarks);
582 
583  // Fuse with current:
584  fuseWith(auxMap, true);
585 
586  // Observation was successfully inserted into the map
587  // --------------------------------------------------------
588  return true;
589  }
590  else
591  {
592  /********************************************************************
593  OBSERVATION TYPE: Unknown
594  ********************************************************************/
595  return false;
596  }
597 
598  MRPT_END
599 }
600 
601 /*---------------------------------------------------------------
602  computeMatchingWith2D
603  ---------------------------------------------------------------*/
604 void CLandmarksMap::computeMatchingWith2D(
605  const mrpt::maps::CMetricMap* otherMap, const CPose2D& otherMapPose,
606  float maxDistForCorrespondence, float maxAngularDistForCorrespondence,
607  const CPose2D& angularDistPivotPoint, TMatchingPairList& correspondences,
608  float& correspondencesRatio, float* sumSqrDist, bool onlyKeepTheClosest,
609  bool onlyUniqueRobust) const
610 {
611  MRPT_UNUSED_PARAM(onlyKeepTheClosest);
612  MRPT_UNUSED_PARAM(onlyUniqueRobust);
613  MRPT_UNUSED_PARAM(sumSqrDist);
614  MRPT_UNUSED_PARAM(angularDistPivotPoint);
615  MRPT_UNUSED_PARAM(maxDistForCorrespondence);
616  MRPT_UNUSED_PARAM(maxAngularDistForCorrespondence);
617 
618  MRPT_START
619 
620  CLandmarksMap auxMap;
621  CPose3D otherMapPose3D(otherMapPose);
622 
623  correspondencesRatio = 0;
624 
625  // Check the other map class:
626  ASSERT_(otherMap->GetRuntimeClass() == CLASS_ID(CLandmarksMap));
627  const CLandmarksMap* otherMap2 =
628  static_cast<const CLandmarksMap*>(otherMap);
629  std::vector<bool> otherCorrespondences;
630 
631  // Coordinates change:
632  auxMap.changeCoordinatesReference(otherMapPose3D, otherMap2);
633 
634  //// Use the 3D matching method:
635  computeMatchingWith3DLandmarks(
636  otherMap2, correspondences, correspondencesRatio, otherCorrespondences);
637 
638  MRPT_END
639 }
640 
641 /*---------------------------------------------------------------
642  loadSiftFeaturesFromImageObservation
643  ---------------------------------------------------------------*/
644 void CLandmarksMap::loadSiftFeaturesFromImageObservation(
645  const CObservationImage& obs,
646  const mrpt::vision::CFeatureExtraction::TOptions& feat_options)
647 {
648  CImage corImg;
649  CPointPDFGaussian landmark3DPositionPDF;
650  float d = insertionOptions.SIFTsLoadDistanceOfTheMean;
651  float width = insertionOptions.SIFTsLoadEllipsoidWidth;
652  CMatrixDouble33 P, D;
653  CLandmark lm;
654 
655  vision::CFeatureExtraction fExt;
656  vision::CFeatureList siftList; // vision::TSIFTFeatureList siftList;
657  vision::CFeatureList::iterator
658  sift; // vision::TSIFTFeatureList::iterator sift;
659 
660  // Timestamp:
661  lm.timestampLastSeen = obs.timestamp;
662  lm.seenTimesCount = 1;
663 
664  // Remove distortion:
665  corImg =
666  obs.image; // obs.image.correctDistortion(obs.intrinsicParams,obs.distortionParams);
667 
668  // Extract SIFT features:
669  fExt.options = feat_options;
670  fExt.detectFeatures(
671  corImg, siftList); // vision::computeSiftFeatures(corImg, siftList );
672 
673  // Save them as 3D landmarks:
674  landmarks.clear(); // resize( siftList.size() );
675 
676  for (sift = siftList.begin(); sift != siftList.end(); sift++)
677  {
678  // Find the 3D position from the pixels
679  // coordinates and the camera intrinsic matrix:
680  mrpt::math::TPoint3D dir = vision::pixelTo3D(
681  TPixelCoordf((*sift)->x, (*sift)->y),
682  obs.cameraParams.intrinsicParams); // dir = vision::pixelTo3D(
683  // sift->x,sift->y,
684  // obs.intrinsicParams );
685 
686  // Compute the mean and covariance of the landmark gaussian 3D position,
687  // from the unitary direction vector and a given distance:
688  // --------------------------------------------------------------------------
689  landmark3DPositionPDF.mean = CPoint3D(dir); // The mean is easy :-)
690  landmark3DPositionPDF.mean *= d;
691 
692  // The covariance:
693  P = math::generateAxisBaseFromDirection(dir.x, dir.y, dir.z);
694 
695  // Diagonal matrix (with the "size" of the ellipsoid)
696  D(0, 0) = square(0.5 * d);
697  D(1, 1) = square(width);
698  D(2, 2) = square(width);
699 
700  // Finally, compute the covariance!
701  landmark3DPositionPDF.cov = CMatrixDouble33(P * D * P.transpose());
702 
703  // Save into the landmarks vector:
704  // --------------------------------------------
705  lm.features.resize(1);
706  lm.features[0] = (*sift);
707 
708  CPoint3D Normal = landmark3DPositionPDF.mean;
709  Normal *= -1 / Normal.norm();
710 
711  lm.normal = Normal;
712 
713  lm.pose_mean = landmark3DPositionPDF.mean;
714 
715  lm.pose_cov_11 = landmark3DPositionPDF.cov(0, 0);
716  lm.pose_cov_22 = landmark3DPositionPDF.cov(1, 1);
717  lm.pose_cov_33 = landmark3DPositionPDF.cov(2, 2);
718  lm.pose_cov_12 = landmark3DPositionPDF.cov(0, 1);
719  lm.pose_cov_13 = landmark3DPositionPDF.cov(0, 2);
720  lm.pose_cov_23 = landmark3DPositionPDF.cov(1, 2);
721 
722  landmarks.push_back(lm);
723  }
724 
725 } // end loadSiftFeaturesFromImageObservation
726 
727 /*---------------------------------------------------------------
728  loadSiftFeaturesFromStereoImagesObservation
729  ---------------------------------------------------------------*/
730 void CLandmarksMap::loadSiftFeaturesFromStereoImageObservation(
731  const CObservationStereoImages& obs, mrpt::maps::CLandmark::TLandmarkID fID,
732  const mrpt::vision::CFeatureExtraction::TOptions& feat_options)
733 {
734  MRPT_START
735 
736  vision::CFeatureExtraction fExt;
737  vision::CFeatureList leftSiftList, rightSiftList;
738  vision::CMatchedFeatureList matchesList;
739  vision::TMatchingOptions matchingOptions;
740  vision::TStereoSystemParams stereoParams;
741 
742  CLandmarksMap landmarkMap;
743 
744  // Extract SIFT features:
745  fExt.options = feat_options; // OLD:
746  // fExt.options.SIFTOptions.implementation =
747  // vision::CFeatureExtraction::Hess;
748 
749  // Default: Hess implementation
750  fExt.detectFeatures(
751  obs.imageLeft, leftSiftList, fID,
752  insertionOptions.SIFTs_numberOfKLTKeypoints);
753  fExt.detectFeatures(
754  obs.imageRight, rightSiftList, fID,
755  insertionOptions.SIFTs_numberOfKLTKeypoints);
756 
757  matchingOptions.matching_method =
758  vision::TMatchingOptions::mmDescriptorSIFT;
759  matchingOptions.epipolar_TH = insertionOptions.SIFTs_epipolar_TH;
760  matchingOptions.EDD_RATIO = insertionOptions.SiftCorrRatioThreshold;
761  matchingOptions.maxEDD_TH = insertionOptions.SiftEDDThreshold;
762  vision::matchFeatures(
763  leftSiftList, rightSiftList, matchesList, matchingOptions);
764 
765  if (insertionOptions.PLOT_IMAGES)
766  {
767  std::cerr << "Warning: insertionOptions.PLOT_IMAGES has no effect "
768  "since MRPT 0.9.1\n";
769  }
770 
771  // obs.imageLeft.saveToFile("LImage.jpg");
772  // obs.imageRight.saveToFile("RImage.jpg");
773  // FILE *fmt;
774  // fmt = os::fopen( "matchesRBPF.txt", "at" );
775  // os::fprintf( fmt, "%d\n", (unsigned int)matchesList.size() );
776  // os::fclose(fmt);
777  // matchesList.saveToTextFile("matches.txt");
778 
779  // Feature Projection to 3D
780  // Parameters of the stereo rig
781 
782  stereoParams.K = obs.leftCamera.intrinsicParams;
783  stereoParams.stdPixel = insertionOptions.SIFTs_stdXY;
784  stereoParams.stdDisp = insertionOptions.SIFTs_stdDisparity;
785  stereoParams.baseline = obs.rightCameraPose.x();
786  stereoParams.minZ = 0.0f;
787  stereoParams.maxZ = insertionOptions.SIFTs_stereo_maxDepth;
788 
789  size_t numM = matchesList.size();
790  vision::projectMatchedFeatures(matchesList, stereoParams, *this);
791 
792  // Add Timestamp and the "Seen-Times" counter
793  TCustomSequenceLandmarks::iterator ii;
794  for (ii = landmarks.begin(); ii != landmarks.end(); ii++)
795  {
796  (*ii).timestampLastSeen = obs.timestamp;
797  (*ii).seenTimesCount = 1;
798  }
799 
800  printf(
801  "%u (out of %u) corrs!\n", static_cast<unsigned>(landmarks.size()),
802  static_cast<unsigned>(numM));
803 
804  // CLandmarksMap::_maxMapID = fID;
805 
806  // Project landmarks according to the ref. camera pose:
807  changeCoordinatesReference(mrpt::poses::CPose3D(obs.cameraPose));
808 
809  MRPT_END
810 }
811 
812 /*---------------------------------------------------------------
813  changeCoordinatesReference
814  ---------------------------------------------------------------*/
815 void CLandmarksMap::changeCoordinatesReference(const CPose3D& newOrg)
816 {
817  TSequenceLandmarks::iterator lm;
818 
819  CMatrixDouble44 HM;
820  newOrg.getHomogeneousMatrix(HM);
821 
822  // Build the rotation only transformation:
823  double R11 = HM.get_unsafe(0, 0);
824  double R12 = HM.get_unsafe(0, 1);
825  double R13 = HM.get_unsafe(0, 2);
826  double R21 = HM.get_unsafe(1, 0);
827  double R22 = HM.get_unsafe(1, 1);
828  double R23 = HM.get_unsafe(1, 2);
829  double R31 = HM.get_unsafe(2, 0);
830  double R32 = HM.get_unsafe(2, 1);
831  double R33 = HM.get_unsafe(2, 2);
832 
833  double c11, c22, c33, c12, c13, c23;
834 
835  // Change the reference of each individual landmark:
836  // ----------------------------------------------------
837  for (lm = landmarks.begin(); lm != landmarks.end(); lm++)
838  {
839  // Transform the pose mean & covariance:
840  // ---------------------------------------------------------
841  newOrg.composePoint(lm->pose_mean, lm->pose_mean);
842 
843  float C11 = lm->pose_cov_11;
844  float C22 = lm->pose_cov_22;
845  float C33 = lm->pose_cov_33;
846  float C12 = lm->pose_cov_12;
847  float C13 = lm->pose_cov_13;
848  float C23 = lm->pose_cov_23;
849 
850  // The covariance: cov = M * cov * (~M);
851  c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) +
852  R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
853  R13 * (C13 * R11 + C23 * R12 + C33 * R13);
854  c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 +
855  (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
856  (C13 * R11 + C23 * R12 + C33 * R13) * R23;
857  c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 +
858  (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
859  (C13 * R11 + C23 * R12 + C33 * R13) * R33;
860  c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) +
861  R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
862  R23 * (C13 * R21 + C23 * R22 + C33 * R23);
863  c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 +
864  (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
865  (C13 * R21 + C23 * R22 + C33 * R23) * R33;
866  c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) +
867  R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
868  R33 * (C13 * R31 + C23 * R32 + C33 * R33);
869 
870  // save into the landmark:
871  lm->pose_cov_11 = c11;
872  lm->pose_cov_22 = c22;
873  lm->pose_cov_33 = c33;
874  lm->pose_cov_12 = c12;
875  lm->pose_cov_13 = c13;
876  lm->pose_cov_23 = c23;
877 
878  // Rotate the normal: lm->normal = rot + lm->normal;
879  // ---------------------------------------------------------
880  float Nx = lm->normal.x;
881  float Ny = lm->normal.y;
882  float Nz = lm->normal.z;
883 
884  lm->normal.x = Nx * R11 + Ny * R12 + Nz * R13;
885  lm->normal.y = Nx * R21 + Ny * R22 + Nz * R23;
886  lm->normal.z = Nx * R31 + Ny * R32 + Nz * R33;
887  }
888 
889  // For updating the KD-Tree
890  landmarks.hasBeenModifiedAll();
891 }
892 
893 /*---------------------------------------------------------------
894  changeCoordinatesReference
895  ---------------------------------------------------------------*/
896 void CLandmarksMap::changeCoordinatesReference(
897  const CPose3D& newOrg, const mrpt::maps::CLandmarksMap* otherMap)
898 {
899  TSequenceLandmarks::const_iterator lm;
900  CLandmark newLandmark;
901 
902  CMatrixDouble44 HM;
903  newOrg.getHomogeneousMatrix(HM);
904 
905  // Build the rotation only transformation:
906  double R11 = HM.get_unsafe(0, 0);
907  double R12 = HM.get_unsafe(0, 1);
908  double R13 = HM.get_unsafe(0, 2);
909  double R21 = HM.get_unsafe(1, 0);
910  double R22 = HM.get_unsafe(1, 1);
911  double R23 = HM.get_unsafe(1, 2);
912  double R31 = HM.get_unsafe(2, 0);
913  double R32 = HM.get_unsafe(2, 1);
914  double R33 = HM.get_unsafe(2, 2);
915 
916  double c11, c22, c33, c12, c13, c23;
917 
918  landmarks.clear();
919 
920  // Change the reference of each individual landmark:
921  // ----------------------------------------------------
922  for (lm = otherMap->landmarks.begin(); lm != otherMap->landmarks.end();
923  lm++)
924  {
925  // Transform the pose mean & covariance:
926  // ---------------------------------------------------------
927  // The mean: mean = newReferenceBase + mean;
928  newOrg.composePoint(lm->pose_mean, newLandmark.pose_mean);
929 
930  float C11 = lm->pose_cov_11;
931  float C22 = lm->pose_cov_22;
932  float C33 = lm->pose_cov_33;
933  float C12 = lm->pose_cov_12;
934  float C13 = lm->pose_cov_13;
935  float C23 = lm->pose_cov_23;
936 
937  // The covariance: cov = M * cov * (~M);
938  c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) +
939  R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
940  R13 * (C13 * R11 + C23 * R12 + C33 * R13);
941  c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 +
942  (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
943  (C13 * R11 + C23 * R12 + C33 * R13) * R23;
944  c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 +
945  (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
946  (C13 * R11 + C23 * R12 + C33 * R13) * R33;
947  c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) +
948  R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
949  R23 * (C13 * R21 + C23 * R22 + C33 * R23);
950  c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 +
951  (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
952  (C13 * R21 + C23 * R22 + C33 * R23) * R33;
953  c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) +
954  R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
955  R33 * (C13 * R31 + C23 * R32 + C33 * R33);
956 
957  // save into the landmark:
958  newLandmark.pose_cov_11 = c11;
959  newLandmark.pose_cov_22 = c22;
960  newLandmark.pose_cov_33 = c33;
961  newLandmark.pose_cov_12 = c12;
962  newLandmark.pose_cov_13 = c13;
963  newLandmark.pose_cov_23 = c23;
964 
965  // Rotate the normal: lm->normal = rot + lm->normal;
966  // ---------------------------------------------------------
967  float Nx = lm->normal.x;
968  float Ny = lm->normal.y;
969  float Nz = lm->normal.z;
970 
971  newLandmark.normal.x = Nx * R11 + Ny * R12 + Nz * R13;
972  newLandmark.normal.y = Nx * R21 + Ny * R22 + Nz * R23;
973  newLandmark.normal.z = Nx * R31 + Ny * R32 + Nz * R33;
974 
975  newLandmark.ID = lm->ID;
976 
977  newLandmark.features = lm->features;
978 
979  newLandmark.timestampLastSeen = lm->timestampLastSeen;
980  newLandmark.seenTimesCount = lm->seenTimesCount;
981 
982  landmarks.push_back(newLandmark);
983  }
984 }
985 
986 /*---------------------------------------------------------------
987  fuseWith
988  ---------------------------------------------------------------*/
989 void CLandmarksMap::fuseWith(CLandmarksMap& other, bool justInsertAllOfThem)
990 {
991  MRPT_START
992 
993  // std::cout << "Entrando en fuseWith" << std::endl;
994 
995  std::vector<bool> otherCorrs(other.size(), false);
996  TMatchingPairList corrs;
997  TMatchingPairList::iterator corrIt;
998  float corrsRatio;
999  CLandmark *thisLM, *otherLM;
1000  int i, n;
1001  bool verbose = true; // false;
1002  TTimeStamp lastestTime = 0;
1003  unsigned int nRemoved = 0;
1004 
1005  if (!justInsertAllOfThem)
1006  {
1007  // 1) Compute matching between the global and the new map:
1008  // ---------------------------------------------------------
1009  computeMatchingWith3DLandmarks(&other, corrs, corrsRatio, otherCorrs);
1010 
1011  // 2) Fuse correspondences
1012  // ---------------------------------------------------------
1013  for (corrIt = corrs.begin(); corrIt != corrs.end(); corrIt++)
1014  {
1015  thisLM = landmarks.get(corrIt->this_idx);
1016  otherLM = other.landmarks.get(corrIt->other_idx);
1017 
1018  // Fuse their poses:
1019  CPointPDFGaussian P, P1, P2;
1020 
1021  thisLM->getPose(P1);
1022  otherLM->getPose(P2);
1023 
1024  P.bayesianFusion(P1, P2);
1025 
1026  landmarks.isToBeModified(corrIt->this_idx);
1027  thisLM->setPose(P);
1028 
1029  // Update "seen" data:
1030  thisLM->seenTimesCount++;
1031  thisLM->timestampLastSeen = otherLM->timestampLastSeen;
1032 
1033  landmarks.hasBeenModified(corrIt->this_idx);
1034 
1035  } // end foreach corrs
1036  }
1037 
1038  // 3) Add new landmarks from the other map:
1039  // ---------------------------------------------------------
1040  n = other.size();
1041  for (i = 0; i < n; i++)
1042  {
1043  // Find the lastest time.
1044  lastestTime =
1045  max(lastestTime, other.landmarks.get(i)->timestampLastSeen);
1046 
1047  if (!otherCorrs[i])
1048  {
1049  // No corrs: A NEW LANDMARK!
1050  landmarks.push_back(*other.landmarks.get(i));
1051  }
1052  } // end foreach other landmark
1053 
1054  if (!justInsertAllOfThem)
1055  {
1056  // 4) Remove landmarks that have been not seen the required
1057  // number of times:
1058  // ---------------------------------------------------------
1059  n = landmarks.size();
1060  for (i = n - 1; i >= 0; i--)
1061  {
1062  if (landmarks.get(i)->getType() !=
1063  featNotDefined) // Occupancy features
1064  {
1065  TTimeStamp t =
1066  lastestTime - landmarks.get(i)->timestampLastSeen;
1067  double tt =
1068  (int64_t)t * 0.0000001; // (int64_t) required by MSVC6
1069  if (tt > fuseOptions.ellapsedTime &&
1070  landmarks.get(i)->seenTimesCount < fuseOptions.minTimesSeen)
1071  {
1072  landmarks.erase(i);
1073  nRemoved++;
1074  }
1075  }
1076  }
1077  }
1078 
1079  if (verbose)
1080  {
1081  printf(
1082  "[CLandmarksMap::fuseWith] %u fused/ %u new/ %u removed -> %u "
1083  "total\n",
1084  static_cast<unsigned int>(corrs.size()),
1085  static_cast<unsigned int>(other.size() - corrs.size()),
1086  static_cast<unsigned int>(nRemoved),
1087  static_cast<unsigned int>(landmarks.size()));
1088  FILE* f = os::fopen("Fused.txt", "at");
1089  fprintf(
1090  f, "%u\t%u\t%u\t%u\n", static_cast<unsigned int>(corrs.size()),
1091  static_cast<unsigned int>(other.size() - corrs.size()),
1092  static_cast<unsigned int>(nRemoved),
1093  static_cast<unsigned int>(landmarks.size()));
1094  os::fclose(f);
1095  }
1096 
1097  MRPT_END
1098 }
1099 
1100 /*---------------------------------------------------------------
1101  computeMatchingWith3DLandmarks
1102  ---------------------------------------------------------------*/
1103 void CLandmarksMap::computeMatchingWith3DLandmarks(
1104  const mrpt::maps::CLandmarksMap* anotherMap,
1105  TMatchingPairList& correspondences, float& correspondencesRatio,
1106  std::vector<bool>& otherCorrespondences) const
1107 {
1108  MRPT_START
1109 
1110  TSequenceLandmarks::const_iterator thisIt, otherIt;
1111  unsigned int nThis, nOther;
1112  int maxIdx;
1113  float desc;
1114  unsigned int i, n, j, k;
1115  TMatchingPair match;
1116  double lik_dist, lik_desc, lik, maxLik;
1117  // double maxLikDist = -1, maxLikDesc =
1118  // -1;
1119  CPointPDFGaussian pointPDF_k, pointPDF_j;
1120  std::vector<bool> thisLandmarkAssigned;
1121  double K_desc = 0.0;
1122  double K_dist = 0.0;
1123 
1124  // FILE *f = os::fopen( "flik.txt", "wt"
1125  //);
1126 
1127  // Get the number of landmarks:
1128  nThis = landmarks.size();
1129  nOther = anotherMap->landmarks.size();
1130 
1131  // Initially no LM has a correspondence:
1132  thisLandmarkAssigned.resize(nThis, false);
1133 
1134  // Initially, set all landmarks without correspondences:
1135  correspondences.clear();
1136  otherCorrespondences.clear();
1137  otherCorrespondences.resize(nOther, false);
1138  correspondencesRatio = 0;
1139 
1140  // Method selection:
1141  // 0. Our Method.
1142  // 1. Sim, Elinas, Griffin, Little.
1143 
1144  switch (insertionOptions.SIFTMatching3DMethod)
1145  {
1146  case 0:
1147  // Our method: Filter out by the likelihood of the 3D position and
1148  // compute the likelihood of the Euclidean descriptor distance
1149 
1150  // "Constants" for the distance computation
1151  K_desc =
1152  -0.5 / square(likelihoodOptions.SIFTs_sigma_descriptor_dist);
1153  K_dist = -0.5 / square(likelihoodOptions.SIFTs_mahaDist_std);
1154 
1155  // CDynamicGrid<vector_int> *gridLandmarks =
1156  // landmarks.getGrid();
1157  // vector_int closeLandmarksList;
1158 
1159  for (k = 0, otherIt = anotherMap->landmarks.begin();
1160  otherIt != anotherMap->landmarks.end(); otherIt++, k++)
1161  {
1162  // Load into "pointPDF_k" the PDF of landmark "otherIt":
1163  otherIt->getPose(pointPDF_k);
1164 
1165  if (otherIt->getType() == featSIFT)
1166  {
1167  // minDist = minDist2 = 1e10f;
1168  maxLik = -1;
1169  maxIdx = -1;
1170 
1171  /*
1172  // Get the list of close landmarks:
1173  // ---------------------------------------------
1174  int cx0 = gridLandmarks->x2idx( otherIt->pose_mean.x,
1175  otherIt->pose_mean.y );
1176  int cx0 = gridLandmarks->x2idx( otherIt->pose_mean.x,
1177  otherIt->pose_mean.y );
1178 
1179  closeLandmarksList.clear();
1180  closeLandmarksList.reserve(300);
1181  ...
1182  */
1183 
1184  for (j = 0, thisIt = landmarks.begin();
1185  thisIt != landmarks.end(); thisIt++, j++)
1186  {
1187  if (thisIt->getType() == featSIFT &&
1188  thisIt->features.size() ==
1189  otherIt->features.size() &&
1190  !thisIt->features.empty() && thisIt->features[0] &&
1191  otherIt->features[0] &&
1192  thisIt->features[0]->descriptors.SIFT.size() ==
1193  otherIt->features[0]->descriptors.SIFT.size())
1194  {
1195  // Compute "coincidence probability":
1196  // --------------------------------------
1197  // Load into "pointPDF_j" the PDF of landmark
1198  // "otherIt":
1199  thisIt->getPose(pointPDF_j);
1200 
1201  // Compute lik:
1202  // lik_dist =
1203  // pointPDF_k.productIntegralNormalizedWith(
1204  // &pointPDF_j );
1205  CMatrixDouble dij(1, 3), Cij(3, 3), Cij_1;
1206  double distMahaFlik2;
1207 
1208  // Distance between means:
1209  dij(0, 0) =
1210  pointPDF_k.mean.x() - pointPDF_j.mean.x();
1211  dij(0, 1) =
1212  pointPDF_k.mean.y() - pointPDF_j.mean.y();
1213  dij(0, 2) =
1214  pointPDF_k.mean.z() - pointPDF_j.mean.z();
1215 
1216  // Equivalent covariance from "i" to "j":
1217  Cij =
1218  CMatrixDouble(pointPDF_k.cov + pointPDF_j.cov);
1219  Cij_1 = Cij.inv();
1220 
1221  distMahaFlik2 = dij.multiply_HCHt_scalar(
1222  Cij_1); //( dij * Cij_1 * (~dij) )(0,0);
1223 
1224  lik_dist =
1225  exp(K_dist * distMahaFlik2); // Likelihood
1226  // regarding the
1227  // spatial
1228  // distance
1229 
1230  if (lik_dist > 1e-2)
1231  {
1232  // Compute distance between descriptors:
1233  // --------------------------------------
1234 
1235  // MODIFICATION 19-SEPT-2007
1236  // ONLY COMPUTE THE EUCLIDEAN DISTANCE BETWEEN
1237  // DESCRIPTORS IF IT HAS NOT BEEN COMPUTED
1238  // BEFORE
1239  // Make the pair of points
1240  std::pair<mrpt::maps::CLandmark::TLandmarkID,
1241  mrpt::maps::CLandmark::TLandmarkID>
1242  mPair(thisIt->ID, otherIt->ID);
1243 
1244  if (CLandmarksMap::_mEDD[mPair] == 0)
1245  {
1246  n = otherIt->features[0]
1247  ->descriptors.SIFT.size();
1248  desc = 0;
1249  for (i = 0; i < n; i++)
1250  desc += square(
1251  otherIt->features[0]
1252  ->descriptors.SIFT[i] -
1253  thisIt->features[0]
1254  ->descriptors.SIFT[i]);
1255 
1256  CLandmarksMap::_mEDD[mPair] = desc;
1257  // std::cout << "[fuseWith] - Nueva entrada!
1258  // - (LIK): " << exp( K_desc * desc ) << "
1259  // -> " << "(" << mPair.first << "," <<
1260  // mPair.second << ")" << std::endl;
1261 
1262  } // end if
1263  else
1264  {
1265  desc = CLandmarksMap::_mEDD[mPair];
1266  // std::cout << "[fuseWith] - Ya esta
1267  // calculado!: " << "(" << mPair.first <<
1268  // "," << mPair.second << ")" << ": " <<
1269  // desc << std::endl;
1270  }
1271 
1272  lik_desc = exp(K_desc * desc); // Likelihood
1273  // regarding the
1274  // descriptor
1275  }
1276  else
1277  {
1278  lik_desc = 1e-3;
1279  }
1280 
1281  // Likelihood of the correspondence
1282  // --------------------------------------
1283  lik = lik_dist * lik_desc;
1284 
1285  // os::fprintf( f,
1286  //"%i\t%i\t%f\t%f\t%f\n", k, j, lik_desc, lik_dist,
1287  // lik );
1288 
1289  if (lik > maxLik)
1290  {
1291  // maxLikDist =
1292  // lik_dist;
1293  // maxLikDesc =
1294  // lik_desc;
1295  maxLik = lik;
1296  maxIdx = j;
1297  }
1298 
1299  } // end of this landmark is SIFT
1300 
1301  } // End of for each "this", j
1302  // os::fprintf(f, "%i\t%i\t%f\t%f\t%f\n", maxIdx, k,
1303  // maxLikDist, maxLikDesc, maxLik);
1304 
1305  // Is it a correspondence?
1306  if (maxLik > insertionOptions.SiftLikelihoodThreshold)
1307  {
1308  // TODO: Solve in a better way the multiple
1309  // correspondences case!!!
1310  // ****************************************************************
1311  // If a previous correspondence for this LM was found,
1312  // discard this one!
1313  if (!thisLandmarkAssigned[maxIdx])
1314  {
1315  thisLandmarkAssigned[maxIdx] = true;
1316 
1317  // OK: A correspondence found!!
1318  otherCorrespondences[k] = true;
1319 
1320  match.this_idx = maxIdx;
1321  match.this_x = landmarks.get(maxIdx)->pose_mean.x;
1322  match.this_y = landmarks.get(maxIdx)->pose_mean.y;
1323  match.this_z = landmarks.get(maxIdx)->pose_mean.z;
1324 
1325  match.other_idx = k;
1326  match.other_x =
1327  anotherMap->landmarks.get(k)->pose_mean.x;
1328  match.other_y =
1329  anotherMap->landmarks.get(k)->pose_mean.y;
1330  match.other_z =
1331  anotherMap->landmarks.get(k)->pose_mean.z;
1332 
1333  correspondences.push_back(match);
1334  }
1335  }
1336 
1337  } // end of "otherIt" is SIFT
1338 
1339  } // end of other it., k
1340 
1341  // Compute the corrs ratio:
1342  correspondencesRatio =
1343  correspondences.size() / static_cast<float>(nOther);
1344  // os::fclose(f);
1345 
1346  break;
1347 
1348  case 1:
1349 
1350  // IMPLEMENTATION OF THE METHOD DESCRIBED IN [VISION-BASED SLAM
1351  // USING THE RBPF][SIM, ELINAS, GRIFFIN, LITTLE]
1352  // 1. Compute Euclidean descriptor distance (EDD).
1353  // 2. Matching based on a Threshold.
1354  // 3. Compute likelihood based only on the position of the 3D
1355  // landmarks.
1356 
1357  // 1.- COMPUTE EDD
1358 
1359  ASSERT_(!anotherMap->landmarks.begin()->features.empty())
1360  ASSERT_(!landmarks.begin()->features.empty())
1361 
1362  unsigned int dLen = anotherMap->landmarks.begin()
1363  ->features[0]
1364  ->descriptors.SIFT.size();
1365  for (k = 0, otherIt = anotherMap->landmarks.begin();
1366  otherIt != anotherMap->landmarks.end(); otherIt++, k++)
1367  {
1368  double mEDD = -1.0;
1369  unsigned int mEDDidx = 0;
1370  for (j = 0, thisIt = landmarks.begin();
1371  thisIt != landmarks.end(); thisIt++, j++)
1372  {
1373  // Compute EDD
1374  double EDD = 0.0;
1375  for (i = 0; i < dLen; i++)
1376  EDD += square(
1377  otherIt->features[0]->descriptors.SIFT[i] -
1378  thisIt->features[0]->descriptors.SIFT[i]);
1379 
1380  EDD = sqrt(EDD);
1381 
1382  if (EDD > mEDD)
1383  {
1384  mEDD = EDD;
1385  mEDDidx = j;
1386  } // end if
1387  } // end for j
1388 
1389  if (mEDD > insertionOptions.SiftEDDThreshold)
1390  {
1391  // There is a correspondence
1392  if (!thisLandmarkAssigned[mEDDidx]) // If there is not
1393  // multiple
1394  // correspondence
1395  {
1396  thisLandmarkAssigned[mEDDidx] = true;
1397 
1398  // OK: A correspondence found!!
1399  otherCorrespondences[k] = true;
1400 
1401  otherIt->getPose(pointPDF_k);
1402  thisIt->getPose(pointPDF_j);
1403 
1404  match.this_idx = j;
1405  match.this_x = landmarks.get(mEDDidx)->pose_mean.x;
1406  match.this_y = landmarks.get(mEDDidx)->pose_mean.y;
1407  match.this_z = landmarks.get(mEDDidx)->pose_mean.z;
1408 
1409  match.other_idx = k;
1410  match.other_x =
1411  anotherMap->landmarks.get(k)->pose_mean.x;
1412  match.other_y =
1413  anotherMap->landmarks.get(k)->pose_mean.y;
1414  match.other_z =
1415  anotherMap->landmarks.get(k)->pose_mean.z;
1416 
1417  correspondences.push_back(match);
1418 
1419  } // end if multiple correspondence
1420 
1421  } // end if mEDD
1422 
1423  } // end for k
1424 
1425  correspondencesRatio =
1426  correspondences.size() / static_cast<float>(nOther);
1427 
1428  break;
1429 
1430  } // end switch
1431 
1432  MRPT_END
1433 }
1434 
1435 /*---------------------------------------------------------------
1436  saveToTextFile
1437  ---------------------------------------------------------------*/
1438 bool CLandmarksMap::saveToTextFile(std::string file)
1439 {
1440  MRPT_START
1441 
1442  FILE* f = os::fopen(file.c_str(), "wt");
1443  if (!f) return false;
1444 
1445  // os::fprintf(f,"%% Map of landmarks - file dumped by
1446  // mrpt::maps::CLandmarksMap\n");
1447  // os::fprintf(f,"%% Columns are: X Y Z TYPE(TFeatureType) TIMES_SEEN
1448  // TIME_OF_LAST_OBSERVATION [SIFT DESCRIPTOR] ID\n");
1449  // os::fprintf(f,"%%
1450  // -----------------------------------------------------------------------------------------------------\n");
1451 
1452  for (TSequenceLandmarks::iterator it = landmarks.begin();
1453  it != landmarks.end(); ++it)
1454  {
1455  os::fprintf(
1456  f, "%10f %10f %10f %4i %4u %10f", it->pose_mean.x, it->pose_mean.y,
1457  it->pose_mean.z, static_cast<int>(it->getType()),
1458  it->seenTimesCount, it->timestampLastSeen == INVALID_TIMESTAMP
1459  ? 0
1460  : mrpt::system::extractDayTimeFromTimestamp(
1461  it->timestampLastSeen));
1462 
1463  if (it->getType() == featSIFT)
1464  {
1465  ASSERT_(!it->features.empty() && it->features[0])
1466 
1467  for (unsigned int i = 0;
1468  i < it->features[0]->descriptors.SIFT.size(); i++)
1469  os::fprintf(f, " %u ", it->features[0]->descriptors.SIFT[i]);
1470  }
1471  os::fprintf(f, " %i ", (int)it->ID);
1472 
1473  os::fprintf(f, "\n");
1474  }
1475 
1476  os::fclose(f);
1477 
1478  return true;
1479 
1480  MRPT_END
1481 }
1482 
1483 /*---------------------------------------------------------------
1484  saveToMATLABScript3D
1485  ---------------------------------------------------------------*/
1486 bool CLandmarksMap::saveToMATLABScript3D(
1487  std::string file, const char* style, float confInterval) const
1488 {
1489  FILE* f = os::fopen(file.c_str(), "wt");
1490  if (!f) return false;
1491 
1492  // Header:
1493  os::fprintf(
1494  f, "%%-------------------------------------------------------\n");
1495  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
1496  os::fprintf(f, "%% 'CLandmarksMap::saveToMATLABScript3D'\n");
1497  os::fprintf(f, "%%\n");
1498  os::fprintf(f, "%% ~ MRPT ~\n");
1499  os::fprintf(
1500  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
1501  os::fprintf(f, "%% http://www.isa.uma.es/ \n");
1502  os::fprintf(
1503  f, "%%-------------------------------------------------------\n\n");
1504 
1505  // Main code:
1506  os::fprintf(f, "hold on;\n\n");
1507 
1508  for (TSequenceLandmarks::const_iterator it = landmarks.begin();
1509  it != landmarks.end(); ++it)
1510  {
1511  os::fprintf(
1512  f, "m=[%.4f %.4f %.4f];", it->pose_mean.x, it->pose_mean.y,
1513  it->pose_mean.z);
1514  os::fprintf(
1515  f, "c=[%.8f %.8f %.8f;%.8f %.8f %.8f;%.8f %.8f %.8f]; ",
1516  it->pose_cov_11, it->pose_cov_12, it->pose_cov_13, it->pose_cov_12,
1517  it->pose_cov_22, it->pose_cov_23, it->pose_cov_13, it->pose_cov_23,
1518  it->pose_cov_33);
1519 
1520  os::fprintf(
1521  f,
1522  "error_ellipse(c,m,'conf',%f,'style','%s','numPointsEllipse',10);"
1523  "\n",
1524  confInterval, style);
1525  }
1526 
1527  os::fprintf(f, "axis equal;grid on;xlabel('x'); ylabel('y'); zlabel('z');");
1528 
1529  os::fclose(f);
1530  return true;
1531 }
1532 /**/
1533 
1534 /*---------------------------------------------------------------
1535  saveToMATLABScript2D
1536  ---------------------------------------------------------------*/
1537 bool CLandmarksMap::saveToMATLABScript2D(
1538  std::string file, const char* style, float stdCount)
1539 {
1540  FILE* f = os::fopen(file.c_str(), "wt");
1541  if (!f) return false;
1542 
1543  const int ELLIPSE_POINTS = 30;
1544  std::vector<float> X, Y, COS, SIN;
1545  std::vector<float>::iterator x, y, Cos, Sin;
1546  double ang;
1547  CMatrixD cov(2, 2), eigVal, eigVec, M;
1548 
1549  X.resize(ELLIPSE_POINTS);
1550  Y.resize(ELLIPSE_POINTS);
1551  COS.resize(ELLIPSE_POINTS);
1552  SIN.resize(ELLIPSE_POINTS);
1553 
1554  // Fill the angles:
1555  for (Cos = COS.begin(), Sin = SIN.begin(), ang = 0; Cos != COS.end();
1556  Cos++, Sin++, ang += (M_2PI / (ELLIPSE_POINTS - 1)))
1557  {
1558  *Cos = cos(ang);
1559  *Sin = sin(ang);
1560  }
1561 
1562  // Header:
1563  os::fprintf(
1564  f, "%%-------------------------------------------------------\n");
1565  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
1566  os::fprintf(f, "%% 'CLandmarksMap::saveToMATLABScript2D'\n");
1567  os::fprintf(f, "%%\n");
1568  os::fprintf(f, "%% ~ MRPT ~\n");
1569  os::fprintf(
1570  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
1571  os::fprintf(f, "%% http://www.isa.uma.es/ \n");
1572  os::fprintf(
1573  f, "%%-------------------------------------------------------\n\n");
1574 
1575  // Main code:
1576  os::fprintf(f, "hold on;\n\n");
1577 
1578  for (TSequenceLandmarks::iterator it = landmarks.begin();
1579  it != landmarks.end(); ++it)
1580  {
1581  // Compute the eigen-vectors & values:
1582  cov(0, 0) = it->pose_cov_11;
1583  cov(1, 1) = it->pose_cov_22;
1584  cov(0, 1) = cov(1, 0) = it->pose_cov_12;
1585 
1586  cov.eigenVectors(eigVec, eigVal);
1587  eigVal = eigVal.array().sqrt().matrix();
1588  M = eigVal * eigVec.transpose();
1589 
1590  // Compute the points of the ellipsoid:
1591  // ----------------------------------------------
1592  for (x = X.begin(), y = Y.begin(), Cos = COS.begin(), Sin = SIN.begin();
1593  x != X.end(); x++, y++, Cos++, Sin++)
1594  {
1595  *x =
1596  (it->pose_mean.x +
1597  stdCount * (*Cos * M(0, 0) + *Sin * M(1, 0)));
1598  *y =
1599  (it->pose_mean.y +
1600  stdCount * (*Cos * M(0, 1) + *Sin * M(1, 1)));
1601  }
1602 
1603  // Save the code to plot the ellipsoid:
1604  // ----------------------------------------------
1605  os::fprintf(f, "plot([ ");
1606  for (x = X.begin(); x != X.end(); x++)
1607  {
1608  os::fprintf(f, "%.4f", *x);
1609  if (x != (X.end() - 1)) os::fprintf(f, ",");
1610  }
1611  os::fprintf(f, "],[ ");
1612  for (y = Y.begin(); y != Y.end(); y++)
1613  {
1614  os::fprintf(f, "%.4f", *y);
1615  if (y != (Y.end() - 1)) os::fprintf(f, ",");
1616  }
1617 
1618  os::fprintf(f, "],'%s');\n", style);
1619 
1620  // os::fprintf(f,"error_ellipse(c,m,'conf',0.99,'style','%s','numPointsEllipse',10);\n",style);
1621  }
1622 
1623  os::fprintf(f, "\naxis equal;\n");
1624  os::fclose(f);
1625  return true;
1626 }
1627 
1628 /*---------------------------------------------------------------
1629  loadOccupancyFeaturesFrom2DRangeScan
1630  ---------------------------------------------------------------*/
1631 void CLandmarksMap::loadOccupancyFeaturesFrom2DRangeScan(
1632  const CObservation2DRangeScan& obs, const CPose3D* robotPose,
1633  unsigned int downSampleFactor)
1634 {
1635  unsigned int i, n = obs.scan.size();
1636  double Th, dTh; // angle of the ray
1637  double J11, J12, J21, J22; // The jacobian elements.
1638  double d;
1639  CPose3D sensorGlobalPose;
1640 
1641  // Empty the map:
1642  this->clear();
1643 
1644  // Sensor pose in 3D:
1645  if (!robotPose)
1646  sensorGlobalPose = obs.sensorPose;
1647  else
1648  sensorGlobalPose = *robotPose + obs.sensorPose;
1649 
1650  // Starting direction:
1651  if (obs.rightToLeft)
1652  {
1653  Th = -0.5 * obs.aperture;
1654  dTh = obs.aperture / n;
1655  }
1656  else
1657  {
1658  Th = +0.5 * obs.aperture;
1659  dTh = -obs.aperture / n;
1660  }
1661 
1662  // Measurement uncertainties:
1663  double var_d = square(0.005); // square(obs.stdError);
1664  double var_th = square(dTh / 10.0);
1665 
1666  // For each range:
1667  for (i = 0; i < n; i += downSampleFactor, Th += downSampleFactor * dTh)
1668  {
1669  // If it is a valid ray:
1670  if (obs.validRange[i])
1671  {
1672  CLandmark newLandmark;
1673 
1674  // Timestamp:
1675  newLandmark.timestampLastSeen = obs.timestamp;
1676  newLandmark.seenTimesCount = 1;
1677 
1678  newLandmark.createOneFeature();
1679  newLandmark.features[0]->type = featNotDefined;
1680 
1681  d = obs.scan[i];
1682 
1683  // Compute the landmark in 2D:
1684  // -----------------------------------------------
1685  // Descriptor:
1686  newLandmark.features[0]->orientation = Th;
1687  newLandmark.features[0]->scale = d;
1688 
1689  // Mean:
1690  newLandmark.pose_mean.x = (cos(Th) * d);
1691  newLandmark.pose_mean.y = (sin(Th) * d);
1692  newLandmark.pose_mean.z = 0;
1693 
1694  // Normal direction:
1695  newLandmark.normal = newLandmark.pose_mean;
1696  newLandmark.normal *= -1.0f / newLandmark.pose_mean.norm();
1697 
1698  // Jacobian:
1699  J11 = -d * sin(Th);
1700  J12 = cos(Th);
1701  J21 = d * cos(Th);
1702  J22 = sin(Th);
1703 
1704  // Covariance matrix:
1705  newLandmark.pose_cov_11 = (J11 * J11 * var_th + J12 * J12 * var_d);
1706  newLandmark.pose_cov_12 = (J11 * J21 * var_th + J12 * J22 * var_d);
1707  newLandmark.pose_cov_22 = (J21 * J21 * var_th + J22 * J22 * var_d);
1708  newLandmark.pose_cov_33 = (square(0.005)); // var_d;
1709  newLandmark.pose_cov_13 = newLandmark.pose_cov_23 = 0;
1710 
1711  // Append it:
1712  // -----------------------------------------------
1713  landmarks.push_back(newLandmark);
1714 
1715  } // end of valid ray.
1716 
1717  } // end for n
1718 
1719  // Take landmarks to 3D according to the robot & sensor 3D poses:
1720  // -----------------------------------------------
1721  changeCoordinatesReference(sensorGlobalPose);
1722 }
1723 
1724 /*---------------------------------------------------------------
1725  METHOD DESCRIBED IN PAPER
1726  -------------------------
1727 
1728  ICRA 2007,....
1729 
1730  ---------------------------------------------------------------*/
1731 double CLandmarksMap::computeLikelihood_RSLC_2007(
1732  const CLandmarksMap* s, const CPose2D& sensorPose)
1733 {
1734  MRPT_UNUSED_PARAM(sensorPose);
1735  MRPT_START
1736 
1737  double lik = 1.0;
1738  TSequenceLandmarks::const_iterator itOther;
1739  CLandmark* lm; //*itClosest;
1740  double corr;
1741  double PrNoCorr;
1742  CPointPDFGaussian poseThis, poseOther;
1743  // double STD_THETA = DEG2RAD(0.15);
1744  // double STD_DIST = 0.5f;
1745  double nFoundCorrs = 0;
1746  vector_int* corrs;
1747  unsigned int cx, cy, cx_1, cx_2, cy_1, cy_2;
1748 
1749  // s->saveToMATLABScript2D(std::string("ver_sensed.m"));
1750  // saveToMATLABScript2D(std::string("ver_ref.m"),"r");
1751 
1752  CDynamicGrid<vector_int>* grid = landmarks.getGrid();
1753  // grid->saveToTextFile( "debug_grid.txt" );
1754 
1755  // For each landmark in the observations:
1756  for (itOther = s->landmarks.begin(); itOther != s->landmarks.end();
1757  itOther++)
1758  {
1759  itOther->getPose(poseOther);
1760 
1761  cx = cy = grid->y2idx(itOther->pose_mean.y);
1762 
1763  cx_1 = max(0, grid->x2idx(itOther->pose_mean.x - 0.10f));
1764  cx_2 =
1765  min(static_cast<int>(grid->getSizeX()) - 1,
1766  grid->x2idx(itOther->pose_mean.x + 0.10f));
1767  cy_1 = max(0, grid->y2idx(itOther->pose_mean.y - 0.10f));
1768  cy_2 =
1769  min(static_cast<int>(grid->getSizeY()) - 1,
1770  grid->y2idx(itOther->pose_mean.y + 0.10f));
1771 
1772  // The likelihood of no correspondence starts at "1" and will be
1773  // modified next:
1774  PrNoCorr = 1;
1775 
1776  // For each landmark in this map: Compute its correspondence likelihood
1777  // and conditioned observation likelihood:
1778  // itClosest = nullptr;
1779 
1780  // Look at landmarks in sourronding cells only:
1781  for (cx = cx_1; cx <= cx_2; cx++)
1782  for (cy = cy_1; cy <= cy_2; cy++)
1783  {
1784  corrs = grid->cellByIndex(cx, cy);
1785  ASSERT_(corrs != nullptr);
1786  if (!corrs->empty())
1787  for (vector_int::iterator it = corrs->begin();
1788  it != corrs->end(); ++it)
1789  {
1790  lm = landmarks.get(*it);
1791 
1792  // Compute the "correspondence" in the range [0,1]:
1793  // -------------------------------------------------------------
1794 
1795  // Shortcut:
1796  if (fabs(lm->pose_mean.x - itOther->pose_mean.x) >
1797  0.15f ||
1798  fabs(lm->pose_mean.y - itOther->pose_mean.y) >
1799  0.15f)
1800  {
1801  // Absolutely no correspondence, not need to compute
1802  // it:
1803  corr = 0;
1804  }
1805  else
1806  {
1807  lm->getPose(poseThis);
1808  corr = poseOther.productIntegralNormalizedWith2D(
1809  poseThis);
1810  }
1811 
1812  // The likelihood of no corresp. is proportional to the
1813  // product of all "1-CORRij":
1814  // -------------------------------------------------------------
1815  PrNoCorr *= 1 - corr;
1816 
1817  } // end of each landmark in this map.
1818  }
1819 
1820  // Only consider this "point" if it has some real chance to has a
1821  // correspondence:
1822  nFoundCorrs += 1 - PrNoCorr;
1823 
1824  /**** DEBUG **** /
1825  {
1826  //FILE *f=os::fopen("debug_corrs.txt","wt");
1827  //for (Cij=v_Cij.begin(),pZj=v_pZj.begin(); pZj!=v_pZj.end();
1828  Cij++,pZj++)
1829  //{
1830  // os::fprintf(f,"%e %e\n", *Cij, *pZj);
1831  //}
1832 
1833  //os::fprintf(f,"\n INDIV LIK=%e\n lik=%e\n
1834  closestObstacleInLine=%e\n measured
1835  range=%e\n",indivLik,lik,closestObstacleInLine,
1836  itOther->descriptors.SIFT[1]);
1837  //os::fprintf(f,"
1838  closestObstacleDirection=%e\n",closestObstacleDirection);
1839  //os::fclose(f);
1840 
1841  printf("\n lik=%e\n closestObstacleInLine=%e\n measured
1842  range=%e\n",lik,closestObstacleInLine, itOther->descriptors.SIFT[1]);
1843  if (itClosest)
1844  printf(" closest=(%.03f,%.03f)\n", itClosest->pose_mean.x,
1845  itClosest->pose_mean.y);
1846  else printf(" closest=nullptr\n");
1847 
1848  printf(" P(no corrs)=%e\n", PrNoCorr );
1849  mrpt::system::pause();
1850  }
1851  / ***************/
1852 
1853  lik *= 1 - PrNoCorr;
1854 
1855  } // enf for each landmark in the other map.
1856 
1857  lik = nFoundCorrs / static_cast<double>(s->size());
1858 
1859  lik = log(lik);
1860 
1861  MRPT_CHECK_NORMAL_NUMBER(lik);
1862  return lik;
1863 
1864  MRPT_END
1865 }
1866 
1867 /*---------------------------------------------------------------
1868 
1869  TCustomSequenceLandmarks
1870 
1871  ---------------------------------------------------------------*/
1872 CLandmarksMap::TCustomSequenceLandmarks::TCustomSequenceLandmarks()
1873  : m_landmarks(),
1874  m_grid(-10.0f, 10.0f, -10.0f, 10.f, 0.20f),
1875  m_largestDistanceFromOrigin(),
1876  m_largestDistanceFromOriginIsUpdated(false)
1877 {
1878 }
1879 
1880 void CLandmarksMap::TCustomSequenceLandmarks::clear()
1881 {
1882  m_landmarks.clear();
1883 
1884  // Erase the grid:
1885  m_grid.clear();
1886 
1887  m_largestDistanceFromOriginIsUpdated = false;
1888 }
1889 
1890 void CLandmarksMap::TCustomSequenceLandmarks::push_back(const CLandmark& l)
1891 {
1892  // Resize grid if necesary:
1893  vector_int dummyEmpty;
1894 
1895  m_grid.resize(
1896  min(m_grid.getXMin(), l.pose_mean.x - 0.1),
1897  max(m_grid.getXMax(), l.pose_mean.x + 0.1),
1898  min(m_grid.getYMin(), l.pose_mean.y - 0.1),
1899  max(m_grid.getYMax(), l.pose_mean.y + 0.1), dummyEmpty);
1900 
1901  m_landmarks.push_back(l);
1902 
1903  // Add to the grid:
1904  vector_int* cell = m_grid.cellByPos(l.pose_mean.x, l.pose_mean.y);
1905  ASSERT_(cell);
1906  cell->push_back(m_landmarks.size() - 1);
1907 
1908  m_largestDistanceFromOriginIsUpdated = false;
1909 }
1910 
1911 CLandmark* CLandmarksMap::TCustomSequenceLandmarks::get(unsigned int indx)
1912 {
1913  return &m_landmarks[indx];
1914 }
1915 
1916 const CLandmark* CLandmarksMap::TCustomSequenceLandmarks::get(
1917  unsigned int indx) const
1918 {
1919  return &m_landmarks[indx];
1920 }
1921 
1922 void CLandmarksMap::TCustomSequenceLandmarks::isToBeModified(unsigned int indx)
1923 {
1924  vector_int* cell = m_grid.cellByPos(
1925  m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);
1926 
1927  vector_int::iterator it;
1928  for (it = cell->begin(); it != cell->end(); it++)
1929  {
1930  if (*it == static_cast<int>(indx))
1931  {
1932  cell->erase(it);
1933  return;
1934  }
1935  }
1936 
1937  m_largestDistanceFromOriginIsUpdated = false;
1938 }
1939 
1940 void CLandmarksMap::TCustomSequenceLandmarks::erase(unsigned int indx)
1941 {
1942  m_landmarks.erase(m_landmarks.begin() + indx);
1943  m_largestDistanceFromOriginIsUpdated = false;
1944 }
1945 
1946 void CLandmarksMap::TCustomSequenceLandmarks::hasBeenModified(unsigned int indx)
1947 {
1948  vector_int dummyEmpty;
1949 
1950  // Resize grid if necesary:
1951  m_grid.resize(
1952  min(m_grid.getXMin(), m_landmarks[indx].pose_mean.x),
1953  max(m_grid.getXMax(), m_landmarks[indx].pose_mean.x),
1954  min(m_grid.getYMin(), m_landmarks[indx].pose_mean.y),
1955  max(m_grid.getYMax(), m_landmarks[indx].pose_mean.y), dummyEmpty);
1956 
1957  // Add to the grid:
1958  vector_int* cell = m_grid.cellByPos(
1959  m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);
1960  cell->push_back(indx);
1961  m_largestDistanceFromOriginIsUpdated = false;
1962 }
1963 
1964 void CLandmarksMap::TCustomSequenceLandmarks::hasBeenModifiedAll()
1965 {
1966  MRPT_START
1967 
1968  TSequenceLandmarks::iterator it;
1969  unsigned int idx;
1970  double min_x = -10.0, max_x = 10.0;
1971  double min_y = -10.0, max_y = 10.0;
1972  vector_int dummyEmpty;
1973 
1974  // Clear cells:
1975  m_grid.clear();
1976 
1977  // Resize the grid to the outer limits of landmarks:
1978  for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end();
1979  idx++, it++)
1980  {
1981  min_x = min(min_x, it->pose_mean.x);
1982  max_x = max(max_x, it->pose_mean.x);
1983  min_y = min(min_y, it->pose_mean.y);
1984  max_y = max(max_y, it->pose_mean.y);
1985  }
1986  m_grid.resize(min_x, max_x, min_y, max_y, dummyEmpty);
1987 
1988  // Add landmarks to cells:
1989  for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end();
1990  idx++, it++)
1991  {
1992  vector_int* cell = m_grid.cellByPos(it->pose_mean.x, it->pose_mean.y);
1993  cell->push_back(idx);
1994  }
1995 
1996  m_largestDistanceFromOriginIsUpdated = false;
1997  MRPT_END
1998 }
1999 
2000 /*---------------------------------------------------------------
2001  getLargestDistanceFromOrigin
2002 ---------------------------------------------------------------*/
2003 float CLandmarksMap::TCustomSequenceLandmarks::getLargestDistanceFromOrigin()
2004  const
2005 {
2006  // Updated?
2007  if (!m_largestDistanceFromOriginIsUpdated)
2008  {
2009  // NO: Update it:
2010  float maxDistSq = 0, d;
2011  for (const_iterator it = begin(); it != end(); ++it)
2012  {
2013  d = square(it->pose_mean.x) + square(it->pose_mean.y) +
2014  square(it->pose_mean.z);
2015  maxDistSq = max(d, maxDistSq);
2016  }
2017 
2018  m_largestDistanceFromOrigin = sqrt(maxDistSq);
2019  m_largestDistanceFromOriginIsUpdated = true;
2020  }
2021  return m_largestDistanceFromOrigin;
2022 }
2023 
2024 /*---------------------------------------------------------------
2025  computeLikelihood_SIFT_LandmarkMap
2026  ---------------------------------------------------------------*/
2027 double CLandmarksMap::computeLikelihood_SIFT_LandmarkMap(
2028  CLandmarksMap* theMap, TMatchingPairList* correspondences,
2029  std::vector<bool>* otherCorrespondences)
2030 // double CLandmarksMap::computeLikelihood_SIFT_LandmarkMap( CLandmarksMap
2031 // *theMap )
2032 {
2033  // std::cout << "Entrando en computeLikelihood -- MAPA_OTHER: " <<
2034  // theMap->landmarks.size() << std::endl;
2035  // std::cout << "Entrando en computeLikelihood -- MAPA_THIS: " <<
2036  // this->landmarks.size() << std::endl;
2037  double lik = 0; // For 'traditional'
2038  double lik_i;
2039  unsigned long distDesc;
2040  double likByDist, likByDesc;
2041 
2042  // FILE *f;
2043 
2044  // int nFeaturesThis = this->size();
2045  // int nFeaturesAuxMap = theMap->size();
2046  TSequenceLandmarks::iterator lm1, lm2;
2047  std::vector<unsigned char>::iterator it1, it2;
2048 
2049  // Fast look-up, precomputed, variables:
2050  // double sigmaDist3 = 4.0 *
2051  // likelihoodOptions.SIFTs_sigma_euclidean_dist;
2052 
2053  /**** FAMD ****/
2054  double K_dist = -0.5 / square(likelihoodOptions.SIFTs_mahaDist_std);
2055  // double K_dist = - 0.5;
2056  /**************/
2057  double K_desc =
2058  -0.5 / square(likelihoodOptions.SIFTs_sigma_descriptor_dist);
2059 
2060  unsigned int idx1, idx2;
2061  CPointPDFGaussian lm1_pose, lm2_pose;
2062  CMatrixD dij(1, 3), Cij(3, 3), Cij_1;
2063  double distMahaFlik2;
2064  int decimation = likelihoodOptions.SIFTs_decimation;
2065 
2066  // USE INSERTOPTIONS METHOD
2067  switch (insertionOptions.SIFTLikelihoodMethod)
2068  {
2069  case 0: // Our method
2070 
2071  // lik = 1e-9; // For consensus
2072  lik = 1.0; // For traditional
2073 
2074  for (idx1 = 0, lm1 = theMap->landmarks.begin();
2075  lm1 < theMap->landmarks.end();
2076  lm1 += decimation, idx1 += decimation) // Other theMap LM1
2077  {
2078  if (lm1->getType() == featSIFT)
2079  {
2080  // Get the pose of lm1 as an object:
2081  lm1->getPose(lm1_pose);
2082 
2083  lik_i = 0; // Counter
2084 
2085  for (idx2 = 0, lm2 = landmarks.begin();
2086  lm2 != landmarks.end();
2087  lm2++, idx2++) // This theMap LM2
2088  {
2089  if (lm2->getType() == featSIFT)
2090  {
2091  // Get the pose of lm2 as an object:
2092  lm2->getPose(lm2_pose);
2093 
2094  // Compute the likelihood according to mahalanobis
2095  // distance:
2096  // Distance between means:
2097  dij(0, 0) = lm1->pose_mean.x - lm2->pose_mean.x;
2098  dij(0, 1) = lm1->pose_mean.y - lm2->pose_mean.y;
2099  dij(0, 2) = lm1->pose_mean.z - lm2->pose_mean.z;
2100 
2101  // std::cout << "ED POSICION: " << sqrt(
2102  // dij(0,0)*dij(0,0) + dij(0,1)*dij(0,1) +
2103  // dij(0,2)*dij(0,2) ) << std::endl;
2104  // Equivalent covariance from "i" to "j":
2105  Cij = CMatrixDouble(lm1_pose.cov + lm2_pose.cov);
2106  Cij_1 = Cij.inv();
2107 
2108  distMahaFlik2 = dij.multiply_HCHt_scalar(
2109  Cij_1); //( dij * Cij_1 * (~dij) )(0,0);
2110 
2111  likByDist = exp(K_dist * distMahaFlik2);
2112  // std::cout << likByDist << std::endl;
2113 
2114  if (likByDist > 1e-2)
2115  {
2116  // If the EUCLIDEAN distance is not too large,
2117  // we compute the Descriptor distance
2118  // Compute Likelihood by descriptor
2119  // Compute distance between descriptors
2120 
2121  // IF the EDD has been already computed, we skip
2122  // this step!
2123  std::pair<mrpt::maps::CLandmark::TLandmarkID,
2124  mrpt::maps::CLandmark::TLandmarkID>
2125  mPair(lm2->ID, lm1->ID);
2126  // std::cout << "Par: (" << lm2->ID << "," <<
2127  // lm1->ID << ") -> ";
2128 
2129  if (CLandmarksMap::_mEDD[mPair] == 0)
2130  {
2131  distDesc = 0;
2132  ASSERT_(
2133  !lm1->features.empty() &&
2134  !lm2->features.empty())
2135  ASSERT_(
2136  lm1->features[0] && lm2->features[0])
2137  ASSERT_(
2138  lm1->features[0]
2139  ->descriptors.SIFT.size() ==
2140  lm2->features[0]
2141  ->descriptors.SIFT.size())
2142 
2143  for (it1 = lm1->features[0]
2144  ->descriptors.SIFT.begin(),
2145  it2 = lm2->features[0]
2146  ->descriptors.SIFT.begin();
2147  it1 !=
2148  lm1->features[0]
2149  ->descriptors.SIFT.end();
2150  it1++, it2++)
2151  distDesc += square(*it1 - *it2);
2152 
2153  // Insert into the vector of Euclidean
2154  // distances
2155  CLandmarksMap::_mEDD[mPair] = distDesc;
2156  // std::cout << "Nueva entrada! - (LIK): "
2157  // << exp( K_desc * distDesc ) << " -> " <<
2158  // "(" << mPair.first << "," << mPair.second
2159  // << ")" << std::endl;
2160 
2161  } // end if
2162  else
2163  {
2164  distDesc = (unsigned long)
2165  CLandmarksMap::_mEDD[mPair];
2166  // std::cout << "Ya esta calculado!: " <<
2167  // "(" << mPair.first << "," << mPair.second
2168  // << ")" << ": " << distDesc << std::endl;
2169  }
2170 
2171  likByDesc = exp(K_desc * distDesc);
2172 
2173  // likByDesc = max(likByDesc, 1e-5);
2174  // printf("LIK (%d,%d) - (Dist) %f , (Desc) %f -
2175  // %u\n", idx1, idx2, likByDist, likByDesc,
2176  // distDesc );
2177 
2178  // if (likByDesc>0.9)
2179  //{
2180  // printf("LM1: %d (%.3f,%.3f,%.3f), LM2: %d
2181  //(%.3f,%.3f,%.3f)\n", idx1, lm1->pose_mean.x,
2182  // lm1->pose_mean.y, lm1->pose_mean.z,
2183  // idx2,
2184  // lm2->pose_mean.x, lm2->pose_mean.y,
2185  // lm2->pose_mean.z );
2186  // mrpt::system::pause();
2187  //}
2188 
2189  lik_i += likByDist *
2190  likByDesc; // Cumulative Likelihood
2191  }
2192  else
2193  {
2194  // If the EUCLIDEAN distance is too large, we
2195  // assume that the cumulative likelihood is
2196  // (almost) zero.
2197  lik_i += 1e-10f;
2198  }
2199  } // end if
2200 
2201  } // end for "lm2"
2202  // lik += (0.1 + 0.9*lik_i); // (CONSENSUS) Total
2203  // likelihood (assuming independent probabilities)
2204  // lik += lik_i; // (CONSENSUS) Total
2205  // likelihood
2206  // (assuming independent probabilities)
2207  lik *= (0.1 + 0.9 * lik_i); // (TRADITIONAL) Total
2208  // likelihood (assuming
2209  // independent probabilities)
2210  // lik *= lik_i; // (TRADITIONAL) Total
2211  // likelihood
2212  // (assuming independent probabilities)
2213  }
2214  } // end for "lm1"
2215  // std::cout << "LIK OBS:" << lik << std::endl;
2216 
2217  // f = os::fopen("likelihood","a+");
2218  // os::fprintf(f, "%e\n", lik);
2219  // os::fclose(f);
2220  // std::cout << "Fin del computeLikelihood: " << lik << std::endl;
2221  // system("pause");
2222 
2223  break;
2224 
2225  case 1: // SIM, ELINAS, GRIFFIN, LITTLE
2226  double dist;
2227  unsigned int k;
2228  TMatchingPairList::iterator itCorr;
2229 
2230  lik = 1.0f;
2231 
2232  // Check if the Matches are inserted into the function
2233  if (correspondences == nullptr)
2234  THROW_EXCEPTION(
2235  "When using this method with SIFTLikelihoodMethod = 1, "
2236  "TMatchingPairList *correspondence can not be NULL");
2237 
2238  if (otherCorrespondences == nullptr)
2239  THROW_EXCEPTION(
2240  "When using this method with SIFTLikelihoodMethod = 1, "
2241  "std::vector<bool> *otherCorrespondences can not be NULL");
2242 
2243  for (itCorr = correspondences->begin();
2244  itCorr != correspondences->end(); itCorr++)
2245  {
2246  CLandmark* lm1 = theMap->landmarks.get(itCorr->other_idx);
2247  CLandmark* lm2 = landmarks.get(itCorr->this_idx);
2248 
2249  dij(0, 0) = lm1->pose_mean.x - lm2->pose_mean.x;
2250  dij(0, 1) = lm1->pose_mean.y - lm2->pose_mean.y;
2251  dij(0, 2) = lm1->pose_mean.z - lm2->pose_mean.z;
2252 
2253  // Equivalent covariance from "i" to "j":
2254  Cij = CMatrixDouble(lm1_pose.cov + lm2_pose.cov);
2255  Cij_1 = Cij.inv();
2256 
2257  distMahaFlik2 = dij.multiply_HCHt_scalar(
2258  Cij_1); // ( dij * Cij_1 * (~dij) )(0,0);
2259 
2260  dist = min(
2261  (double)likelihoodOptions.SIFTnullCorrespondenceDistance,
2262  distMahaFlik2);
2263 
2264  lik *= exp(-0.5 * dist);
2265 
2266  } // end for correspondences
2267 
2268  // We complete the likelihood with the null correspondences
2269  for (k = 1; k <= (theMap->size() - correspondences->size()); k++)
2270  lik *= likelihoodOptions.SIFTnullCorrespondenceDistance;
2271 
2272  break;
2273 
2274  } // end switch
2275 
2276  /** /
2277  saveToMATLABScript3D(std::string(("ver_ref.m")),"b.");
2278  theMap->saveToMATLABScript3D(std::string(("ver_sensed.m")),"r.");
2279  theMap->saveToTextFile(std::string("debug_map1.txt"));
2280  this->saveToTextFile(std::string("debug_map2.txt"));
2281  / **/
2282 
2283  lik = log(lik);
2284  MRPT_CHECK_NORMAL_NUMBER(lik);
2285  return lik;
2286 }
2287 
2288 /*---------------------------------------------------------------
2289  TInsertionOptions
2290  ---------------------------------------------------------------*/
2291 CLandmarksMap::TInsertionOptions::TInsertionOptions()
2292  : insert_SIFTs_from_monocular_images(true),
2293  insert_SIFTs_from_stereo_images(true),
2294  insert_Landmarks_from_range_scans(true),
2295  SiftCorrRatioThreshold(0.4f),
2296  SiftLikelihoodThreshold(0.5f),
2297 
2298  SiftEDDThreshold(200.0f),
2299  SIFTMatching3DMethod(0),
2300  SIFTLikelihoodMethod(0),
2301 
2302  SIFTsLoadDistanceOfTheMean(3.0f),
2303  SIFTsLoadEllipsoidWidth(0.05f),
2304 
2305  SIFTs_stdXY(2.0f),
2306  SIFTs_stdDisparity(1.0f),
2307 
2308  SIFTs_numberOfKLTKeypoints(60),
2309  SIFTs_stereo_maxDepth(15.0f),
2310 
2311  SIFTs_epipolar_TH(1.5f),
2312  PLOT_IMAGES(false),
2313 
2314  SIFT_feat_options(vision::featSIFT)
2315 {
2316 }
2317 
2318 /*---------------------------------------------------------------
2319  dumpToTextStream
2320  ---------------------------------------------------------------*/
2321 void CLandmarksMap::TInsertionOptions::dumpToTextStream(
2322  mrpt::utils::CStream& out) const
2323 {
2324  out.printf(
2325  "\n----------- [CLandmarksMap::TInsertionOptions] ------------ \n\n");
2326 
2327  out.printf(
2328  "insert_SIFTs_from_monocular_images = %c\n",
2329  insert_SIFTs_from_monocular_images ? 'Y' : 'N');
2330  out.printf(
2331  "insert_SIFTs_from_stereo_images = %c\n",
2332  insert_SIFTs_from_stereo_images ? 'Y' : 'N');
2333  out.printf(
2334  "insert_Landmarks_from_range_scans = %c\n",
2335  insert_Landmarks_from_range_scans ? 'Y' : 'N');
2336  out.printf("\n");
2337  out.printf(
2338  "SiftCorrRatioThreshold = %f\n",
2339  SiftCorrRatioThreshold);
2340  out.printf(
2341  "SiftLikelihoodThreshold = %f\n",
2342  SiftLikelihoodThreshold);
2343  out.printf(
2344  "SiftEDDThreshold = %f\n", SiftEDDThreshold);
2345  out.printf(
2346  "SIFTMatching3DMethod = %d\n", SIFTMatching3DMethod);
2347  out.printf(
2348  "SIFTLikelihoodMethod = %d\n", SIFTLikelihoodMethod);
2349 
2350  out.printf(
2351  "SIFTsLoadDistanceOfTheMean = %f\n",
2352  SIFTsLoadDistanceOfTheMean);
2353  out.printf(
2354  "SIFTsLoadEllipsoidWidth = %f\n",
2355  SIFTsLoadEllipsoidWidth);
2356  out.printf("\n");
2357  out.printf("SIFTs_stdXY = %f\n", SIFTs_stdXY);
2358  out.printf(
2359  "SIFTs_stdDisparity = %f\n", SIFTs_stdDisparity);
2360  out.printf("\n");
2361  out.printf(
2362  "SIFTs_numberOfKLTKeypoints = %i\n",
2363  SIFTs_numberOfKLTKeypoints);
2364  out.printf(
2365  "SIFTs_stereo_maxDepth = %f\n",
2366  SIFTs_stereo_maxDepth);
2367  out.printf(
2368  "SIFTs_epipolar_TH = %f\n", SIFTs_epipolar_TH);
2369  out.printf(
2370  "PLOT_IMAGES = %c\n",
2371  PLOT_IMAGES ? 'Y' : 'N');
2372 
2373  SIFT_feat_options.dumpToTextStream(out);
2374 
2375  out.printf("\n");
2376 }
2377 
2378 /*---------------------------------------------------------------
2379  loadFromConfigFile
2380  ---------------------------------------------------------------*/
2381 void CLandmarksMap::TInsertionOptions::loadFromConfigFile(
2382  const mrpt::utils::CConfigFileBase& iniFile, const std::string& section)
2383 {
2384  insert_SIFTs_from_monocular_images = iniFile.read_bool(
2385  section.c_str(), "insert_SIFTs_from_monocular_images",
2386  insert_SIFTs_from_monocular_images);
2387  insert_SIFTs_from_stereo_images = iniFile.read_bool(
2388  section.c_str(), "insert_SIFTs_from_stereo_images",
2389  insert_SIFTs_from_stereo_images);
2390  insert_Landmarks_from_range_scans = iniFile.read_bool(
2391  section.c_str(), "insert_Landmarks_from_range_scans",
2392  insert_Landmarks_from_range_scans);
2393  SiftCorrRatioThreshold = iniFile.read_float(
2394  section.c_str(), "SiftCorrRatioThreshold", SiftCorrRatioThreshold);
2395  SiftLikelihoodThreshold = iniFile.read_float(
2396  section.c_str(), "SiftLikelihoodThreshold", SiftLikelihoodThreshold);
2397  SiftEDDThreshold = iniFile.read_float(
2398  section.c_str(), "SiftEDDThreshold", SiftEDDThreshold);
2399  SIFTMatching3DMethod = iniFile.read_int(
2400  section.c_str(), "SIFTMatching3DMethod", SIFTMatching3DMethod);
2401  SIFTLikelihoodMethod = iniFile.read_int(
2402  section.c_str(), "SIFTLikelihoodMethod", SIFTLikelihoodMethod);
2403  SIFTsLoadDistanceOfTheMean = iniFile.read_float(
2404  section.c_str(), "SIFTsLoadDistanceOfTheMean",
2405  SIFTsLoadDistanceOfTheMean);
2406  SIFTsLoadEllipsoidWidth = iniFile.read_float(
2407  section.c_str(), "SIFTsLoadEllipsoidWidth", SIFTsLoadEllipsoidWidth);
2408  SIFTs_stdXY =
2409  iniFile.read_float(section.c_str(), "SIFTs_stdXY", SIFTs_stdXY);
2410  SIFTs_stdDisparity = iniFile.read_float(
2411  section.c_str(), "SIFTs_stdDisparity", SIFTs_stdDisparity);
2412  SIFTs_numberOfKLTKeypoints = iniFile.read_int(
2413  section.c_str(), "SIFTs_numberOfKLTKeypoints",
2414  SIFTs_numberOfKLTKeypoints);
2415  SIFTs_stereo_maxDepth = iniFile.read_float(
2416  section.c_str(), "SIFTs_stereo_maxDepth", SIFTs_stereo_maxDepth);
2417  SIFTs_epipolar_TH = iniFile.read_float(
2418  section.c_str(), "SIFTs_epipolar_TH", SIFTs_epipolar_TH);
2419  PLOT_IMAGES =
2420  iniFile.read_bool(section.c_str(), "PLOT_IMAGES", PLOT_IMAGES);
2421 
2422  SIFT_feat_options.loadFromConfigFile(iniFile, section);
2423 }
2424 
2425 /*---------------------------------------------------------------
2426  TLikelihoodOptions
2427  ---------------------------------------------------------------*/
2428 CLandmarksMap::TLikelihoodOptions::TLikelihoodOptions()
2429  : rangeScan2D_decimation(20),
2430  SIFTs_sigma_euclidean_dist(0.30f),
2431  SIFTs_sigma_descriptor_dist(100.0f),
2432  SIFTs_mahaDist_std(4.0f),
2433  SIFTnullCorrespondenceDistance(4.0f),
2434  SIFTs_decimation(1),
2435  SIFT_feat_options(vision::featSIFT),
2436  beaconRangesStd(0.08f),
2437  beaconRangesUseObservationStd(false),
2438  extRobotPoseStd(0.05f),
2439  GPSOrigin(),
2440  GPS_sigma(1.0f)
2441 {
2442 }
2443 
2444 CLandmarksMap::TLikelihoodOptions::TGPSOrigin::TGPSOrigin()
2445  : longitude(-4.47763833333333),
2446  latitude(36.71559000000000),
2447  altitude(42.3),
2448  ang(0),
2449  x_shift(0),
2450  y_shift(0),
2451  min_sat(4)
2452 {
2453 }
2454 
2455 /*---------------------------------------------------------------
2456  dumpToTextStream
2457  ---------------------------------------------------------------*/
2458 void CLandmarksMap::TLikelihoodOptions::dumpToTextStream(
2459  mrpt::utils::CStream& out) const
2460 {
2461  out.printf(
2462  "\n----------- [CLandmarksMap::TLikelihoodOptions] ------------ \n\n");
2463 
2464  out.printf(
2465  "rangeScan2D_decimation = %i\n",
2466  rangeScan2D_decimation);
2467  out.printf(
2468  "SIFTs_sigma_euclidean_dist = %f\n",
2469  SIFTs_sigma_euclidean_dist);
2470  out.printf(
2471  "SIFTs_sigma_descriptor_dist = %f\n",
2472  SIFTs_sigma_descriptor_dist);
2473  out.printf(
2474  "SIFTs_mahaDist_std = %f\n", SIFTs_mahaDist_std);
2475  out.printf(
2476  "SIFTs_decimation = %i\n", SIFTs_decimation);
2477  out.printf(
2478  "SIFTnullCorrespondenceDistance = %f\n",
2479  SIFTnullCorrespondenceDistance);
2480  out.printf(
2481  "beaconRangesStd = %f\n", beaconRangesStd);
2482  out.printf(
2483  "beaconRangesUseObservationStd = %c\n",
2484  beaconRangesUseObservationStd ? 'Y' : 'N');
2485  out.printf(
2486  "extRobotPoseStd = %f\n", extRobotPoseStd);
2487 
2488  out.printf(
2489  "GPSOrigin:LATITUDE = %f\n", GPSOrigin.latitude);
2490  out.printf(
2491  "GPSOrigin:LONGITUDE = %f\n", GPSOrigin.longitude);
2492  out.printf(
2493  "GPSOrigin:ALTITUDE = %f\n", GPSOrigin.altitude);
2494  out.printf("GPSOrigin:Rotation_Angle = %f\n", GPSOrigin.ang);
2495  out.printf(
2496  "GPSOrigin:x_shift = %f\n", GPSOrigin.x_shift);
2497  out.printf(
2498  "GPSOrigin:y_shift = %f\n", GPSOrigin.y_shift);
2499  out.printf(
2500  "GPSOrigin:min_sat = %i\n", GPSOrigin.min_sat);
2501 
2502  out.printf("GPS_sigma = %f (m)\n", GPS_sigma);
2503 
2504  SIFT_feat_options.dumpToTextStream(out);
2505 
2506  out.printf("\n");
2507 }
2508 
2509 /*---------------------------------------------------------------
2510  loadFromConfigFile
2511  ---------------------------------------------------------------*/
2512 void CLandmarksMap::TLikelihoodOptions::loadFromConfigFile(
2513  const mrpt::utils::CConfigFileBase& iniFile, const std::string& section)
2514 {
2515  rangeScan2D_decimation = iniFile.read_int(
2516  section.c_str(), "rangeScan2D_decimation", rangeScan2D_decimation);
2517  SIFTs_sigma_euclidean_dist = iniFile.read_double(
2518  section.c_str(), "SIFTs_sigma_euclidean_dist",
2519  SIFTs_sigma_euclidean_dist);
2520  SIFTs_sigma_descriptor_dist = iniFile.read_double(
2521  section.c_str(), "SIFTs_sigma_descriptor_dist",
2522  SIFTs_sigma_descriptor_dist);
2523  SIFTs_mahaDist_std = iniFile.read_float(
2524  section.c_str(), "SIFTs_mahaDist_std", SIFTs_mahaDist_std);
2525  SIFTs_decimation =
2526  iniFile.read_int(section.c_str(), "SIFTs_decimation", SIFTs_decimation);
2527  SIFTnullCorrespondenceDistance = iniFile.read_float(
2528  section.c_str(), "SIFTnullCorrespondenceDistance",
2529  SIFTnullCorrespondenceDistance);
2530 
2531  GPSOrigin.latitude = iniFile.read_double(
2532  section.c_str(), "GPSOriginLatitude", GPSOrigin.latitude);
2533  GPSOrigin.longitude = iniFile.read_double(
2534  section.c_str(), "GPSOriginLongitude", GPSOrigin.longitude);
2535  GPSOrigin.altitude = iniFile.read_double(
2536  section.c_str(), "GPSOriginAltitude", GPSOrigin.altitude);
2537  GPSOrigin.ang =
2538  iniFile.read_double(section.c_str(), "GPSOriginAngle", GPSOrigin.ang) *
2539  M_PI / 180;
2540  GPSOrigin.x_shift = iniFile.read_double(
2541  section.c_str(), "GPSOriginXshift", GPSOrigin.x_shift);
2542  GPSOrigin.y_shift = iniFile.read_double(
2543  section.c_str(), "GPSOriginYshift", GPSOrigin.y_shift);
2544  GPSOrigin.min_sat =
2545  iniFile.read_int(section.c_str(), "GPSOriginMinSat", GPSOrigin.min_sat);
2546 
2547  GPS_sigma = iniFile.read_float(section.c_str(), "GPSSigma", GPS_sigma);
2548 
2549  beaconRangesStd =
2550  iniFile.read_float(section.c_str(), "beaconRangesStd", beaconRangesStd);
2551  beaconRangesUseObservationStd = iniFile.read_bool(
2552  section.c_str(), "beaconRangesUseObservationStd",
2553  beaconRangesUseObservationStd);
2554 
2555  extRobotPoseStd =
2556  iniFile.read_float(section.c_str(), "extRobotPoseStd", extRobotPoseStd);
2557 
2558  SIFT_feat_options.loadFromConfigFile(iniFile, section);
2559 }
2560 
2561 /*---------------------------------------------------------------
2562  TFuseOptions
2563  ---------------------------------------------------------------*/
2564 CLandmarksMap::TFuseOptions::TFuseOptions()
2565  : minTimesSeen(2), ellapsedTime(4.0f)
2566 {
2567 }
2568 
2569 /*---------------------------------------------------------------
2570  isEmpty
2571  ---------------------------------------------------------------*/
2572 bool CLandmarksMap::isEmpty() const { return size() == 0; }
2573 /*---------------------------------------------------------------
2574  simulateBeaconReadings
2575  ---------------------------------------------------------------*/
2576 void CLandmarksMap::simulateBeaconReadings(
2577  const CPose3D& in_robotPose, const CPoint3D& in_sensorLocationOnRobot,
2578  mrpt::obs::CObservationBeaconRanges& out_Observations) const
2579 {
2580  TSequenceLandmarks::const_iterator it;
2581  mrpt::obs::CObservationBeaconRanges::TMeasurement newMeas;
2582  CPoint3D point3D, beacon3D;
2583  CPointPDFGaussian beaconPDF;
2584 
2585  // Compute the 3D position of the sensor:
2586  point3D = in_robotPose + in_sensorLocationOnRobot;
2587 
2588  // Clear output data:
2589  out_Observations.sensedData.clear();
2590  out_Observations.timestamp = mrpt::system::getCurrentTime();
2591 
2592  // For each BEACON landmark in the map:
2593  for (it = landmarks.begin(); it != landmarks.end(); it++)
2594  {
2595  if (it->getType() == featBeacon)
2596  {
2597  // Get the 3D position of the beacon (just the mean value):
2598  it->getPose(beaconPDF);
2599  beacon3D = beaconPDF.mean;
2600 
2601  float range = point3D.distanceTo(beacon3D);
2602 
2603  // Add noise:
2604  range += out_Observations.stdError *
2605  getRandomGenerator().drawGaussian1D_normalized();
2606  range = max(0.0f, range);
2607 
2608  if (range >= out_Observations.minSensorDistance &&
2609  range <= out_Observations.maxSensorDistance)
2610  {
2611  // Fill out:
2612  newMeas.beaconID = it->ID;
2613  newMeas.sensorLocationOnRobot = in_sensorLocationOnRobot;
2614  newMeas.sensedDistance = range;
2615 
2616  // Insert:
2617  out_Observations.sensedData.push_back(newMeas);
2618  }
2619  } // end if beacon
2620  } // end for it
2621  // Done!
2622 }
2623 
2624 /*---------------------------------------------------------------
2625  saveMetricMapRepresentationToFile
2626  ---------------------------------------------------------------*/
2627 void CLandmarksMap::saveMetricMapRepresentationToFile(
2628  const std::string& filNamePrefix) const
2629 {
2630  MRPT_START
2631 
2632  // Matlab:
2633  std::string fil1(filNamePrefix + std::string("_3D.m"));
2634  saveToMATLABScript3D(fil1);
2635 
2636  // 3D Scene:
2637  opengl::COpenGLScene scene;
2638  mrpt::opengl::CSetOfObjects::Ptr obj3D =
2639  mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
2640  getAs3DObject(obj3D);
2641 
2642  opengl::CGridPlaneXY::Ptr objGround =
2643  mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
2644  -100, 100, -100, 100, 0, 1);
2645 
2646  scene.insert(obj3D);
2647  scene.insert(objGround);
2648 
2649  std::string fil2(filNamePrefix + std::string("_3D.3Dscene"));
2650  CFileOutputStream f(fil2.c_str());
2651  f << scene;
2652 
2653  MRPT_END
2654 }
2655 
2656 /*---------------------------------------------------------------
2657  getAs3DObject
2658  ---------------------------------------------------------------*/
2659 void CLandmarksMap::getAs3DObject(
2660  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
2661 {
2662  if (!genericMapParams.enableSaveAs3DObject) return;
2663 
2664  // TODO: Generate patchs in 3D, etc...
2665 
2666  // Save 3D ellipsoids
2667  CPointPDFGaussian pointGauss;
2668  for (TCustomSequenceLandmarks::const_iterator it = landmarks.begin();
2669  it != landmarks.end(); ++it)
2670  {
2671  opengl::CEllipsoid::Ptr ellip =
2672  mrpt::make_aligned_shared<opengl::CEllipsoid>();
2673 
2674  it->getPose(pointGauss);
2675 
2676  ellip->setPose(pointGauss.mean);
2677  ellip->setCovMatrix(pointGauss.cov);
2678  ellip->enableDrawSolid3D(false);
2679  ellip->setQuantiles(3.0);
2680  ellip->set3DsegmentsCount(10);
2681  ellip->setColor(0, 0, 1);
2682  ellip->setName(
2683  mrpt::format("LM.ID=%u", static_cast<unsigned int>(it->ID)));
2684  ellip->enableShowName(true);
2685 
2686  outObj->insert(ellip);
2687  }
2688 }
2689 /**** FAMD ****/
2690 mrpt::maps::CLandmark::TLandmarkID CLandmarksMap::getMapMaxID()
2691 {
2692  return _mapMaxID;
2693 }
2694 /**** END FAMD ****/
2695 
2696 const CLandmark* CLandmarksMap::TCustomSequenceLandmarks::getByID(
2697  CLandmark::TLandmarkID ID) const
2698 {
2699  for (size_t indx = 0; indx < m_landmarks.size(); indx++)
2700  {
2701  if (m_landmarks[indx].ID == ID) return &m_landmarks[indx];
2702  }
2703  return nullptr;
2704 }
2705 
2706 // CLandmark* CLandmarksMap::TCustomSequenceLandmarks::getByID(
2707 // CLandmark::TLandmarkID ID )
2708 //{
2709 // for(size_t indx = 0; indx < m_landmarks.size(); indx++)
2710 // {
2711 // if( m_landmarks[indx].ID == ID )
2712 // return &m_landmarks[indx];
2713 // }
2714 // return nullptr;
2715 //}
2716 
2717 const CLandmark* CLandmarksMap::TCustomSequenceLandmarks::getByBeaconID(
2718  unsigned int ID) const
2719 {
2720  for (size_t indx = 0; indx < m_landmarks.size(); indx++)
2721  {
2722  if (m_landmarks[indx].ID == ID) return &m_landmarks[indx];
2723  }
2724  return nullptr;
2725 }
2726 
2727 /*---------------------------------------------------------------
2728  Computes the ratio in [0,1] of correspondences between "this" and the
2729  "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
2730  * In the case of a multi-metric map, this returns the average between the
2731  maps. This method always return 0 for grid maps.
2732  * \param otherMap [IN] The other map to compute the matching
2733  with.
2734  * \param otherMapPose [IN] The 6D pose of the other map as seen
2735  from "this".
2736  * \param maxDistForCorr [IN] The minimum distance between 2
2737  non-probabilistic map elements for counting them as a correspondence.
2738  * \param maxMahaDistForCorr [IN] The minimum Mahalanobis distance
2739  between 2 probabilistic map elements for counting them as a correspondence.
2740  *
2741  * \return The matching ratio [0,1]
2742  * \sa computeMatchingWith2D
2743  ----------------------------------------------------------------*/
2744 float CLandmarksMap::compute3DMatchingRatio(
2745  const mrpt::maps::CMetricMap* otherMap2,
2746  const mrpt::poses::CPose3D& otherMapPose,
2747  const TMatchingRatioParams& params) const
2748 {
2749  MRPT_START
2750 
2751  // Compare to a similar map only:
2752  const CLandmarksMap* otherMap = nullptr;
2753 
2754  if (otherMap2->GetRuntimeClass() == CLASS_ID(CLandmarksMap))
2755  otherMap = static_cast<const CLandmarksMap*>(otherMap2);
2756 
2757  if (!otherMap) return 0;
2758 
2759  TCustomSequenceLandmarks::const_iterator itThis, itOther;
2760  std::deque<CPointPDFGaussian::Ptr> poses3DThis, poses3DOther;
2761  std::deque<CPointPDFGaussian::Ptr>::iterator itPoseThis, itPoseOther;
2762  CPointPDFGaussian tempPose;
2763  size_t nThis = landmarks.size();
2764  size_t nOther = otherMap->landmarks.size();
2765  size_t otherLandmarkWithCorrespondence = 0;
2766 
2767  // Checks:
2768  if (!nThis) return 0;
2769  if (!nOther) return 0;
2770 
2771  // The transformation:
2772  CMatrixDouble44 pose3DMatrix;
2773  otherMapPose.getHomogeneousMatrix(pose3DMatrix);
2774  float Tx = pose3DMatrix.get_unsafe(0, 3);
2775  float Ty = pose3DMatrix.get_unsafe(1, 3);
2776  float Tz = pose3DMatrix.get_unsafe(2, 3);
2777 
2778  // ---------------------------------------------------------------------------------------------------------------
2779  // Is there any "contact" between the spheres that contain all the points
2780  // from each map after translating them??
2781  // (Note that we can avoid computing the rotation of all the points if this
2782  // test fail, with a great speed up!)
2783  // ---------------------------------------------------------------------------------------------------------------
2784  if (sqrt(square(Tx) + square(Ty) + square(Tz)) >=
2785  (landmarks.getLargestDistanceFromOrigin() +
2786  otherMap->landmarks.getLargestDistanceFromOrigin() + 1.0f))
2787  return 0; // There is no contact!
2788 
2789  // Prepare:
2790  poses3DOther.resize(nOther);
2791  for (size_t i = 0; i < nOther; i++)
2792  poses3DOther[i] = mrpt::make_aligned_shared<CPointPDFGaussian>();
2793 
2794  poses3DThis.resize(nThis);
2795  for (size_t i = 0; i < nThis; i++)
2796  poses3DThis[i] = mrpt::make_aligned_shared<CPointPDFGaussian>();
2797 
2798  // Save 3D poses of the other map with transformed coordinates:
2799  for (itOther = otherMap->landmarks.begin(),
2800  itPoseOther = poses3DOther.begin();
2801  itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
2802  {
2803  itOther->getPose(**itPoseOther);
2804  (*itPoseOther)->changeCoordinatesReference(otherMapPose);
2805  }
2806 
2807  // And the 3D poses of "this" map:
2808  for (itThis = landmarks.begin(), itPoseThis = poses3DThis.begin();
2809  itThis != landmarks.end(); itThis++, itPoseThis++)
2810  {
2811  itThis->getPose(**itPoseThis);
2812  }
2813 
2814  // Check whether each "other"'s LM has a correspondence or not:
2815  for (itOther = otherMap->landmarks.begin(),
2816  itPoseOther = poses3DOther.begin();
2817  itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
2818  {
2819  for (itThis = landmarks.begin(), itPoseThis = poses3DThis.begin();
2820  itThis != landmarks.end(); itThis++, itPoseThis++)
2821  {
2822  CMatrixDouble COV =
2823  CMatrixDouble((*itPoseThis)->cov + (*itPoseOther)->cov);
2824  TPoint3D D(
2825  (*itPoseThis)->mean.x() - (*itPoseOther)->mean.x(),
2826  (*itPoseThis)->mean.y() - (*itPoseOther)->mean.y(),
2827  (*itPoseThis)->mean.z() - (*itPoseOther)->mean.z());
2828  CMatrixDouble d(1, 3);
2829  d(0, 0) = D.x;
2830  d(0, 1) = D.y;
2831  d(0, 2) = D.z;
2832 
2833  float distMaha = sqrt(
2834  d.multiply_HCHt_scalar(
2835  COV.inv())); //(d*COV.inv()*(~d))(0,0) );
2836 
2837  if (distMaha < params.maxMahaDistForCorr)
2838  {
2839  // Now test the SIFT descriptors:
2840  if (!itThis->features.empty() && !itOther->features.empty() &&
2841  itThis->features[0]->descriptors.SIFT.size() ==
2842  itOther->features[0]->descriptors.SIFT.size())
2843  {
2844  unsigned long descrDist = 0;
2845  std::vector<unsigned char>::const_iterator it1, it2;
2846  for (it1 = itThis->features[0]->descriptors.SIFT.begin(),
2847  it2 = itOther->features[0]->descriptors.SIFT.begin();
2848  it1 != itThis->features[0]->descriptors.SIFT.end();
2849  it1++, it2++)
2850  descrDist += square(*it1 - *it2);
2851 
2852  float descrDist_f =
2853  sqrt(static_cast<float>(descrDist)) /
2854  itThis->features[0]->descriptors.SIFT.size();
2855 
2856  if (descrDist_f < 1.5f)
2857  {
2858  otherLandmarkWithCorrespondence++;
2859  break; // Go to the next "other" LM
2860  }
2861  }
2862  }
2863  } // for each in "this"
2864 
2865  } // for each in "other"
2866 
2867  return static_cast<float>(otherLandmarkWithCorrespondence) / nOther;
2868 
2869  MRPT_END
2870 }
2871 
2872 /*---------------------------------------------------------------
2873  auxParticleFilterCleanUp
2874  ---------------------------------------------------------------*/
2875 void CLandmarksMap::auxParticleFilterCleanUp()
2876 {
2877  // std::cout << "mEDD:" << std::endl;
2878  // std::cout << "-----------------------" << std::endl;
2879  // std::map<std::pair<mrpt::maps::CLandmark::TLandmarkID,
2880  // mrpt::maps::CLandmark::TLandmarkID>, unsigned long>::iterator itmEDD;
2881  // for(itmEDD = CLandmarksMap::_mEDD.begin(); itmEDD !=
2882  // CLandmarksMap::_mEDD.end(); itmEDD++)
2883  // std::cout << "(" << itmEDD->first.first << "," << itmEDD->first.second
2884  //<< ")" << ": " << itmEDD->second << std::endl;
2885 
2886  CLandmarksMap::_mEDD.clear();
2887  CLandmarksMap::_maxIDUpdated = false;
2888  // TODO: Paco...
2889 }
2890 
2891 /*---------------------------------------------------------------
2892  simulateRangeBearingReadings
2893  ---------------------------------------------------------------*/
2894 void CLandmarksMap::simulateRangeBearingReadings(
2895  const CPose3D& in_robotPose, const CPose3D& in_sensorLocationOnRobot,
2896  CObservationBearingRange& out_Observations, bool sensorDetectsIDs,
2897  const float in_stdRange, const float in_stdYaw, const float in_stdPitch,
2898  vector_size_t* out_real_associations, const double spurious_count_mean,
2899  const double spurious_count_std) const
2900 {
2901  TSequenceLandmarks::const_iterator it;
2902  size_t idx;
2903  mrpt::obs::CObservationBearingRange::TMeasurement newMeas;
2904  CPoint3D beacon3D;
2905  CPointPDFGaussian beaconPDF;
2906 
2907  if (out_real_associations) out_real_associations->clear();
2908 
2909  // Compute the 3D position of the sensor:
2910  const CPose3D point3D = in_robotPose + CPose3D(in_sensorLocationOnRobot);
2911 
2912  // Clear output data:
2913  out_Observations.validCovariances = false;
2914  out_Observations.sensor_std_range = in_stdRange;
2915  out_Observations.sensor_std_yaw = in_stdYaw;
2916  out_Observations.sensor_std_pitch = in_stdPitch;
2917 
2918  out_Observations.sensedData.clear();
2919  out_Observations.timestamp = mrpt::system::getCurrentTime();
2920  out_Observations.sensorLocationOnRobot = in_sensorLocationOnRobot;
2921 
2922  // For each BEACON landmark in the map:
2923  // ------------------------------------------
2924  for (idx = 0, it = landmarks.begin(); it != landmarks.end(); ++it, ++idx)
2925  {
2926  // Get the 3D position of the beacon (just the mean value):
2927  it->getPose(beaconPDF);
2928  beacon3D = CPoint3D(beaconPDF.mean);
2929 
2930  // Compute yaw,pitch,range:
2931  double range, yaw, pitch;
2932  point3D.sphericalCoordinates(beacon3D, range, yaw, pitch);
2933 
2934  // Add noises:
2935  range += in_stdRange * getRandomGenerator().drawGaussian1D_normalized();
2936  yaw += in_stdYaw * getRandomGenerator().drawGaussian1D_normalized();
2937  pitch += in_stdPitch * getRandomGenerator().drawGaussian1D_normalized();
2938 
2939  yaw = math::wrapToPi(yaw);
2940  range = max(0.0, range);
2941 
2942  if (range >= out_Observations.minSensorDistance &&
2943  range <= out_Observations.maxSensorDistance &&
2944  fabs(yaw) <= 0.5f * out_Observations.fieldOfView_yaw &&
2945  fabs(pitch) <= 0.5f * out_Observations.fieldOfView_pitch)
2946  {
2947  // Fill out:
2948  if (sensorDetectsIDs)
2949  newMeas.landmarkID = it->ID;
2950  else
2951  newMeas.landmarkID = INVALID_LANDMARK_ID;
2952  newMeas.range = range;
2953  newMeas.yaw = yaw;
2954  newMeas.pitch = pitch;
2955 
2956  // Insert:
2957  out_Observations.sensedData.push_back(newMeas);
2958 
2959  if (out_real_associations)
2960  out_real_associations->push_back(idx); // Real indices.
2961  }
2962  } // end for it
2963 
2964  const double fSpurious =
2965  getRandomGenerator().drawGaussian1D(spurious_count_mean, spurious_count_std);
2966  size_t nSpurious = 0;
2967  if (spurious_count_std != 0 || spurious_count_mean != 0)
2968  nSpurious = static_cast<size_t>(
2969  mrpt::utils::round_long(std::max(0.0, fSpurious)));
2970 
2971  // For each spurios reading to generate:
2972  // ------------------------------------------
2973  for (size_t i = 0; i < nSpurious; i++)
2974  {
2975  // Make up yaw,pitch,range out from thin air:
2976  // (the conditionals on yaw & pitch are to generate 2D observations if
2977  // we are in 2D, which we learn from a null std.dev.)
2978  const double range = getRandomGenerator().drawUniform(
2979  out_Observations.minSensorDistance,
2980  out_Observations.maxSensorDistance);
2981  const double yaw = (out_Observations.sensor_std_yaw == 0)
2982  ? 0
2983  : getRandomGenerator().drawUniform(
2984  -0.5f * out_Observations.fieldOfView_yaw,
2985  0.5f * out_Observations.fieldOfView_yaw);
2986  const double pitch =
2987  (out_Observations.sensor_std_pitch == 0)
2988  ? 0
2989  : getRandomGenerator().drawUniform(
2990  -0.5f * out_Observations.fieldOfView_pitch,
2991  0.5f * out_Observations.fieldOfView_pitch);
2992 
2993  // Fill out:
2994  newMeas.landmarkID =
2995  INVALID_LANDMARK_ID; // Always invalid ID since it's spurious
2996  newMeas.range = range;
2997  newMeas.yaw = yaw;
2998  newMeas.pitch = pitch;
2999 
3000  // Insert:
3001  out_Observations.sensedData.push_back(newMeas);
3002 
3003  if (out_real_associations)
3004  out_real_associations->push_back(
3005  std::string::npos); // Real index: spurious
3006  } // end for it
3007 
3008  // Done!
3009 }
mrpt::maps::CMetricMap::clear
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
mrpt::maps::CLandmarksMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CLandmarksMap.cpp:2659
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:25
mrpt::vision::projectMatchedFeatures
void projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::utils::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
Definition: vision_utils.cpp:988
mrpt::system::TTimeStamp
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:32
mrpt::obs::CObservationBearingRange::fieldOfView_yaw
float fieldOfView_yaw
Information about the.
Definition: CObservationBearingRange.h:45
mrpt::random::CRandomGenerator::drawUniform
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
Definition: RandomGenerators.h:103
mrpt::utils::TPixelCoordf
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
mrpt::vision::TMatchingOptions::EDD_RATIO
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
Definition: vision/include/mrpt/vision/types.h:423
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:135
range
GLsizei range
Definition: glext.h:5907
mrpt::maps::TMetricMapInitializer
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Definition: TMetricMapInitializer.h:34
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::getGrid
mrpt::utils::CDynamicGrid< vector_int > * getGrid()
Definition: CLandmarksMap.h:180
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:75
mrpt::maps::CLandmarksMap::TInsertionOptions::SiftEDDThreshold
float SiftEDDThreshold
[For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as corres...
Definition: CLandmarksMap.h:274
mrpt::vision::featNotDefined
Non-defined feature (also used for Occupancy features)
Definition: vision/include/mrpt/vision/types.h:54
mrpt::obs::CObservationImage
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
Definition: CObservationImage.h:35
mrpt::obs::gnss::Message_NMEA_GGA
NMEA datum: GGA.
Definition: gnss_messages_ascii_nmea.h:23
CFileOutputStream.h
mrpt::maps::CLandmarksMap::size
size_t size() const
Returns the stored landmarks count.
Definition: CLandmarksMap.cpp:170
mrpt::maps::CLandmarksMap::TMapDefinition::insertionOpts
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOpts
Definition: CLandmarksMap.h:709
mrpt::utils::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:143
mrpt::maps::CLandmarksMap::computeMatchingWith3DLandmarks
void computeMatchingWith3DLandmarks(const mrpt::maps::CLandmarksMap *otherMap, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: In this class...
Definition: CLandmarksMap.cpp:1103
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
t
GLdouble GLdouble t
Definition: glext.h:3689
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:79
mrpt::maps::CLandmarksMap::TInsertionOptions::insert_SIFTs_from_stereo_images
bool insert_SIFTs_from_stereo_images
If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D ...
Definition: CLandmarksMap.h:252
CObservationImage.h
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:25
mrpt::maps::CLandmark::createOneFeature
void createOneFeature()
Creates one feature in the vector "features", calling the appropriate constructor of the smart pointe...
Definition: CLandmark.h:110
mrpt::obs::CObservationStereoImages::imageLeft
mrpt::utils::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
Definition: CObservationStereoImages.h:75
mrpt::maps::CLandmark::features
std::vector< mrpt::vision::CFeature::Ptr > features
The set of features from which the landmark comes.
Definition: CLandmark.h:44
mrpt::mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::obs::CObservationBearingRange::TMeasurement::range
float range
The sensed landmark distance, in meters.
Definition: CObservationBearingRange.h:59
mrpt::obs::gnss::Message_NMEA_GGA::content_t::latitude_degrees
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
Definition: gnss_messages_ascii_nmea.h:37
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:206
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:30
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFTLikelihoodMethod
unsigned int SIFTLikelihoodMethod
[For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 0: Our method -...
Definition: CLandmarksMap.h:291
mrpt::utils::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:106
mrpt::maps::TMapGenericParams::enableSaveAs3DObject
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
Definition: metric_map_types.h:95
mrpt::maps::CLandmarksMap::TLikelihoodOptions::beaconRangesStd
float beaconRangesStd
The standard deviation used for Beacon ranges likelihood (default=0.08) [meters]. ...
Definition: CLandmarksMap.h:387
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::getByID
const CLandmark * getByID(CLandmark::TLandmarkID ID) const
Returns the landmark with a given landmrk ID, or nullptr if not found.
Definition: CLandmarksMap.cpp:2696
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
mrpt::obs::CObservationImage::cameraParams
mrpt::utils::TCamera cameraParams
Intrinsic and distortion parameters of the camera.
Definition: CObservationImage.h:57
mrpt::utils::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:118
mrpt::system::getCurrentTime
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:73
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:169
mrpt::poses::CPointPDFGaussian::mean
CPoint3D mean
The mean value.
Definition: CPointPDFGaussian.h:44
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFTs_stereo_maxDepth
float SIFTs_stereo_maxDepth
Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation.
Definition: CLandmarksMap.h:320
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: CColouredPointsMap.h:313
mrpt::maps::CLandmarksMap::fuseWith
void fuseWith(CLandmarksMap &other, bool justInsertAllOfThem=false)
Fuses the contents of another map with this one, updating "this" object with the result.
Definition: CLandmarksMap.cpp:989
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: CColouredPointsMap.h:112
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
mrpt::vision::CFeatureExtraction::detectFeatures
void detectFeatures(const mrpt::utils::CImage &img, CFeatureList &feats, const unsigned int init_ID=0, const unsigned int nDesiredFeatures=0, const TImageROI &ROI=TImageROI()) const
Extract features from the image based on the method defined in TOptions.
Definition: CFeatureExtraction_common.cpp:44
CObservationBeaconRanges.h
mrpt::utils::CDynamicGrid::x2idx
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:268
n
GLenum GLsizei n
Definition: glext.h:5074
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::obs::CObservationVisualLandmarks
Declares a class derived from "CObservation" that stores a Landmarks Map as seen from a stereo camera...
Definition: CObservationVisualLandmarks.h:28
mrpt::obs::gnss::Message_NMEA_GGA::content_t::satellitesUsed
uint32_t satellitesUsed
The number of satelites used to compute this estimation.
Definition: gnss_messages_ascii_nmea.h:56
mrpt::maps::CLandmarksMap::fuseOptions
struct mrpt::maps::CLandmarksMap::TFuseOptions fuseOptions
mrpt::vision::CFeatureExtraction::options
TOptions options
Set all the parameters of the desired method here.
Definition: CFeatureExtraction.h:317
mrpt::maps::CLandmarksMap::TMapDefinition::TPairIdBeacon
std::pair< mrpt::math::TPoint3D, unsigned int > TPairIdBeacon
Definition: CLandmarksMap.h:705
mrpt::math::TPoint3D::norm
double norm() const
Point norm.
Definition: lightweight_geom_data.h:467
begin
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:29
mrpt::utils::round_long
long round_long(const T value)
Returns the closer integer (long) to x.
Definition: round.h:37
mrpt::maps::CLandmarksMap::TLikelihoodOptions::extRobotPoseStd
float extRobotPoseStd
The standard deviation used for external robot poses likelihood (default=0.05) [meters].
Definition: CLandmarksMap.h:398
mrpt::system::extractDayTimeFromTimestamp
double extractDayTimeFromTimestamp(const mrpt::system::TTimeStamp t)
Returns the number of seconds ellapsed from midnight in the given timestamp.
Definition: datetime.cpp:290
mrpt::obs::CObservationRobotPose
An observation providing an alternative robot pose from an external source.
Definition: CObservationRobotPose.h:24
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:539
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:533
mrpt::vision::CFeatureExtraction::TOptions
The set of parameters for all the detectors & descriptor algorithms.
Definition: CFeatureExtraction.h:95
mrpt::obs::CObservation::GetRuntimeClass
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::obs::CObservationBeaconRanges
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Definition: CObservationBeaconRanges.h:27
mrpt::utils::CDynamicGrid::getSizeY
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:256
mrpt::vision::CFeatureExtraction::TOptions::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Definition: CFeatureExtraction_common.cpp:409
mrpt::obs::CObservationBeaconRanges::TMeasurement::beaconID
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
Definition: CObservationBeaconRanges.h:55
std
STL namespace.
mrpt::maps::CMetricMap::genericMapParams
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:281
M_PI
#define M_PI
Definition: bits.h:92
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::iterator
internal::TSequenceLandmarks::iterator iterator
Definition: CLandmarksMap.h:161
mrpt::vision::TMatchingOptions::epipolar_TH
float epipolar_TH
Epipolar constraint (rows of pixels)
Definition: vision/include/mrpt/vision/types.h:417
CGridPlaneXY.h
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::math::CMatrixDouble33
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:55
s
GLdouble s
Definition: glext.h:3676
mrpt::maps::CLandmark::TLandmarkID
int64_t TLandmarkID
The type for the IDs of landmarks.
Definition: CLandmark.h:41
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
mrpt::maps::CLandmarksMap::simulateBeaconReadings
void simulateBeaconReadings(const mrpt::poses::CPose3D &in_robotPose, const mrpt::poses::CPoint3D &in_sensorLocationOnRobot, mrpt::obs::CObservationBeaconRanges &out_Observations) const
Simulates a noisy reading toward each of the beacons in the landmarks map, if any.
Definition: CLandmarksMap.cpp:2576
CObservationRobotPose.h
mrpt::obs::CObservationRobotPose::sensorPose
mrpt::poses::CPose3D sensorPose
The pose of the sensor on the robot/vehicle.
Definition: CObservationRobotPose.h:35
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::getByBeaconID
const CLandmark * getByBeaconID(unsigned int ID) const
Returns the landmark with a given beacon ID, or nullptr if not found.
Definition: CLandmarksMap.cpp:2717
mrpt::maps::CLandmarksMap::saveMetricMapRepresentationToFile
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Definition: CLandmarksMap.cpp:2627
mrpt::random::CRandomGenerator::drawGaussian1D
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
Definition: RandomGenerators.h:155
width
GLenum GLsizei width
Definition: glext.h:3531
mrpt::maps::CLandmarksMap::internal_clear
void internal_clear() override
Internal method called by clear()
Definition: CLandmarksMap.cpp:166
M_2PI
#define M_2PI
Definition: mrpt_macros.h:437
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:39
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:55
mrpt::utils::CDynamicGrid::y2idx
int y2idx(double y) const
Definition: CDynamicGrid.h:272
mrpt::maps::CLandmarksMap::TLikelihoodOptions::beaconRangesUseObservationStd
bool beaconRangesUseObservationStd
(Default: false) If true, beaconRangesStd is ignored and each individual CObservationBeaconRanges::st...
Definition: CLandmarksMap.h:391
mrpt::maps::CLandmarksMap::_mapMaxID
static mrpt::maps::CLandmark::TLandmarkID _mapMaxID
Definition: CLandmarksMap.h:216
mrpt::vision::TStereoSystemParams::stdPixel
float stdPixel
Standard deviation of the error in feature detection.
Definition: vision/include/mrpt/vision/types.h:296
mrpt::obs::CObservationBearingRange::TMeasurement::yaw
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
Definition: CObservationBearingRange.h:66
mrpt::maps::CLandmarksMap::TInsertionOptions::insert_SIFTs_from_monocular_images
bool insert_SIFTs_from_monocular_images
If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D feature...
Definition: CLandmarksMap.h:247
mrpt::vision::pixelTo3D
mrpt::math::TPoint3D pixelTo3D(const mrpt::utils::TPixelCoordf &xy, const mrpt::math::CMatrixDouble33 &A)
Extract a UNITARY 3D vector in the direction of a 3D point, given from its (x,y) pixels coordinates...
Definition: vision_utils.cpp:169
mrpt::utils::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:118
mrpt::obs::CObservationStereoImages::cameraPose
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
Definition: CObservationStereoImages.h:103
mrpt::obs::CObservationBearingRange::TMeasurement::landmarkID
int32_t landmarkID
The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark...
Definition: CObservationBearingRange.h:70
mrpt::poses::CPointPDFGaussian::bayesianFusion
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
Definition: CPointPDFGaussian.cpp:152
mrpt::maps::CLandmarksMap::getMapMaxID
mrpt::maps::CLandmark::TLandmarkID getMapMaxID()
Definition: CLandmarksMap.cpp:2690
mrpt::obs::gnss::Message_NMEA_GGA::content_t::altitude_meters
double altitude_meters
The measured altitude, in meters (A).
Definition: gnss_messages_ascii_nmea.h:46
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFTsLoadDistanceOfTheMean
float SIFTsLoadDistanceOfTheMean
[For SIFT landmarks only] The distance (in meters) of the mean value of landmarks, for the initial position PDF (Default = 3m)
Definition: CLandmarksMap.h:299
mrpt::vision::TMatchingOptions::mmDescriptorSIFT
Matching by Euclidean distance between SIFT descriptors.
Definition: vision/include/mrpt/vision/types.h:374
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:41
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFTs_numberOfKLTKeypoints
int SIFTs_numberOfKLTKeypoints
Number of points to extract in the image.
Definition: CLandmarksMap.h:315
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::maps::CLandmark::setPose
void setPose(const mrpt::poses::CPointPDFGaussian &p)
Sets the pose from an object:
Definition: CLandmark.cpp:73
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
mrpt::maps::CLandmarksMap::TLikelihoodOptions::SIFTs_decimation
int SIFTs_decimation
Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood compu...
Definition: CLandmarksMap.h:372
mrpt::maps::CLandmarksMap
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:75
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::getLargestDistanceFromOrigin
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
Definition: CLandmarksMap.cpp:2003
MRPT_END
#define MRPT_END
Definition: CColouredPointsMap.h:430
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: CColouredPointsMap.h:366
mrpt::poses::CPoseOrPoint::norm
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:252
mrpt::utils::CFileOutputStream
This CStream derived class allow using a file as a write-only, binary stream.
Definition: CFileOutputStream.h:26
int64_t
__int64 int64_t
Definition: rptypes.h:49
mrpt::vision::TStereoSystemParams
Parameters associated to a stereo system.
Definition: vision/include/mrpt/vision/types.h:255
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: CColouredPointsMap.h:182
mrpt::maps::CLandmarksMap::readFromStream
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CLandmarksMap.cpp:199
mrpt::poses::CPointPDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPointPDFGaussian.h:46
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::push_back
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
Definition: CLandmarksMap.cpp:1890
mrpt::math::CMatrixDouble
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixTemplateNumeric.h:147
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFTs_stdXY
float SIFTs_stdXY
[For SIFT landmarks only] The standard deviation (in pixels) for the SIFTs detector (This is used for...
Definition: CLandmarksMap.h:311
mrpt::vision::featSIFT
Scale Invariant Feature Transform [LOWE&#39;04].
Definition: vision/include/mrpt/vision/types.h:62
mrpt::utils::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:93
mrpt::utils::CDynamicGrid::getSizeX
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:254
mrpt::obs::CObservationBeaconRanges::sensedData
std::deque< TMeasurement > sensedData
The list of observed ranges.
Definition: CObservationBeaconRanges.h:59
end
GLuint GLuint end
Definition: glext.h:3528
mrpt::poses::CPose3D::sphericalCoordinates
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
Definition: CPose3D.cpp:388
mrpt::obs::gnss::Message_NMEA_GGA::content_t::longitude_degrees
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
Definition: gnss_messages_ascii_nmea.h:39
mrpt::obs::CObservationStereoImages
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
Definition: CObservationStereoImages.h:43
mrpt::obs::CObservationBearingRange::fieldOfView_pitch
float fieldOfView_pitch
Information about the sensor: The "field-of-view" of the sensor, in radians (for pitch )...
Definition: CObservationBearingRange.h:48
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< CSetOfObjects > Ptr
Definition: CSetOfObjects.h:32
mrpt::maps::CLandmarksMap::~CLandmarksMap
virtual ~CLandmarksMap()
Virtual destructor.
Definition: CLandmarksMap.cpp:162
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:19
mrpt::obs::CObservation2DRangeScan::validRange
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It&#39;s false (=0) on no reflected rays, referenced to elements in scan.
Definition: CObservation2DRangeScan.h:115
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:19
CObservationGPS.h
mrpt::vision
Classes for computer vision, detectors, features, etc.
Definition: bundle_adjustment.h:25
mrpt::math::TPoint3D::x
double x
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:388
mrpt::maps::CLandmark::getPose
void getPose(mrpt::poses::CPointPDFGaussian &p) const
Returns the pose as an object:
Definition: CLandmark.cpp:54
mrpt::vision::CFeatureExtraction::TOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CFeatureExtraction_common.cpp:488
mrpt::maps::CLandmarksMap::internal_computeObservationLikelihood
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the wor...
Definition: CLandmarksMap.cpp:234
DEG2RAD
#define DEG2RAD
Definition: CColouredPointsMap.h:112
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
mrpt::utils::CDynamicGrid::cellByIndex
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
Definition: CDynamicGrid.h:234
mrpt::obs::CObservationGPS::getMsgByClass
MSG_CLASS & getMsgByClass()
Returns a reference to the message in the list CObservationGPS::messages of the requested class...
Definition: CObservationGPS.h:149
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:32
mrpt::obs::CObservationStereoImages::leftCamera
mrpt::utils::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
Definition: CObservationStereoImages.h:100
mrpt::vision::CFeatureList
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:305
CPointPDFGaussian.h
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:545
mrpt::obs::CObservationBeaconRanges::stdError
float stdError
The "sigma" of the sensor, assuming a zero-mean Gaussian noise model.
Definition: CObservationBeaconRanges.h:38
mrpt::obs::CObservationStereoImages::imageRight
mrpt::utils::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
Definition: CObservationStereoImages.h:79
mrpt::maps::CLandmark::timestampLastSeen
mrpt::system::TTimeStamp timestampLastSeen
The last time that this landmark was observed.
Definition: CLandmark.h:77
mrpt::obs::CObservationBearingRange::sensorLocationOnRobot
mrpt::poses::CPose3D sensorLocationOnRobot
The position of the sensor on the robot.
Definition: CObservationBearingRange.h:52
INVALID_TIMESTAMP
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:16
mrpt::maps::CLandmarksMap::changeCoordinatesReference
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
Definition: CLandmarksMap.cpp:815
mrpt::utils::TCamera::intrinsicParams
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:57
mrpt::maps::CLandmarksMap::TLikelihoodOptions::SIFT_feat_options
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map...
Definition: CLandmarksMap.h:380
CObservation2DRangeScan.h
mrpt::maps::CLandmarksMap::insertionOptions
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOptions
mrpt::maps::CLandmarksMap::TFuseOptions::minTimesSeen
unsigned int minTimesSeen
Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds...
Definition: CLandmarksMap.h:454
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:405
mrpt::maps::CLandmarksMap::_mEDD
static std::map< std::pair< mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID >, double > _mEDD
Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks.
Definition: CLandmarksMap.h:215
mrpt::maps::CLandmarksMap::saveToTextFile
bool saveToTextFile(std::string file)
Save to a text file.
Definition: CLandmarksMap.cpp:1438
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensedDistance
float sensedDistance
The sensed range itself (in meters).
Definition: CObservationBeaconRanges.h:53
mrpt::maps::CLandmarksMap::TLikelihoodOptions::rangeScan2D_decimation
unsigned int rangeScan2D_decimation
The number of rays from a 2D range scan will be decimated by this factor (default = 1...
Definition: CLandmarksMap.h:361
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:83
mrpt::maps::CLandmarksMap::TMapDefinition::likelihoodOpts
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOpts
Definition: CLandmarksMap.h:710
mrpt::maps::CLandmarksMap::writeToStream
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CLandmarksMap.cpp:176
mrpt::obs::CObservationVisualLandmarks::refCameraPose
mrpt::poses::CPose3D refCameraPose
The 3D pose of the reference camera relative to robot coordinates.
Definition: CObservationVisualLandmarks.h:37
MRPT_START
#define MRPT_START
Definition: CColouredPointsMap.h:426
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
mrpt::maps::CLandmark
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
MAP_DEFINITION_REGISTER
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Definition: TMetricMapTypesRegistry.h:95
mrpt::obs::CObservation::timestamp
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:58
mrpt::math::generateAxisBaseFromDirection
CMatrixTemplateNumeric< T > generateAxisBaseFromDirection(T dx, T dy, T dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
Definition: geometry.h:1121
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:55
mrpt::obs::CObservationRobotPose::pose
mrpt::poses::CPose3DPDFGaussian pose
The observed robot pose.
Definition: CObservationRobotPose.h:32
mrpt::poses::CPointPDFGaussian::productIntegralNormalizedWith2D
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
Definition: CPointPDFGaussian.cpp:259
COpenGLScene.h
mrpt::maps::CLandmarksMap::TFuseOptions::ellapsedTime
float ellapsedTime
See "minTimesSeen".
Definition: CLandmarksMap.h:458
mrpt::obs::CObservationGPS::has_GGA_datum
internal_msg_test_proxy< gnss::NMEA_GGA > has_GGA_datum
Evaluates as a bool; true if the corresponding field exists in messages.
Definition: CObservationGPS.h:256
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:55
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
mrpt::maps::CLandmarksMap::TInsertionOptions::PLOT_IMAGES
bool PLOT_IMAGES
Indicates if the images (as well as the SIFT detected features) should be shown in a window...
Definition: CLandmarksMap.h:330
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::obs::CObservation
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:41
mrpt::maps::CLandmark::normal
mrpt::math::TPoint3D normal
The "normal" to the landmark, i.e.
Definition: CLandmark.h:50
mrpt::maps::CLandmarksMap::TMapDefinition::initialBeacons
std::deque< TPairIdBeacon > initialBeacons
Initial contents of the map, especified by a set of 3D Beacons with associated IDs.
Definition: CLandmarksMap.h:708
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFTsLoadEllipsoidWidth
float SIFTsLoadEllipsoidWidth
[For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perp...
Definition: CLandmarksMap.h:305
mrpt::maps::CLandmarksMap::loadSiftFeaturesFromStereoImageObservation
void loadSiftFeaturesFromStereoImageObservation(const mrpt::obs::CObservationStereoImages &obs, mrpt::maps::CLandmark::TLandmarkID fID, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of...
Definition: CLandmarksMap.cpp:730
mrpt::utils::CConfigFileBase::read_double
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:94
mrpt::poses::CPose3D::composePoint
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:453
mrpt::maps::CLandmarksMap::simulateRangeBearingReadings
void simulateRangeBearingReadings(const mrpt::poses::CPose3D &robotPose, const mrpt::poses::CPose3D &sensorLocationOnRobot, mrpt::obs::CObservationBearingRange &observations, bool sensorDetectsIDs=true, const float stdRange=0.01f, const float stdYaw=mrpt::utils::DEG2RAD(0.1f), const float stdPitch=mrpt::utils::DEG2RAD(0.1f), vector_size_t *real_associations=nullptr, const double spurious_count_mean=0, const double spurious_count_std=0) const
Simulates a noisy bearing-range observation of all the beacons (landamrks with type glBeacon) in the ...
Definition: CLandmarksMap.cpp:2894
mrpt::obs::CObservationStereoImages::rightCameraPose
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
Definition: CObservationStereoImages.h:111
mrpt::maps::CLandmark::getType
mrpt::vision::TFeatureType getType() const
Gets the type of the first feature in its feature vector.
Definition: CLandmark.h:100
CLandmark.h
mrpt::vision::matchFeatures
size_t matchFeatures(const CFeatureList &list1, const CFeatureList &list2, CMatchedFeatureList &matches, const TMatchingOptions &options=TMatchingOptions(), const TStereoSystemParams &params=TStereoSystemParams())
Find the matches between two lists of features which must be of the same type.
Definition: vision_utils.cpp:457
mrpt::obs::CObservationBearingRange
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
Definition: CObservationBearingRange.h:31
mrpt::vision::TMatchingOptions
A structure containing options for the matching.
Definition: vision/include/mrpt/vision/types.h:362
mrpt::maps::CLandmarksMap::internal_insertObservation
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
Definition: CLandmarksMap.cpp:479
CObservationStereoImages.h
in
GLuint in
Definition: glext.h:7274
mrpt::maps::CLandmarksMap::TInsertionOptions::TInsertionOptions
TInsertionOptions()
Initilization of default parameters.
Definition: CLandmarksMap.cpp:2291
mrpt::obs::CObservationVisualLandmarks::landmarks
mrpt::maps::CLandmarksMap landmarks
The landmarks, with coordinates origin in the camera reference system.
Definition: CObservationVisualLandmarks.h:40
mrpt::maps::CLandmarksMap::loadSiftFeaturesFromImageObservation
void loadSiftFeaturesFromImageObservation(const mrpt::obs::CObservationImage &obs, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an image observation (Previous content...
Definition: CLandmarksMap.cpp:644
mrpt::obs::gnss::Message_NMEA_GGA::fields
content_t fields
Message content, accesible by individual fields.
Definition: gnss_messages_ascii_nmea.h:67
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:60
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
Definition: CObservation2DRangeScan.h:121
ASSERT_
#define ASSERT_(f)
Definition: CColouredPointsMap.h:310
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensorLocationOnRobot
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
Definition: CObservationBeaconRanges.h:51
mrpt::obs::CObservationImage::image
mrpt::utils::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
Definition: CObservationImage.h:61
mrpt::maps::CLandmarksMap::TInsertionOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CLandmarksMap.cpp:2381
INVALID_LANDMARK_ID
#define INVALID_LANDMARK_ID
Used for CObservationBearingRange::TMeasurement::beaconID and others.
Definition: CObservation.h:25
mrpt::maps::CLandmarksMap::auxParticleFilterCleanUp
virtual void auxParticleFilterCleanUp() override
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
Definition: CLandmarksMap.cpp:2875
mrpt::vision::CFeatureList::iterator
TInternalFeatList::iterator iterator
Definition: CFeature.h:366
mrpt::vector_size_t
std::vector< size_t > vector_size_t
Definition: types_simple.h:26
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::maps::CLandmarksMap::computeMatchingWith2D
void computeMatchingWith2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const mrpt::poses::CPose2D &angularDistPivotPoint, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=nullptr, bool onlyKeepTheClosest=false, bool onlyUniqueRobust=false) const
Definition: CLandmarksMap.cpp:604
mrpt::maps::CLandmarksMap::TInsertionOptions::insert_Landmarks_from_range_scans
bool insert_Landmarks_from_range_scans
If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for ...
Definition: CLandmarksMap.h:257
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::const_iterator
internal::TSequenceLandmarks::const_iterator const_iterator
Definition: CLandmarksMap.h:165
mrpt::maps::CLandmarksMap::computeLikelihood_RSLC_2007
double computeLikelihood_RSLC_2007(const CLandmarksMap *s, const mrpt::poses::CPose2D &sensorPose)
Computes the (logarithmic) likelihood function for a sensed observation "o" according to "this" map...
Definition: CLandmarksMap.cpp:1731
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:254
CEllipsoid.h
mrpt::vector_int
std::vector< int32_t > vector_int
Definition: types_simple.h:24
mrpt::math::cov
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:148
mrpt::vision::TMatchingOptions::maxEDD_TH
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
Definition: vision/include/mrpt/vision/types.h:421
mrpt::maps::CLandmarksMap::saveToMATLABScript2D
bool saveToMATLABScript2D(std::string file, const char *style="b", float stdCount=2.0f)
Save to a MATLAB script which displays 2D error ellipses for the map (top-view, projection on the XY ...
Definition: CLandmarksMap.cpp:1537
mrpt::vision::featBeacon
A especial case: this is not an image feature, but a 2D/3D beacon (used for range-only SLAM from mrpt...
Definition: vision/include/mrpt/vision/types.h:67
mrpt::obs::CObservationBearingRange::validCovariances
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
Definition: CObservationBearingRange.h:87
mrpt::maps::CLandmarksMap::saveToMATLABScript3D
bool saveToMATLABScript3D(std::string file, const char *style="b", float confInterval=0.95f) const
Save to a MATLAB script which displays 3D error ellipses for the map.
Definition: CLandmarksMap.cpp:1486
mrpt::maps::CLandmarksMap::TLikelihoodOptions::GPSOrigin
struct mrpt::maps::CLandmarksMap::TLikelihoodOptions::TGPSOrigin GPSOrigin
x
GLenum GLint x
Definition: glext.h:3538
mrpt::obs::CObservation2DRangeScan::scan
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters.
Definition: CObservation2DRangeScan.h:103
mrpt::maps::CLandmark::pose_mean
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:47
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:381
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFTs_epipolar_TH
float SIFTs_epipolar_TH
Maximum distance (in pixels) from a point to a certain epipolar line to be considered a potential mat...
Definition: CLandmarksMap.h:325
mrpt::vision::TMatchingOptions::matching_method
TMatchingMethod matching_method
Matching method.
Definition: vision/include/mrpt/vision/types.h:415
mrpt::obs::CObservationGPS
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
Definition: CObservationGPS.h:69
mrpt::maps::CLandmarksMap::TInsertionOptions::SiftLikelihoodThreshold
float SiftLikelihoodThreshold
[For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0...
Definition: CLandmarksMap.h:267
mrpt::maps::CLandmarksMap::TLikelihoodOptions::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Definition: CLandmarksMap.cpp:2458
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFT_feat_options
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map...
Definition: CLandmarksMap.h:340
mrpt::obs::CObservationBearingRange::sensor_std_range
float sensor_std_range
Taken into account only if validCovariances=false: the standard deviation of the sensor noise model f...
Definition: CObservationBearingRange.h:96
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:25
mrpt::opengl::COpenGLScene::insert
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
Definition: COpenGLScene.cpp:189
mrpt::maps::CLandmarksMap::TLikelihoodOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CLandmarksMap.cpp:2512
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::maps::CLandmarksMap::TLikelihoodOptions::TGPSOrigin::ang
double ang
These 3 params allow rotating and shifting GPS coordinates with other 2D maps (e.g.
Definition: CLandmarksMap.h:421
params
GLenum const GLfloat * params
Definition: glext.h:3534
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: CObservation2DRangeScan.h:129
mrpt::maps::CLandmarksMap::TInsertionOptions::SiftCorrRatioThreshold
float SiftCorrRatioThreshold
[For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as c...
Definition: CLandmarksMap.h:262
mrpt::maps::CLandmarksMap::isEmpty
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Definition: CLandmarksMap.cpp:2572
mrpt::maps::CLandmark::seenTimesCount
uint32_t seenTimesCount
The number of times that this landmark has been seen.
Definition: CLandmark.h:79
mrpt::maps::CLandmarksMap::TInsertionOptions::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Definition: CLandmarksMap.cpp:2321
CObservationVisualLandmarks.h
mrpt::maps::CLandmarksMap::landmarks
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
mrpt::utils::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
mrpt::poses::CPose3D::getHomogeneousMatrix
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.h:221
mrpt::maps::CLandmarksMap::loadOccupancyFeaturesFrom2DRangeScan
void loadOccupancyFeaturesFrom2DRangeScan(const mrpt::obs::CObservation2DRangeScan &obs, const mrpt::poses::CPose3D *robotPose=nullptr, unsigned int downSampleFactor=1)
Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous cont...
Definition: CLandmarksMap.cpp:1631
mrpt::math::distance
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1885
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:34
CLandmarksMap.h
mrpt::maps::CLandmarksMap::TLikelihoodOptions::TGPSOrigin::min_sat
unsigned int min_sat
Minimum number of sats to take into account the data.
Definition: CLandmarksMap.h:423
mrpt::poses::CPointPDFGaussian
A gaussian distribution for 3D points.
Definition: CPointPDFGaussian.h:25
mrpt::utils::CStream::printf
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:597
mrpt::maps::CLandmarksMap::TLikelihoodOptions::GPS_sigma
float GPS_sigma
A constant "sigma" for GPS localization data (in meters)
Definition: CLandmarksMap.h:427
mrpt::maps::CLandmarksMap::compute3DMatchingRatio
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
Definition: CLandmarksMap.cpp:2744
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:123
mrpt::vision::TStereoSystemParams::stdDisp
float stdDisp
Standard deviation of the error in disparity computation.
Definition: vision/include/mrpt/vision/types.h:300
mrpt::vision::CFeatureExtraction
The central class from which images can be analyzed in search of different kinds of interest points a...
Definition: CFeatureExtraction.h:82
mrpt::maps::CLandmark::ID
TLandmarkID ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
Definition: CLandmark.h:75
mrpt::obs::CObservationBearingRange::sensedData
TMeasurementList sensedData
The list of observed ranges:
Definition: CObservationBearingRange.h:80
mrpt::opengl::CEllipsoid::Ptr
std::shared_ptr< CEllipsoid > Ptr
Definition: CEllipsoid.h:49
mrpt::random::CRandomGenerator::drawGaussian1D_normalized
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.
Definition: RandomGenerator.cpp:63
mrpt::maps::internal::TSequenceLandmarks
std::vector< CLandmark > TSequenceLandmarks
Definition: CLandmarksMap.h:31
mrpt::vision::TStereoSystemParams::K
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
Definition: vision/include/mrpt/vision/types.h:289
mrpt::maps::CLandmarksMap::likelihoodOptions
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOptions
mrpt::maps::CLandmarksMap::computeLikelihood_SIFT_LandmarkMap
double computeLikelihood_SIFT_LandmarkMap(CLandmarksMap *map, mrpt::utils::TMatchingPairList *correspondences=nullptr, std::vector< bool > *otherCorrespondences=nullptr)
Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
Definition: CLandmarksMap.cpp:2027
mrpt::vision::CMatchedFeatureList
A list of features.
Definition: CFeature.h:504
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFTMatching3DMethod
unsigned int SIFTMatching3DMethod
[For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method)) 0: Our method -> E...
Definition: CLandmarksMap.h:283


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