00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2008 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 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 00063 class MRPTDLLIMPEXP CPose3DInterpolator : public mrpt::utils::CSerializable 00064 { 00065 // This must be added to any CSerializable derived class: 00066 DEFINE_SERIALIZABLE( CPose3DInterpolator ) 00067 00068 private: 00069 typedef mrpt::utils::map_serializable< mrpt::system::TTimeStamp, CPose3D > TPath; 00070 00071 TPath m_path; 00072 double maxTimeInterpolation; 00073 00074 public: 00075 typedef TPath::iterator iterator; 00076 typedef TPath::const_iterator const_iterator; 00077 00078 iterator begin() { return m_path.begin(); } 00079 const_iterator begin() const { return m_path.begin(); } 00080 00081 iterator end() { return m_path.end(); } 00082 const_iterator end() const { return m_path.end(); } 00083 00084 iterator lower_bound( const mrpt::system::TTimeStamp & t) { return m_path.lower_bound(t); } 00085 const_iterator lower_bound( const mrpt::system::TTimeStamp & t) const { return m_path.lower_bound(t); } 00086 00087 iterator upper_bound( const mrpt::system::TTimeStamp & t) { return m_path.upper_bound(t); } 00088 const_iterator upper_bound( const mrpt::system::TTimeStamp & t) const { return m_path.upper_bound(t); } 00089 00090 iterator erase(iterator element_to_erase) { m_path.erase(element_to_erase++); return element_to_erase; } 00091 00092 size_t size() const { return m_path.size(); } 00093 bool empty() const { return m_path.empty(); } 00094 00097 CPose3DInterpolator(); 00098 00102 void insert( mrpt::system::TTimeStamp t, const CPose3D &p); 00103 00110 CPose3D &interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const; 00111 00113 void clear(); 00114 00118 void setMaxTimeInterpolation( double time ); 00119 00121 double getMaxTimeInterpolation( ); 00122 00126 bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose ); 00127 00136 bool saveToTextFile(const std::string &s) const; 00137 00142 bool loadFromTextFile(const std::string &s); 00143 00147 void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const; 00148 00149 00150 }; // End of class def. 00151 00152 } // End of namespace 00153 } // End of namespace 00154 00155 #endif
Page generated by Doxygen 1.5.8 for MRPT 0.6.5 SVN:exported at Mon Jan 12 13:00:16 UTC 2009 |