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