Add right and left versions to plus() and minus()

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)
 {

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)