Main MRPT website > C++ reference for MRPT 1.5.3
interpolate.hpp
Go to the documentation of this file.
1 #ifndef SOPHUS_INTERPOLATE_HPP
2 #define SOPHUS_INTERPOLATE_HPP
3 
4 #include <Eigen/Eigenvalues>
5 
7 
8 namespace Sophus {
9 
10 // This function interpolates between two Lie group elements ``foo_T_bar``
11 // and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1].
12 //
13 // It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar``
14 // and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns
15 // ``foo_T_bar``.
16 //
17 // (Since interpolation on Lie groups is inverse-invariant, we can equivalently
18 // think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the
19 // return value being ``quiz_T_foo``.)
20 //
21 // Precondition: ``p`` must be in [0, 1].
22 //
23 template <class G, class Scalar2>
25  G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) {
26  using Scalar = typename G::Scalar;
27  Scalar inter_p(p);
28  SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1),
29  "p (%) must in [0, 1].");
30  return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());
31 }
32 
33 } // namespace Sophus
34 
35 #endif // SOPHUS_INTERPOLATE_HPP
#define SOPHUS_ENSURE(expr, description,...)
Definition: common.hpp:129
enable_if_t< interp_details::Traits< G >::supported, G > interpolate(G const &foo_T_bar, G const &foo_T_baz, Scalar2 p=Scalar2(0.5f))
Definition: interpolate.hpp:24
typename std::enable_if< B, T >::type enable_if_t
Definition: common.hpp:201



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Mon Oct 30 10:27:08 UTC 2017