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/slam/CObservationImage.h>
00034 #include <mrpt/utils/CSerializable.h>
00035 #include <mrpt/math/CMatrix.h>
00036 #include <mrpt/utils/stl_extensions.h>
00037
00038 namespace mrpt
00039 {
00040 namespace slam
00041 {
00042
00043 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CColouredPointsMap, CPointsMap )
00044
00045
00050 class MRPTDLLIMPEXP CColouredPointsMap : public CPointsMap
00051 {
00052
00053 DEFINE_SERIALIZABLE( CColouredPointsMap )
00054
00055 protected:
00057 vector_float_serializable m_color_R,m_color_G,m_color_B;
00058
00060 vector_float_serializable m_min_dist;
00061
00062 public:
00065 virtual ~CColouredPointsMap();
00066
00069 CColouredPointsMap();
00070
00073 void copyFrom(const CPointsMap &obj);
00074
00084 void loadFromRangeScan(
00085 const CObservation2DRangeScan &rangeScan,
00086 const CPose3D *robotPose = NULL );
00087
00091 bool load2D_from_text_file(std::string file);
00092
00096 bool load3D_from_text_file(std::string file);
00097
00101 bool save3D_and_colour_to_text_file(const std::string &file) const;
00102
00105 void clear();
00106
00117 void fuseWith( CPointsMap *otherMap,
00118 float minDistForFuse = 0.02f,
00119 std::vector<bool> *notFusedPoints = NULL);
00120
00124 void setPoint(const size_t &index,CPoint2D &p);
00125
00129 void setPoint(const size_t &index,CPoint3D &p);
00130
00134 void setPoint(const size_t &index,float x, float y);
00135
00139 void setPoint(const size_t &index,float x, float y, float z);
00140
00143 void insertPoint( float x, float y, float z = 0 );
00144
00147 void insertPoint( CPoint3D p );
00148
00153 void applyDeletionMask( std::vector<bool> &mask );
00154
00157 void insertPoint( float x, float y, float z, float R, float G, float B );
00158
00161 void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const;
00162
00170 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00171
00184 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00185
00189 void auxParticleFilterCleanUp();
00190
00195 void reserve(size_t newLength);
00196
00199 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00200
00203 bool colourFromObservation( const CObservationImage &obs, const CPose3D &robotPose );
00204
00209 enum TColouringMethod
00210 {
00211 cmFromHeightRelativeToSensor = 0
00212 };
00213
00215 struct MRPTDLLIMPEXP TColourOptions : public utils::CLoadableOptions
00216 {
00218 TColourOptions( );
00219 virtual ~TColourOptions() {}
00222 void loadFromConfigFile(
00223 const mrpt::utils::CConfigFileBase &source,
00224 const std::string §ion);
00225
00228 void dumpToTextStream(utils::CStream &out);
00229
00230 TColouringMethod scheme;
00231 float z_min,z_max;
00232 float d_max;
00233 };
00234
00235 TColourOptions colorScheme;
00236
00237 void resetPointsMinDist( float defValue = 2000.0f );
00238
00239 };
00240
00241 }
00242 }
00243
00244 #endif