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 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 00044 /** A trajectory in time and in 6D (CPose3D) that interpolates using splines the intervals between a set of given time-referenced poses. 00045 * To insert new points into the sequence, use the "insert" method, and for getting an interpolated point, use "interpolate" method. For example: 00046 \code 00047 CPose3DInterpolator path; 00048 00049 path.setInterpolationMethod( CPose3DInterpolator::imSpline ); 00050 00051 path.insert( t0, CPose3D(...) ); 00052 path.insert( t1, CPose3D(...) ); 00053 path.insert( t2, CPose3D(...) ); 00054 path.insert( t3, CPose3D(...) ); 00055 00056 CPose3D p; 00057 bool valid; 00058 00059 cout << "Pose at t: " << path.interpolate(t,p,valid) << endl; 00060 \endcode 00061 * 00062 * Time is represented with mrpt::system::TTimeStamp. See mrpt::system for methods and utilities to manage these time references. 00063 * 00064 * See TInterpolatorMethod for the list of interpolation methods. 00065 * 00066 * \sa CPoseOrPoint 00067 */ 00068 class MRPTDLLIMPEXP CPose3DInterpolator : public mrpt::utils::CSerializable 00069 { 00070 // This must be added to any CSerializable derived class: 00071 DEFINE_SERIALIZABLE( CPose3DInterpolator ) 00072 00073 private: 00074 typedef std::map< mrpt::system::TTimeStamp, CPose3D > TPath; 00075 TPath m_path; //!< The sequence of poses 00076 00077 public: 00078 typedef TPath::iterator iterator; 00079 typedef TPath::const_iterator const_iterator; 00080 00081 /** Type to select the interpolation method in CPose3DInterpolator::setInterpolationMethod 00082 * - imSpline: Spline interpolation using 4 points (2 before + 2 after the query point). 00083 * - imLinear2Neig: Linear interpolation between the previous and next neightbour. 00084 * - imLinear4Neig: Linear interpolation using the linear fit of the 4 closer points (2 before + 2 after the query point). 00085 * - imSSLLLL : Use Spline for X and Y, and Linear Least squares for Z, yaw, pitch and roll. 00086 */ 00087 enum TInterpolatorMethod 00088 { 00089 imSpline = 0, 00090 imLinear2Neig, 00091 imLinear4Neig, 00092 imSSLLLL, 00093 imSSLSLL 00094 }; 00095 00096 iterator begin() { return m_path.begin(); } 00097 const_iterator begin() const { return m_path.begin(); } 00098 00099 iterator end() { return m_path.end(); } 00100 const_iterator end() const { return m_path.end(); } 00101 00102 iterator lower_bound( const mrpt::system::TTimeStamp & t) { return m_path.lower_bound(t); } 00103 const_iterator lower_bound( const mrpt::system::TTimeStamp & t) const { return m_path.lower_bound(t); } 00104 00105 iterator upper_bound( const mrpt::system::TTimeStamp & t) { return m_path.upper_bound(t); } 00106 const_iterator upper_bound( const mrpt::system::TTimeStamp & t) const { return m_path.upper_bound(t); } 00107 00108 iterator erase(iterator element_to_erase) { m_path.erase(element_to_erase++); return element_to_erase; } 00109 00110 size_t size() const { return m_path.size(); } 00111 bool empty() const { return m_path.empty(); } 00112 00113 /** Creates an empty interpolator (with no points). 00114 */ 00115 CPose3DInterpolator(); 00116 00117 /** Inserts a new pose in the sequence. 00118 * It overwrites any previously existing pose at exactly the same time. 00119 */ 00120 void insert( mrpt::system::TTimeStamp t, const CPose3D &p); 00121 00122 /** Returns the pose at a given time, or interpolates using splines if there is not an exact match. 00123 * \param t The time of the point to interpolate. 00124 * \param out_interp The output interpolated pose. 00125 * \param out_valid_interp Whether there was information enough to compute the interpolation. 00126 * \return A reference to out_interp 00127 */ 00128 CPose3D &interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const; 00129 00130 /** Clears the current sequence of poses */ 00131 void clear(); 00132 00133 /** Set value of the maximum time to consider interpolation. 00134 * If set to a negative value, the check is disabled (default behavior). 00135 */ 00136 void setMaxTimeInterpolation( double time ); 00137 00138 /** Set value of the maximum time to consider interpolation */ 00139 double getMaxTimeInterpolation( ); 00140 00141 /** Get the previous CPose3D in the map with a minimum defined distance 00142 * \return true if pose was found, false otherwise. 00143 */ 00144 bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose ); 00145 00146 /** Saves the points in the interpolator to a text file, with this format: 00147 * Each row contains these elements separated by spaces: 00148 * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t). 00149 * - x y z: The 3D position in meters. 00150 * - yaw pitch roll: The angles, in radians 00151 * \sa loadFromTextFile 00152 * \return true on success, false on any error. 00153 */ 00154 bool saveToTextFile(const std::string &s) const; 00155 00156 /** Saves the points in the interpolator to a text file, with the same format that saveToTextFile, but interpolating the path with the given period in seconds. 00157 * \sa loadFromTextFile 00158 * \return true on success, false on any error. 00159 */ 00160 bool saveInterpolatedToTextFile(const std::string &s, double period) const; 00161 00162 /** Loads from a text file, in the format described by saveToTextFile. 00163 * \return true on success, false on any error. 00164 * \exception std::exception On invalid file format 00165 */ 00166 bool loadFromTextFile(const std::string &s); 00167 00168 /** Computes the bounding box in X,Y,Z of the whole vehicle path. 00169 * \exception std::exception On empty path 00170 */ 00171 void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const; 00172 00173 /** Computes the bounding box in X,Y,Z of the whole vehicle path. 00174 * \exception std::exception On empty path 00175 */ 00176 void getBoundingBox(mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const; 00177 00178 /** Change the method used to interpolate the robot path. 00179 * The default method at construction is "imSpline". 00180 * \sa getInterpolationMethod 00181 */ 00182 void setInterpolationMethod( TInterpolatorMethod method); 00183 00184 /** Returns the currently set interpolation method. 00185 * \sa setInterpolationMethod 00186 */ 00187 TInterpolatorMethod getInterpolationMethod() const; 00188 00189 00190 private: 00191 double maxTimeInterpolation; //!< Maximum time considered to interpolate. If the difference between the desired timestamp where to interpolate and the next timestamp stored in the map is bigger than this value, the interpolation will not be done. 00192 00193 TInterpolatorMethod m_method; 00194 00195 }; // End of class def. 00196 00197 } // End of namespace 00198 } // End of namespace 00199 00200 #endif
Page generated by Doxygen 1.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009 |