From 27156ec8c14e935f71bb258ae18b84bbf908cde6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Dec 2014 16:10:47 +0100 Subject: [PATCH] Split up derivative tests --- gtsam/base/testLie.h | 72 ++++++------------------------ gtsam/geometry/tests/testPose2.cpp | 46 ++++++++++++++++--- gtsam/geometry/tests/testPose3.cpp | 34 +++++++++++++- gtsam/geometry/tests/testRot2.cpp | 57 +++++++++++++++-------- gtsam/geometry/tests/testRot3.cpp | 44 +++++++++++++++--- 5 files changed, 160 insertions(+), 93 deletions(-) diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h index 3a2e0b5e9..62eda0c68 100644 --- a/gtsam/base/testLie.h +++ b/gtsam/base/testLie.h @@ -30,17 +30,12 @@ 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, bool advanced) { + 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)); @@ -48,79 +43,38 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_, 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)); +} +// Do a comprehensive test of Lie Group Chart derivatives +template +void testChartDerivatives(TestResult& result_, const std::string& name_, + const G& t1, const G& t2) { - 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)); - - if (!advanced) return; + Matrix H1, H2; + typedef traits_x T; // 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,flag) \ - { gtsam::testLieGroupDerivatives(result_, name_, t1, t2, flag); } +#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \ + { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); } + +#define CHECK_CHART_DERIVATIVES(t1,t2) \ + { gtsam::testChartDerivatives(result_, name_, t1, t2); } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 03fec8fc8..fa6bd203c 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -153,7 +153,7 @@ namespace screw { Pose2 expected(expectedR, expectedT); } -TEST(Pose3, expmap_c) +TEST(Pose2, expmap_c) { EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6)); @@ -768,12 +768,44 @@ TEST(Pose2, align_4) { } //****************************************************************************** -TEST(Pose2 , Traits) { - 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); - CHECK_LIE_GROUP_DERIVATIVES(t1,t2,true); +Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5))); +Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0)); + +//****************************************************************************** +TEST(Pose2 , Invariants) { + Pose2 id; + + check_group_invariants(id,id); + check_group_invariants(id,T1); + check_group_invariants(T2,id); + check_group_invariants(T2,T1); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,T1); + check_manifold_invariants(T2,id); + check_manifold_invariants(T2,T1); + +} + +//****************************************************************************** +TEST(Pose2 , LieGroupDerivatives) { + Pose2 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T1); + +} + +//****************************************************************************** +TEST(Pose2 , ChartDerivatives) { + Pose2 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T1); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 28d3e7427..d40344d35 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -746,10 +746,40 @@ TEST( Pose3, stream) } //****************************************************************************** -TEST(Pose3 , Traits) { +TEST(Pose3 , Invariants) { + Pose3 id; + + check_group_invariants(id,id); + check_group_invariants(id,T3); + check_group_invariants(T2,id); check_group_invariants(T2,T3); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,T3); + check_manifold_invariants(T2,id); check_manifold_invariants(T2,T3); - CHECK_LIE_GROUP_DERIVATIVES(T2,T3,false); + +} + +//****************************************************************************** +TEST(Pose3 , LieGroupDerivatives) { + Pose3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T3); + +} + +//****************************************************************************** +TEST(Pose3 , ChartDerivatives) { + Pose3 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 45fecb244..37054fd83 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -15,10 +15,10 @@ * @author Frank Dellaert */ -#include -#include -#include #include +#include +#include +#include using namespace gtsam; @@ -155,26 +155,45 @@ TEST( Rot2, relativeBearing ) CHECK(assert_equal(expectedH,actualH)); } -/* ************************************************************************* */ -Vector w = (Vector(1) << 0.27).finished(); +//****************************************************************************** +Rot2 T1(0.1); +Rot2 T2(0.2); + +//****************************************************************************** +TEST(Rot2 , Invariants) { + Rot2 id; + + check_group_invariants(id,id); + check_group_invariants(id,T1); + check_group_invariants(T2,id); + check_group_invariants(T2,T1); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,T1); + check_manifold_invariants(T2,id); + check_manifold_invariants(T2,T1); -// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? -// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector1 testDexpL(const Vector& dw) { - Vector1 y = Rot2::Logmap(Rot2::Expmap(-w) * Rot2::Expmap(w + dw)); - return y; } -TEST( Rot2, ExpmapDerivative) { - Matrix actualDexpL = Rot2::ExpmapDerivative(w); - Matrix expectedDexpL = numericalDerivative11( - boost::function( - boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); +//****************************************************************************** +TEST(Rot2 , LieGroupDerivatives) { + Rot2 id; - Matrix actualDexpInvL = Rot2::LogmapDerivative(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T1); + +} + +//****************************************************************************** +TEST(Rot2 , ChartDerivatives) { + Rot2 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T1); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 69fdd5a12..96346e382 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -652,12 +652,44 @@ TEST( Rot3, slerp) } //****************************************************************************** -TEST(Rot3 , Traits) { - Rot3 R1(Rot3::rodriguez(Vector3(0, 0, 1), 1)); - Rot3 R2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); - check_group_invariants(R1, R2); - check_manifold_invariants(R1, R2); - CHECK_LIE_GROUP_DERIVATIVES(R1,R2,false); +Rot3 T1(Rot3::rodriguez(Vector3(0, 0, 1), 1)); +Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); + +//****************************************************************************** +TEST(Rot3 , Invariants) { + Rot3 id; + + check_group_invariants(id,id); + check_group_invariants(id,T1); + check_group_invariants(T2,id); + check_group_invariants(T2,T1); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,T1); + check_manifold_invariants(T2,id); + check_manifold_invariants(T2,T1); + +} + +//****************************************************************************** +TEST(Rot3 , LieGroupDerivatives) { + Rot3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T1); + +} + +//****************************************************************************** +TEST(Rot3 , ChartDerivatives) { + Rot3 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T1); } /* ************************************************************************* */