lio-cpp/lio/include/sophus/interpolate.hpp

/// @file
/// Interpolation for Lie groups.

#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 = typename G::Scalar>
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