From 2624e9876d8950e30cb6b2d6ee2f644e9f9d450c Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 25 Dec 2014 19:42:21 +0100 Subject: [PATCH] Testing machinery --- gtsam/base/testLie.h | 125 +++++++++++++++++++++++++++++ gtsam/geometry/tests/testPose2.cpp | 62 +------------- 2 files changed, 127 insertions(+), 60 deletions(-) create mode 100644 gtsam/base/testLie.h diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h new file mode 100644 index 000000000..aadfb4f07 --- /dev/null +++ b/gtsam/base/testLie.h @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file chartTesting.h + * @brief + * @date November, 2014 + * @author Paul Furgale + */ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace gtsam { + +// Do a comprehensive test of Lie Group derivatives +template +void testLieGroupDerivatives(TestResult& result_, + const std::string& name_, + const G& t1, const G& t2) { + + G id; + Matrix H1, H2; + typedef traits_x T; + + // Inverse + + EXPECT(assert_equal(id,T::Inverse(id, H1))); + EXPECT(assert_equal(numericalDerivative11(T::Inverse, id),H1)); + + EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); + EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1)); + + EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); + EXPECT(assert_equal(numericalDerivative11(T::Inverse, t2),H1)); + + // Compose + + EXPECT(assert_equal(t1, T::Compose(id, t1, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t1), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t1), H2)); + + EXPECT(assert_equal(t2, T::Compose(id, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t2), H2)); + + EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2)); + + // Between + + EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2)); + + EXPECT(assert_equal(id.inverse() * t1,T::Between(id, t1, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Between, id, t1), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Between, id, t1), H2)); + + EXPECT(assert_equal(id.inverse() * t2,T::Between(id, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Between, id, t2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Between, id, t2), H2)); + + // Retract + + typename G::TangentVector z = T::Local(id, id); + EXPECT(assert_equal(id, T::Retract(id,z, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Retract, id, z), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Retract, id, z), H2)); + + typename G::TangentVector w1 = T::Local(id, t1); + EXPECT(assert_equal(t1, T::Retract(id,w1, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w1), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w1), H2)); + + typename G::TangentVector w2 = T::Local(id, t2); + EXPECT(assert_equal(t2, T::Retract(id,w2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w2), H2)); + + typename G::TangentVector w12 = T::Local(t1, t2); + EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2)); + + // Local + + EXPECT(assert_equal(z, id.localCoordinates(id, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Local, id, id), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Local, id, id), H2)); + + EXPECT(assert_equal(w1, id.localCoordinates(t1, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Local, id, t1), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Local, id, t1), H2)); + + EXPECT(assert_equal(w2, id.localCoordinates(t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Local, id, t2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Local, id, t2), H2)); + + EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2)); + +} +} // namespace gtsam + +/// \brief Perform a concept check on the default chart for a type. +/// \param value An instantiation of the type to be tested. +#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \ + { gtsam::testLieGroupDerivatives(result_, name_, t1,t2); } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 94ab8c634..c1e3e86ee 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include @@ -769,69 +769,11 @@ TEST(Pose2, align_4) { //****************************************************************************** TEST(Pose2 , Traits) { - Pose2 id; Pose2 t1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5))); Pose2 t2(M_PI / 2.0, Point2(0.0, 2.0)); check_group_invariants(t1, t2); check_manifold_invariants(t1, t2); - - Matrix H1, H2; - typedef traits_x T; - - EXPECT(assert_equal(t1, T::Compose(id, t1, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t1), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t1), H2)); - - EXPECT(assert_equal(t2, T::Compose(id, t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Compose, id, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Compose, id, t2), H2)); - - EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2)); - - EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2)); - - EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); - EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1)); - - Vector3 z = T::Local(id, id); - EXPECT(assert_equal(id, T::Retract(id,z, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Retract, id, z), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Retract, id, z), H2)); - - EXPECT(assert_equal(z, id.localCoordinates(id, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Local, id, id), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Local, id, id), H2)); - - Vector3 w1 = T::Local(id, t1); - EXPECT(assert_equal(t1, T::Retract(id,w1, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w1), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w1), H2)); - - EXPECT(assert_equal(w1, id.localCoordinates(t1, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Local, id, t1), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Local, id, t1), H2)); - - Vector3 w2 = T::Local(id, t2); - EXPECT(assert_equal(t2, T::Retract(id,w2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Retract, id, w2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Retract, id, w2), H2)); - - EXPECT(assert_equal(w2, id.localCoordinates(t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Local, id, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Local, id, t2), H2)); - - Vector3 w12 = T::Local(t1, t2); - EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2)); - - EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2)); + CHECK_LIE_GROUP_DERIVATIVES(t1,t2); } /* ************************************************************************* */