CPoseInterpolatorBase.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/Clock.h>
#include <mrpt/poses/Lie/Euclidean.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/poses/poses_frwds.h>
#include <mrpt/typemeta/TEnumType.h>

namespace mrpt::poses
{
/** Type to select the interpolation method in CPoseInterpolatorBase derived
 * classes.
 *  - imSpline: Spline interpolation using 4 points (2 before + 2 after the
 * query point).
 *  - imLinear2Neig: Linear interpolation between the previous and next
 * neightbour.
 *  - imLinear4Neig: Linear interpolation using the linear fit of the 4 closer
 * points (2 before + 2 after the query point).
 *  - imSSLLLL : Use Spline for X and Y, and Linear Least squares for Z, yaw,
 * pitch and roll.
 *  - imSSLSLL : Use Spline for X, Y and yaw, and Linear Lesat squares for Z,
 * pitch and roll.
 *  - imLinearSlerp: Linear for X,Y,Z, Slerp for 3D angles.
 *  - imSplineSlerp: Spline for X,Y,Z, Slerp for 3D angles.
 * \ingroup interpolation_grp poses_grp
 */
enum TInterpolatorMethod
{
    imSpline = 0,
    imLinear2Neig,
    imLinear4Neig,
    imSSLLLL,
    imSSLSLL,
    imLinearSlerp,
    imSplineSlerp
};

/** Base class for SE(2)/SE(3) interpolators. See docs for derived classes.
 * \ingroup interpolation_grp poses_grp
 */
template <int DIM>
class CPoseInterpolatorBase
{
   public:
    /** Default ctor: empty sequence of poses */
    CPoseInterpolatorBase();

    /** @name Type definitions and STL-like container interface
     * @{ */

    /** TPose2D or TPose3D */
    using pose_t = typename Lie::SE<DIM>::light_type;
    /** CPose2D or CPose3D */
    using cpose_t = typename Lie::SE<DIM>::type;
    /** TPoint2D or TPoint3D */
    using point_t = typename Lie::Euclidean<DIM>::light_type;

    using TTimePosePair = std::pair<mrpt::Clock::time_point, pose_t>;
    using TPath = std::map<mrpt::Clock::time_point, pose_t>;
    using iterator = typename TPath::iterator;
    using const_iterator = typename TPath::const_iterator;
    using reverse_iterator = typename TPath::reverse_iterator;
    using const_reverse_iterator = typename TPath::const_reverse_iterator;

    inline iterator begin() { return m_path.begin(); }
    inline const_iterator begin() const { return m_path.begin(); }
    inline const_iterator cbegin() const { return m_path.cbegin(); }
    inline iterator end() { return m_path.end(); }
    inline const_iterator end() const { return m_path.end(); }
    inline const_iterator cend() const { return m_path.cend(); }
    inline reverse_iterator rbegin() { return m_path.rbegin(); }
    inline const_reverse_iterator rbegin() const { return m_path.rbegin(); }
    inline reverse_iterator rend() { return m_path.rend(); }
    inline const_reverse_iterator rend() const { return m_path.rend(); }
    iterator lower_bound(const mrpt::Clock::time_point& t)
    {
        return m_path.lower_bound(t);
    }
    const_iterator lower_bound(const mrpt::Clock::time_point& t) const
    {
        return m_path.lower_bound(t);
    }

    iterator upper_bound(const mrpt::Clock::time_point& t)
    {
        return m_path.upper_bound(t);
    }
    const_iterator upper_bound(const mrpt::Clock::time_point& t) const
    {
        return m_path.upper_bound(t);
    }

    iterator erase(iterator element_to_erase)
    {
        m_path.erase(element_to_erase++);
        return element_to_erase;
    }

    size_t size() const { return m_path.size(); }
    bool empty() const { return m_path.empty(); }
    iterator find(const mrpt::Clock::time_point& t) { return m_path.find(t); }
    const_iterator find(const mrpt::Clock::time_point& t) const
    {
        return m_path.find(t);
    }
    pose_t& at(const mrpt::Clock::time_point& t) { return m_path.at(t); }
    const pose_t& at(const mrpt::Clock::time_point& t) const
    {
        return m_path.at(t);
    }
    /** @} */

    /** Inserts a new pose in the sequence.
     *  It overwrites any previously existing pose at exactly the same time.
     */
    void insert(const mrpt::Clock::time_point& t, const pose_t& p);
    /** Overload (slower) */
    void insert(const mrpt::Clock::time_point& t, const cpose_t& p);

    /** Returns the pose at a given time, or interpolates using splines if there
     * is not an exact match.
     * \param t The time of the point to interpolate.
     * \param out_interp The output interpolated pose.
     * \param out_valid_interp Whether there was information enough to compute
     * the interpolation.
     * \return A reference to out_interp
     */
    pose_t& interpolate(
        const mrpt::Clock::time_point& t, pose_t& out_interp,
        bool& out_valid_interp) const;
    /** \overload (slower) */
    cpose_t& interpolate(
        const mrpt::Clock::time_point& t, cpose_t& out_interp,
        bool& out_valid_interp) const;

    /** Clears the current sequence of poses */
    void clear();

    /** Set value of the maximum time to consider interpolation.
     * If set to a negative value, the check is disabled (default behavior). */
    void setMaxTimeInterpolation(const mrpt::Clock::duration& time);
    /** Set value of the maximum time to consider interpolation */
    mrpt::Clock::duration getMaxTimeInterpolation();

    /** Get the previous CPose3D in the map with a minimum defined distance.
     * \return true if pose was found, false otherwise */
    bool getPreviousPoseWithMinDistance(
        const mrpt::Clock::time_point& t, double distance, pose_t& out_pose);
    /** \overload (slower) */
    bool getPreviousPoseWithMinDistance(
        const mrpt::Clock::time_point& t, double distance, cpose_t& out_pose);

    /** Saves the points in the interpolator to a text file, with this format:
     *  Each row contains these elements separated by spaces:
     *  - Timestamp: As a "double time_t" (see mrpt::system::timestampTotime_t).
     *  - x y z: The 3D position in meters.
     *  - yaw pitch roll: The angles, in radians
     * \sa loadFromTextFile
     * \return true on success, false on any error.
     */
    bool saveToTextFile(const std::string& s) const;

    /** 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.
     * \sa loadFromTextFile
     * \return true on success, false on any error.
     */
    bool saveInterpolatedToTextFile(
        const std::string& s, const mrpt::Clock::duration& period) const;

    /** Loads from a text file, in the format described by saveToTextFile.
     * \return true on success, false on any error.
     * \exception std::exception On invalid file format
     */
    bool loadFromTextFile(const std::string& s);

    /** Computes the bounding box in all Euclidean coordinates of the whole
     * path. \exception std::exception On empty path */
    void getBoundingBox(point_t& minCorner, point_t& maxCorner) const;

    /** Change the method used to interpolate the robot path. The default method
     * at construction is "imSpline". \sa getInterpolationMethod() */
    void setInterpolationMethod(TInterpolatorMethod method);
    /** Returns the currently set interpolation method.  \sa
     * setInterpolationMethod() */
    TInterpolatorMethod getInterpolationMethod() const;

    /** Filters by averaging one of the components of the pose data within the
     * interpolator. The width of the filter is set by the number of samples.
     * \param component [IN]    The index of the component to filter: 0 (x),
     * 1 (y), 2 (z), 3 (yaw), 4 (pitch) or 5 (roll)
     * \param samples       [IN]    The width of the average filter.
     */
    void filter(unsigned int component, unsigned int samples);

   protected:
    /** The sequence of poses */
    TPath m_path;
    /** 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. */
    mrpt::Clock::duration maxTimeInterpolation;
    TInterpolatorMethod m_method{mrpt::poses::imLinearSlerp};

    void impl_interpolation(
        const TTimePosePair& p1, const TTimePosePair& p2,
        const TTimePosePair& p3, const TTimePosePair& p4,
        const TInterpolatorMethod method, const mrpt::Clock::time_point& td,
        pose_t& out_interp) const;

};  // End of class def.
}  // namespace mrpt::poses
MRPT_ENUM_TYPE_BEGIN(mrpt::poses::TInterpolatorMethod)
MRPT_FILL_ENUM_MEMBER(mrpt::poses, imSpline);
MRPT_FILL_ENUM_MEMBER(mrpt::poses, imLinear2Neig);
MRPT_FILL_ENUM_MEMBER(mrpt::poses, imLinear4Neig);
MRPT_FILL_ENUM_MEMBER(mrpt::poses, imSSLLLL);
MRPT_FILL_ENUM_MEMBER(mrpt::poses, imLinearSlerp);
MRPT_FILL_ENUM_MEMBER(mrpt::poses, imSplineSlerp);
MRPT_ENUM_TYPE_END()