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