Program Listing for File interpolate.hpp

↰ Return to documentation for file (sophus/interpolate.hpp)

#pragma once

#include <Eigen/Eigenvalues>

#include "interpolate_details.hpp"

namespace Sophus {

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].", SOPHUS_FMT_ARG(inter_p));
  return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());
}

}  // namespace Sophus