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 CColouredPointsMap_H
00029 #define CColouredPointsMap_H
00030
00031 #include <mrpt/slam/CPointsMap.h>
00032 #include <mrpt/slam/CObservation2DRangeScan.h>
00033 #include <mrpt/utils/CSerializable.h>
00034 #include <mrpt/math/CMatrix.h>
00035
00036 namespace mrpt
00037 {
00038 namespace slam
00039 {
00040
00041 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CColouredPointsMap, CPointsMap )
00042
00043
00048 class MRPTDLLIMPEXP CColouredPointsMap : public CPointsMap
00049 {
00050
00051 DEFINE_SERIALIZABLE( CColouredPointsMap )
00052
00053 protected:
00055 vector_float m_color_R,m_color_G,m_color_B;
00056
00057 public:
00060 virtual ~CColouredPointsMap();
00061
00064 CColouredPointsMap();
00065
00068 void copyFrom(const CPointsMap &obj);
00069
00079 void loadFromRangeScan(
00080 const CObservation2DRangeScan &rangeScan,
00081 const CPose3D *robotPose = NULL );
00082
00086 bool load2D_from_text_file(std::string file);
00087
00091 bool load3D_from_text_file(std::string file);
00092
00095 void clear();
00096
00107 void fuseWith( CPointsMap *otherMap,
00108 float minDistForFuse = 0.02f,
00109 std::vector<bool> *notFusedPoints = NULL);
00110
00114 void setPoint(const size_t &index,CPoint2D &p);
00115
00119 void setPoint(const size_t &index,CPoint3D &p);
00120
00124 void setPoint(const size_t &index,float x, float y);
00125
00129 void setPoint(const size_t &index,float x, float y, float z);
00130
00133 void insertPoint( float x, float y, float z = 0 );
00134
00137 void insertPoint( CPoint3D p );
00138
00143 void applyDeletionMask( std::vector<bool> &mask );
00144
00147 void insertPoint( float x, float y, float z, float R, float G, float B );
00148
00151 void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const;
00152
00160 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00161
00174 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00175
00179 void auxParticleFilterCleanUp();
00180
00185 void reserve(size_t newLength);
00186
00189 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00190
00191
00196 enum TColouringMethod
00197 {
00198 cmFromHeightRelativeToSensor = 0
00199 };
00200
00202 struct MRPTDLLIMPEXP TColourOptions : public utils::CLoadableOptions
00203 {
00205 TColourOptions( );
00206 virtual ~TColourOptions() {}
00209 void loadFromConfigFile(
00210 const mrpt::utils::CConfigFileBase &source,
00211 const std::string §ion);
00212
00215 void dumpToTextStream(utils::CStream &out);
00216
00217 TColouringMethod scheme;
00218 float z_min,z_max;
00219 };
00220
00221 TColourOptions colorScheme;
00222
00223 };
00224
00225 }
00226 }
00227
00228 #endif