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 CSENSORYFRAME_H 00029 #define CSENSORYFRAME_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 contained observations objects are automatically deleted on 00069 * this object's 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 public: 00078 /** Default constructor 00079 */ 00080 CSensoryFrame(); 00081 00082 /** Copy constructor 00083 */ 00084 CSensoryFrame( const CSensoryFrame &); 00085 00086 /** @name Cached points map 00087 @{ */ 00088 protected: 00089 /** A points map, build only under demand by the methods getAuxPointsMap() and buildAuxPointsMap(). 00090 * It's a generic smart pointer to avoid depending here in the library mrpt-obs on classes on other libraries. 00091 */ 00092 mutable mrpt::slam::CMetricMapPtr m_cachedMap; 00093 00094 void internal_buildAuxPointsMap( const void *options = NULL ) const; //!< Internal method, used from buildAuxPointsMap() 00095 00096 public: 00097 00098 /** Returns the cached points map representation of the scan, if already build with buildAuxPointsMap(), or NULL otherwise. 00099 * Usage: 00100 * \code 00101 * mrpt::slam::CPointsMap *map = obs->getAuxPointsMap<mrpt::slam::CPointsMap>(); 00102 * \endcode 00103 * \sa buildAuxPointsMap 00104 */ 00105 template <class POINTSMAP> 00106 inline const POINTSMAP* getAuxPointsMap() const { 00107 return static_cast<POINTSMAP*>(m_cachedMap.pointer()); 00108 } 00109 00110 /** Returns a cached points map representing this laser scan, building it upon the first call. 00111 * \param options Can be NULL to use default point maps' insertion options, or a pointer to a "CPointsMap::TInsertionOptions" structure to override some params. 00112 * Usage: 00113 * \code 00114 * mrpt::slam::CPointsMap *map = sf->buildAuxPointsMap<mrpt::slam::CPointsMap>(&options or NULL); 00115 * \endcode 00116 * \sa getAuxPointsMap 00117 */ 00118 template <class POINTSMAP> 00119 inline const POINTSMAP *buildAuxPointsMap( const void *options = NULL ) const { 00120 internal_buildAuxPointsMap(options); 00121 return static_cast<POINTSMAP*>(m_cachedMap.pointer()); 00122 } 00123 00124 /** @} */ 00125 00126 00127 /** Copy 00128 */ 00129 CSensoryFrame& operator =( const CSensoryFrame &o); 00130 00131 /** Destructor. 00132 */ 00133 virtual ~CSensoryFrame(); 00134 00135 /** Clear all current observations. 00136 */ 00137 void clear(); 00138 00139 /** Insert all the observations in this SF into a metric map or any kind (see mrpt::slam::CMetricMap). 00140 * It calls CObservation::insertObservationInto for all stored observation. 00141 * \param theMap The map where this observation is to be inserted: the map will be updated. 00142 * \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) 00143 * 00144 * \return Returns true if the map has been updated, or false if this observations 00145 * has nothing to do with a metric map (for example, a sound observation). 00146 * 00147 * \sa mrpt::slam::CMetricMap, CObservation::insertObservationInto, CMetricMap::insertObservation 00148 */ 00149 bool insertObservationsInto( mrpt::slam::CMetricMap *theMap, const CPose3D *robotPose = NULL ) const; 00150 00151 /** Insert all the observations in this SF into a metric map or any kind (see mrpt::slam::CMetricMap). 00152 * It calls CObservation::insertObservationInto for all stored observation. 00153 * \param theMap The map where this observation is to be inserted: the map will be updated. 00154 * \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) 00155 * 00156 * \return Returns true if the map has been updated, or false if this observations 00157 * has nothing to do with a metric map (for example, a sound observation). 00158 * 00159 * \sa mrpt::slam::CMetricMap, CObservation::insertObservationInto, CMetricMap::insertObservation 00160 */ 00161 inline bool insertObservationsInto( mrpt::slam::CMetricMapPtr &theMap, const CPose3D *robotPose = NULL ) const 00162 { 00163 return insertObservationsInto(theMap.pointer(), robotPose); 00164 } 00165 00166 00167 /** 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. 00168 * \sa moveFrom 00169 */ 00170 void operator += (const CSensoryFrame &sf); 00171 00172 /** 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. 00173 * \sa moveFrom 00174 */ 00175 void operator += (const CObservationPtr &obs); 00176 00177 /** 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. 00178 * \sa operator += 00179 */ 00180 void moveFrom(CSensoryFrame &sf); 00181 00182 /** 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. 00183 */ 00184 void push_back(const CObservationPtr &obs); 00185 00186 /** 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. 00187 */ 00188 void insert(const CObservationPtr &obs); 00189 00190 /** 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. 00191 * Example: 00192 * \code 00193 CObservationImagePtr obs = m_SF->getObservationByClass<CObservationImage>(); 00194 * \endcode 00195 * By default (ith=0), the first observation is returned. 00196 */ 00197 template <typename T> 00198 typename T::SmartPtr getObservationByClass( const size_t &ith = 0 ) const 00199 { 00200 MRPT_START; 00201 size_t foundCount = 0; 00202 const mrpt::utils::TRuntimeClassId* class_ID = T::classinfo; 00203 for (const_iterator it = begin();it!=end();++it) 00204 if ( (*it)->GetRuntimeClass()->derivedFrom( class_ID ) ) 00205 if (foundCount++ == ith) 00206 return typename T::SmartPtr(*it); 00207 return typename T::SmartPtr(); // Not found: return empty smart pointer 00208 MRPT_END; 00209 } 00210 00211 /** You can use CSensoryFrame::begin to get a iterator to the first element. 00212 */ 00213 typedef std::deque<CObservationPtr>::iterator iterator; 00214 00215 /** You can use CSensoryFrame::begin to get a iterator to the first element. 00216 */ 00217 typedef std::deque<CObservationPtr>::const_iterator const_iterator; 00218 00219 /** Returns a iterator to the first observation: this is an example of usage: 00220 * \code 00221 * CSensoryFrame sf; 00222 * ... 00223 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00224 * { 00225 * (*it)->... // (*it) is a "CObservation*" 00226 * } 00227 * 00228 * \endcode 00229 */ 00230 const_iterator begin() const { return m_observations.begin(); } 00231 00232 /** Returns a iterator to the end of the list of observations: this is an example of usage: 00233 * \code 00234 * CSensoryFrame sf; 00235 * ... 00236 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00237 * { 00238 * (*it)->... // (*it) is a "CObservation*" 00239 * } 00240 * 00241 * \endcode 00242 */ 00243 const_iterator end() const { return m_observations.end(); } 00244 00245 /** Returns a iterator to the first observation: this is an example of usage: 00246 * \code 00247 * CSensoryFrame sf; 00248 * ... 00249 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00250 * { 00251 * (*it)->... // (*it) is a "CObservation*" 00252 * } 00253 * 00254 * \endcode 00255 */ 00256 iterator begin() { return m_observations.begin(); } 00257 00258 /** Returns a iterator to the end of the list of observations: this is an example of usage: 00259 * \code 00260 * CSensoryFrame sf; 00261 * ... 00262 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00263 * { 00264 * (*it)->... // (*it) is a "CObservation*" 00265 * } 00266 * 00267 * \endcode 00268 */ 00269 iterator end() { return m_observations.end(); } 00270 00271 00272 /** Returns the number of observations in the list. 00273 */ 00274 size_t size() const; 00275 00276 /** Removes the i'th observation in the list (0=first). 00277 */ 00278 void eraseByIndex(const size_t &idx); 00279 00280 /** Removes the given observation in the list, and return an iterator to the next element (or this->end() if it was the last one). 00281 */ 00282 iterator erase( const iterator &it); 00283 00284 /** Removes all the observations that match a given sensorLabel. 00285 */ 00286 void eraseByLabel(const std::string &label); 00287 00288 /** Returns the i'th observation in the list (0=first). 00289 * \sa begin, size 00290 */ 00291 CObservationPtr getObservationByIndex( const size_t &idx ) const; 00292 00293 /** Returns the i'th observation in the list (0=first), and as a different smart pointer type: 00294 * \code 00295 * sf.getObservationByIndexAs<CObservationStereoImagesPtr>(i); 00296 * \endcode 00297 * \sa begin, size 00298 */ 00299 template <typename T> 00300 T getObservationByIndexAs( const size_t &idx ) const 00301 { 00302 return static_cast<T>(getObservationByIndex(idx)); 00303 } 00304 00305 /** Returns the i'th observation in the list with the given "sensorLabel" (0=first). 00306 * \return The observation, or NULL if not found. 00307 * \sa begin, size 00308 */ 00309 CObservationPtr getObservationBySensorLabel( const std::string &label, const size_t &idx = 0) const; 00310 00311 /** Returns the i'th observation in the list with the given "sensorLabel" (0=first), and as a different smart pointer type: 00312 * \code 00313 * sf.getObservationBySensorLabelAs<CObservationStereoImagesPtr>(i); 00314 * \endcode 00315 * \sa begin, size 00316 */ 00317 template <typename T> 00318 T getObservationBySensorLabelAs( const std::string &label, const size_t &idx = 0) const 00319 { 00320 return T(getObservationBySensorLabel(label,idx)); 00321 } 00322 00323 /** Efficiently swaps the contents of two objects. 00324 */ 00325 void swap( CSensoryFrame &sf ); 00326 00327 protected: 00328 /** The set of observations taken at the same time instant. See the top of this page for instructions on accessing this. 00329 */ 00330 //std::deque<CObservation*> m_observations; 00331 std::deque<CObservationPtr> m_observations; 00332 00333 }; // End of class def. 00334 00335 00336 } // End of namespace 00337 } // End of namespace 00338 00339 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011 |