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 
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 = T_b_rel_a.log();
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 
79 public:
80  PlanarInterpolator(const Sophus::SE3f& T_a, const Sophus::SE3f& T_b);
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 
98 public:
99  SlidingInterpolator(const Sophus::SE3f& T_a, const Sophus::SE3f& T_b);
100  Sophus::SE3f operator()(const double& t);
101 
102 private:
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_ */
Interpolator< Sophus::SE3f > interpolator
Mathematical tools for linear algebra.
Interpolate between two lie group objects.
Group::Tangent tangent
Interpolator(const Group &T_a, const Group &T_b)
Group operator()(const double &t)
Sophus::SE3f::Tangent tangent
Sophus::SE2f::Tangent tangent


ecl_linear_algebra
Author(s): Daniel Stonier
autogenerated on Mon Feb 28 2022 22:18:38