Split up derivative tests

gtsam/base/testLie.h

@@ -30,17 +30,12 @@ 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, bool advanced) {
+    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));
 
@@ -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<typename G>
+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)));
+  Matrix H1, H2;
-  EXPECT(assert_equal(numericalDerivative21(T::Between, id, t1), H1));
+  typedef traits_x<G> T;
-  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;
 
   // 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.
+#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \
-/// \param value An instantiation of the type to be tested.
+    { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); }
-#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2,flag) \
+
-    { gtsam::testLieGroupDerivatives(result_, name_, t1, t2, flag); }
+#define CHECK_CHART_DERIVATIVES(t1,t2) \
+    { gtsam::testChartDerivatives(result_, name_, t1, t2); }

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<Pose2>(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 t1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
+Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
-  Pose2 t2(M_PI / 2.0, Point2(0.0, 2.0));
+
-  check_group_invariants(t1, t2);
+//******************************************************************************
-  check_manifold_invariants(t1, t2);
+TEST(Pose2 , Invariants) {
-  CHECK_LIE_GROUP_DERIVATIVES(t1,t2,true);
+  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);
 }
 
 /* ************************************************************************* */

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);
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot2.cpp

@@ -15,10 +15,10 @@
  * @author  Frank Dellaert
  */
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Rot2.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/testLie.h>
+#include <CppUnitLite/TestHarness.h>
 
 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);
+TEST(Rot2 , LieGroupDerivatives) {
-  Matrix expectedDexpL = numericalDerivative11<Vector, Vector1>(
+  Rot2 id;
-      boost::function<Vector(const Vector&)>(
-          boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2);
-  EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
 
-  Matrix actualDexpInvL = Rot2::LogmapDerivative(w);
+  CHECK_LIE_GROUP_DERIVATIVES(id,id);
-  EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
+  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);
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -652,12 +652,44 @@ TEST( Rot3, slerp)
 }
 
 //******************************************************************************
-TEST(Rot3 , Traits) {
+Rot3 T1(Rot3::rodriguez(Vector3(0, 0, 1), 1));
-  Rot3 R1(Rot3::rodriguez(Vector3(0, 0, 1), 1));
+Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
-  Rot3 R2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
+
-  check_group_invariants(R1, R2);
+//******************************************************************************
-  check_manifold_invariants(R1, R2);
+TEST(Rot3 , Invariants) {
-  CHECK_LIE_GROUP_DERIVATIVES(R1,R2,false);
+  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);
 }
 
 /* ************************************************************************* */