Testing machinery

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 <gtsam/base/Lie.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <CppUnitLite/TestResult.h>
+#include <CppUnitLite/Test.h>
+#include <CppUnitLite/Failure.h>
+
+namespace gtsam {
+
+// Do a comprehensive test of Lie Group derivatives
+template<typename G>
+void testLieGroupDerivatives(TestResult& result_,
+                      const std::string& name_,
+                      const G& t1, const G& t2) {
+
+  G id;
+  Matrix H1, H2;
+  typedef traits_x<G> 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); }

gtsam/geometry/tests/testPose2.cpp

@@ -18,7 +18,7 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/testLie.h>
 #include <gtsam/base/lieProxies.h>
 
 #include <CppUnitLite/TestHarness.h>
@@ -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);
-
+  CHECK_LIE_GROUP_DERIVATIVES(t1,t2);
-  Matrix H1, H2;
-  typedef traits_x<Pose2> 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));
 }
 
 /* ************************************************************************* */