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