00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00044 DEFINE_SERIALIZABLE_PRE( CSensoryFrame )
00045
00046
00071 class MRPTDLLIMPEXP CSensoryFrame : public mrpt::utils::CSerializable
00072 {
00073
00074 DEFINE_SERIALIZABLE( CSensoryFrame )
00075
00076
00077
00078 static mrpt::utils::CLASSINIT _init_CSensorialFrame;
00079 static mrpt::utils::TRuntimeClassId classCSensorialFrame;
00080
00081 private:
00083 mutable CObservation2DRangeScan::CAuxMapWrapper m_auxMap;
00084
00085 public:
00088 CSensoryFrame();
00089
00092 CSensoryFrame( const CSensoryFrame &);
00093
00096 const CPointsMap *buildAuxPointsMap( CPointsMap::TInsertionOptions *ops = NULL ) const;
00097
00100 CSensoryFrame& operator =( const CSensoryFrame &o);
00101
00104 virtual ~CSensoryFrame();
00105
00108 void clear();
00109
00112 void clearWithoutDelete();
00113
00123 bool insertObservationsInto( CMetricMap *theMap, const CPose3D *robotPose = NULL ) const;
00124
00134 bool insertObservationsInto( CMetricMapPtr &theMap, const CPose3D *robotPose = NULL ) const
00135 {
00136 return insertObservationsInto(theMap.pointer(),robotPose);
00137 }
00138
00147 float likelihoodWith( const CSensoryFrame *anotherSF, const CPosePDF *anotherSFPose = NULL ) const;
00148
00152 void operator += (const CSensoryFrame &sf);
00153
00157 void operator += (const CObservationPtr &obs);
00158
00162 void moveFrom(CSensoryFrame &sf);
00163
00166 void push_back(const CObservationPtr &obs);
00167
00170 void insert(const CObservationPtr &obs);
00171
00179 template <typename T>
00180 typename T::SmartPtr getObservationByClass( const size_t &ith = 0 ) const
00181 {
00182 MRPT_TRY_START;
00183 size_t foundCount = 0;
00184 const mrpt::utils::TRuntimeClassId* class_ID = T::classinfo;
00185 for (const_iterator it = begin();it!=end();it++)
00186 if ( (*it)->GetRuntimeClass()->derivedFrom( class_ID ) )
00187 if (foundCount++ == ith)
00188 return typename T::SmartPtr(*it);
00189 return typename T::SmartPtr();
00190 MRPT_TRY_END;
00191 }
00192
00195 typedef std::deque<CObservationPtr>::iterator iterator;
00196
00199 typedef std::deque<CObservationPtr>::const_iterator const_iterator;
00200
00212 const_iterator begin() const { return m_observations.begin(); }
00213
00225 const_iterator end() const { return m_observations.end(); }
00226
00238 iterator begin() { return m_observations.begin(); }
00239
00251 iterator end() { return m_observations.end(); }
00252
00253
00256 size_t size() const;
00257
00260 void eraseByIndex(const size_t &idx);
00261
00264 iterator erase( const iterator &it);
00265
00268 void eraseByLabel(const std::string &label);
00269
00273 CObservationPtr getObservationByIndex( const size_t &idx ) const;
00274
00281 template <typename T>
00282 T getObservationByIndexAs( const size_t &idx ) const
00283 {
00284 return static_cast<T>(getObservationByIndex(idx));
00285 }
00286
00291 CObservationPtr getObservationBySensorLabel( const std::string &label, const size_t &idx = 0) const;
00292
00299 template <typename T>
00300 T getObservationBySensorLabelAs( const std::string &label, const size_t &idx = 0) const
00301 {
00302 return T(getObservationBySensorLabel(label,idx));
00303 }
00304
00307 void swap( CSensoryFrame &sf );
00308
00309 protected:
00312
00313 std::deque<CObservationPtr> m_observations;
00314
00315 };
00316
00317
00318 }
00319 }
00320
00321 #endif