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 CPose3DInterpolator_H
00029 #define CPose3DInterpolator_H
00030
00031 #include <mrpt/poses/CPose.h>
00032 #include <mrpt/poses/CPoint3D.h>
00033 #include <mrpt/system/os.h>
00034 #include <mrpt/utils/stl_extensions.h>
00035
00036 namespace mrpt
00037 {
00038 namespace poses
00039 {
00040 DEFINE_SERIALIZABLE_PRE( CPose3DInterpolator )
00041
00042 typedef std::pair<mrpt::system::TTimeStamp, mrpt::poses::CPose3D> TTimePosePair;
00043
00068 class MRPTDLLIMPEXP CPose3DInterpolator : public mrpt::utils::CSerializable
00069 {
00070
00071 DEFINE_SERIALIZABLE( CPose3DInterpolator )
00072
00073 private:
00074 typedef mrpt::utils::map_serializable< mrpt::system::TTimeStamp, CPose3D > TPath;
00075 TPath m_path;
00076
00077 public:
00078 typedef TPath::iterator iterator;
00079 typedef TPath::const_iterator const_iterator;
00080
00087 enum TInterpolatorMethod
00088 {
00089 imSpline = 0,
00090 imLinear2Neig,
00091 imLinear4Neig,
00092 imSSLLLL
00093 };
00094
00095 iterator begin() { return m_path.begin(); }
00096 const_iterator begin() const { return m_path.begin(); }
00097
00098 iterator end() { return m_path.end(); }
00099 const_iterator end() const { return m_path.end(); }
00100
00101 iterator lower_bound( const mrpt::system::TTimeStamp & t) { return m_path.lower_bound(t); }
00102 const_iterator lower_bound( const mrpt::system::TTimeStamp & t) const { return m_path.lower_bound(t); }
00103
00104 iterator upper_bound( const mrpt::system::TTimeStamp & t) { return m_path.upper_bound(t); }
00105 const_iterator upper_bound( const mrpt::system::TTimeStamp & t) const { return m_path.upper_bound(t); }
00106
00107 iterator erase(iterator element_to_erase) { m_path.erase(element_to_erase++); return element_to_erase; }
00108
00109 size_t size() const { return m_path.size(); }
00110 bool empty() const { return m_path.empty(); }
00111
00114 CPose3DInterpolator();
00115
00119 void insert( mrpt::system::TTimeStamp t, const CPose3D &p);
00120
00127 CPose3D &interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const;
00128
00130 void clear();
00131
00135 void setMaxTimeInterpolation( double time );
00136
00138 double getMaxTimeInterpolation( );
00139
00143 bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose );
00144
00153 bool saveToTextFile(const std::string &s) const;
00154
00159 bool saveInterpolatedToTextFile(const std::string &s, double period) const;
00160
00165 bool loadFromTextFile(const std::string &s);
00166
00170 void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const;
00171
00172
00177 void setInterpolationMethod( TInterpolatorMethod method);
00178
00182 TInterpolatorMethod getInterpolationMethod() const;
00183
00184
00185 private:
00186 double maxTimeInterpolation;
00187
00188 TInterpolatorMethod m_method;
00189
00190 };
00191
00192 }
00193 }
00194
00195 #endif