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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020