interpolate.hpp

#ifndef SOPHUS_INTERPOLATE_HPP
#define SOPHUS_INTERPOLATE_HPP

#include <Eigen/Eigenvalues>

#include "interpolate_details.hpp"

namespace Sophus {

// This function interpolates between two Lie group elements ``foo_T_bar``
// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1].
//
// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar``
// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns
// ``foo_T_bar``.
//
// (Since interpolation on Lie groups is inverse-invariant, we can equivalently
// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the
// return value being ``quiz_T_foo``.)
//
// Precondition: ``p`` must be in [0, 1].
//
template <class G, class Scalar2>
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)) {
  using Scalar = typename G::Scalar;
  Scalar inter_p(p);
  SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1),
                "p (%) must in [0, 1].");
  return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());
}

}  // namespace Sophus

#endif  // SOPHUS_INTERPOLATE_HPP