diff --git a/src/three_D_tools.h b/src/three_D_tools.h index 44512070ff18026d6172d0841e3ac1ee962402b0..8ed81869500d4fbcab5c0091e78f972eb1e2a85b 100644 --- a/src/three_D_tools.h +++ b/src/three_D_tools.h @@ -31,6 +31,7 @@ * - exp_SE3: go from tangent space to delta manifold (equivalent to exp() in rotations) * - plus: D2 = D1 * exp_SE3(d) * - minus: d = log_SE3( D1.inv() * D2 ) + * - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t) */ @@ -375,6 +376,22 @@ inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1, return ret; } +template<typename D1, typename D2, typename D3> +inline void interpolate(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + const typename D1::Scalar t, + MatrixBase<D3>& sum) +{ + Matrix<typename D1::Scalar, 6, 1> dd; + Matrix<typename D1::Scalar, 7, 1> ret; + + minus(d1, d2, dd); + plus(d1, dd * t, ret); + + return ret; +} + + } // namespace three_d } // namespace wolf