1234567891011121314151617181920212223242526272829303132333435363738 |
- #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());
- }
- }
- #endif
|