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_CUSTOM_BASE( CPose3DInterpolator, mrpt::utils::CSerializable ) 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 BASE_IMPEXP 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 * - imSSLSLL : Use Spline for X, Y and yaw, and Linear Lesat squares for Z, pitch and roll. 00090 */ 00091 enum TInterpolatorMethod 00092 { 00093 imSpline = 0, 00094 imLinear2Neig, 00095 imLinear4Neig, 00096 imSSLLLL, 00097 imSSLSLL 00098 }; 00099 00100 inline iterator begin() { return m_path.begin(); } 00101 inline const_iterator begin() const { return m_path.begin(); } 00102 00103 inline iterator end() { return m_path.end(); } 00104 inline const_iterator end() const { return m_path.end(); } 00105 00106 inline reverse_iterator rbegin() { return m_path.rbegin(); } 00107 inline const_reverse_iterator rbegin() const { return m_path.rbegin(); } 00108 00109 inline reverse_iterator rend() { return m_path.rend(); } 00110 inline const_reverse_iterator rend() const { return m_path.rend(); } 00111 00112 iterator lower_bound( const mrpt::system::TTimeStamp & t) { return m_path.lower_bound(t); } 00113 const_iterator lower_bound( const mrpt::system::TTimeStamp & t) const { return m_path.lower_bound(t); } 00114 00115 iterator upper_bound( const mrpt::system::TTimeStamp & t) { return m_path.upper_bound(t); } 00116 const_iterator upper_bound( const mrpt::system::TTimeStamp & t) const { return m_path.upper_bound(t); } 00117 00118 iterator erase(iterator element_to_erase) { m_path.erase(element_to_erase++); return element_to_erase; } 00119 00120 size_t size() const { return m_path.size(); } 00121 bool empty() const { return m_path.empty(); } 00122 00123 /** Creates an empty interpolator (with no points). 00124 */ 00125 CPose3DInterpolator(); 00126 00127 /** Inserts a new pose in the sequence. 00128 * It overwrites any previously existing pose at exactly the same time. 00129 */ 00130 void insert( mrpt::system::TTimeStamp t, const CPose3D &p); 00131 00132 /** Returns the pose at a given time, or interpolates using splines if there is not an exact match. 00133 * \param t The time of the point to interpolate. 00134 * \param out_interp The output interpolated pose. 00135 * \param out_valid_interp Whether there was information enough to compute the interpolation. 00136 * \return A reference to out_interp 00137 */ 00138 CPose3D &interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const; 00139 00140 /** Clears the current sequence of poses */ 00141 void clear(); 00142 00143 /** Set value of the maximum time to consider interpolation. 00144 * If set to a negative value, the check is disabled (default behavior). 00145 */ 00146 void setMaxTimeInterpolation( double time ); 00147 00148 /** Set value of the maximum time to consider interpolation */ 00149 double getMaxTimeInterpolation( ); 00150 00151 /** Get the previous CPose3D in the map with a minimum defined distance 00152 * \return true if pose was found, false otherwise. 00153 */ 00154 bool getPreviousPoseWithMinDistance( const mrpt::system::TTimeStamp &t, double distance, CPose3D &out_pose ); 00155 00156 /** Saves the points in the interpolator to a text file, with this format: 00157 * Each row contains these elements separated by spaces: 00158 * - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t). 00159 * - x y z: The 3D position in meters. 00160 * - yaw pitch roll: The angles, in radians 00161 * \sa loadFromTextFile 00162 * \return true on success, false on any error. 00163 */ 00164 bool saveToTextFile(const std::string &s) const; 00165 00166 /** 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. 00167 * \sa loadFromTextFile 00168 * \return true on success, false on any error. 00169 */ 00170 bool saveInterpolatedToTextFile(const std::string &s, double period) const; 00171 00172 /** Loads from a text file, in the format described by saveToTextFile. 00173 * \return true on success, false on any error. 00174 * \exception std::exception On invalid file format 00175 */ 00176 bool loadFromTextFile(const std::string &s); 00177 00178 /** Computes the bounding box in X,Y,Z of the whole vehicle path. 00179 * \exception std::exception On empty path 00180 */ 00181 void getBoundingBox(CPoint3D &minCorner, CPoint3D &maxCorner) const; 00182 00183 /** Computes the bounding box in X,Y,Z of the whole vehicle path. 00184 * \exception std::exception On empty path 00185 */ 00186 void getBoundingBox(mrpt::math::TPoint3D &minCorner, mrpt::math::TPoint3D &maxCorner) const; 00187 00188 /** Change the method used to interpolate the robot path. 00189 * The default method at construction is "imSpline". 00190 * \sa getInterpolationMethod 00191 */ 00192 void setInterpolationMethod( TInterpolatorMethod method); 00193 00194 /** Returns the currently set interpolation method. 00195 * \sa setInterpolationMethod 00196 */ 00197 TInterpolatorMethod getInterpolationMethod() const; 00198 00199 /** Filters by averaging one of the components of the CPose3D data within the interpolator. The width of the filter is set by the number of samples. 00200 * \param component [IN] The index of the component to filter: 0 (x), 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll). 00201 * \param samples [IN] The width of the average filter. 00202 */ 00203 void filter( unsigned int component, unsigned int samples ); 00204 00205 00206 private: 00207 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. 00208 00209 TInterpolatorMethod m_method; 00210 00211 }; // End of class def. 00212 00213 } // End of namespace 00214 } // End of namespace 00215 00216 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
