39 lines
1.3 KiB
C++
39 lines
1.3 KiB
C++
|
/// @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
|