Fixed SE3::inverse and wrote a gtest

include/core/math/SE3.h

@@ -108,7 +108,7 @@ inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q,
     MatrixSizeCheck<3, 1>::check(p);
     MatrixSizeCheck<3, 1>::check(ip);
 
-    ip = - q.conjugate() * p ;
+    ip = -(q.conjugate() * p) ;
     iq =   q.conjugate() ;
 }
 

test/gtest_SE3.cpp

@@ -103,6 +103,34 @@ TEST(SE3, log_of_power)
     ASSERT_MATRIX_APPROX(tau3, log(pose3), 1e-8);
 }
 
+TEST(SE3, inverse)
+{
+    Vector3d p; p.setRandom();
+    Vector4d qvec; qvec.setRandom().normalized();
+    Quaterniond q(qvec);
+
+    Vector3d pi_true = -(q.conjugate() * p);
+    Quaterniond qi_true = q.conjugate();
+
+    // separate API
+    Vector3d pi_out;
+    Quaterniond qi_out;
+
+    inverse(p, q, pi_out, qi_out);
+    ASSERT_MATRIX_APPROX(pi_out, pi_true, 1e-8);
+    ASSERT_MATRIX_APPROX(qi_out.coeffs(), qi_true.coeffs(), 1e-8);
+
+    // Vector7d API
+    Vector7d pose; pose << p, q.coeffs();
+    Vector7d posei_true; posei_true << pi_true, qi_true.coeffs();
+    Vector7d posei_out;
+    inverse(pose, posei_out);
+    ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8);
+
+    posei_out = inverse(pose);
+    ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8);
+}
+
 TEST(SE3, composeBlocks)
 {
     Vector3d p1, p2, pc;
@@ -401,6 +429,7 @@ TEST(SE3, interpolate_third)
     ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
 }
 
+
 TEST(SE3, toSE3_I)
 {
     Vector7d pose; pose << 0,0,0, 0,0,0,1;