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/CPointsMap.h> 00034 #include <mrpt/slam/CObservation2DRangeScan.h> 00035 00036 00037 namespace mrpt 00038 { 00039 namespace slam 00040 { 00041 class CSimplePointsMap; 00042 00043 // This must be added to any CSerializable derived class: 00044 DEFINE_SERIALIZABLE_PRE( CSensoryFrame ) 00045 00046 /** Declares a class for storing a "sensory frame", thus 00047 * a set of "observations" of the robot, taken exactly 00048 * from the same location.The "observations" can be of 00049 * many different kinds.<br> 00050 * New observations can be added using:<br> 00051 * \code 00052 CObservationXXXPtr o = CObservationXXX::Create(); // Create a smart pointer containing an object of class "CObservationXXX" 00053 o->(...) 00054 00055 CSensoryFrame sf; 00056 sf.insert(o); 00057 \endcode 00058 00059 The following methods are equivalent for adding new observations to a "sensory frame": 00060 * CSensoryFrame::operator += 00061 * CSensoryFrame::push_back 00062 * CSensoryFrame::insert 00063 00064 To examine the objects within a sensory frame, the following methods exist: 00065 * CSensoryFrame::getObservationByClass : Looks for some specific observation class. 00066 * CSensoryFrame::begin : To iterate over all observations. 00067 * CSensoryFrame::getObservationByIndex : To query by index. 00068 00069 Notice that observations objects are automatically deleted on 00070 Sensorial Frame destruction or clear. 00071 * \sa CObservation 00072 */ 00073 class MRPTDLLIMPEXP CSensoryFrame : public mrpt::utils::CSerializable 00074 { 00075 // This must be added to any CSerializable derived class: 00076 DEFINE_SERIALIZABLE( CSensoryFrame ) 00077 // ------------------------------------------------------------------------------- 00078 // HACK: For compatibility with old datasets (See CSensoryFrame.cpp) 00079 // ------------------------------------------------------------------------------- 00080 static mrpt::utils::CLASSINIT _init_CSensorialFrame; 00081 static mrpt::utils::TRuntimeClassId classCSensorialFrame; 00082 00083 private: 00084 /** Auxiliary points map, built only under request through buildAuxPointsMap */ 00085 mutable CObservation2DRangeScan::CAuxMapWrapper m_auxMap; 00086 00087 public: 00088 /** Default constructor 00089 */ 00090 CSensoryFrame(); 00091 00092 /** Copy constructor 00093 */ 00094 CSensoryFrame( const CSensoryFrame &); 00095 00096 /** Build (only on the first call), and return a simple points map built from the observations in the SF (options can be optionally provided to the map building process) 00097 */ 00098 const CPointsMap *buildAuxPointsMap( CPointsMap::TInsertionOptions *ops = NULL ) const; 00099 00100 /** Copy 00101 */ 00102 CSensoryFrame& operator =( const CSensoryFrame &o); 00103 00104 /** Destructor. 00105 */ 00106 virtual ~CSensoryFrame(); 00107 00108 /** Clear all current observations. 00109 */ 00110 void clear(); 00111 00112 /** Dump all observations in a metric map. It calls CObservation::insertObservationInto for all stored observation. 00113 * \param theMap The map where this observation is to be inserted: the map will be updated. 00114 * \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) 00115 * 00116 * \return Returns true if the map has been updated, or false if this observations 00117 * has nothing to do with a metric map (for example, a sound observation). 00118 * 00119 * \sa CObservation::insertObservationInto, CMetricMap::insertObservation 00120 */ 00121 bool insertObservationsInto( CMetricMap *theMap, const CPose3D *robotPose = NULL ) const; 00122 00123 /** Dump all observations in a metric map. It calls CObservation::insertObservationInto for all stored observation. 00124 * \param theMap The map where this observation is to be inserted: the map will be updated. 00125 * \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) 00126 * 00127 * \return Returns true if the map has been updated, or false if this observations 00128 * has nothing to do with a metric map (for example, a sound observation). 00129 * 00130 * \sa CObservation::insertObservationInto, CMetricMap::insertObservation 00131 */ 00132 bool insertObservationsInto( CMetricMapPtr &theMap, const CPose3D *robotPose = NULL ) const 00133 { 00134 return insertObservationsInto(theMap.pointer(),robotPose); 00135 } 00136 00137 /** Returns the average of "likelihoodWith" evaluated between all the observations of this and another sensoryframe. 00138 * 00139 * \param anotherSF The other observations to compute likelihood with. 00140 * \param anotherSFPose If known, the belief about the relative robot pose when the other observations were taken can be supplied here, or NULL if it is unknown. 00141 * 00142 * \return Returns a likelihood measurement, in the range [0,1]. 00143 * \exception std::exception On any error, as another observation being of an invalid class. 00144 */ 00145 float likelihoodWith( const CSensoryFrame *anotherSF, const CPosePDF *anotherSFPose = NULL ) const; 00146 00147 /** 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. 00148 * \sa moveFrom 00149 */ 00150 void operator += (const CSensoryFrame &sf); 00151 00152 /** 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. 00153 * \sa moveFrom 00154 */ 00155 void operator += (const CObservationPtr &obs); 00156 00157 /** 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. 00158 * \sa operator += 00159 */ 00160 void moveFrom(CSensoryFrame &sf); 00161 00162 /** 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. 00163 */ 00164 void push_back(const CObservationPtr &obs); 00165 00166 /** 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. 00167 */ 00168 void insert(const CObservationPtr &obs); 00169 00170 /** 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. 00171 * Example: 00172 * \code 00173 CObservationImagePtr obs = m_SF->getObservationByClass<CObservationImage>(); 00174 * \endcode 00175 * By default (ith=0), the first observation is returned. 00176 */ 00177 template <typename T> 00178 typename T::SmartPtr getObservationByClass( const size_t &ith = 0 ) const 00179 { 00180 MRPT_START; 00181 size_t foundCount = 0; 00182 const mrpt::utils::TRuntimeClassId* class_ID = T::classinfo; 00183 for (const_iterator it = begin();it!=end();++it) 00184 if ( (*it)->GetRuntimeClass()->derivedFrom( class_ID ) ) 00185 if (foundCount++ == ith) 00186 return typename T::SmartPtr(*it); 00187 return typename T::SmartPtr(); // Not found: return empty smart pointer 00188 MRPT_END; 00189 } 00190 00191 /** You can use CSensoryFrame::begin to get a iterator to the first element. 00192 */ 00193 typedef std::deque<CObservationPtr>::iterator iterator; 00194 00195 /** You can use CSensoryFrame::begin to get a iterator to the first element. 00196 */ 00197 typedef std::deque<CObservationPtr>::const_iterator const_iterator; 00198 00199 /** Returns a iterator to the first observation: this is an example of usage: 00200 * \code 00201 * CSensoryFrame sf; 00202 * ... 00203 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00204 * { 00205 * (*it)->... // (*it) is a "CObservation*" 00206 * } 00207 * 00208 * \endcode 00209 */ 00210 const_iterator begin() const { return m_observations.begin(); } 00211 00212 /** Returns a iterator to the end of the list of observations: this is an example of usage: 00213 * \code 00214 * CSensoryFrame sf; 00215 * ... 00216 * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it) 00217 * { 00218 * (*it)->... // (*it) is a "CObservation*" 00219 * } 00220 * 00221 * \endcode 00222 */ 00223 const_iterator end() const { return m_observations.end(); } 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 iterator begin() { 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 iterator end() { return m_observations.end(); } 00250 00251 00252 /** Returns the number of observations in the list. 00253 */ 00254 size_t size() const; 00255 00256 /** Removes the i'th observation in the list (0=first). 00257 */ 00258 void eraseByIndex(const size_t &idx); 00259 00260 /** Removes the given observation in the list, and return an iterator to the next element (or this->end() if it was the last one). 00261 */ 00262 iterator erase( const iterator &it); 00263 00264 /** Removes all the observations that match a given sensorLabel. 00265 */ 00266 void eraseByLabel(const std::string &label); 00267 00268 /** Returns the i'th observation in the list (0=first). 00269 * \sa begin, size 00270 */ 00271 CObservationPtr getObservationByIndex( const size_t &idx ) const; 00272 00273 /** Returns the i'th observation in the list (0=first), and as a different smart pointer type: 00274 * \code 00275 * sf.getObservationByIndexAs<CObservationStereoImagesPtr>(i); 00276 * \endcode 00277 * \sa begin, size 00278 */ 00279 template <typename T> 00280 T getObservationByIndexAs( const size_t &idx ) const 00281 { 00282 return static_cast<T>(getObservationByIndex(idx)); 00283 } 00284 00285 /** Returns the i'th observation in the list with the given "sensorLabel" (0=first). 00286 * \return The observation, or NULL if not found. 00287 * \sa begin, size 00288 */ 00289 CObservationPtr getObservationBySensorLabel( const std::string &label, const size_t &idx = 0) const; 00290 00291 /** Returns the i'th observation in the list with the given "sensorLabel" (0=first), and as a different smart pointer type: 00292 * \code 00293 * sf.getObservationBySensorLabelAs<CObservationStereoImagesPtr>(i); 00294 * \endcode 00295 * \sa begin, size 00296 */ 00297 template <typename T> 00298 T getObservationBySensorLabelAs( const std::string &label, const size_t &idx = 0) const 00299 { 00300 return T(getObservationBySensorLabel(label,idx)); 00301 } 00302 00303 /** Efficiently swaps the contents of two objects. 00304 */ 00305 void swap( CSensoryFrame &sf ); 00306 00307 protected: 00308 /** The set of observations taken at the same time instant. See the top of this page for instructions on accessing this. 00309 */ 00310 //std::deque<CObservation*> m_observations; 00311 std::deque<CObservationPtr> m_observations; 00312 00313 }; // End of class def. 00314 00315 00316 } // End of namespace 00317 } // End of namespace 00318 00319 #endif
| Page generated by Doxygen 1.6.2 for MRPT 0.8.1 SVN:exported at Mon Feb 15 22:01:07 UTC 2010 |