interpolators.hpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Ifdefs
6 *****************************************************************************/
7 
8 #ifndef yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_
9 #define yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_
10 
11 /*****************************************************************************
12 ** Includes
13 *****************************************************************************/
14 
15 #include <ecl/exceptions/standard_exception.hpp>
16 #include <ecl/linear_algebra.hpp>
17 #include <iomanip>
18 #include <iostream>
19 #include <sophus/se3.hpp>
20 #include <sophus/se2.hpp>
21 #include <sophus/so2.hpp>
22 #include <sophus/so3.hpp>
23 #include <string>
24 
25 #include "formatters.hpp"
26 
27 /*****************************************************************************
28 ** Namespace
29 *****************************************************************************/
30 
31 namespace Sophus {
32 
33 /*****************************************************************************
34 ** Interfaces
35 *****************************************************************************/
36 
37 
52 template <typename Group>
53 class Interpolator {
54 public:
55  Interpolator(const Group& T_a, const Group& T_b)
56  : T_a(T_a)
57  {
58  Group T_b_rel_a = T_b*T_a.inverse();
59  tangent = Group::log(T_b_rel_a);
60  }
61  Group operator()(const double& t) {
62  return Group::exp(t*tangent)*T_a;
63  }
64 private:
65  Group T_a;
66  typename Group::Tangent tangent;
67 };
68 
78 class PlanarInterpolator {
79 public:
80  PlanarInterpolator(const Sophus::SE3f& T_a, const Sophus::SE3f& T_b) throw(ecl::StandardException);
81  Sophus::SE3f operator()(const double& t);
82 
83 private:
84  Sophus::SE3f T_a;
85  Sophus::SE2f t_a;
86  Sophus::SE2f::Tangent tangent;
87 };
88 
97 class SlidingInterpolator {
98 public:
99  SlidingInterpolator(const Sophus::SE3f& T_a, const Sophus::SE3f& T_b);
100  Sophus::SE3f operator()(const double& t);
101 
102 private:
103  Interpolator<Sophus::SE3f> interpolator;
104  Sophus::SE3f T_a, T_b;
105  Sophus::SE3f::Tangent tangent;
106 };
107 
108 
109 } // namespace Sophus
110 
111 #endif /* yocs_math_toolkit_SOPHUS_INTERPOLATERS_HPP_ */
Standard exception type, provides code location and error string.
Group::Tangent tangent
Interpolator(const Group &T_a, const Group &T_b)
Group operator()(const double &t)


xbot_driver
Author(s): Roc, wangpeng@droid.ac.cn
autogenerated on Sat Oct 10 2020 03:27:37