interpolate.hpp

Go to the documentation of this file.

 
#ifndef SOPHUS_INTERPOLATE_HPP
#define SOPHUS_INTERPOLATE_HPP
 
#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].");
  return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());
}
 
}  // namespace Sophus
 
#endif  // SOPHUS_INTERPOLATE_HPP