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