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 CMultiMetricMap_H
00029 #define CMultiMetricMap_H
00030
00031 #include <mrpt/slam/COccupancyGridMap2D.h>
00032 #include <mrpt/slam/CGasConcentrationGridMap2D.h>
00033 #include <mrpt/slam/CHeightGridMap2D.h>
00034 #include <mrpt/slam/CSimplePointsMap.h>
00035 #include <mrpt/slam/CColouredPointsMap.h>
00036 #include <mrpt/slam/CLandmarksMap.h>
00037 #include <mrpt/slam/CBeaconMap.h>
00038 #include <mrpt/slam/CMetricMap.h>
00039 #include <mrpt/utils/CSerializable.h>
00040 #include <mrpt/utils/CLoadableOptions.h>
00041
00042 namespace mrpt
00043 {
00044 namespace slam
00045 {
00046 class TSetOfMetricMapInitializers;
00047
00048 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CMultiMetricMap , CMetricMap )
00049
00050
00068 class MRPTDLLIMPEXP CMultiMetricMap : public CMetricMap
00069 {
00070
00071 DEFINE_SERIALIZABLE( CMultiMetricMap )
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081 static CLASSINIT _init_CHybridMetricMap;
00082 static TRuntimeClassId classCHybridMetricMap;
00083
00084 protected:
00087 void deleteAllMaps();
00088
00089 public:
00090 typedef std::pair<CPoint3D,unsigned int> TPairIdBeacon;
00091
00094 bool isEmpty() const;
00095
00098 struct MRPTDLLIMPEXP TOptions : public utils::CLoadableOptions
00099 {
00100 TOptions() : likelihoodMapSelection(mapFuseAll),
00101 enableInsertion_pointsMap(true),
00102 enableInsertion_landmarksMap(true),
00103 enableInsertion_gridMaps(true),
00104 enableInsertion_gasGridMaps(true),
00105 enableInsertion_beaconMap(true),
00106 enableInsertion_heightMaps(true),
00107 enableInsertion_colourPointsMaps(true)
00108 {
00109 }
00110
00113 void loadFromConfigFile(
00114 const mrpt::utils::CConfigFileBase &source,
00115 const std::string §ion);
00116
00119 void dumpToTextStream(CStream &out);
00120
00124 enum TMapSelectionForLikelihood
00125 {
00126 mapFuseAll = -1,
00127 mapGrid = 0,
00128 mapPoints,
00129 mapLandmarks,
00130 mapGasGrid,
00131 mapBeacon,
00132 mapHeight,
00133 mapColourPoints
00134 } likelihoodMapSelection;
00135
00138 bool enableInsertion_pointsMap;
00139
00142 bool enableInsertion_landmarksMap;
00143
00146 bool enableInsertion_gridMaps;
00147
00150 bool enableInsertion_gasGridMaps;
00151
00154 bool enableInsertion_beaconMap;
00155
00158 bool enableInsertion_heightMaps;
00159
00162 bool enableInsertion_colourPointsMaps;
00163
00164
00165 } options;
00166
00169 std::deque<CSimplePointsMapPtr> m_pointsMaps;
00170
00173 CLandmarksMapPtr m_landmarksMap;
00174
00177 CBeaconMapPtr m_beaconMap;
00178
00181 std::deque<COccupancyGridMap2DPtr> m_gridMaps;
00182
00185 std::deque<CGasConcentrationGridMap2DPtr> m_gasGridMaps;
00186
00189 std::deque<CHeightGridMap2DPtr> m_heightMaps;
00190
00193 CColouredPointsMapPtr m_colourPointsMap;
00194
00200 CMultiMetricMap(
00201 const mrpt::slam::TSetOfMetricMapInitializers *initializers = NULL,
00202 const TOptions *opts = NULL );
00203
00206 void setListOfMaps( const mrpt::slam::TSetOfMetricMapInitializers *initializers );
00207
00209 CMultiMetricMap(const mrpt::slam::CMultiMetricMap &other );
00210
00213 mrpt::slam::CMultiMetricMap &operator = ( const mrpt::slam::CMultiMetricMap &other );
00214
00217 virtual ~CMultiMetricMap( );
00218
00221 void clear();
00222
00231 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00232
00237 float getNewStaticPointsRatio(
00238 CPointsMap *points,
00239 CPose2D &takenFrom );
00240
00247 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00248
00253 void computeMatchingWith2D(
00254 const CMetricMap *otherMap,
00255 const CPose2D &otherMapPose,
00256 float maxDistForCorrespondence,
00257 float maxAngularDistForCorrespondence,
00258 const CPose2D &angularDistPivotPoint,
00259 TMatchingPairList &correspondences,
00260 float &correspondencesRatio,
00261 float *sumSqrDist = NULL,
00262 bool onlyKeepTheClosest = false,
00263 bool onlyUniqueRobust = false ) const;
00264
00275 float compute3DMatchingRatio(
00276 const CMetricMap *otherMap,
00277 const CPose3D &otherMapPose,
00278 float minDistForCorr = 0.10f,
00279 float minMahaDistForCorr = 2.0f
00280 ) const;
00281
00284 void saveMetricMapRepresentationToFile(
00285 const std::string &filNamePrefix
00286 ) const;
00287
00291 void auxParticleFilterCleanUp();
00292
00295 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00296
00301 bool canComputeObservationLikelihood( const CObservation *obs );
00302
00305 unsigned int m_ID;
00306
00307 };
00308
00313 struct MRPTDLLIMPEXP TMetricMapInitializer
00314 {
00317 TMetricMapInitializer();
00318
00321 TRuntimeClassIdPtr metricMapClassType;
00322
00325 bool m_disableSaveAs3DObject;
00326
00329 struct MRPTDLLIMPEXP TOccGridMap2DOptions
00330 {
00333 TOccGridMap2DOptions();
00334
00337 float min_x,max_x,min_y,max_y,resolution;
00338
00341 COccupancyGridMap2D::TInsertionOptions insertionOpts;
00342
00345 COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts;
00346
00347 } occupancyGridMap2D_options;
00348
00351 struct MRPTDLLIMPEXP CPointsMapOptions
00352 {
00355 CPointsMapOptions();
00356
00359 CPointsMap::TInsertionOptions insertionOpts;
00360
00361 } pointsMapOptions_options;
00362
00365 struct MRPTDLLIMPEXP CGasConcentrationGridMap2DOptions
00366 {
00369 CGasConcentrationGridMap2DOptions();
00370
00373 float min_x,max_x,min_y,max_y,resolution;
00374
00377 CGasConcentrationGridMap2D::TMapRepresentation mapType;
00378
00381 CGasConcentrationGridMap2D::TInsertionOptions insertionOpts;
00382
00383 } gasGridMap_options;
00384
00387 struct MRPTDLLIMPEXP CLandmarksMapOptions
00388 {
00391 CLandmarksMapOptions();
00392
00395 std::deque<CMultiMetricMap::TPairIdBeacon> initialBeacons;
00396
00399 CLandmarksMap::TInsertionOptions insertionOpts;
00400
00403 CLandmarksMap::TLikelihoodOptions likelihoodOpts;
00404
00405 } landmarksMap_options;
00406
00407
00410 struct MRPTDLLIMPEXP CBeaconMapOptions
00411 {
00414 CBeaconMapOptions();
00415
00418 CBeaconMap::TLikelihoodOptions likelihoodOpts;
00419 CBeaconMap::TInsertionOptions insertionOpts;
00420
00421 } beaconMap_options;
00422
00425 struct MRPTDLLIMPEXP CHeightGridMap2DOptions
00426 {
00429 CHeightGridMap2DOptions();
00430
00433 float min_x,max_x,min_y,max_y,resolution;
00434
00437 CHeightGridMap2D::TMapRepresentation mapType;
00438
00441 CHeightGridMap2D::TInsertionOptions insertionOpts;
00442
00443 } heightMap_options;
00444
00447 struct MRPTDLLIMPEXP CColouredPointsMapOptions
00448 {
00451 CColouredPointsMapOptions();
00452
00455 CPointsMap::TInsertionOptions insertionOpts;
00456
00458 CColouredPointsMap::TColourOptions colourOpts;
00459
00460 } colouredPointsMapOptions_options;
00461
00462 };
00463
00469 class MRPTDLLIMPEXP TSetOfMetricMapInitializers : public utils::CLoadableOptions
00470 {
00471 protected:
00472 std::deque<TMetricMapInitializer> m_list;
00473
00474 public:
00475 size_t size() const { return m_list.size(); }
00476 void push_back( const TMetricMapInitializer &o ) { m_list.push_back(o); }
00477
00478 typedef std::deque<TMetricMapInitializer>::iterator iterator;
00479 typedef std::deque<TMetricMapInitializer>::const_iterator const_iterator;
00480
00481 iterator begin() { return m_list.begin(); }
00482 const_iterator begin() const { return m_list.begin(); }
00483
00484 iterator end() { return m_list.end(); }
00485 const_iterator end() const { return m_list.end(); }
00486
00487 void clear() { m_list.clear(); }
00488
00489
00490 TSetOfMetricMapInitializers() : m_list(), options()
00491 {}
00492
00493
00496 CMultiMetricMap::TOptions options;
00497
00617 void loadFromConfigFile(
00618 const mrpt::utils::CConfigFileBase &source,
00619 const std::string §ionName);
00620
00623 void dumpToTextStream(
00624 CStream &out);
00625 };
00626
00627
00628 }
00629 }
00630
00631 #endif