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 CBeaconMap_H
00029 #define CBeaconMap_H
00030
00031 #include <mrpt/slam/CMetricMap.h>
00032 #include <mrpt/slam/CBeacon.h>
00033 #include <mrpt/utils/CSerializable.h>
00034 #include <mrpt/math/CMatrix.h>
00035 #include <mrpt/utils/CDynamicGrid.h>
00036 #include <mrpt/utils/CLoadableOptions.h>
00037
00038 namespace mrpt
00039 {
00040 namespace slam
00041 {
00042 using namespace mrpt::utils;
00043 using namespace mrpt::math;
00044
00045 class CObservationBeaconRanges;
00046
00049 typedef std::deque<CBeacon> TSequenceBeacons;
00050
00051 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CBeaconMap, CMetricMap )
00052
00053
00069 class MRPTDLLIMPEXP CBeaconMap : public CMetricMap
00070 {
00071
00072 DEFINE_SERIALIZABLE( CBeaconMap )
00073
00074 public:
00077 TSequenceBeacons m_beacons;
00078
00081 CBeaconMap();
00082
00085 virtual ~CBeaconMap();
00086
00097 float compute3DMatchingRatio(
00098 const CMetricMap *otherMap,
00099 const CPose3D &otherMapPose,
00100 float minDistForCorr = 0.10f,
00101 float minMahaDistForCorr = 2.0f
00102 ) const;
00103
00106 struct MRPTDLLIMPEXP TLikelihoodOptions : public utils::CLoadableOptions
00107 {
00108 public:
00111 TLikelihoodOptions();
00112
00115 void loadFromConfigFile(
00116 const mrpt::utils::CConfigFileBase &source,
00117 const std::string §ion);
00118
00121 void dumpToTextStream(utils::CStream &out);
00122
00125 float rangeStd;
00126
00127 } likelihoodOptions;
00128
00131 struct MRPTDLLIMPEXP TInsertionOptions : public utils::CLoadableOptions
00132 {
00133 public:
00136 TInsertionOptions();
00137
00140 void loadFromConfigFile(
00141 const mrpt::utils::CConfigFileBase &source,
00142 const std::string §ion);
00143
00146 void dumpToTextStream(utils::CStream &out);
00147
00151 bool insertAsMonteCarlo;
00152
00155 float maxElevation_deg,minElevation_deg;
00156
00159 unsigned int MC_numSamplesPerMeter;
00160
00163 float MC_maxStdToGauss;
00164
00167 float MC_thresholdNegligible;
00168
00171 bool MC_performResampling;
00172
00175 float MC_afterResamplingNoise;
00176
00179 float SOG_thresholdNegligible;
00180
00183 float SOG_maxDistBetweenGaussians;
00184
00187 float SOG_separationConstant;
00188
00189 } insertionOptions;
00190
00198 bool saveToMATLABScript3D(
00199 std::string file,
00200 const char *style="b",
00201 float confInterval = 0.95f ) const;
00202
00205 void clear();
00206
00209 size_t size() const;
00210
00218 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00219
00233 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00234
00254 void computeMatchingWith2D(
00255 const CMetricMap *otherMap,
00256 const CPose2D &otherMapPose,
00257 float maxDistForCorrespondence,
00258 float maxAngularDistForCorrespondence,
00259 const CPose2D &angularDistPivotPoint,
00260 TMatchingPairList &correspondences,
00261 float &correspondencesRatio,
00262 float *sumSqrDist = NULL,
00263 bool onlyKeepTheClosest = false,
00264 bool onlyUniqueRobust = false ) const;
00265
00274 void computeMatchingWith3DLandmarks(
00275 const mrpt::slam::CBeaconMap *otherMap,
00276 TMatchingPairList &correspondences,
00277 float &correspondencesRatio,
00278 std::vector<bool> &otherCorrespondences) const;
00279
00282 void changeCoordinatesReference( const CPose3D &newOrg );
00283
00286 void changeCoordinatesReference( const CPose3D &newOrg, const mrpt::slam::CBeaconMap *otherMap );
00287
00288
00291 bool isEmpty() const;
00292
00299 void simulateBeaconReadings(
00300 const CPose3D &in_robotPose,
00301 const CPoint3D &in_sensorLocationOnRobot,
00302 CObservationBeaconRanges &out_Observations ) const;
00303
00310 void saveMetricMapRepresentationToFile(
00311 const std::string &filNamePrefix ) const;
00312
00319 void saveToTextFile(const std::string &fil) const;
00320
00323 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00324
00328 void auxParticleFilterCleanUp();
00329
00332 const CBeacon * getBeaconByID( CBeacon::TBeaconID id ) const;
00333
00336 CBeacon * getBeaconByID( CBeacon::TBeaconID id );
00337
00338 };
00339
00340
00341 }
00342 }
00343
00344 #endif