.. _program_listing_file__tmp_ws_src_ecl_core_ecl_linear_algebra_include_ecl_linear_algebra_sophus_interpolators.hpp:

Program Listing for File interpolators.hpp
==========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_ecl_core_ecl_linear_algebra_include_ecl_linear_algebra_sophus_interpolators.hpp>` (``/tmp/ws/src/ecl_core/ecl_linear_algebra/include/ecl/linear_algebra/sophus/interpolators.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

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