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 = T_b_rel_a.log();
  }
  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);
  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_ */