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