interpolators.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_
#define yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ecl/exceptions/standard_exception.hpp>
#include <ecl/linear_algebra.hpp>
#include <iomanip>
#include <iostream>
#include <sophus/se3.hpp>
#include <sophus/se2.hpp>
#include <sophus/so2.hpp>
#include <sophus/so3.hpp>
#include <string>

#include "formatters.hpp"

/*****************************************************************************
** Namespace
*****************************************************************************/

namespace Sophus {

/*****************************************************************************
** Interfaces
*****************************************************************************/


template <typename Group>
class Interpolator {
public:
  Interpolator(const Group& T_a, const Group& T_b)
  : T_a(T_a)
  {
    Group T_b_rel_a = T_b*T_a.inverse();
    tangent = Group::log(T_b_rel_a);
  }
  Group operator()(const double& t) {
    return Group::exp(t*tangent)*T_a;
  }
private:
  Group T_a;
  typename Group::Tangent tangent;
};

class PlanarInterpolator {
public:
  PlanarInterpolator(const Sophus::SE3f& T_a, const Sophus::SE3f& T_b) throw(ecl::StandardException);
  Sophus::SE3f operator()(const double& t);

private:
  Sophus::SE3f T_a;
  Sophus::SE2f t_a;
  Sophus::SE2f::Tangent tangent;
};

class SlidingInterpolator {
public:
  SlidingInterpolator(const Sophus::SE3f& T_a, const Sophus::SE3f& T_b);
  Sophus::SE3f operator()(const double& t);

private:
  Interpolator<Sophus::SE3f> interpolator;
  Sophus::SE3f T_a, T_b;
  Sophus::SE3f::Tangent tangent;
};


} // namespace Sophus

#endif /* yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_ */