Main MRPT website > C++ reference
MRPT logo

CPose3DInterpolator.h

Go to the documentation of this file.
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