From 9f47d66e82a019b8bc0e2dfcd27ad1b68c5bea5e Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Thu, 9 Jun 2022 20:44:42 +0200
Subject: [PATCH] [skip-ci] Implemented inverse composite for SE3

---
 include/core/math/SE3.h | 26 +++++++++++++-------------
 test/gtest_SE3.cpp      | 22 ++++++++++++++++++++++
 2 files changed, 35 insertions(+), 13 deletions(-)

diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 749f8a433..791793554 100644
--- a/include/core/math/SE3.h
+++ b/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>
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index 239d9a3cb..a87610379 100644
--- a/test/gtest_SE3.cpp
+++ b/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;
-- 
GitLab