MRPT  2.0.3
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 }
mrpt::math::cov
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
os.h
mrpt::containers::CDynamicGrid::getSizeY
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:244
mrpt::maps::CLandmark::ID
TLandmarkID ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
Definition: CLandmark.h:75
mrpt::obs::CObservationBeaconRanges::TMeasurement
Each one of the measurements.
Definition: CObservationBeaconRanges.h:38
mrpt::maps::CLandmarksMap::TLikelihoodOptions::TLikelihoodOptions
TLikelihoodOptions()
Definition: CLandmarksMap.cpp:2251
mrpt::vision::CFeatureExtraction
The central class from which images can be analyzed in search of different kinds of interest points a...
Definition: CFeatureExtraction.h:71
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::end
iterator end()
Definition: CLandmarksMap.h:161
mrpt::vision::TMatchingOptions::maxEDD_TH
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
Definition: vision/include/mrpt/vision/types.h:401
mrpt::maps::CLandmarksMap::_mEDD
static std::map< std::pair< mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID >, double > _mEDD
Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks.
Definition: CLandmarksMap.h:213
mrpt::img::TCamera::intrinsicParams
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates):
Definition: TCamera.h:50
ASSERT_ABOVE_
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
mrpt::maps::CLandmarksMap::simulateRangeBearingReadings
void simulateRangeBearingReadings(const mrpt::poses::CPose3D &robotPose, const mrpt::poses::CPose3D &sensorLocationOnRobot, mrpt::obs::CObservationBearingRange &observations, bool sensorDetectsIDs=true, const float stdRange=0.01f, const float stdYaw=mrpt::DEG2RAD(0.1f), const float stdPitch=mrpt::DEG2RAD(0.1f), std::vector< size_t > *real_associations=nullptr, const double spurious_count_mean=0, const double spurious_count_std=0) const
Simulates a noisy bearing-range observation of all the beacons (landamrks with type glBeacon) in the ...
Definition: CLandmarksMap.cpp:2689
mrpt::maps::CLandmarksMap::TMapDefinition::TPairIdBeacon
std::pair< mrpt::math::TPoint3D, unsigned int > TPairIdBeacon
Definition: CLandmarksMap.h:703
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::getByID
const CLandmark * getByID(CLandmark::TLandmarkID ID) const
Returns the landmark with a given landmrk ID, or nullptr if not found.
Definition: CLandmarksMap.cpp:2502
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensedDistance
float sensedDistance
The sensed range itself (in meters).
Definition: CObservationBeaconRanges.h:50
mrpt::random::CRandomGenerator::drawGaussian1D_normalized
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.
Definition: RandomGenerator.cpp:96
mrpt::containers::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
mrpt::maps::CLandmarksMap::saveToMATLABScript3D
bool saveToMATLABScript3D(std::string file, const char *style="b", float confInterval=0.95f) const
Save to a MATLAB script which displays 3D error ellipses for the map.
Definition: CLandmarksMap.cpp:1405
mrpt::maps::CLandmarksMap::_maxIDUpdated
static bool _maxIDUpdated
Definition: CLandmarksMap.h:215
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
mrpt::obs::CObservation2DRangeScan::rightToLeft
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
Definition: CObservation2DRangeScan.h:116
mrpt::opengl::CSetOfObjects::Create
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
mrpt::poses::CPointPDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPointPDFGaussian.h:44
INVALID_TIMESTAMP
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
mrpt::obs::gnss::Message_NMEA_GGA::content_t::longitude_degrees
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
Definition: gnss_messages_ascii_nmea.h:35
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
geometry.h
mrpt::math::TPoint3D_< double >
mrpt::obs::CObservationBeaconRanges::minSensorDistance
float minSensorDistance
Info about sensor.
Definition: CObservationBeaconRanges.h:33
mrpt::opengl::COpenGLScene::insert
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport).
Definition: COpenGLScene.cpp:177
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
verbose
params verbose
Definition: chessboard_stereo_camera_calib_unittest.cpp:59
mrpt::maps::CLandmarksMap::TLikelihoodOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CLandmarksMap.cpp:2266
mrpt::vision::matchFeatures
size_t matchFeatures(const CFeatureList &list1, const CFeatureList &list2, CMatchedFeatureList &matches, const TMatchingOptions &options=TMatchingOptions(), const TStereoSystemParams &params=TStereoSystemParams())
Find the matches between two lists of features which must be of the same type.
Definition: vision_utils.cpp:315
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< mrpt::opengl ::CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:31
mrpt::maps::TMapGenericParams::enableSaveAs3DObject
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects
Definition: metric_map_types.h:84
mrpt::obs::CObservationBearingRange::TMeasurement
Each one of the measurements:
Definition: CObservationBearingRange.h:51
INVALID_LANDMARK_ID
#define INVALID_LANDMARK_ID
Used for CObservationBearingRange::TMeasurement::beaconID and others.
Definition: CObservation.h:27
mrpt::maps::CLandmark::TLandmarkID
int64_t TLandmarkID
The type for the IDs of landmarks.
Definition: CLandmark.h:41
mrpt::obs::CObservationVisualLandmarks
Declares a class derived from "CObservation" that stores a Landmarks Map as seen from a stereo camera...
Definition: CObservationVisualLandmarks.h:25
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::poses::CPose3D::sphericalCoordinates
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
Definition: CPose3D.cpp:312
mrpt::maps::CLandmark::seenTimesCount
uint32_t seenTimesCount
The number of times that this landmark has been seen.
Definition: CLandmark.h:81
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::iterator
internal::TSequenceLandmarks::iterator iterator
Definition: CLandmarksMap.h:159
mrpt::maps::CLandmarksMap::TLikelihoodOptions::SIFTs_mahaDist_std
float SIFTs_mahaDist_std
Definition: CLandmarksMap.h:366
mrpt::tfest::TMatchingPair::other_z
float other_z
Definition: TMatchingPair.h:53
mrpt::maps::CLandmarksMap::TLikelihoodOptions::SIFTnullCorrespondenceDistance
float SIFTnullCorrespondenceDistance
Definition: CLandmarksMap.h:367
mrpt::poses::CPointPDFGaussian
A gaussian distribution for 3D points.
Definition: CPointPDFGaussian.h:22
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::maps::CLandmark::getPose
void getPose(mrpt::poses::CPointPDFGaussian &p) const
Returns the pose as an object:
Definition: CLandmark.cpp:32
mrpt::vision
Copyright (C) 2010 Hauke Strasdat Imperial College London Copyright (c) 2005-2020,...
Definition: bundle_adjustment.h:35
mrpt::vision::TStereoSystemParams::stdDisp
float stdDisp
Standard deviation of the error in disparity computation.
Definition: vision/include/mrpt/vision/types.h:277
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:56
CFileOutputStream.h
mrpt::poses::CPointPDFGaussian::productIntegralNormalizedWith2D
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
Definition: CPointPDFGaussian.cpp:251
mrpt::obs::CObservationBearingRange::sensor_std_yaw
float sensor_std_yaw
Definition: CObservationBearingRange.h:91
mrpt::math::TPoint3D_data::z
T z
Definition: TPoint3D.h:29
mrpt::obs::CObservation2DRangeScan::getScanRange
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
Definition: CObservation2DRangeScan.cpp:496
mrpt::tfest::TMatchingPair::other_idx
uint32_t other_idx
Definition: TMatchingPair.h:51
mrpt::maps::CLandmarksMap::size
size_t size() const
Returns the stored landmarks count.
Definition: CLandmarksMap.cpp:147
CObservationVisualLandmarks.h
mrpt::obs::gnss::Message_NMEA_GGA
NMEA datum: GGA.
Definition: gnss_messages_ascii_nmea.h:19
mrpt::system::extractDayTimeFromTimestamp
double extractDayTimeFromTimestamp(const mrpt::system::TTimeStamp t)
Returns the number of seconds ellapsed from midnight in the given timestamp.
Definition: datetime.cpp:197
mrpt::obs::CObservation2DRangeScan::getScanSize
size_t getScanSize() const
Get number of scan rays.
Definition: CObservation2DRangeScan.cpp:557
mrpt::obs::gnss::Message_NMEA_GGA::fields
content_t fields
Message content, accesible by individual fields.
Definition: gnss_messages_ascii_nmea.h:63
mrpt::vision::TMatchingOptions::epipolar_TH
float epipolar_TH
Epipolar constraint (rows of pixels)
Definition: vision/include/mrpt/vision/types.h:397
MAP_DEFINITION_REGISTER
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Definition: TMetricMapTypesRegistry.h:91
mrpt::vision::CFeatureList::iterator
TInternalFeatList::iterator iterator
Definition: CFeature.h:331
mrpt::obs::CObservationBeaconRanges::maxSensorDistance
float maxSensorDistance
Definition: CObservationBeaconRanges.h:33
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:64
mrpt::maps::CLandmarksMap::loadSiftFeaturesFromStereoImageObservation
void loadSiftFeaturesFromStereoImageObservation(const mrpt::obs::CObservationStereoImages &obs, mrpt::maps::CLandmark::TLandmarkID fID, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of...
Definition: CLandmarksMap.cpp:681
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::poses::CPoseOrPoint::norm
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:256
mrpt::round_long
long round_long(const T value)
Returns the closer integer (long) to x.
Definition: round.h:39
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:14
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::math::distance
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
mrpt::maps::TMetricMapInitializer
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Definition: TMetricMapInitializer.h:32
mrpt::maps::CLandmarksMap::TInsertionOptions::SIFTLikelihoodMethod
unsigned int SIFTLikelihoodMethod
[For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 0: Our method -...
Definition: CLandmarksMap.h:289
mrpt::obs::CObservationImage::image
mrpt::img::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
Definition: CObservationImage.h:57
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::getLargestDistanceFromOrigin
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
Definition: CLandmarksMap.cpp:1920
mrpt::maps::CMetricMap::genericMapParams
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:274
mrpt::tfest::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
mrpt::vision::CFeatureList::begin
iterator begin()
Definition: CFeature.h:337
mrpt::vision::TMatchingOptions::mmDescriptorSIFT
@ mmDescriptorSIFT
Matching by Euclidean distance between SIFT descriptors.
Definition: vision/include/mrpt/vision/types.h:354
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::math::MatrixBase::eig_symmetric
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
Definition: MatrixBase_impl.h:131
mrpt::maps::CLandmarksMap::insertionOptions
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOptions
mrpt::maps::CLandmarksMap::TLikelihoodOptions::SIFTs_sigma_descriptor_dist
double SIFTs_sigma_descriptor_dist
Definition: CLandmarksMap.h:365
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::maps::CLandmarksMap::loadOccupancyFeaturesFrom2DRangeScan
void loadOccupancyFeaturesFrom2DRangeScan(const mrpt::obs::CObservation2DRangeScan &obs, const mrpt::poses::CPose3D *robotPose=nullptr, unsigned int downSampleFactor=1)
Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous cont...
Definition: CLandmarksMap.cpp:1551
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
CObservationGPS.h
mrpt::obs::CObservationStereoImages::imageRight
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
Definition: CObservationStereoImages.h:56
mrpt::obs::CObservationBearingRange::TMeasurement::pitch
float pitch
Definition: CObservationBearingRange.h:61
vision-precomp.h
mrpt::obs::CObservationBearingRange::sensedData
TMeasurementList sensedData
The list of observed ranges:
Definition: CObservationBearingRange.h:75
mrpt::maps::CLandmark::pose_cov_12
float pose_cov_12
Definition: CLandmark.h:51
mrpt::maps::CLandmark::pose_cov_13
float pose_cov_13
Definition: CLandmark.h:52
mrpt::maps::CLandmark::pose_cov_22
float pose_cov_22
Definition: CLandmark.h:51
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise.
Definition: exceptions.h:125
mrpt::math::generateAxisBaseFromDirection
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
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::getByBeaconID
const CLandmark * getByBeaconID(unsigned int ID) const
Returns the landmark with a given beacon ID, or nullptr if not found.
Definition: CLandmarksMap.cpp:2523
CObservationStereoImages.h
mrpt::obs::CObservation::timestamp
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
mrpt::maps::CLandmarksMap::TMapDefinition::initialBeacons
std::deque< TPairIdBeacon > initialBeacons
Initial contents of the map, especified by a set of 3D Beacons with associated IDs.
Definition: CLandmarksMap.h:706
mrpt::maps::CLandmarksMap::TInsertionOptions::TInsertionOptions
TInsertionOptions()
Initilization of default parameters.
Definition: CLandmarksMap.cpp:2135
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
random.h
mrpt::vision::CFeatureExtraction::detectFeatures
void detectFeatures(const mrpt::img::CImage &img, CFeatureList &feats, const unsigned int init_ID=0, const unsigned int nDesiredFeatures=0, const TImageROI &ROI=TImageROI())
Extract features from the image based on the method defined in TOptions.
Definition: CFeatureExtraction_common.cpp:37
mrpt::maps::CLandmarksMap::TMapDefinition::likelihoodOpts
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOpts
Definition: CLandmarksMap.h:708
mrpt::containers::CDynamicGrid::x2idx
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:256
mrpt::vision::TStereoSystemParams::K
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
Definition: vision/include/mrpt/vision/types.h:266
mrpt::vision::CFeatureList
A list of visual features, to be used as output by detectors, as input/output by trackers,...
Definition: CFeature.h:275
mrpt::obs::CObservationBeaconRanges::sensedData
std::deque< TMeasurement > sensedData
The list of observed ranges.
Definition: CObservationBeaconRanges.h:56
mrpt::math::MatrixBase::inverse_LLt
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
Definition: MatrixBase_impl.h:195
mrpt::config::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:130
mrpt::math::TPoint3D_::norm
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
mrpt::poses::CPointPDFGaussian::bayesianFusion
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
Definition: CPointPDFGaussian.cpp:156
mrpt::obs::CObservationBearingRange::validCovariances
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
Definition: CObservationBearingRange.h:82
mrpt::maps::CLandmarksMap::getMapMaxID
mrpt::maps::CLandmark::TLandmarkID getMapMaxID()
Definition: CLandmarksMap.cpp:2496
mrpt::math::CMatrixDouble
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixDynamic.h:589
mrpt::obs::gnss::Message_NMEA_GGA::content_t::latitude_degrees
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
Definition: gnss_messages_ascii_nmea.h:33
mrpt::maps::CLandmarksMap::TInsertionOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CLandmarksMap.cpp:2145
mrpt::maps::CLandmarksMap::auxParticleFilterCleanUp
void auxParticleFilterCleanUp() override
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
Definition: CLandmarksMap.cpp:2680
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
mrpt::obs::gnss::Message_NMEA_GGA::content_t::satellitesUsed
uint32_t satellitesUsed
The number of satelites used to compute this estimation.
Definition: gnss_messages_ascii_nmea.h:52
mrpt::containers::CDynamicGrid::getSizeX
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:242
mrpt::img
Definition: CCanvas.h:16
mrpt::vision::CMatchedFeatureList
A list of features.
Definition: CFeature.h:494
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::erase
void erase(unsigned int indx)
Definition: CLandmarksMap.cpp:1856
mrpt::obs::CObservation::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::random::CRandomGenerator::drawUniform
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
Definition: RandomGenerators.h:142
CLandmarksMap.h
mrpt::math::CMatrixFixed< double, 3, 3 >
mrpt::obs::CObservationBearingRange::TMeasurement::yaw
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
Definition: CObservationBearingRange.h:61
mrpt::obs::CObservationStereoImages::leftCamera
mrpt::img::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras.
Definition: CObservationStereoImages.h:77
CEllipsoid3D.h
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
mrpt::obs::CObservationBeaconRanges
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Definition: CObservationBeaconRanges.h:24
mrpt::poses::CPose3D::getHomogeneousMatrix
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.cpp:781
mrpt::obs::CObservationStereoImages::imageLeft
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
Definition: CObservationStereoImages.h:52
mrpt::tfest::TMatchingPair::this_x
float this_x
Definition: TMatchingPair.h:52
mrpt::obs::CObservation2DRangeScan::aperture
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees).
Definition: CObservation2DRangeScan.h:114
mrpt::poses::CPose3D::composePoint
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...
mrpt::tfest::TMatchingPair::this_idx
uint32_t this_idx
Definition: TMatchingPair.h:50
mrpt::maps::CLandmark
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
mrpt::maps::CLandmarksMap::TMapDefinition::insertionOpts
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOpts
Definition: CLandmarksMap.h:707
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
COpenGLScene.h
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::obs::CObservationBeaconRanges::stdError
float stdError
The "sigma" of the sensor, assuming a zero-mean Gaussian noise model.
Definition: CObservationBeaconRanges.h:35
mrpt::maps::CLandmark::features
std::vector< mrpt::vision::CFeature > features
The set of features from which the landmark comes.
Definition: CLandmark.h:44
mrpt::maps::CLandmarksMap::TLikelihoodOptions::SIFTs_decimation
int SIFTs_decimation
Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood compu...
Definition: CLandmarksMap.h:370
mrpt::obs::CObservationBearingRange::TMeasurement::landmarkID
int32_t landmarkID
The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark...
Definition: CObservationBearingRange.h:65
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
CPointPDFGaussian.h
mrpt::maps::CLandmarksMap::compute3DMatchingRatio
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map,...
Definition: CLandmarksMap.cpp:2550
mrpt::math::multiply_HCHt
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::TCustomSequenceLandmarks::push_back
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
Definition: CLandmarksMap.cpp:1806
mrpt::tfest::TMatchingPair::other_y
float other_y
Definition: TMatchingPair.h:53
mrpt::opengl::CEllipsoid3D::Ptr
std::shared_ptr< mrpt::opengl ::CEllipsoid3D > Ptr
Definition: CEllipsoid3D.h:42
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
ops_matrices.h
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::size
size_t size() const
Definition: CLandmarksMap.h:163
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::hasBeenModified
void hasBeenModified(unsigned int indx)
Definition: CLandmarksMap.cpp:1862
mrpt::obs::CObservationBeaconRanges::TMeasurement::beaconID
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
Definition: CObservationBeaconRanges.h:52
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
mrpt::vision::pixelTo3D
mrpt::math::TPoint3D pixelTo3D(const mrpt::img::TPixelCoordf &xy, const mrpt::math::CMatrixDouble33 &A)
Extract a UNITARY 3D vector in the direction of a 3D point, given from its (x,y) pixels coordinates,...
Definition: vision_utils.cpp:137
mrpt::maps::CLandmarksMap::landmarks
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
mrpt::vision::CFeatureExtraction::TOptions
The set of parameters for all the detectors & descriptor algorithms.
Definition: CFeatureExtraction.h:87
mrpt::math::size
size_t size(const MATRIXLIKE &m, const int dim)
Definition: math/include/mrpt/math/bits_math.h:21
mrpt::math::MatrixBase::setDiagonal
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
mrpt::system::TTimeStamp
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
mrpt::maps::internal::TSequenceLandmarks
std::vector< CLandmark > TSequenceLandmarks
Definition: CLandmarksMap.h:30
mrpt::d2f
float d2f(const double d)
shortcut for static_cast<float>(double)
Definition: core/include/mrpt/core/bits_math.h:189
mrpt::maps::CLandmarksMap::isEmpty
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Definition: CLandmarksMap.cpp:2381
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::begin
iterator begin()
Definition: CLandmarksMap.h:160
mrpt::vision::TStereoSystemParams::stdPixel
float stdPixel
Standard deviation of the error in feature detection.
Definition: vision/include/mrpt/vision/types.h:273
mrpt::obs::CObservationBearingRange::maxSensorDistance
float maxSensorDistance
Definition: CObservationBearingRange.h:35
mrpt::obs::CObservationBearingRange
This observation represents a number of range-bearing value pairs, each one for a detected landmark,...
Definition: CObservationBearingRange.h:28
CObservationRobotPose.h
mrpt::obs::gnss::Message_NMEA_GGA::content_t::altitude_meters
double altitude_meters
The measured altitude, in meters (A).
Definition: gnss_messages_ascii_nmea.h:42
mrpt::obs::CObservationBearingRange::sensorLocationOnRobot
mrpt::poses::CPose3D sensorLocationOnRobot
The position of the sensor on the robot.
Definition: CObservationBearingRange.h:47
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::maps::CLandmarksMap::saveMetricMapRepresentationToFile
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
Definition: CLandmarksMap.cpp:2436
mrpt::vision::CFeatureExtraction::options
TOptions options
Set all the parameters of the desired method here before calling detectFeatures()
Definition: CFeatureExtraction.h:295
mrpt::config::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:118
mrpt::containers::CDynamicGrid::y2idx
int y2idx(double y) const
Definition: CDynamicGrid.h:260
mrpt::maps::CLandmark::pose_mean
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:47
mrpt::vision::featBeacon
@ featBeacon
A especial case: this is not an image feature, but a 2D/3D beacon (used for range-only SLAM from mrpt...
Definition: vision/include/mrpt/vision/types.h:59
mrpt::obs::gnss::pitch
double pitch
Definition: gnss_messages_novatel.h:306
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::math::MatrixVectorBase::transpose
auto transpose()
Definition: MatrixVectorBase.h:159
mrpt::maps::CLandmark::normal
mrpt::math::TPoint3D normal
The "normal" to the landmark, i.e.
Definition: CLandmark.h:50
mrpt::tfest::TMatchingPair::this_y
float this_y
Definition: TMatchingPair.h:52
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
mrpt::maps::CLandmarksMap::loadSiftFeaturesFromImageObservation
void loadSiftFeaturesFromImageObservation(const mrpt::obs::CObservationImage &obs, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an image observation (Previous content...
Definition: CLandmarksMap.cpp:598
mrpt::math::TPoint3D_data::y
T y
Definition: TPoint3D.h:29
mrpt::maps::CLandmarksMap::TMapDefinition
Definition: CLandmarksMap.h:702
mrpt::vision::featSIFT
@ featSIFT
Scale Invariant Feature Transform [LOWE'04].
Definition: vision/include/mrpt/vision/types.h:54
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::const_iterator
internal::TSequenceLandmarks::const_iterator const_iterator
Definition: CLandmarksMap.h:164
mrpt::vision::CFeatureList::end
iterator end()
Definition: CFeature.h:338
mrpt::maps::CLandmarksMap::TInsertionOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CLandmarksMap.cpp:2204
mrpt::vision::TStereoSystemParams::baseline
float baseline
Baseline.
Definition: vision/include/mrpt/vision/types.h:269
mrpt::obs::CObservationBearingRange::fieldOfView_pitch
float fieldOfView_pitch
Information about the sensor: The "field-of-view" of the sensor, in radians (for pitch ).
Definition: CObservationBearingRange.h:43
mrpt::opengl::COpenGLScene::saveToFile
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
Definition: COpenGLScene.cpp:293
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
mrpt::system::getCurrentTime
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.h:82
mrpt::maps::CLandmarksMap::computeLikelihood_SIFT_LandmarkMap
double computeLikelihood_SIFT_LandmarkMap(CLandmarksMap *map, mrpt::tfest::TMatchingPairList *correspondences=nullptr, std::vector< bool > *otherCorrespondences=nullptr)
Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
Definition: CLandmarksMap.cpp:1944
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
CObservationBeaconRanges.h
mrpt::obs::CObservationGPS
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
Definition: CObservationGPS.h:67
mrpt::obs::CObservationBearingRange::sensor_std_pitch
float sensor_std_pitch
Definition: CObservationBearingRange.h:91
mrpt::containers::CDynamicGrid::cellByIndex
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
Definition: CDynamicGrid.h:222
mrpt::obs::CObservationBearingRange::sensor_std_range
float sensor_std_range
Taken into account only if validCovariances=false: the standard deviation of the sensor noise model f...
Definition: CObservationBearingRange.h:91
mrpt::containers::CDynamicGrid
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:38
mrpt::maps::CLandmarksMap::likelihoodOptions
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOptions
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::hasBeenModifiedAll
void hasBeenModifiedAll()
Definition: CLandmarksMap.cpp:1880
mrpt::math::CMatrixFixed::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixFixed.h:251
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
mrpt::maps::CLandmark::pose_cov_33
float pose_cov_33
Definition: CLandmark.h:51
mrpt::maps::CLandmarksMap::changeCoordinatesReference
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
Definition: CLandmarksMap.cpp:766
mrpt::vision::projectMatchedFeatures
void projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::img::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
Definition: vision_utils.cpp:770
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
mrpt::maps::CLandmarksMap::TFuseOptions::TFuseOptions
TFuseOptions()
Initialization.
mrpt::obs::CObservationStereoImages::cameraPose
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
Definition: CObservationStereoImages.h:80
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::isToBeModified
void isToBeModified(unsigned int indx)
Definition: CLandmarksMap.cpp:1838
mrpt::math::MatrixVectorBase::setZero
void setZero()
Definition: MatrixVectorBase.h:112
mrpt::math::multiply_HCHt_scalar
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
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::clear
void clear()
Definition: CLandmarksMap.cpp:1796
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
dir
const auto dir
Definition: chessboard_stereo_camera_calib_unittest.cpp:28
mrpt::math::TPoint3D_data::x
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
mrpt::tfest::TMatchingPair::this_z
float this_z
Definition: TMatchingPair.h:52
mrpt::maps::CLandmarksMap::_mapMaxID
static mrpt::maps::CLandmark::TLandmarkID _mapMaxID
Definition: CLandmarksMap.h:214
mrpt::vision::TStereoSystemParams::maxZ
float maxZ
Maximum allowed distance.
Definition: vision/include/mrpt/vision/types.h:280
mrpt::poses::CPoint3D::asTPoint
mrpt::math::TPoint3D asTPoint() const
Definition: CPoint3D.cpp:164
mrpt::random::CRandomGenerator::drawGaussian1D
return_t drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
Definition: RandomGenerators.h:194
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::vision::TStereoSystemParams
Parameters associated to a stereo system.
Definition: vision/include/mrpt/vision/types.h:233
mrpt::math::MatrixVectorBase::array
auto array()
Definition: MatrixVectorBase.h:170
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
mrpt::obs::CObservationBearingRange::fieldOfView_yaw
float fieldOfView_yaw
Information about the.
Definition: CObservationBearingRange.h:40
mrpt::maps::CLandmarksMap::simulateBeaconReadings
void simulateBeaconReadings(const mrpt::poses::CPose3D &in_robotPose, const mrpt::poses::CPoint3D &in_sensorLocationOnRobot, mrpt::obs::CObservationBeaconRanges &out_Observations) const
Simulates a noisy reading toward each of the beacons in the landmarks map, if any.
Definition: CLandmarksMap.cpp:2385
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
mrpt::vision::featNotDefined
@ featNotDefined
Non-defined feature (also used for Occupancy features)
Definition: vision/include/mrpt/vision/types.h:47
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
M_2PI
#define M_2PI
Definition: common.h:58
CObservationImage.h
mrpt::maps::CLandmarksMap
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::maps::CLandmark::createOneFeature
void createOneFeature()
Creates one feature in the vector "features", calling the appropriate constructor of the smart pointe...
Definition: CLandmark.h:112
mrpt::obs::CObservationBearingRange::minSensorDistance
float minSensorDistance
Definition: CObservationBearingRange.h:35
mrpt::maps
Definition: CBeacon.h:21
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::get
CLandmark * get(unsigned int indx)
Definition: CLandmarksMap.cpp:1827
mrpt::obs::CObservationRobotPose
An observation providing an alternative robot pose from an external source.
Definition: CObservationRobotPose.h:21
CLandmark.h
mrpt::obs::CObservationBearingRange::TMeasurement::range
float range
The sensed landmark distance, in meters.
Definition: CObservationBearingRange.h:54
mrpt::vision::TMatchingOptions::matching_method
TMatchingMethod matching_method
Matching method.
Definition: vision/include/mrpt/vision/types.h:395
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:31
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::vision::TStereoSystemParams::minZ
float minZ
Maximum allowed distance.
Definition: vision/include/mrpt/vision/types.h:283
mrpt::maps::CLandmark::pose_cov_11
float pose_cov_11
Definition: CLandmark.h:51
mrpt::poses::CPointPDFGaussian::mean
CPoint3D mean
The mean value.
Definition: CPointPDFGaussian.h:42
mrpt::obs::CObservation2DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
Definition: CObservation2DRangeScan.h:122
CGridPlaneXY.h
mrpt::obs::CObservation2DRangeScan::getScanRangeValidity
bool getScanRangeValidity(const size_t i) const
It's false (=0) on no reflected rays, referenced to elements in scan.
Definition: CObservation2DRangeScan.cpp:529
mrpt::maps::CLandmarksMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CLandmarksMap.cpp:2466
mrpt::maps::CLandmark::timestampLastSeen
mrpt::system::TTimeStamp timestampLastSeen
The last time that this landmark was observed.
Definition: CLandmark.h:78
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensorLocationOnRobot
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
Definition: CObservationBeaconRanges.h:48
mrpt::maps::CLandmark::pose_cov_23
float pose_cov_23
Definition: CLandmark.h:52
mrpt::obs::CObservationImage
Declares a class derived from "CObservation" that encapsules an image from a camera,...
Definition: CObservationImage.h:32
mrpt::math::CMatrixDynamic< double >
mrpt::obs::CObservationStereoImages::rightCameraPose
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
Definition: CObservationStereoImages.h:88
mrpt::vision::TMatchingOptions::EDD_RATIO
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
Definition: vision/include/mrpt/vision/types.h:403
mrpt::containers
Definition: bimap.h:14
mrpt::obs::CObservationStereoImages
Observation class for either a pair of left+right or left+disparity images from a stereo camera.
Definition: CObservationStereoImages.h:38
mrpt::tfest::TMatchingPair::other_x
float other_x
Definition: TMatchingPair.h:53
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
CObservation2DRangeScan.h
mrpt::maps::CLandmarksMap::TLikelihoodOptions::TGPSOrigin::TGPSOrigin
TGPSOrigin()
mrpt::system
Definition: backtrace.h:14
mrpt::maps::CLandmarksMap::TLikelihoodOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CLandmarksMap.cpp:2322
mrpt::obs::CObservationImage::cameraParams
mrpt::img::TCamera cameraParams
Intrinsic and distortion parameters of the camera.
Definition: CObservationImage.h:53
mrpt::vision::TMatchingOptions
A structure containing options for the matching.
Definition: vision/include/mrpt/vision/types.h:342
mrpt::maps::CLandmark::setPose
void setPose(const mrpt::poses::CPointPDFGaussian &p)
Sets the pose from an object:
Definition: CLandmark.cpp:51



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020