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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST