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 CSimplePointsMap_H 00029 #define CSimplePointsMap_H 00030 00031 #include <mrpt/slam/CPointsMap.h> 00032 #include <mrpt/slam/CObservation2DRangeScan.h> 00033 #include <mrpt/slam/CObservation3DRangeScan.h> 00034 #include <mrpt/utils/CSerializable.h> 00035 #include <mrpt/math/CMatrix.h> 00036 00037 namespace mrpt 00038 { 00039 namespace slam 00040 { 00041 00042 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CSimplePointsMap , CPointsMap ) 00043 00044 /** A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. 00045 * This class stores the coordinates (x,y,z) and a "weight", or counter of how many times that point has been seen, used only if points fusion is enabled in the options structure. 00046 * \sa CMetricMap, CPoint, mrpt::utils::CSerializable 00047 */ 00048 class MRPTDLLIMPEXP CSimplePointsMap : public CPointsMap 00049 { 00050 // This must be added to any CSerializable derived class: 00051 DEFINE_SERIALIZABLE( CSimplePointsMap ) 00052 public: 00053 00054 /** Destructor 00055 */ 00056 virtual ~CSimplePointsMap(); 00057 00058 /** Default constructor 00059 */ 00060 CSimplePointsMap(); 00061 00062 /** Copy operator 00063 */ 00064 void copyFrom(const CPointsMap &obj); 00065 00066 /** Transform the range scan into a set of cartesian coordinated 00067 * points. The options in "insertionOptions" are considered in this method. 00068 * \param rangeScan The scan to be inserted into this map 00069 * \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. 00070 * 00071 * NOTE: Only ranges marked as "valid=true" in the observation will be inserted 00072 * 00073 * \sa CObservation2DRangeScan 00074 */ 00075 void loadFromRangeScan( 00076 const CObservation2DRangeScan &rangeScan, 00077 const CPose3D *robotPose = NULL ); 00078 00079 /** Enter the set of cartesian coordinated points from the 3D range scan into 00080 * the map. The options in "insertionOptions" are considered in this method. 00081 * \param rangeScan The 3D scan to be inserted into this map 00082 * \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. 00083 * 00084 * NOTE: Only ranges marked as "valid=true" in the observation will be inserted 00085 * 00086 * \sa CObservation3DRangeScan 00087 */ 00088 void loadFromRangeScan( 00089 const CObservation3DRangeScan &rangeScan, 00090 const CPose3D *robotPose = NULL ); 00091 00092 /** Load from a text file. In each line there are a point coordinates. 00093 * Returns false if any error occured, true elsewere. 00094 */ 00095 bool load2D_from_text_file(std::string file); 00096 00097 /** Load from a text file. In each line there are a point coordinates. 00098 * Returns false if any error occured, true elsewere. 00099 */ 00100 bool load3D_from_text_file(std::string file); 00101 00102 /** Clear the map, erasing all the points. 00103 */ 00104 void clear(); 00105 00106 /** Insert the contents of another map into this one, fusing the previous content with the new one. 00107 * This means that points very close to existing ones will be "fused", rather than "added". This prevents 00108 * the unbounded increase in size of these class of maps. 00109 * NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done 00110 * before calling this method. 00111 * \param otherMap The other map whose points are to be inserted into this one. 00112 * \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. 00113 * \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. 00114 * \sa insertAnotherMap 00115 */ 00116 void fuseWith( CPointsMap *otherMap, 00117 float minDistForFuse = 0.02f, 00118 std::vector<bool> *notFusedPoints = NULL); 00119 00120 /** Insert the contents of another map into this one, without fusing close points. 00121 * \param otherMap The other map whose points are to be inserted into this one. 00122 * \param otherPose The pose of the other map in the coordinates of THIS map 00123 * \sa fuseWith 00124 */ 00125 void insertAnotherMap( 00126 CPointsMap *otherMap, 00127 CPose2D otherPose); 00128 00129 /** Changes a given point from map, as a 2D point. First index is 0. 00130 * \exception Throws std::exception on index out of bound. 00131 */ 00132 virtual void setPoint(size_t index,CPoint2D &p); 00133 00134 /** Changes a given point from map, as a 3D point. First index is 0. 00135 * \exception Throws std::exception on index out of bound. 00136 */ 00137 virtual void setPoint(size_t index,CPoint3D &p); 00138 00139 /** Changes a given point from map. First index is 0. 00140 * \exception Throws std::exception on index out of bound. 00141 */ 00142 virtual void setPoint(size_t index,float x, float y); 00143 00144 /** Changes a given point from map. First index is 0. 00145 * \exception Throws std::exception on index out of bound. 00146 */ 00147 virtual void setPoint(size_t index,float x, float y, float z); 00148 00149 /** Provides a way to insert individual points into the map: 00150 */ 00151 void insertPoint( float x, float y, float z = 0 ); 00152 00153 /** Provides a way to insert individual points into the map: 00154 */ 00155 void insertPoint( const mrpt::math::TPoint3D &new_pnt ) { 00156 this->insertPoint(new_pnt.x,new_pnt.y,new_pnt.z); 00157 } 00158 00159 /** Remove from the map the points marked in a bool's array as "true". 00160 * 00161 * \exception std::exception If mask size is not equal to points count. 00162 */ 00163 void applyDeletionMask( std::vector<bool> &mask ); 00164 00165 /** Insert the observation information into this map. This method must be implemented 00166 * in derived classes. 00167 * \param obs The observation 00168 * \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) 00169 * 00170 * \sa CObservation::insertObservationInto 00171 */ 00172 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00173 00174 /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map. 00175 * The implementation in this class checks if the points map has an associated gridmap, i.e. I am into a CHibridMetricMap. 00176 * If this is so, it always return 1.0 therefore the gridmap can return a more appropiate value. If this map is a standalone 00177 * metric map, it returns the points-matching ratio as the likelihood, using 1.5 times the parameter "minDistBetweenLaserPoints" (from 00178 * "CPointsMap::insertionOptions") as the matching distance threshold. 00179 * 00180 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00181 * \param obs The observation. 00182 * \return This method returns a likelihood in the range [0,1]. 00183 * 00184 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00185 */ 00186 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 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 00200 /** Set all the points at once from vectors with X,Y and Z coordinates. */ 00201 void setAllPoints(const vector_float &X,const vector_float &Y,const vector_float &Z); 00202 00203 /** Set all the points at once from vectors with X and Y coordinates (Z=0). */ 00204 void setAllPoints(const vector_float &X,const vector_float &Y); 00205 00206 }; // End of class def. 00207 00208 } // End of namespace 00209 } // End of namespace 00210 00211 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:27:43 EDT 2009 |