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 CSENSORIALFRAME_H 00029 #define CSENSORIALFRAME_H 00030 00031 #include <mrpt/slam/CObservation.h> 00032 #include <mrpt/utils/CSerializable.h> 00033 #include <mrpt/slam/CObservation2DRangeScan.h> 00034 00035 00036 namespace mrpt 00037 { 00038 namespace slam 00039 { 00040 class CMetricMap; 00041 00042 // This must be added to any CSerializable derived class: 00043 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CSensoryFrame, mrpt::utils::CSerializable, OBS_IMPEXP ) 00044 00045 /** Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximately at the same time as one "snapshot" of the environment. 00046 * It can contain "observations" of many different kinds. 00047 * 00048 * New observations can be added using: 00049 * 00050 * \code 00051 CObservationXXXPtr o = CObservationXXX::Create(); // Create a smart pointer containing an object of class "CObservationXXX" 00052 o->(...) 00053 00054 CSensoryFrame sf; 00055 sf.insert(o); 00056 \endcode 00057 00058 The following methods are equivalent for adding new observations to a "sensory frame": 00059 * CSensoryFrame::operator += 00060 * CSensoryFrame::push_back 00061 * CSensoryFrame::insert 00062 00063 To examine the objects within a sensory frame, the following methods exist: 00064 * CSensoryFrame::getObservationByClass : Looks for some specific observation class. 00065 * CSensoryFrame::begin : To iterate over all observations. 00066 * CSensoryFrame::getObservationByIndex : To query by index. 00067 00068 Notice that observations objects are automatically deleted on 00069 Sensorial Frame destruction or clear. 00070 * \sa CObservation 00071 */ 00072 class OBS_IMPEXP CSensoryFrame : public mrpt::utils::CSerializable 00073 { 00074 // This must be added to any CSerializable derived class: 00075 DEFINE_SERIALIZABLE( CSensoryFrame ) 00076 // ------------------------------------------------------------------------------- 00077 // HACK: For compatibility with old datasets (See CSensoryFrame.cpp) 00078 // ------------------------------------------------------------------------------- 00079 static mrpt::utils::CLASSINIT _init_CSensorialFrame; 00080 static mrpt::utils::TRuntimeClassId classCSensorialFrame; 00081 00082 00083 public: 00084 /** Default constructor 00085 */ 00086 CSensoryFrame(); 00087 00088 /** Copy constructor 00089 */ 00090 CSensoryFrame( const CSensoryFrame &); 00091 00092 /** @name Cached points map 00093 @{ */ 00094 protected: 00095 /** A points map, build only under demand by the methods getAuxPointsMap() and buildAuxPointsMap(). 00096 * It's a generic smart pointer to avoid depending here in the library mrpt-obs on classes on other libraries. 00097 */ 00098 mutable mrpt::slam::CMetricMapPtr m_cachedMap; 00099 00100 void internal_buildAuxPointsMap( const void *options = NULL ) const; //!< Internal method, used from buildAuxPointsMap() 00101 00102 public: 00103 00104 /** Returns the cached points map representation of the scan, if already build with buildAuxPointsMap(), or NULL otherwise. 00105 * Usage: 00106 * \code 00107 * mrpt::slam::CPointsMap *map = obs->getAuxPointsMap<mrpt::slam::CPointsMap>(); 00108 * \endcode 00109 * \sa buildAuxPointsMap 00110 */ 00111 template <class POINTSMAP> 00112 inline const POINTSMAP* getAuxPointsMap() const { 00113 return static_cast<POINTSMAP*>(m_cachedMap.pointer()); 00114 } 00115 00116 /** Returns a cached points map representing this laser scan, building it upon the first call. 00117 * \param options Can be NULL to use default point maps' insertion options, or a pointer to a "CPointsMap::TInsertionOptions" structure to override some params. 00118 * Usage: 00119 * \code 00120 * mrpt::slam::CPointsMap *map = sf->buildAuxPointsMap<mrpt::slam::CPointsMap>(&options or NULL); 00121 * \endcode 00122 * \sa getAuxPointsMap 00123 */ 00124 template <class POINTSMAP> 00125 inline const POINTSMAP *buildAuxPointsMap( const void *options = NULL ) const { 00126 internal_buildAuxPointsMap(options); 00127 return static_cast<POINTSMAP*>(m_cachedMap.pointer()); 00128 } 00129 00130 /** @} */ 00131 00132 00133 /** Copy 00134 */ 00135 CSensoryFrame& operator =( const CSensoryFrame &o); 00136 00137 /** Destructor. 00138 */ 00139 virtual ~CSensoryFrame(); 00140 00141 /** Clear all current observations. 00142 */ 00143 void clear(); 00144 00145 /** Insert all the observations in this SF into a metric map or any kind (see mrpt::slam::CMetricMap). 00146 * It calls CObservation::insertObservationInto for all stored observation. 00147 * \param theMap The map where this observation is to be inserted: the map will be updated. 00148 * \param robotPose The pose of the robot base for this observation, relative to the target metric map. Set to NULL (default) to use (0,0,0deg) 00149 * 00150 * \return Returns true if the map has been updated, or false if this observations 00151 * has nothing to do with a metric map (for example, a sound observation). 00152 * 00153 * \sa mrpt::slam::CMetricMap, CObservation::insertObservationInto, CMetricMap::insertObservation 00154 */ 00155 bool insertObservationsInto( mrpt::slam::CMetricMap *theMap, const CPose3D *robotPose = NULL ) const; 00156 00157 /** Insert all the observations in this SF into a metric map or any kind (see mrpt::slam::CMetricMap). 00158 * It calls CObservation::insertObservationInto for all stored observation. 00159 * \param theMap The map where this observation is to be inserted: the map will be updated. 00160 * \param robotPose The pose of the robot base for this observation, relative to the target metric map. Set to NULL (default) to use (0,0,0deg) 00161 * 00162 * \return Returns true if the map has been updated, or false if this observations 00163 * has nothing to do with a metric map (for example, a sound observation). 00164 * 00165 * \sa mrpt::slam::CMetricMap, CObservation::insertObservationInto, CMetricMap::insertObservation 00166 */ 00167 inline bool insertObservationsInto( mrpt::slam::CMetricMapPtr &theMap, const CPose3D *robotPose = NULL ) const 00168 { 00169 return insertObservationsInto(theMap.pointer(), robotPose); 00170 } 00171 00172 00173 /** You can use "sf1+=sf2;" to add observations in sf2 to sf1. Objects are copied, not referenced, thus the source can be safely deleted next. 00174 * \sa moveFrom 00175 */ 00176 void operator += (const CSensoryFrame &sf); 00177 00178 /** You can use "sf+=obs;" to add the observation "obs" to the "sf1". Objects are copied, using the smart pointer, thus the original pointer can be safely deleted next. 00179 * \sa moveFrom 00180 */ 00181 void operator += (const CObservationPtr &obs); 00182 00183 /** Copies all the observation from another object, then erase them from the origin object (this method is fast since only pointers are copied); Previous objects in this objects are not deleted. 00184 * \sa operator += 00185 */ 00186 void moveFrom(CSensoryFrame &sf); 00187 00188 /** Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the passed object, this class will do at destructor or when appropriate. 00189 */ 00190 void push_back(const CObservationPtr &obs); 00191 00192 /** Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the passed object, this class will do at destructor or when appropriate. 00193 */ 00194 void insert(const CObservationPtr &obs); 00195 00196 /** Returns the i'th observation of a given class (or of a descendant class), or NULL if there is no such observation in the array. 00197 * Example: 00198 * \code 00199 CObservationImagePtr obs = m_SF->getObservationByClass<CObservationImage>(); 00200 * \endcode 00201 * By default (ith=0), the first observation is returned. 00202 */ 00203 template <typename T> 00204 typename T::SmartPtr getObservationByClass( const size_t &ith = 0 ) const 00205 { 00206 MRPT_START; 00207 size_t foundCount = 0; 00208 const mrpt::utils::TRuntimeClassId* class_ID = T::classinfo; 00209 for (const_iterator it = begin();it!=end();++it) 00210 if ( (*it)->GetRuntimeClass()->derivedFrom( class_ID ) ) 00211 if (foundCount++ == ith) 00212 return typename T::SmartPtr(*it); 00213 return typename T::SmartPtr(); // Not found: return empty smart pointer 00214 MRPT_END; 00215 } 00216 00217 /** You can use CSensoryFrame::begin to get a iterator to the first element. 00218 */ 00219 typedef std::deque<CObservationPtr>::iterator iterator; 00220 00221 /** You can use CSensoryFrame::begin to get a iterator to the first element. 00222 */ 00223 typedef std::deque<CObservationPtr>::const_iterator const_iterator; 00224 00225 /** Returns a iterator to the first observation: this is an example of usage: 00226 * \code 00227 * CSensoryFrame sf; 00228 * ... 00229 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00230 * { 00231 * (*it)->... // (*it) is a "CObservation*" 00232 * } 00233 * 00234 * \endcode 00235 */ 00236 const_iterator begin() const { return m_observations.begin(); } 00237 00238 /** Returns a iterator to the end of the list of observations: this is an example of usage: 00239 * \code 00240 * CSensoryFrame sf; 00241 * ... 00242 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00243 * { 00244 * (*it)->... // (*it) is a "CObservation*" 00245 * } 00246 * 00247 * \endcode 00248 */ 00249 const_iterator end() const { return m_observations.end(); } 00250 00251 /** Returns a iterator to the first observation: this is an example of usage: 00252 * \code 00253 * CSensoryFrame sf; 00254 * ... 00255 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00256 * { 00257 * (*it)->... // (*it) is a "CObservation*" 00258 * } 00259 * 00260 * \endcode 00261 */ 00262 iterator begin() { return m_observations.begin(); } 00263 00264 /** Returns a iterator to the end of the list of observations: this is an example of usage: 00265 * \code 00266 * CSensoryFrame sf; 00267 * ... 00268 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00269 * { 00270 * (*it)->... // (*it) is a "CObservation*" 00271 * } 00272 * 00273 * \endcode 00274 */ 00275 iterator end() { return m_observations.end(); } 00276 00277 00278 /** Returns the number of observations in the list. 00279 */ 00280 size_t size() const; 00281 00282 /** Removes the i'th observation in the list (0=first). 00283 */ 00284 void eraseByIndex(const size_t &idx); 00285 00286 /** Removes the given observation in the list, and return an iterator to the next element (or this->end() if it was the last one). 00287 */ 00288 iterator erase( const iterator &it); 00289 00290 /** Removes all the observations that match a given sensorLabel. 00291 */ 00292 void eraseByLabel(const std::string &label); 00293 00294 /** Returns the i'th observation in the list (0=first). 00295 * \sa begin, size 00296 */ 00297 CObservationPtr getObservationByIndex( const size_t &idx ) const; 00298 00299 /** Returns the i'th observation in the list (0=first), and as a different smart pointer type: 00300 * \code 00301 * sf.getObservationByIndexAs<CObservationStereoImagesPtr>(i); 00302 * \endcode 00303 * \sa begin, size 00304 */ 00305 template <typename T> 00306 T getObservationByIndexAs( const size_t &idx ) const 00307 { 00308 return static_cast<T>(getObservationByIndex(idx)); 00309 } 00310 00311 /** Returns the i'th observation in the list with the given "sensorLabel" (0=first). 00312 * \return The observation, or NULL if not found. 00313 * \sa begin, size 00314 */ 00315 CObservationPtr getObservationBySensorLabel( const std::string &label, const size_t &idx = 0) const; 00316 00317 /** Returns the i'th observation in the list with the given "sensorLabel" (0=first), and as a different smart pointer type: 00318 * \code 00319 * sf.getObservationBySensorLabelAs<CObservationStereoImagesPtr>(i); 00320 * \endcode 00321 * \sa begin, size 00322 */ 00323 template <typename T> 00324 T getObservationBySensorLabelAs( const std::string &label, const size_t &idx = 0) const 00325 { 00326 return T(getObservationBySensorLabel(label,idx)); 00327 } 00328 00329 /** Efficiently swaps the contents of two objects. 00330 */ 00331 void swap( CSensoryFrame &sf ); 00332 00333 protected: 00334 /** The set of observations taken at the same time instant. See the top of this page for instructions on accessing this. 00335 */ 00336 //std::deque<CObservation*> m_observations; 00337 std::deque<CObservationPtr> m_observations; 00338 00339 }; // End of class def. 00340 00341 00342 } // End of namespace 00343 } // End of namespace 00344 00345 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
