diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h index 749f8a4332466e614fa048496b471652347515ce..7917935549619fbc58cc917e834d5ae735d90d41 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 239d9a3cb0597c371ac1c34673e1344b17883298..a876103795f7afe0401c4a5db48290dd95c3775c 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;