00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CLandmarksMap_H
00029 #define CLandmarksMap_H
00030
00031 #include <mrpt/slam/CMetricMap.h>
00032 #include <mrpt/slam/CLandmark.h>
00033 #include <mrpt/slam/CObservationImage.h>
00034
00035 #include <mrpt/slam/CObservation2DRangeScan.h>
00036 #include <mrpt/slam/CObservationGPS.h>
00037 #include <mrpt/slam/CObservationBearingRange.h>
00038 #include <mrpt/utils/CSerializable.h>
00039 #include <mrpt/math/CMatrix.h>
00040 #include <mrpt/utils/CDynamicGrid.h>
00041 #include <mrpt/utils/CLoadableOptions.h>
00042 #include <mrpt/gui/CDisplayWindow.h>
00043
00044 #include <map>
00045
00046 namespace mrpt
00047 {
00048 namespace slam
00049 {
00050 class CObservationBeaconRanges;
00051 class CObservationStereoImages;
00052
00055 typedef std::vector<CLandmark> TSequenceLandmarks;
00056
00057 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CLandmarksMap, CMetricMap )
00058
00059
00084 class MRPTDLLIMPEXP CLandmarksMap : public CMetricMap
00085 {
00086
00087 DEFINE_SERIALIZABLE( CLandmarksMap )
00088
00089 private:
00090
00093 mrpt::gui::CDisplayWindow *wind1, *wind2;
00094
00095 public:
00098 struct MRPTDLLIMPEXP TCustomSequenceLandmarks
00099 {
00100 private:
00103 TSequenceLandmarks m_landmarks;
00104
00108 CDynamicGrid<vector_int> m_grid;
00109
00113 mutable float m_largestDistanceFromOrigin;
00114
00118 mutable bool m_largestDistanceFromOriginIsUpdated;
00119
00120 public:
00123 TCustomSequenceLandmarks();
00124
00125 typedef TSequenceLandmarks::iterator iterator;
00126 iterator begin() { return m_landmarks.begin(); };
00127 iterator end() { return m_landmarks.end(); };
00128 void clear();
00129 size_t size() const { return m_landmarks.size(); };
00130
00131 typedef TSequenceLandmarks::const_iterator const_iterator;
00132 const_iterator begin() const { return m_landmarks.begin(); };
00133 const_iterator end() const { return m_landmarks.end(); };
00134
00137 void push_back( CLandmark *lm);
00138 CLandmark* get(unsigned int indx);
00139 const CLandmark* get(unsigned int indx) const;
00140 void isToBeModified(unsigned int indx);
00141 void hasBeenModified(unsigned int indx);
00142 void hasBeenModifiedAll();
00143 void erase(unsigned int indx);
00144
00145 CDynamicGrid<vector_int>* getGrid() { return &m_grid; }
00146
00149 const CLandmark* getByID( CLandmark::TLandmarkID ID ) const;
00150
00153 const CLandmark* getByBeaconID( unsigned int ID ) const;
00154
00157 float getLargestDistanceFromOrigin() const;
00158
00159 } landmarks;
00160
00163 CLandmarksMap();
00164
00167 virtual ~CLandmarksMap();
00168
00169
00170
00173 static std::map<std::pair<mrpt::slam::CLandmark::TLandmarkID, mrpt::slam::CLandmark::TLandmarkID>, double> _mEDD;
00174 static mrpt::slam::CLandmark::TLandmarkID _mapMaxID;
00175 static bool _maxIDUpdated;
00176
00177 mrpt::slam::CLandmark::TLandmarkID getMapMaxID();
00178
00179
00190 float compute3DMatchingRatio(
00191 const CMetricMap *otherMap,
00192 const CPose3D &otherMapPose,
00193 float minDistForCorr = 0.10f,
00194 float minMahaDistForCorr = 2.0f
00195 ) const;
00196
00199 struct MRPTDLLIMPEXP TInsertionOptions : public utils::CLoadableOptions
00200 {
00201 public:
00204 TInsertionOptions( );
00205
00208 void loadFromConfigFile(
00209 const mrpt::utils::CConfigFileBase &source,
00210 const std::string §ion);
00211
00214 void dumpToTextStream(utils::CStream &out);
00215
00218 bool insert_SIFTs_from_monocular_images;
00219
00222 bool insert_SIFTs_from_stereo_images;
00223
00226 bool insert_Landmarks_from_range_scans;
00227
00230 float SiftCorrRatioThreshold;
00231
00234 float SiftLikelihoodThreshold;
00235
00236
00239 float SiftEDDThreshold;
00240
00245 unsigned int SIFTMatching3DMethod;
00246
00251 unsigned int SIFTLikelihoodMethod;
00252
00253
00254
00257 float SIFTsLoadDistanceOfTheMean;
00258
00261 float SIFTsLoadEllipsoidWidth;
00262
00265 float SIFTs_stdXY, SIFTs_stdDisparity;
00266
00269 int SIFTs_numberOfKLTKeypoints;
00270
00273 float SIFTs_stereo_maxDepth;
00274
00277 float SIFTs_epipolar_TH;
00278
00281 bool PLOT_IMAGES;
00282
00283 } insertionOptions;
00284
00287 struct MRPTDLLIMPEXP TLikelihoodOptions : public utils::CLoadableOptions
00288 {
00289 public:
00292 TLikelihoodOptions();
00293
00296 void loadFromConfigFile(
00297 const mrpt::utils::CConfigFileBase &source,
00298 const std::string §ion);
00299
00302 void dumpToTextStream(utils::CStream &out);
00303
00306 unsigned int rangeScan2D_decimation;
00307
00308 double SIFTs_sigma_euclidean_dist;
00309
00310 double SIFTs_sigma_descriptor_dist;
00311
00312 float SIFTs_mahaDist_std;
00313
00314 float SIFTnullCorrespondenceDistance;
00315
00318 int SIFTs_decimation;
00319
00322 float beaconRangesStd;
00323
00326 float alphaRatio;
00327
00330 float beaconMaxRange;
00331
00335 struct MRPTDLLIMPEXP TGPSOrigin
00336 {
00337 public:
00338 TGPSOrigin();
00339
00342 double longitude;
00343
00346 double latitude;
00347
00350 double altitude;
00351
00358 double ang,
00359 x_shift,
00360 y_shift;
00361
00364 unsigned int min_sat;
00365
00366 } GPSOrigin;
00367
00370 float GPS_sigma;
00371
00372
00373
00374 } likelihoodOptions;
00375
00378 struct MRPTDLLIMPEXP TInsertionResults
00379 {
00383 unsigned int nSiftL, nSiftR;
00384
00385
00386 } insertionResults;
00387
00390 struct MRPTDLLIMPEXP TFuseOptions
00391 {
00394 TFuseOptions();
00395
00398 unsigned int minTimesSeen;
00399
00402 float ellapsedTime;
00403
00404 } fuseOptions;
00405
00406
00412 bool saveToTextFile(std::string file);
00413
00421 bool saveToMATLABScript2D(
00422 std::string file,
00423 const char *style="b",
00424 float stdCount = 2.0f );
00425
00433 bool saveToMATLABScript3D(
00434 std::string file,
00435 const char *style="b",
00436 float confInterval = 0.95f ) const ;
00437
00440 void clear();
00441
00444 size_t size() const;
00445
00448
00449
00454 double computeLikelihood_RSLC_2007( const CLandmarksMap *s, const CPose2D &sensorPose);
00455
00460 void loadSiftFeaturesFromImageObservation( const CObservationImage &obs );
00461
00466 void loadSiftFeaturesFromStereoImageObservation( const CObservationStereoImages &obs, mrpt::slam::CLandmark::TLandmarkID fID );
00467
00473 void loadOccupancyFeaturesFrom2DRangeScan(
00474 const CObservation2DRangeScan &obs,
00475 const CPose3D *robotPose = NULL,
00476 unsigned int downSampleFactor = 1);
00477
00485 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00486
00500 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00501
00502 void computeMatchingWith2D(
00503 const CMetricMap *otherMap,
00504 const CPose2D &otherMapPose,
00505 float maxDistForCorrespondence,
00506 float maxAngularDistForCorrespondence,
00507 const CPose2D &angularDistPivotPoint,
00508 TMatchingPairList &correspondences,
00509 float &correspondencesRatio,
00510 float *sumSqrDist = NULL,
00511 bool onlyKeepTheClosest = false,
00512 bool onlyUniqueRobust = false ) const;
00513
00521 void computeMatchingWith3DLandmarks(
00522 const mrpt::slam::CLandmarksMap *otherMap,
00523 TMatchingPairList &correspondences,
00524 float &correspondencesRatio,
00525 std::vector<bool> &otherCorrespondences) const;
00526
00529 void changeCoordinatesReference( const CPose3D &newOrg );
00530
00533 void changeCoordinatesReference( const CPose3D &newOrg, const mrpt::slam::CLandmarksMap *otherMap );
00534
00540 void fuseWith( CLandmarksMap &other, bool justInsertAllOfThem = false );
00541
00545 double computeLikelihood_SIFT_LandmarkMap( CLandmarksMap *map,
00546 TMatchingPairList *correspondences = NULL,
00547 std::vector<bool> *otherCorrespondences = NULL);
00548
00551 bool isEmpty() const;
00552
00559 void simulateBeaconReadings(
00560 const CPose3D &in_robotPose,
00561 const CPoint3D &in_sensorLocationOnRobot,
00562 CObservationBeaconRanges &out_Observations ) const;
00563
00575 void simulateRangeBearingReadings(
00576 const CPose3D &in_robotPose,
00577 const CPose3D &in_sensorLocationOnRobot,
00578 CObservationBearingRange &out_Observations,
00579 bool sensorDetectsIDs = true,
00580 const float &in_stdRange = 0.01f,
00581 const float &in_stdYaw = DEG2RAD(0.1f),
00582 const float &in_stdPitch = DEG2RAD(0.1f)
00583 ) const;
00584
00585
00591 void saveMetricMapRepresentationToFile(
00592 const std::string &filNamePrefix ) const;
00593
00596 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00597
00601 void auxParticleFilterCleanUp();
00602
00603 };
00604
00605
00606 }
00607 }
00608
00609 #endif