Program Listing for File interpolators.hpp
↰ Return to documentation for file (include/ecl/linear_algebra/sophus/interpolators.hpp
)
/*****************************************************************************
** Ifdefs
*****************************************************************************/
#ifndef yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_
#define yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_
/*****************************************************************************
** Includes
*****************************************************************************/
#include <ecl/exceptions/standard_exception.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_ */