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


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019