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