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>
14 
16 #include <mrpt/maps/CLandmark.h>
25 #include <mrpt/system/os.h>
26 #include <mrpt/random.h>
27 
29 #include <mrpt/opengl/CEllipsoid.h>
31 
32 using namespace mrpt;
33 using namespace mrpt::math;
34 using namespace mrpt::maps;
35 using namespace mrpt::obs;
36 using namespace mrpt::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;
44 
45 
46 // =========== Begin of Map definition ============
47 MAP_DEFINITION_REGISTER("CLandmarksMap,landmarksMap", mrpt::maps::CLandmarksMap)
48 
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);
92 
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;
117  return obj;
118 }
119 // =========== End of Map definition Block =========
120 
121 
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  ---------------------------------------------------------------*/
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:
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 
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 
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
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
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 
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 
341  return ret;
342 
343  } // end of likelihood of CObservationBeaconRanges
344  else
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 
376  return ret;
377 
378  } // end of likelihood of CObservation
379  else
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);
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 
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
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
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 
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,
697  const mrpt::vision::CFeatureExtraction::TOptions & feat_options
698  )
699 {
700  MRPT_START
701 
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 
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
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 {
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 {
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;
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  ---------------------------------------------------------------*/
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;
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;
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 
1714 {
1715  m_landmarks.clear();
1716 
1717  // Erase the grid:
1718  m_grid.clear();
1719 
1720  m_largestDistanceFromOriginIsUpdated = false;
1721 }
1722 
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 
1745 {
1746  return &m_landmarks[indx];
1747 }
1748 
1750 {
1751  return &m_landmarks[indx];
1752 }
1753 
1755 {
1756  vector_int *cell = m_grid.cellByPos(m_landmarks[indx].pose_mean.x,m_landmarks[indx].pose_mean.y);
1757 
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 
1772 {
1773  m_landmarks.erase( m_landmarks.begin() + indx );
1774  m_largestDistanceFromOriginIsUpdated = false;
1775 }
1776 
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 
1795 {
1796  MRPT_START
1797 
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 ---------------------------------------------------------------*/
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  ---------------------------------------------------------------*/
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();
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
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;
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++ )
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  ---------------------------------------------------------------*/
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  ---------------------------------------------------------------*/
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  ---------------------------------------------------------------*/
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  ---------------------------------------------------------------*/
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 
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  ---------------------------------------------------------------*/
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 
2204 
2205  out.printf("\n");
2206 }
2207 
2208 /*---------------------------------------------------------------
2209  loadFromConfigFile
2210  ---------------------------------------------------------------*/
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  ---------------------------------------------------------------*/
2244  minTimesSeen ( 2 ),
2245  ellapsedTime ( 4.0f )
2246 {
2247 }
2248 
2249 /*---------------------------------------------------------------
2250  isEmpty
2251  ---------------------------------------------------------------*/
2253 {
2254  return size()==0;
2255 }
2256 
2257 /*---------------------------------------------------------------
2258  simulateBeaconReadings
2259  ---------------------------------------------------------------*/
2261  const CPose3D &in_robotPose,
2262  const CPoint3D &in_sensorLocationOnRobot,
2263  mrpt::obs::CObservationBeaconRanges &out_Observations ) const
2264 {
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  ---------------------------------------------------------------*/
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 {
2342 
2343  // TODO: Generate patchs in 3D, etc...
2344 
2345  // Save 3D ellipsoids
2346  CPointPDFGaussian pointGauss;
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 ****/
2368 {
2369  return _mapMaxID;
2370 }
2371 /**** END FAMD ****/
2372 
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 
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  ----------------------------------------------------------------*/
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 
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;
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  ---------------------------------------------------------------*/
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();
2530  // TODO: Paco...
2531 }
2532 
2533 
2534 /*---------------------------------------------------------------
2535  simulateRangeBearingReadings
2536  ---------------------------------------------------------------*/
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 {
2551  size_t idx;
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 }
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:223
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
void VISION_IMPEXP projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::utils::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
float fieldOfView_yaw
Information about the sensor: Ranges, in meters (0: there is no limits)
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
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.
Parameters for CMetricMap::compute3DMatchingRatio()
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
Non-defined feature (also used for Occupancy features)
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
static CEllipsoidPtr Create()
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOpts
GLenum GLenum range
Definition: glew.h:7127
CPose3D mean
The mean value.
#define min(a, b)
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:30
void createOneFeature()
Creates one feature in the vector "features", calling the appropriate constructor of the smart pointe...
bool BASE_IMPEXP isNaN(float v) MRPT_NO_THROWS
mrpt::utils::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
double norm() const
Point norm.
float range
The sensed landmark distance, in meters.
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
unsigned int SIFTLikelihoodMethod
[For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 0: Our method -...
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
float beaconRangesStd
The standard deviation used for Beacon ranges likelihood (default=0.08) [meters]. ...
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
mrpt::utils::TCamera cameraParams
Intrinsic and distortion parameters of the camera.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CPoint3D mean
The mean value.
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
#define MRPT_CHECK_NORMAL_NUMBER(val)
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Scalar * iterator
Definition: eigen_plugins.h:23
Declares a class derived from "CObservation" that stores a Landmarks Map as seen from a stereo camera...
uint32_t satellitesUsed
The number of satelites used to compute this estimation.
TOptions options
Set all the parameters of the desired method here before calling "detectFeatures".
GLdouble GLdouble t
Definition: glew.h:1303
std::pair< mrpt::math::TPoint3D, unsigned int > TPairIdBeacon
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.
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
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.
long round_long(const T value)
Returns the closer integer (long) to x.
Definition: round.h:48
float extRobotPoseStd
The standard deviation used for external robot poses likelihood (default=0.05) [meters].
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
An observation providing an alternative robot pose from an external source.
The set of parameters for all the detectors & descriptor algorithms.
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
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
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.
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
TMapGenericParams genericMapParams
Common params to all maps.
#define M_PI
Definition: bits.h:78
internal::TSequenceLandmarks::iterator iterator
float epipolar_TH
Epipolar constraint (rows of pixels)
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 ...
const Scalar * const_iterator
Definition: eigen_plugins.h:24
double z
X,Y,Z coordinates.
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.
int64_t TLandmarkID
The type for the IDs of landmarks.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
double drawGaussian1D_normalized(double *likelihood=NULL)
Generate a normalized (mean=0, std=1) normally distributed sample.
mrpt::poses::CPose3D sensorPose
The pose of the sensor on the robot/vehicle.
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:220
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
GLuint in
Definition: glew.h:7146
GLdouble l
Definition: glew.h:5092
#define M_2PI
Definition: mrpt_macros.h:380
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
bool beaconRangesUseObservationStd
(Default: false) If true, beaconRangesStd is ignored and each individual CObservationBeaconRanges::st...
static mrpt::maps::CLandmark::TLandmarkID _mapMaxID
float stdPixel
Standard deviation of the error in feature detection.
std::vector< mrpt::vision::CFeaturePtr > features
The set of features from which the landmark comes.
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
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...
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
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.
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
int32_t landmarkID
The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark...
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:241
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
mrpt::maps::CLandmark::TLandmarkID getMapMaxID()
double altitude_meters
The measured altitude, in meters (A).
double productIntegralNormalizedWith2D(const CPointPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
Matching by Euclidean distance between SIFT descriptors.
GLdouble s
Definition: glew.h:1295
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void setPose(const mrpt::poses::CPointPDFGaussian &p)
Sets the pose from an object:
Definition: CLandmark.cpp:72
GLuint GLuint end
Definition: glew.h:1167
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
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)...
GLsizei n
Definition: glew.h:5051
int SIFTs_decimation
Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood compu...
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.
A class for storing a map of 3D probabilistic landmarks.
virtual void auxParticleFilterCleanUp() MRPT_OVERRIDE
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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
This CStream derived class allow using a file as a write-only, binary stream.
__int64 int64_t
Definition: rptypes.h:51
Parameters associated to a stereo system.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
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
Scale Invariant Feature Transform [LOWE'04].
A list of TMatchingPair.
Definition: TMatchingPair.h:78
std::deque< TMeasurement > sensedData
The list of observed ranges.
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
float fieldOfView_pitch
Information about the sensor: The "field-of-view" of the sensor, in radians (for pitch )...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< char > > validRange
It's false (=0) on no reflected rays, referenced to elements in scan.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLhandleARB obj
Definition: glew.h:3276
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
int version
Definition: mrpt_jpeglib.h:898
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
#define DEG2RAD
GLfloat GLfloat p
Definition: glew.h:10113
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
size_t size() const
Returns the stored landmarks count.
bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
mrpt::maps::CLandmarksMap CLandmarksMap
Backward compatible typedef.
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.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
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
MSG_CLASS & getMsgByClass()
Returns a reference to the message in the list CObservationGPS::messages of the requested class...
A class used to store a 3D point.
Definition: CPoint3D.h:32
mrpt::utils::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
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.
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:211
const CLandmark * getByID(CLandmark::TLandmarkID ID) const
Returns the landmark with a given landmrk ID, or NULL if not found.
float stdError
The "sigma" of the sensor, assuming a zero-mean Gaussian noise model.
mrpt::utils::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
mrpt::system::TTimeStamp timestampLastSeen
The last time that this landmark was observed.
mrpt::poses::CPose3D sensorLocationOnRobot
The position of the sensor on the robot.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:54
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map...
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOptions
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.
float sensedDistance
The sensed range itself (in meters).
unsigned int rangeScan2D_decimation
The number of rays from a 2D range scan will be decimated by this factor (default = 1...
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOpts
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
void getPose(mrpt::poses::CPointPDFGaussian &p) const
Returns the pose as an object:
Definition: CLandmark.cpp:53
GLsizeiptr size
Definition: glew.h:1586
mrpt::poses::CPose3D refCameraPose
The 3D pose of the reference camera relative to robot coordinates.
#define MRPT_START
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.
const CLandmark * getByBeaconID(unsigned int ID) const
Returns the landmark with a given beacon ID, or NULL if not found.
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
static CGridPlaneXYPtr Create()
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
uint64_t TLandmarkID
Unique IDs for landmarks.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
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
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
mrpt::poses::CPose3DPDFGaussian pose
The observed robot pose.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
internal_msg_test_proxy< gnss::NMEA_GGA > has_GGA_datum
Evaluates as a bool; true if the corresponding field exists in messages.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot's observation.
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.
std::deque< TPairIdBeacon > initialBeacons
Initial contents of the map, especified by a set of 3D Beacons with associated IDs.
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
void loadSiftFeaturesFromStereoImageObservation(const mrpt::obs::CObservationStereoImages &obs, mrpt::maps::CLandmark::TLandmarkID fID, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of...
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
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.
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
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...
A structure containing options for the matching.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
TInsertionOptions()
Initilization of default parameters.
mrpt::maps::CLandmarksMap landmarks
The landmarks, with coordinates origin in the camera reference system.
void loadSiftFeaturesFromImageObservation(const mrpt::obs::CObservationImage &obs, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an image observation (Previous content...
content_t fields
Message content, accesible by individual fields.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:49
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
#define ASSERT_(f)
float maxMahaDistForCorr
(Default: 2.0f) The minimum Mahalanobis distance between 2 probabilistic map elements for counting th...
int y2idx(double y) const
Definition: CDynamicGrid.h:242
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
mrpt::utils::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
GLfloat * params
Definition: glew.h:1436
#define INVALID_LANDMARK_ID
Used for CObservationBearingRange::TMeasurement::beaconID and others.
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.
TInternalFeatList::iterator iterator
Definition: CFeature.h:261
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
internal::TSequenceLandmarks::const_iterator const_iterator
std::vector< int32_t > vector_int
Definition: types_simple.h:23
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
A especial case: this is not an image feature, but a 2D/3D beacon (used for range-only SLAM from mrpt...
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Lightweight 3D point.
GLint GLint GLint GLint GLint GLint GLsizei width
Definition: glew.h:1166
TMatchingMethod matching_method
Matching method.
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
unsigned __int32 uint32_t
Definition: rptypes.h:49
float sensor_std_range
Taken into account only if validCovariances=false: the standard deviation of the sensor noise model f...
struct VISION_IMPEXP mrpt::maps::CLandmarksMap::TLikelihoodOptions::TGPSOrigin GPSOrigin
double ang
These 3 params allow rotating and shifting GPS coordinates with other 2D maps (e.g.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
uint32_t seenTimesCount
The number of times that this landmark has been seen.
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1511
unsigned int min_sat
Minimum number of sats to take into account the data.
A gaussian distribution for 3D points.
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
float GPS_sigma
A constant "sigma" for GPS localization data (in meters)
struct VISION_IMPEXP mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
float stdDisp
Standard deviation of the error in disparity computation.
The central class from which images can be analyzed in search of different kinds of interest points a...
TLandmarkID ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
TMeasurementList sensedData
The list of observed ranges:
std::vector< CLandmark > TSequenceLandmarks
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
static CSetOfObjectsPtr Create()
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOptions
A list of features.
Definition: CFeature.h:361



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018