00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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 namespace mrpt 00039 { 00040 namespace slam 00041 { 00042 00043 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CColouredPointsMap, CPointsMap ) 00044 00045 /** A map of 2D/3D points with individual colours (RGB). 00046 * For different color schemes, see CColouredPointsMap::colorScheme 00047 * Colors are defined in the range [0,1]. 00048 * \sa mrpt::slam::CPointsMap, mrpt::slam::CMetricMap, mrpt::utils::CSerializable 00049 */ 00050 class MRPTDLLIMPEXP CColouredPointsMap : public CPointsMap 00051 { 00052 // This must be added to any CSerializable derived class: 00053 DEFINE_SERIALIZABLE( CColouredPointsMap ) 00054 00055 protected: 00056 /** The color data */ 00057 vector_float m_color_R,m_color_G,m_color_B; 00058 00059 /** Minimum distance from where the points have been seen */ 00060 vector_float m_min_dist; 00061 00062 public: 00063 /** Destructor 00064 */ 00065 virtual ~CColouredPointsMap(); 00066 00067 /** Default constructor 00068 */ 00069 CColouredPointsMap(); 00070 00071 /** Copy operator 00072 */ 00073 void copyFrom(const CPointsMap &obj); 00074 00075 /** Transform the range scan into a set of cartessian coordinated 00076 * points. The options in "insertionOptions" are considered in this method. 00077 * \param rangeScan The scan to be inserted into this map 00078 * \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. 00079 * 00080 * \note Only ranges marked as "valid=true" in the observation will be inserted 00081 * 00082 * \sa CObservation2DRangeScan 00083 */ 00084 void loadFromRangeScan( 00085 const CObservation2DRangeScan &rangeScan, 00086 const CPose3D *robotPose = NULL ); 00087 00088 /** Load from a text file. In each line there are a point coordinates. 00089 * Returns false if any error occured, true elsewere. 00090 */ 00091 bool load2D_from_text_file(std::string file); 00092 00093 /** Load from a text file. In each line there are a point coordinates. 00094 * Returns false if any error occured, true elsewere. 00095 */ 00096 bool load3D_from_text_file(std::string file); 00097 00098 /** 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. 00099 * Returns false if any error occured, true elsewere. 00100 */ 00101 bool save3D_and_colour_to_text_file(const std::string &file) const; 00102 00103 /** Clear the map, erasing all the points. 00104 */ 00105 void clear(); 00106 00107 /** Insert the contents of another map into this one, fusing the previous content with the new one. 00108 * This means that points very close to existing ones will be "fused", rather than "added". This prevents 00109 * the unbounded increase in size of these class of maps. 00110 * NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done 00111 * before calling this method. 00112 * \param otherMap The other map whose points are to be inserted into this one. 00113 * \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. 00114 * \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. 00115 * \sa insertAnotherMap 00116 */ 00117 void fuseWith( CPointsMap *otherMap, 00118 float minDistForFuse = 0.02f, 00119 std::vector<bool> *notFusedPoints = NULL); 00120 00121 /** Changes a given point from map, as a 2D point. First index is 0. 00122 * \exception Throws std::exception on index out of bound. 00123 */ 00124 virtual void setPoint(size_t index,CPoint2D &p); 00125 00126 /** Changes a given point from map, as a 3D point. First index is 0. 00127 * \exception Throws std::exception on index out of bound. 00128 */ 00129 virtual void setPoint(size_t index,CPoint3D &p); 00130 00131 /** Changes a given point from map. First index is 0. 00132 * \exception Throws std::exception on index out of bound. 00133 */ 00134 virtual void setPoint(size_t index,float x, float y); 00135 00136 /** Changes a given point from map. First index is 0. 00137 * \exception Throws std::exception on index out of bound. 00138 */ 00139 virtual void setPoint(size_t index,float x, float y, float z); 00140 00141 /** Provides a way to insert individual points into the map: 00142 */ 00143 void insertPoint( float x, float y, float z = 0 ); 00144 00145 /** Provides a way to insert individual points into the map: 00146 */ 00147 void insertPoint( CPoint3D p ); 00148 00149 /** Remove from the map the points marked in a bool's array as "true". 00150 * 00151 * \exception std::exception If mask size is not equal to points count. 00152 */ 00153 void applyDeletionMask( std::vector<bool> &mask ); 00154 00155 /** Adds a new point given its coordinates and color. 00156 */ 00157 void insertPoint( float x, float y, float z, float R, float G, float B ); 00158 00159 /** Retrieves a point and its color 00160 */ 00161 void getPoint( size_t index, float &x, float &y, float &z, float &R, float &G, float &B ) const; 00162 00163 /** Insert the observation information into this map. This method must be implemented 00164 * in derived classes. 00165 * \param obs The observation 00166 * \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) 00167 * 00168 * \sa CObservation::insertObservationInto 00169 */ 00170 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00171 00172 /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map. 00173 * The implementation in this class checks if the points map has an associated gridmap, i.e. I am into a CHibridMetricMap. 00174 * If this is so, it always return 1.0 therefore the gridmap can return a more appropiate value. If this map is a standalone 00175 * metric map, it returns the points-matching ratio as the likelihood, using 1.5 times the parameter "minDistBetweenLaserPoints" (from 00176 * "CPointsMap::insertionOptions") as the matching distance threshold. 00177 * 00178 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00179 * \param obs The observation. 00180 * \return This method returns a likelihood in the range [0,1]. 00181 * 00182 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00183 */ 00184 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 00185 00186 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00187 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00188 */ 00189 void auxParticleFilterCleanUp(); 00190 00191 /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory. 00192 * This is useful for situations where it is approximately known the final size of the map. This method is more 00193 * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods. 00194 */ 00195 void reserve(size_t newLength); 00196 00197 /** Set all the points at once from vectors with X,Y and Z coordinates. */ 00198 void setAllPoints(const vector_float &X,const vector_float &Y,const vector_float &Z); 00199 00200 /** Set all the points at once from vectors with X and Y coordinates (Z=0). */ 00201 void setAllPoints(const vector_float &X,const vector_float &Y); 00202 00203 /** Override of the default 3D scene builder to account for the individual points' color. 00204 */ 00205 virtual void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00206 00207 /** Colour a set of points from a CObservationImage and the global pose of the robot 00208 */ 00209 bool colourFromObservation( const CObservationImage &obs, const CPose3D &robotPose ); 00210 00211 /** The choices for coloring schemes: 00212 * - cmFromHeightRelativeToSensor: The Z coordinate wrt the sensor will be used to obtain the color using the limits z_min,z_max. 00213 * \sa TColourOptions 00214 */ 00215 enum TColouringMethod 00216 { 00217 cmFromHeightRelativeToSensor = 0 00218 }; 00219 00220 /** The definition of parameters for generating colors from laser scans */ 00221 struct MRPTDLLIMPEXP TColourOptions : public utils::CLoadableOptions 00222 { 00223 /** Initilization of default parameters */ 00224 TColourOptions( ); 00225 virtual ~TColourOptions() {} 00226 /** See utils::CLoadableOptions 00227 */ 00228 void loadFromConfigFile( 00229 const mrpt::utils::CConfigFileBase &source, 00230 const std::string §ion); 00231 00232 /** See utils::CLoadableOptions 00233 */ 00234 void dumpToTextStream(CStream &out) const; 00235 00236 TColouringMethod scheme; 00237 float z_min,z_max; 00238 float d_max; 00239 }; 00240 00241 TColourOptions colorScheme; //!< The options employed when inserting laser scans in the map. 00242 00243 void resetPointsMinDist( float defValue = 2000.0f ); //!< Reset the minimum-observed-distance buffer for all the points to a predefined value 00244 00245 }; // End of class def. 00246 00247 } // End of namespace 00248 } // End of namespace 00249 00250 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |