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