[skip-ci] Implemented inverse composite for SE3

include/core/math/SE3.h

@@ -135,19 +135,19 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
     return id;
 }
 
-// inline void inverse(const VectorComposite& v, VectorComposite& c)
-// {
-//     Map<const Quaternion<double> > qv( & v.at('O')(0) );
-//     Map<const Quaternion<double> > qc( & c.at('O')(0) );
-//     inverse(v.at('P'), qv, c.at('P'), qc);
-// }
-
-// inline VectorComposite inverse(const VectorComposite& v)
-// {
-//     VectorComposite c("PO", {3,4});
-//     inverse(v, c);
-//     return c;
-// }
+inline void inverse(const VectorComposite& v, VectorComposite& c)
+{
+    Map<const Quaternion<double> > qv( & v.at('O')(0) );
+    Map<Quaternion<double> > qc( & c['O'](0) );
+    inverse(v.at('P'), qv, c['P'], qc);
+}
+
+inline VectorComposite inverse(const VectorComposite& v)
+{
+    VectorComposite c("PO", {3,4});
+    inverse(v, c);
+    return c;
+}
 
 
 template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>

test/gtest_SE3.cpp

@@ -131,6 +131,28 @@ TEST(SE3, inverse)
     ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8);
 }
 
+TEST(SE3, inverseComposite)
+{
+    Vector3d p; p.setRandom();
+    Vector4d qvec; qvec.setRandom().normalized();
+    VectorComposite pose_vc("PO", {p, qvec});
+    Quaterniond q(qvec);
+
+    // ground truth
+    Vector3d pi_true = -(q.conjugate() * p);
+    Quaterniond qi_true = q.conjugate();
+
+    VectorComposite pose_vc_out("PO", {3, 4});
+    inverse(pose_vc, pose_vc_out);
+    ASSERT_MATRIX_APPROX(pose_vc_out.at('P'), pi_true, 1e-8);
+    ASSERT_MATRIX_APPROX(pose_vc_out.at('O'), qi_true.coeffs(), 1e-8);
+
+    VectorComposite pose_vc_out_bis = inverse(pose_vc);
+    ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('P'), pi_true, 1e-8);
+    ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('O'), qi_true.coeffs(), 1e-8);
+
+}
+
 TEST(SE3, composeBlocks)
 {
     Vector3d p1, p2, pc;