diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 5d9f5773f..74a1bb0cf 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -29,6 +29,12 @@ namespace internal { constexpr int DimensionSO(int N) { return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2; } + +// Return dynamic identity matrix for given SO(n) dimensionality d +Matrix IdentitySO(size_t n) { + const size_t d = n * (n - 1) / 2; + return Matrix::Identity(d, d); +} } // namespace internal /** @@ -149,6 +155,34 @@ using SOn = SO; using SO3 = SO<3>; using SO4 = SO<4>; +/* + * Fully specialize compose and between, because the derivative is unknowable by + * the LieGroup implementations, who return a fixed-size matrix for H2. + */ + +using DynamicJacobian = OptionalJacobian; + +template <> +SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = internal::IdentitySO(g.rows()); + return derived() * g; +} + +template <> +SOn LieGroup::between(const SOn& g, DynamicJacobian H1, + DynamicJacobian H2) const { + SOn result = derived().inverse() * g; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = internal::IdentitySO(g.rows()); + return result; +} + +/* + * Define the traits. internal::LieGroup provides both Lie group and Testable + */ + template struct traits> : public internal::LieGroup> {}; @@ -179,9 +213,9 @@ using namespace gtsam; /* ************************************************************************* */ TEST(SOn, Concept) { - // BOOST_CONCEPT_ASSERT((IsGroup)); - // BOOST_CONCEPT_ASSERT((IsManifold)); - // BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); } // /* *************************************************************************