interpolators.hpp
Go to the documentation of this file.
00001 
00004 /*****************************************************************************
00005 ** Ifdefs
00006 *****************************************************************************/
00007 
00008 #ifndef yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_
00009 #define yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_
00010 
00011 /*****************************************************************************
00012 ** Includes
00013 *****************************************************************************/
00014 
00015 #include <ecl/exceptions/standard_exception.hpp>
00016 #include <ecl/linear_algebra.hpp>
00017 #include <iomanip>
00018 #include <iostream>
00019 #include <sophus/se3.hpp>
00020 #include <sophus/se2.hpp>
00021 #include <sophus/so2.hpp>
00022 #include <sophus/so3.hpp>
00023 #include <string>
00024 
00025 #include "formatters.hpp"
00026 
00027 /*****************************************************************************
00028 ** Namespace
00029 *****************************************************************************/
00030 
00031 namespace Sophus {
00032 
00033 /*****************************************************************************
00034 ** Interfaces
00035 *****************************************************************************/
00036 
00037 
00052 template <typename Group>
00053 class Interpolator {
00054 public:
00055   Interpolator(const Group& T_a, const Group& T_b)
00056   : T_a(T_a)
00057   {
00058     Group T_b_rel_a = T_b*T_a.inverse();
00059     tangent = Group::log(T_b_rel_a);
00060   }
00061   Group operator()(const double& t) {
00062     return Group::exp(t*tangent)*T_a;
00063   }
00064 private:
00065   Group T_a;
00066   typename Group::Tangent tangent;
00067 };
00068 
00078 class PlanarInterpolator {
00079 public:
00080   PlanarInterpolator(const Sophus::SE3f& T_a, const Sophus::SE3f& T_b) throw(ecl::StandardException);
00081   Sophus::SE3f operator()(const double& t);
00082 
00083 private:
00084   Sophus::SE3f T_a;
00085   Sophus::SE2f t_a;
00086   Sophus::SE2f::Tangent tangent;
00087 };
00088 
00097 class SlidingInterpolator {
00098 public:
00099   SlidingInterpolator(const Sophus::SE3f& T_a, const Sophus::SE3f& T_b);
00100   Sophus::SE3f operator()(const double& t);
00101 
00102 private:
00103   Interpolator<Sophus::SE3f> interpolator;
00104   Sophus::SE3f T_a, T_b;
00105   Sophus::SE3f::Tangent tangent;
00106 };
00107 
00108 
00109 } // namespace Sophus
00110 
00111 #endif /* yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_ */


ecl_linear_algebra
Author(s): Daniel Stonier
autogenerated on Thu Jun 6 2019 21:17:37