diff --git a/src/rotations.h b/src/rotations.h index 36b5515f3f8cfa5aeb858431535eea0467b01b12..72812607c46d73d7ac7c3fa018842c0bba74a943 100644 --- a/src/rotations.h +++ b/src/rotations.h @@ -472,18 +472,43 @@ inline void between(const Eigen::QuaternionBase<D1>& _q1, } template<typename D1, typename D2> -inline Eigen::Quaternion<typename D1::Scalar> plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v) +inline Eigen::Quaternion<typename D1::Scalar> plus_right(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v) { MatrixSizeCheck<3,1>::check(v); return q * exp_q(v); } template<typename D1, typename D2> -inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) +inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus_right(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) { return log_q(q1.conjugate() * q2); } +template<typename D1, typename D2> +inline Eigen::Quaternion<typename D1::Scalar> plus_left(const Eigen::MatrixBase<D2>& v, const Eigen::QuaternionBase<D1>& q) +{ + MatrixSizeCheck<3,1>::check(v); + return exp_q(v) * q; +} + +template<typename D1, typename D2> +inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus_left(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) +{ + return log_q(q2 * q1.conjugate()); +} + +template<typename D1, typename D2> +inline Eigen::Quaternion<typename D1::Scalar> plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v) +{ + return plus_right(q, v); +} + +template<typename D1, typename D2> +inline Eigen::Matrix<typename D2::Scalar, 3, 1> minus(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) +{ + return minus_right(q1, q2); +} + template<typename D1, typename D2> inline Eigen::Matrix<typename D2::Scalar, 3, 1> diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) { diff --git a/src/test/gtest_rotation.cpp b/src/test/gtest_rotation.cpp index 2c23edb122044abb62916782b9893a7d30c07e3b..6eb8d259682b2e56fd9baa43b3af269f291cc0e4 100644 --- a/src/test/gtest_rotation.cpp +++ b/src/test/gtest_rotation.cpp @@ -469,8 +469,11 @@ TEST(Plus, Random) v .setRandom(); Quaternions q2 = q * exp_q(v); + Quaternions q3 = exp_q(v) * q; - ASSERT_QUATERNION_APPROX(plus(q,v), q2, 1e-12); + ASSERT_QUATERNION_APPROX(plus(q,v) , q2, 1e-12); + ASSERT_QUATERNION_APPROX(plus_right(q,v), q2, 1e-12); + ASSERT_QUATERNION_APPROX(plus_left(v,q) , q3, 1e-12); } @@ -496,12 +499,16 @@ TEST(Minus_and_diff, Random) q1 .coeffs().setRandom().normalize(); q2 .coeffs().setRandom().normalize(); - Vector3s v = log_q(q1.conjugate() * q2); + Vector3s vr = log_q(q1.conjugate() * q2); + Vector3s vl = log_q(q2 * q1.conjugate()); - ASSERT_MATRIX_APPROX(minus(q1, q2), v, 1e-12); + ASSERT_MATRIX_APPROX(minus(q1, q2), vr, 1e-12); ASSERT_QUATERNION_APPROX(plus(q1, minus(q1, q2)), q2, 1e-12); - ASSERT_MATRIX_APPROX(diff(q1, q2), v, 1e-12); + ASSERT_MATRIX_APPROX(diff(q1, q2), vr, 1e-12); ASSERT_QUATERNION_APPROX(plus(q1, diff(q1, q2)), q2, 1e-12); + + ASSERT_MATRIX_APPROX(minus_left(q1, q2), vl, 1e-12); + ASSERT_QUATERNION_APPROX(plus_left(minus_left(q1, q2), q1), q2, 1e-12); } TEST(Jacobians, Jr)