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 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 #include <mrpt/maps/link_pragmas.h> 00038 00039 namespace mrpt 00040 { 00041 namespace slam 00042 { 00043 00044 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CSimplePointsMap , CPointsMap, MAPS_IMPEXP ) 00045 00046 /** A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. 00047 * 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. 00048 * \sa CMetricMap, CPoint, mrpt::utils::CSerializable 00049 */ 00050 class MAPS_IMPEXP CSimplePointsMap : public CPointsMap 00051 { 00052 // This must be added to any CSerializable derived class: 00053 DEFINE_SERIALIZABLE( CSimplePointsMap ) 00054 public: 00055 00056 /** Destructor 00057 */ 00058 virtual ~CSimplePointsMap(); 00059 00060 /** Default constructor 00061 */ 00062 CSimplePointsMap(); 00063 00064 /** Copy operator 00065 */ 00066 void copyFrom(const CPointsMap &obj); 00067 00068 /** Transform the range scan into a set of cartesian coordinated 00069 * points. The options in "insertionOptions" are considered in this method. 00070 * \param rangeScan The scan to be inserted into this map 00071 * \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. 00072 * 00073 * NOTE: Only ranges marked as "valid=true" in the observation will be inserted 00074 * 00075 * \sa CObservation2DRangeScan 00076 */ 00077 void loadFromRangeScan( 00078 const CObservation2DRangeScan &rangeScan, 00079 const CPose3D *robotPose = NULL ); 00080 00081 /** Enter the set of cartesian coordinated points from the 3D range scan into 00082 * the map. The options in "insertionOptions" are considered in this method. 00083 * \param rangeScan The 3D scan to be inserted into this map 00084 * \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. 00085 * 00086 * NOTE: Only ranges marked as "valid=true" in the observation will be inserted 00087 * 00088 * \sa CObservation3DRangeScan 00089 */ 00090 void loadFromRangeScan( 00091 const CObservation3DRangeScan &rangeScan, 00092 const CPose3D *robotPose = NULL ); 00093 00094 /** Load from a text file. In each line there are a point coordinates. 00095 * Returns false if any error occured, true elsewere. 00096 */ 00097 bool load2D_from_text_file(std::string file); 00098 00099 /** Load from a text file. In each line there are a point coordinates. 00100 * Returns false if any error occured, true elsewere. 00101 */ 00102 bool load3D_from_text_file(std::string file); 00103 00104 00105 /** Insert the contents of another map into this one, fusing the previous content with the new one. 00106 * This means that points very close to existing ones will be "fused", rather than "added". This prevents 00107 * the unbounded increase in size of these class of maps. 00108 * NOTICE that "otherMap" is neither translated nor rotated here, so if this is desired it must done 00109 * before calling this method. 00110 * \param otherMap The other map whose points are to be inserted into this one. 00111 * \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. 00112 * \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. 00113 * \sa insertAnotherMap 00114 */ 00115 void fuseWith( CPointsMap *otherMap, 00116 float minDistForFuse = 0.02f, 00117 std::vector<bool> *notFusedPoints = NULL); 00118 00119 /** Insert the contents of another map into this one, without fusing close points. 00120 * \param otherMap The other map whose points are to be inserted into this one. 00121 * \param otherPose The pose of the other map in the coordinates of THIS map 00122 * \sa fuseWith 00123 */ 00124 void insertAnotherMap( 00125 CPointsMap *otherMap, 00126 const CPose2D &otherPose); 00127 00128 /** Changes a given point from map, as a 2D point. First index is 0. 00129 * \exception Throws std::exception on index out of bound. 00130 */ 00131 virtual void setPoint(size_t index,CPoint2D &p); 00132 00133 /** Changes a given point from map, as a 3D point. First index is 0. 00134 * \exception Throws std::exception on index out of bound. 00135 */ 00136 virtual void setPoint(size_t index,CPoint3D &p); 00137 00138 /** Changes a given point from map. First index is 0. 00139 * \exception Throws std::exception on index out of bound. 00140 */ 00141 virtual void setPoint(size_t index,float x, float y); 00142 00143 /** Changes a given point from map. First index is 0. 00144 * \exception Throws std::exception on index out of bound. 00145 */ 00146 virtual void setPoint(size_t index,float x, float y, float z); 00147 00148 /** Provides a way to insert individual points into the map: 00149 */ 00150 void insertPoint( float x, float y, float z = 0 ); 00151 00152 /** Provides a way to insert individual points into the map: 00153 */ 00154 void insertPoint( const mrpt::math::TPoint3D &new_pnt ) { 00155 this->insertPoint(new_pnt.x,new_pnt.y,new_pnt.z); 00156 } 00157 00158 /** Remove from the map the points marked in a bool's array as "true". 00159 * 00160 * \exception std::exception If mask size is not equal to points count. 00161 */ 00162 void applyDeletionMask( std::vector<bool> &mask ); 00163 00164 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00165 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00166 */ 00167 void auxParticleFilterCleanUp(); 00168 00169 /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory. 00170 * This is useful for situations where it is approximately known the final size of the map. This method is more 00171 * efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods. 00172 */ 00173 void reserve(size_t newLength); 00174 00175 /** 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). 00176 * \tparam VECTOR can be mrpt::vector_float or std::vector<float> or any other column or row Eigen::Matrix. 00177 */ 00178 template <typename VECTOR> 00179 inline void setAllPointsTemplate(const VECTOR &X,const VECTOR &Y,const VECTOR &Z = VECTOR()) 00180 { 00181 const size_t N = X.size(); 00182 ASSERT_EQUAL_(X.size(),Y.size()) 00183 ASSERT_(Z.size()==0 || Z.size()==X.size()) 00184 x.resize(N); y.resize(N); z.resize(N); 00185 const bool z_valid = Z.empty(); 00186 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]; } 00187 else for (size_t i=0;i<N;i++) { this->x[i]=X[i]; this->y[i]=Y[i]; this->z[i]=0; } 00188 pointWeight.assign(N,1); 00189 mark_as_modified(); 00190 } 00191 00192 /** Set all the points at once from vectors with X,Y and Z coordinates. \sa getAllPoints */ 00193 virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y,const std::vector<float> &Z) 00194 { 00195 setAllPointsTemplate(X,Y,Z); 00196 } 00197 00198 /** Set all the points at once from vectors with X and Y coordinates (Z=0). \sa getAllPoints */ 00199 virtual void setAllPoints(const std::vector<float> &X,const std::vector<float> &Y) 00200 { 00201 setAllPointsTemplate(X,Y); 00202 } 00203 00204 /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it. 00205 * Otherwise, return NULL 00206 */ 00207 virtual const CSimplePointsMap * getAsSimplePointsMap() const { return this; } 00208 virtual CSimplePointsMap * getAsSimplePointsMap() { return this; } 00209 00210 protected: 00211 /** Clear the map, erasing all the points. 00212 */ 00213 virtual void internal_clear(); 00214 00215 /** Insert the observation information into this map. This method must be implemented 00216 * in derived classes. 00217 * \param obs The observation 00218 * \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) 00219 * 00220 * \sa CObservation::insertObservationInto 00221 */ 00222 bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00223 00224 }; // End of class def. 00225 00226 } // End of namespace 00227 } // End of namespace 00228 00229 #endif
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:46:17 UTC 2011 |