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