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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019