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 CBeacon_H
00029 #define CBeacon_H
00030
00031 #include <mrpt/utils/CSerializable.h>
00032 #include <mrpt/math/CMatrix.h>
00033 #include <mrpt/system/os.h>
00034 #include <mrpt/utils/CStringList.h>
00035 #include <mrpt/poses/CPoint3D.h>
00036 #include <mrpt/poses/CPointPDFParticles.h>
00037 #include <mrpt/poses/CPointPDFGaussian.h>
00038 #include <mrpt/poses/CPointPDFSOG.h>
00039
00040 namespace mrpt
00041 {
00042 namespace slam
00043 {
00044 using namespace mrpt::poses;
00045 using namespace mrpt::utils;
00046
00047 class CBeaconMap;
00048 DEFINE_SERIALIZABLE_PRE( CBeacon )
00049
00050
00056 class MRPTDLLIMPEXP CBeacon : public CPointPDF
00057 {
00058
00059 DEFINE_SERIALIZABLE( CBeacon )
00060
00061 public:
00064 typedef int64_t TBeaconID;
00065
00068 enum TTypePDF { pdfMonteCarlo = 0, pdfGauss, pdfSOG };
00069
00073 TTypePDF m_typePDF;
00074
00077 CPointPDFParticles m_locationMC;
00078
00081 CPointPDFGaussian m_locationGauss;
00082
00085 CPointPDFSOG m_locationSOG;
00086
00096 TBeaconID m_ID;
00097
00100 CBeacon();
00101
00104 virtual ~CBeacon();
00105
00108 CPoint3D getEstimatedPoint() const;
00109
00112 CMatrixD getEstimatedCovariance() const ;
00113
00116 void copyFrom(const CPointPDF &o);
00117
00120 void saveToTextFile(const std::string &file) const;
00121
00125 void changeCoordinatesReference( const CPose3D &newReferenceBase );
00126
00129 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00130
00133 void getAsMatlabDrawCommands( utils::CStringList &out_Str ) const;
00134
00137 void drawSingleSample(CPoint3D &outSample) const;
00138
00144 void bayesianFusion( CPointPDF &p1, CPointPDF &p2, const double &minMahalanobisDistToDrop = 0);
00145
00146
00155 void generateObservationModelDistribution(
00156 const float &sensedRange,
00157 CPointPDFSOG &outPDF,
00158 const CBeaconMap *myBeaconMap,
00159 const CPoint3D &sensorPntOnRobot,
00160 const CPoint3D ¢erPoint = CPoint3D(0,0,0),
00161 const float &maxDistanceFromCenter = 0
00162 ) const;
00163
00170 static void generateRingSOG(
00171 const float &sensedRange,
00172 CPointPDFSOG &outPDF,
00173 const CBeaconMap *myBeaconMap,
00174 const CPoint3D &sensorPnt,
00175 const CMatrixD *covarianceCompositionToAdd = NULL,
00176 bool clearPreviousContentsOutPDF = true,
00177 const CPoint3D ¢erPoint = CPoint3D(0,0,0),
00178 const float &maxDistanceFromCenter = 0
00179 );
00180
00181
00182 };
00183
00184
00185 }
00186 }
00187
00188 #endif