00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 class CObservation3DRangeScan; 00045 00046 00047 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CColouredPointsMap, CPointsMap,MAPS_IMPEXP ) 00048 00049 /** A map of 2D/3D points with individual colours (RGB). 00050 * For different color schemes, see CColouredPointsMap::colorScheme 00051 * Colors are defined in the range [0,1]. 00052 * \sa mrpt::slam::CPointsMap, mrpt::slam::CMetricMap, mrpt::utils::CSerializable 00053 * \ingroup mrpt_maps_grp 00054 */ 00055 class MAPS_IMPEXP CColouredPointsMap : public CPointsMap 00056 { 00057 // This must be added to any CSerializable derived class: 00058 DEFINE_SERIALIZABLE( CColouredPointsMap ) 00059 00060 public: 00061 /** Destructor 00062 */ 00063 virtual ~CColouredPointsMap(); 00064 00065 /** Default constructor 00066 */ 00067 CColouredPointsMap(); 00068 00069 // -------------------------------------------- 00070 /** @name Pure virtual interfaces to be implemented by any class derived from CPointsMap 00071 @{ */ 00072 00073 /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory. 00074 * This is useful for situations where it is approximately known the final size of the map. This method is more 00075 * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods. 00076 */ 00077 virtual void reserve(size_t newLength); 00078 00079 /** Resizes all point buffers so they can hold the given number of points: newly created points are set to default values, 00080 * and old contents are not changed. 00081 * \sa reserve, setPoint, setPointFast, setSize 00082 */ 00083 virtual void resize(size_t newLength); 00084 00085 /** Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents 00086 * and leaving all points to default values. 00087 * \sa reserve, setPoint, setPointFast, setSize 00088 */ 00089 virtual void setSize(size_t newLength); 00090 00091 /** Changes the coordinates of the given point (0-based index), *without* checking for out-of-bounds and *without* calling mark_as_modified() \sa setPoint */ 00092 virtual void setPointFast(size_t index,float x, float y, float z); 00093 00094 /** The virtual method for \a insertPoint() *without* calling mark_as_modified() */ 00095 virtual void insertPointFast( float x, float y, float z = 0 ); 00096 00097 /** Virtual assignment operator, to be implemented in derived classes. 00098 */ 00099 virtual void copyFrom(const CPointsMap &obj); 00100 00101 /** Get all the data fields for one point as a vector: [X Y Z R G B] 00102 * Unlike getPointAllFields(), this method does not check for index out of bounds 00103 * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast 00104 */ 00105 virtual void getPointAllFieldsFast( const size_t index, std::vector<float> & point_data ) const { 00106 point_data.resize(6); 00107 point_data[0] = x[index]; 00108 point_data[1] = y[index]; 00109 point_data[2] = z[index]; 00110 point_data[3] = m_color_R[index]; 00111 point_data[4] = m_color_G[index]; 00112 point_data[5] = m_color_B[index]; 00113 } 00114 00115 /** Set all the data fields for one point as a vector: [X Y Z R G B] 00116 * Unlike setPointAllFields(), this method does not check for index out of bounds 00117 * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast 00118 */ 00119 virtual void setPointAllFieldsFast( const size_t index, const std::vector<float> & point_data ) { 00120 ASSERTDEB_(point_data.size()==6) 00121 x[index] = point_data[0]; 00122 y[index] = point_data[1]; 00123 z[index] = point_data[2]; 00124 m_color_R[index] = point_data[3]; 00125 m_color_G[index] = point_data[4]; 00126 m_color_B[index] = point_data[5]; 00127 } 00128 00129 /** See CPointsMap::loadFromRangeScan() */ 00130 virtual void loadFromRangeScan( 00131 const CObservation2DRangeScan &rangeScan, 00132 const CPose3D *robotPose = NULL ); 00133 00134 /** See CPointsMap::loadFromRangeScan() */ 00135 virtual void loadFromRangeScan( 00136 const CObservation3DRangeScan &rangeScan, 00137 const CPose3D *robotPose = NULL ); 00138 00139 protected: 00140 00141 /** Auxiliary method called from within \a addFrom() automatically, to finish the copying of class-specific data */ 00142 virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints); 00143 00144 00145 // Friend methods: 00146 template <class Derived> friend struct detail::loadFromRangeImpl; 00147 template <class Derived> friend struct detail::pointmap_traits; 00148 00149 public: 00150 00151 00152 /** @} */ 00153 // -------------------------------------------- 00154 00155 /** 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. 00156 * Returns false if any error occured, true elsewere. 00157 */ 00158 bool save3D_and_colour_to_text_file(const std::string &file) const; 00159 00160 /** Changes a given point from map. First index is 0. 00161 * \exception Throws std::exception on index out of bound. 00162 */ 00163 virtual void setPoint(size_t index,float x, float y, float z, float R, float G, float B); 00164 00165 // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "setPoint()" 00166 /// \overload 00167 inline void setPoint(size_t index,float x, float y, float z) { 00168 ASSERT_BELOW_(index,this->size()) 00169 setPointFast(index,x,y,z); 00170 mark_as_modified(); 00171 } 00172 /// \overload 00173 inline void setPoint(size_t index,CPoint2D &p) { setPoint(index,p.x(),p.y(),0); } 00174 /// \overload 00175 inline void setPoint(size_t index,CPoint3D &p) { setPoint(index,p.x(),p.y(),p.z()); } 00176 /// \overload 00177 inline void setPoint(size_t index,float x, float y) { setPoint(index,x,y,0); } 00178 00179 00180 /** Adds a new point given its coordinates and color (colors range is [0,1]) */ 00181 virtual void insertPoint( float x, float y, float z, float R, float G, float B ); 00182 // The following overloads must be repeated here (from CPointsMap) due to the shadowing of the above "insertPoint()" 00183 /// \overload of \a insertPoint() 00184 inline void insertPoint( const CPoint3D &p ) { insertPoint(p.x(),p.y(),p.z()); } 00185 /// \overload 00186 inline void insertPoint( const mrpt::math::TPoint3D &p ) { insertPoint(p.x,p.y,p.z); } 00187 /// \overload 00188 inline void insertPoint( float x, float y, float z) { insertPointFast(x,y,z); mark_as_modified(); } 00189 00190 /** Changes just the color of a given point from the map. First index is 0. 00191 * \exception Throws std::exception on index out of bound. 00192 */ 00193 void setPointColor(size_t index,float R, float G, float B); 00194 00195 /** Retrieves a point and its color (colors range is [0,1]) 00196 */ 00197 virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const; 00198 00199 /** Retrieves a point */ 00200 unsigned long getPoint( size_t index, float &x, float &y, float &z) const; 00201 00202 /** Retrieves a point color (colors range is [0,1]) */ 00203 void getPointColor( size_t index, float &R, float &G, float &B ) const; 00204 00205 /** Returns true if the point map has a color field for each point */ 00206 virtual bool hasColorPoints() const { return true; } 00207 00208 /** Override of the default 3D scene builder to account for the individual points' color. 00209 * \sa mrpt::global_settings::POINTSMAPS_3DOBJECT_POINTSIZE 00210 */ 00211 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00212 00213 /** Colour a set of points from a CObservationImage and the global pose of the robot 00214 */ 00215 bool colourFromObservation( const CObservationImage &obs, const CPose3D &robotPose ); 00216 00217 /** The choices for coloring schemes: 00218 * - cmFromHeightRelativeToSensor: The Z coordinate wrt the sensor will be used to obtain the color using the limits z_min,z_max. 00219 * - cmFromIntensityImage: When inserting 3D range scans, take the color from the intensity image channel, if available. 00220 * \sa TColourOptions 00221 */ 00222 enum TColouringMethod 00223 { 00224 cmFromHeightRelativeToSensor = 0, 00225 cmFromHeightRelativeToSensorJet = 0, 00226 cmFromHeightRelativeToSensorGray = 1, 00227 cmFromIntensityImage = 2 00228 }; 00229 00230 /** The definition of parameters for generating colors from laser scans */ 00231 struct MAPS_IMPEXP TColourOptions : public utils::CLoadableOptions 00232 { 00233 /** Initilization of default parameters */ 00234 TColourOptions( ); 00235 virtual ~TColourOptions() {} 00236 /** See utils::CLoadableOptions 00237 */ 00238 void loadFromConfigFile( 00239 const mrpt::utils::CConfigFileBase &source, 00240 const std::string §ion); 00241 00242 /** See utils::CLoadableOptions 00243 */ 00244 void dumpToTextStream(CStream &out) const; 00245 00246 TColouringMethod scheme; 00247 float z_min,z_max; 00248 float d_max; 00249 }; 00250 00251 TColourOptions colorScheme; //!< The options employed when inserting laser scans in the map. 00252 00253 void resetPointsMinDist( float defValue = 2000.0f ); //!< Reset the minimum-observed-distance buffer for all the points to a predefined value 00254 00255 /** @name PCL library support 00256 @{ */ 00257 00258 /** Save the point cloud as a PCL PCD file, in either ASCII or binary format \return false on any error */ 00259 virtual bool savePCDFile(const std::string &filename, bool save_as_binary) const; 00260 00261 /** @} */ 00262 00263 00264 protected: 00265 /** The color data */ 00266 std::vector<float> m_color_R,m_color_G,m_color_B; 00267 00268 /** Minimum distance from where the points have been seen */ 00269 //std::vector<float> m_min_dist; 00270 00271 /** Clear the map, erasing all the points. 00272 */ 00273 virtual void internal_clear(); 00274 00275 /** @name Redefinition of PLY Import virtual methods from CPointsMap 00276 @{ */ 00277 /** In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point. 00278 * \param pt_color Will be NULL if the loaded file does not provide color info. 00279 */ 00280 virtual void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::utils::TColorf *pt_color = NULL); 00281 00282 /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex */ 00283 virtual void PLY_import_set_vertex_count(const size_t N); 00284 /** @} */ 00285 00286 /** @name Redefinition of PLY Export virtual methods from CPointsMap 00287 @{ */ 00288 /** In a base class, will be called after PLY_export_get_vertex_count() once for each exported point. 00289 * \param pt_color Will be NULL if the loaded file does not provide color info. 00290 */ 00291 virtual void PLY_export_get_vertex( 00292 const size_t idx, 00293 mrpt::math::TPoint3Df &pt, 00294 bool &pt_has_color, 00295 mrpt::utils::TColorf &pt_color) const; 00296 /** @} */ 00297 00298 }; // End of class def. 00299 00300 } // End of namespace 00301 } // End of namespace 00302 00303 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |