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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019