00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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 vector_float m_color_R,m_color_G,m_color_B; 00060 00061 /** Minimum distance from where the points have been seen */ 00062 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 /** Provides a way to insert individual points into the map: 00164 */ 00165 void insertPoint( float x, float y, float z = 0 ); 00166 00167 /** Provides a way to insert individual points into the map: 00168 */ 00169 void insertPoint( CPoint3D p ); 00170 00171 /** Remove from the map the points marked in a bool's array as "true". 00172 * 00173 * \exception std::exception If mask size is not equal to points count. 00174 */ 00175 void applyDeletionMask( std::vector<bool> &mask ); 00176 00177 /** Adds a new point given its coordinates and color. 00178 */ 00179 void insertPoint( float x, float y, float z, float R, float G, float B ); 00180 00181 /** Retrieves a point and its color 00182 */ 00183 virtual void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const; 00184 00185 /** Returns true if the point map has a color field for each point */ 00186 virtual bool hasColorPoints() const { return true; } 00187 00188 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00189 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00190 */ 00191 void auxParticleFilterCleanUp(); 00192 00193 /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory. 00194 * This is useful for situations where it is approximately known the final size of the map. This method is more 00195 * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods. 00196 */ 00197 void reserve(size_t newLength); 00198 00199 /** Set all the points at once from vectors with X,Y and Z coordinates. */ 00200 void setAllPoints(const vector_float &X,const vector_float &Y,const vector_float &Z); 00201 00202 /** Set all the points at once from vectors with X and Y coordinates (Z=0). */ 00203 void setAllPoints(const vector_float &X,const vector_float &Y); 00204 00205 /** Override of the default 3D scene builder to account for the individual points' color. 00206 */ 00207 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00208 00209 /** Colour a set of points from a CObservationImage and the global pose of the robot 00210 */ 00211 bool colourFromObservation( const CObservationImage &obs, const CPose3D &robotPose ); 00212 00213 /** The choices for coloring schemes: 00214 * - cmFromHeightRelativeToSensor: The Z coordinate wrt the sensor will be used to obtain the color using the limits z_min,z_max. 00215 * - cmFromIntensityImage: When inserting 3D range scans, take the color from the intensity image channel, if available. 00216 * \sa TColourOptions 00217 */ 00218 enum TColouringMethod 00219 { 00220 cmFromHeightRelativeToSensor = 0, 00221 cmFromHeightRelativeToSensorJet = 0, 00222 cmFromHeightRelativeToSensorGray = 1, 00223 cmFromIntensityImage = 2 00224 }; 00225 00226 /** The definition of parameters for generating colors from laser scans */ 00227 struct MAPS_IMPEXP TColourOptions : public utils::CLoadableOptions 00228 { 00229 /** Initilization of default parameters */ 00230 TColourOptions( ); 00231 virtual ~TColourOptions() {} 00232 /** See utils::CLoadableOptions 00233 */ 00234 void loadFromConfigFile( 00235 const mrpt::utils::CConfigFileBase &source, 00236 const std::string §ion); 00237 00238 /** See utils::CLoadableOptions 00239 */ 00240 void dumpToTextStream(CStream &out) const; 00241 00242 TColouringMethod scheme; 00243 float z_min,z_max; 00244 float d_max; 00245 }; 00246 00247 TColourOptions colorScheme; //!< The options employed when inserting laser scans in the map. 00248 00249 void resetPointsMinDist( float defValue = 2000.0f ); //!< Reset the minimum-observed-distance buffer for all the points to a predefined value 00250 00251 }; // End of class def. 00252 00253 } // End of namespace 00254 } // End of namespace 00255 00256 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
