00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2011 University of Malaga | 00007 | | 00008 | This software was written by the Machine Perception and Intelligent | 00009 | Robotics Lab, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 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 #include <mrpt/maps/link_pragmas.h> 00039 00040 namespace mrpt 00041 { 00042 namespace slam 00043 { 00044 00045 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CColouredPointsMap, CPointsMap,MAPS_IMPEXP ) 00046 00047 /** A map of 2D/3D points with individual colours (RGB). 00048 * For different color schemes, see CColouredPointsMap::colorScheme 00049 * Colors are defined in the range [0,1]. 00050 * \sa mrpt::slam::CPointsMap, mrpt::slam::CMetricMap, mrpt::utils::CSerializable 00051 */ 00052 class MAPS_IMPEXP CColouredPointsMap : public CPointsMap 00053 { 00054 // This must be added to any CSerializable derived class: 00055 DEFINE_SERIALIZABLE( CColouredPointsMap ) 00056 00057 protected: 00058 /** The color data */ 00059 std::vector<float> m_color_R,m_color_G,m_color_B; 00060 00061 /** Minimum distance from where the points have been seen */ 00062 std::vector<float> m_min_dist; 00063 00064 /** Clear the map, erasing all the points. 00065 */ 00066 virtual void internal_clear(); 00067 00068 /** Insert the observation information into this map. This method must be implemented 00069 * in derived classes. 00070 * \param obs The observation 00071 * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg) 00072 * 00073 * \sa CObservation::insertObservationInto 00074 */ 00075 virtual bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00076 00077 public: 00078 /** Destructor 00079 */ 00080 virtual ~CColouredPointsMap(); 00081 00082 /** Default constructor 00083 */ 00084 CColouredPointsMap(); 00085 00086 /** Copy operator 00087 */ 00088 void copyFrom(const CPointsMap &obj); 00089 00090 /** Transform the range scan into a set of cartessian coordinated 00091 * points. The options in "insertionOptions" are considered in this method. 00092 * \param rangeScan The scan to be inserted into this map 00093 * \param robotPose The robot 3D pose, default to (0,0,0|0deg,0deg,0deg). It is used to compute the sensor pose relative to the robot actual pose. Recall sensor pose is embeded in the observation class. 00094 * 00095 * \note Only ranges marked as "valid=true" in the observation will be inserted 00096 * 00097 * \sa CObservation2DRangeScan 00098 */ 00099 void loadFromRangeScan( 00100 const CObservation2DRangeScan &rangeScan, 00101 const CPose3D *robotPose = NULL ); 00102 00103 /** Transform the range scan into a set of cartessian coordinated 00104 * points. The options in "insertionOptions" are considered in this method. 00105 * \param rangeScan The scan to be inserted into this map 00106 * \param robotPose The robot 3D pose, default to (0,0,0|0deg,0deg,0deg). It is used to compute the sensor pose relative to the robot actual pose. Recall sensor pose is embeded in the observation class. 00107 * 00108 * \sa CObservation3DRangeScan 00109 */ 00110 void loadFromRangeScan( 00111 const CObservation3DRangeScan &rangeScan, 00112 const CPose3D *robotPose = NULL ); 00113 00114 /** Load from a text file. In each line there are a point coordinates. 00115 * Returns false if any error occured, true elsewere. 00116 */ 00117 bool load2D_from_text_file(std::string file); 00118 00119 /** Load from a text file. In each line there are a point coordinates. 00120 * Returns false if any error occured, true elsewere. 00121 */ 00122 bool load3D_from_text_file(std::string file); 00123 00124 /** Save to a text file. In each line contains X Y Z (meters) R G B (range [0,1]) for each point in the map. 00125 * Returns false if any error occured, true elsewere. 00126 */ 00127 bool save3D_and_colour_to_text_file(const std::string &file) const; 00128 00129 /** Insert the contents of another map into this one, fusing the previous content with the new one. 00130 * This means that points very close to existing ones will be "fused", rather than "added". This prevents 00131 * the unbounded increase in size of these class of maps. 00132 * NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done 00133 * before calling this method. 00134 * \param otherMap The other map whose points are to be inserted into this one. 00135 * \param minDistForFuse Minimum distance (in meters) between two points, each one in a map, to be considered the same one and be fused rather than added. 00136 * \param notFusedPoints If a pointer is supplied, this list will contain at output a list with a "bool" value per point in "this" map. This will be false/true according to that point having been fused or not. 00137 * \sa insertAnotherMap 00138 */ 00139 void fuseWith( CPointsMap *otherMap, 00140 float minDistForFuse = 0.02f, 00141 std::vector<bool> *notFusedPoints = NULL); 00142 00143 /** Changes a given point from map, as a 2D point. First index is 0. 00144 * \exception Throws std::exception on index out of bound. 00145 */ 00146 virtual void setPoint(size_t index,CPoint2D &p); 00147 00148 /** Changes a given point from map, as a 3D point. First index is 0. 00149 * \exception Throws std::exception on index out of bound. 00150 */ 00151 virtual void setPoint(size_t index,CPoint3D &p); 00152 00153 /** Changes a given point from map. First index is 0. 00154 * \exception Throws std::exception on index out of bound. 00155 */ 00156 virtual void setPoint(size_t index,float x, float y); 00157 00158 /** Changes a given point from map. First index is 0. 00159 * \exception Throws std::exception on index out of bound. 00160 */ 00161 virtual void setPoint(size_t index,float x, float y, float z); 00162 00163 /** Changes a given point from map. First index is 0. 00164 * \exception Throws std::exception on index out of bound. 00165 */ 00166 void setPoint(size_t index,float x, float y, float z, float R, float G, float B); 00167 00168 /** Changes just the color of a given point from the map. First index is 0. 00169 * \exception Throws std::exception on index out of bound. 00170 */ 00171 void setPointColor(size_t index,float R, float G, float B); 00172 00173 /** Provides a way to insert individual points into the map: 00174 */ 00175 void insertPoint( float x, float y, float z = 0 ); 00176 00177 /** Provides a way to insert individual points into the map: 00178 */ 00179 void insertPoint( CPoint3D p ); 00180 00181 /** Remove from the map the points marked in a bool's array as "true". 00182 * 00183 * \exception std::exception If mask size is not equal to points count. 00184 */ 00185 void applyDeletionMask( std::vector<bool> &mask ); 00186 00187 /** Adds a new point given its coordinates and color (colors range is [0,1]) 00188 */ 00189 void insertPoint( float x, float y, float z, float R, float G, float B ); 00190 00191 /** Retrieves a point and its color (colors range is [0,1]) 00192 */ 00193 virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const; 00194 00195 /** Retrieves a point color (colors range is [0,1]) 00196 */ 00197 void getPointColor( size_t index, float &R, float &G, float &B ) const; 00198 00199 /** Returns true if the point map has a color field for each point */ 00200 virtual bool hasColorPoints() const { return true; } 00201 00202 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00203 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00204 */ 00205 void auxParticleFilterCleanUp(); 00206 00207 /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory. 00208 * This is useful for situations where it is approximately known the final size of the map. This method is more 00209 * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods. 00210 */ 00211 void reserve(size_t newLength); 00212 00213 /** Set all the points at once from vectors with X,Y and Z coordinates (if Z is not provided, it will be set to all zeros). 00214 * RGB field of all points will be set to white color. 00215 * \tparam VECTOR can be mrpt::vector_float or std::vector<float> or any other column or row Eigen::Matrix. 00216 */ 00217 template <typename VECTOR> 00218 inline void setAllPointsTemplate(const VECTOR &X,const VECTOR &Y,const VECTOR &Z = VECTOR()) 00219 { 00220 const size_t N = X.size(); 00221 ASSERT_EQUAL_(X.size(),Y.size()) 00222 ASSERT_(Z.size()==0 || Z.size()==X.size()) 00223 x.resize(N); y.resize(N); z.resize(N); 00224 const bool z_valid = Z.empty(); 00225 if (z_valid) for (size_t i=0;i<N;i++) { this->x[i]=X[i]; this->y[i]=Y[i]; this->z[i]=Z[i]; } 00226 else for (size_t i=0;i<N;i++) { this->x[i]=X[i]; this->y[i]=Y[i]; this->z[i]=0; } 00227 pointWeight.assign(N,1); 00228 m_color_R.assign(N,1); 00229 m_color_G.assign(N,1); 00230 m_color_B.assign(N,1); 00231 mark_as_modified(); 00232 } 00233 00234 /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */ 00235 virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y,const std::vector<float> &Z) 00236 { 00237 setAllPointsTemplate(X,Y,Z); 00238 } 00239 00240 /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */ 00241 virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y) 00242 { 00243 setAllPointsTemplate(X,Y); 00244 } 00245 00246 00247 00248 /** Override of the default 3D scene builder to account for the individual points' color. 00249 * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE 00250 */ 00251 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00252 00253 /** Colour a set of points from a CObservationImage and the global pose of the robot 00254 */ 00255 bool colourFromObservation( const CObservationImage &obs, const CPose3D &robotPose ); 00256 00257 /** The choices for coloring schemes: 00258 * - cmFromHeightRelativeToSensor: The Z coordinate wrt the sensor will be used to obtain the color using the limits z_min,z_max. 00259 * - cmFromIntensityImage: When inserting 3D range scans, take the color from the intensity image channel, if available. 00260 * \sa TColourOptions 00261 */ 00262 enum TColouringMethod 00263 { 00264 cmFromHeightRelativeToSensor = 0, 00265 cmFromHeightRelativeToSensorJet = 0, 00266 cmFromHeightRelativeToSensorGray = 1, 00267 cmFromIntensityImage = 2 00268 }; 00269 00270 /** The definition of parameters for generating colors from laser scans */ 00271 struct MAPS_IMPEXP TColourOptions : public utils::CLoadableOptions 00272 { 00273 /** Initilization of default parameters */ 00274 TColourOptions( ); 00275 virtual ~TColourOptions() {} 00276 /** See utils::CLoadableOptions 00277 */ 00278 void loadFromConfigFile( 00279 const mrpt::utils::CConfigFileBase &source, 00280 const std::string §ion); 00281 00282 /** See utils::CLoadableOptions 00283 */ 00284 void dumpToTextStream(CStream &out) const; 00285 00286 TColouringMethod scheme; 00287 float z_min,z_max; 00288 float d_max; 00289 }; 00290 00291 TColourOptions colorScheme; //!< The options employed when inserting laser scans in the map. 00292 00293 void resetPointsMinDist( float defValue = 2000.0f ); //!< Reset the minimum-observed-distance buffer for all the points to a predefined value 00294 00295 /** @name Filter-by-height stuff 00296 @{ */ 00297 public: 00298 /** Enable/disable the filter-by-height functionality \sa setHeightFilterLevels \note Default upon construction is disabled. */ 00299 inline void enableFilterByHeight(bool enable=true) { bFilterByHeight=enable; } 00300 /** Return whether filter-by-height is enabled \sa enableFilterByHeight */ 00301 inline bool isFilterByHeightEnabled() const { return bFilterByHeight; } 00302 00303 /** Set the min/max Z levels for points to be actually inserted in the map (only if \a enableFilterByHeight() was called before). */ 00304 inline void setHeightFilterLevels(const double _z_min, const double _z_max) { z_min=_z_min; z_max=_z_max; } 00305 /** Get the min/max Z levels for points to be actually inserted in the map \sa enableFilterByHeight, setHeightFilterLevels */ 00306 inline void getHeightFilterLevels(double &_z_min, double &_z_max) const { _z_min=z_min; _z_max=z_max; } 00307 00308 private: 00309 /** The minimum and maximum height for a certain laser scan to be inserted into this map 00310 \sa bFilterByHeight 00311 */ 00312 double z_min, z_max; 00313 00314 /** Whether or not filter the input points by height 00315 \sa z_min, z_max 00316 */ 00317 bool bFilterByHeight; 00318 00319 /** @} */ 00320 00321 }; // End of class def. 00322 00323 } // End of namespace 00324 } // End of namespace 00325 00326 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011 |