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_ */