Back to compile flags

gtsam/base/testLie.h

@@ -29,9 +29,8 @@ namespace gtsam {
 
 // Do a comprehensive test of Lie Group derivatives
 template<typename G>
-void testLieGroupDerivatives(TestResult& result_,
+void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
-                      const std::string& name_,
+    const G& t1, const G& t2, bool advanced) {
-                      const G& t1, const G& t2) {
 
   G id;
   Matrix H1, H2;
@@ -76,6 +75,8 @@ void testLieGroupDerivatives(TestResult& result_,
   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);
@@ -121,5 +122,5 @@ void testLieGroupDerivatives(TestResult& result_,
 
 /// \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) \
+#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2,flag) \
-    { gtsam::testLieGroupDerivatives(result_, name_, t1,t2); }
+    { gtsam::testLieGroupDerivatives(result_, name_, t1, t2, flag); }

gtsam/geometry/Pose2.h

@@ -24,8 +24,6 @@
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/base/Lie.h>
 
-#define SLOW_BUT_CORRECT_EXPMAP
-
 namespace gtsam {
 
 /**

gtsam/geometry/Pose3.cpp

@@ -23,8 +23,6 @@
 #include <iostream>
 #include <cmath>
 
-#define GTSAM_POSE3_EXPMAP
-
 using namespace std;
 
 namespace gtsam {
@@ -160,8 +158,13 @@ Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
 #ifdef GTSAM_POSE3_EXPMAP
   return Expmap(xi, H);
 #else
-  if (H) CONCEPT_NOT_IMPLEMENTED;
+  Matrix3 DR;
-  return Pose3(Rot3::Retract(xi.head<3>()), Point3(xi.tail<3>()));
+  Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0);
+  if (H) {
+    *H = I_6x6;
+    H->topLeftCorner<3,3>() = DR;
+  }
+  return Pose3(R, Point3(xi.tail<3>()));
 #endif
 }
 
@@ -170,9 +173,14 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
 #ifdef GTSAM_POSE3_EXPMAP
   return Logmap(T, H);
 #else
-  if (H) CONCEPT_NOT_IMPLEMENTED;
+  Matrix3 DR;
+  Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0);
+  if (H) {
+    *H = I_6x6;
+    H->topLeftCorner<3,3>() = DR;
+  }
   Vector6 xi;
-  xi << Rot3::Logmap(T.rotation()), T.translation().vector();
+  xi << omega, T.translation().vector();
   return xi;
 #endif
 }

gtsam/geometry/Rot3.h

@@ -247,10 +247,10 @@ namespace gtsam {
 
 #ifndef GTSAM_USE_QUATERNIONS
 
-    // Cayley chart around origin, no derivatives
+    // Cayley chart around origin
     struct CayleyChart {
-      static Rot3 Retract(const Vector3& v);
+    static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none);
-      static Vector3 Local(const Rot3& r);
+    static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none);
     };
 
     /// Retraction from R^3 to Rot3 manifold using the Cayley transform

gtsam/geometry/Rot3M.cpp

@@ -206,7 +206,8 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::CayleyChart::Retract(const Vector3& omega) {
+Rot3 Rot3::CayleyChart::Retract(const Vector3& omega, OptionalJacobian<3,3> H) {
+  if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative");
   const double x = omega(0), y = omega(1), z = omega(2);
   const double x2 = x * x, y2 = y * y, z2 = z * z;
   const double xy = x * y, xz = x * z, yz = y * z;
@@ -217,7 +218,8 @@ Rot3 Rot3::CayleyChart::Retract(const Vector3& omega) {
 }
 
 /* ************************************************************************* */
-Vector3 Rot3::CayleyChart::Local(const Rot3& R) {
+Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
+  if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
   // Create a fixed-size matrix
   Matrix3 A = R.matrix();
   // Mathematica closed form optimization (procrastination?) gone wild:
@@ -237,28 +239,16 @@ Vector3 Rot3::CayleyChart::Local(const Rot3& R) {
 Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
   static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
   if (mode == Rot3::EXPMAP) return Expmap(omega, H);
-
+  if (mode == Rot3::CAYLEY) return CayleyChart::Retract(omega, H);
-  if (H) CONCEPT_NOT_IMPLEMENTED;
+  else throw std::runtime_error("Rot3::Retract: unknown mode");
-  if(mode == Rot3::CAYLEY) {
-    return CayleyChart::Retract(omega);
-  } else {
-    assert(false);
-    exit(1);
-  }
 }
 
 /* ************************************************************************* */
 Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
   static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
-  if (mode == Rot3::EXPMAP) return Logmap(R,H);
+  if (mode == Rot3::EXPMAP) return Logmap(R, H);
-
+  if (mode == Rot3::CAYLEY) return CayleyChart::Local(R, H);
-  if (H) CONCEPT_NOT_IMPLEMENTED;
+  else throw std::runtime_error("Rot3::Local: unknown mode");
-  if(mode == Rot3::CAYLEY) {
-    return CayleyChart::Local(R);
-  } else {
-    assert(false);
-    exit(1);
-  }
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPose2.cpp

@@ -773,7 +773,7 @@ TEST(Pose2 , Traits) {
   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);
+  CHECK_LIE_GROUP_DERIVATIVES(t1,t2,true);
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPose3.cpp

@@ -28,8 +28,6 @@ using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
-#define GTSAM_POSE3_EXPMAP
-
 GTSAM_CONCEPT_TESTABLE_INST(Pose3)
 GTSAM_CONCEPT_LIE_INST(Pose3)
 
@@ -463,16 +461,24 @@ TEST( Pose3, transformPose_to)
 }
 
 /* ************************************************************************* */
-#ifndef GTSAM_POSE3_EXPMAP
+TEST(Pose3, Retract_LocalCoordinates)
-TEST(Pose3, localCoordinates_first_order)
 {
-  Vector d12 = repeat(6,0.1);
+  Vector6 d;
+  d << 1,2,3,4,5,6; d/=10;
+  R = Rot3::Retract(d.head<3>());
+  Pose3 t = Pose3::Retract(d);
+  EXPECT(assert_equal(d, Pose3::LocalCoordinates(t)));
+}
+/* ************************************************************************* */
+TEST(Pose3, retract_localCoordinates)
+{
+  Vector6 d12;
+  d12 << 1,2,3,4,5,6; d12/=10;
   Pose3 t1 = T, t2 = t1.retract(d12);
   EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
 }
-#endif
 /* ************************************************************************* */
-TEST(Pose3, localCoordinates_expmap)
+TEST(Pose3, expmap_logmap)
 {
   Vector d12 = repeat(6,0.1);
   Pose3 t1 = T, t2 = t1.expmap(d12);
@@ -480,8 +486,7 @@ TEST(Pose3, localCoordinates_expmap)
 }
 
 /* ************************************************************************* */
-#ifndef GTSAM_POSE3_EXPMAP
+TEST(Pose3, retract_localCoordinates2)
-TEST(Pose3, manifold_first_order)
 {
   Pose3 t1 = T;
   Pose3 t2 = T3;
@@ -491,7 +496,6 @@ TEST(Pose3, manifold_first_order)
   Vector d21 = t2.localCoordinates(t1);
   EXPECT(assert_equal(t1, t2.retract(d21)));
 }
-#endif
 /* ************************************************************************* */
 TEST(Pose3, manifold_expmap)
 {
@@ -678,9 +682,9 @@ TEST( Pose3, ExpmapDerivative1) {
   Matrix6 actualH;
   Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
   Pose3::Expmap(w,actualH);
-  Matrix6 expectedH = numericalDerivative21<Pose3, Vector6,
+  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
-      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none, 1e-2);
+      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
-  EXPECT(assert_equal(expectedH, actualH, 1e-5));
+  EXPECT(assert_equal(expectedH, actualH));
 }
 
 /* ************************************************************************* */
@@ -689,9 +693,9 @@ TEST( Pose3, LogmapDerivative1) {
   Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
   Pose3 p = Pose3::Expmap(w);
   EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
-  Matrix6 expectedH = numericalDerivative21<Vector6, Pose3,
+  Matrix expectedH = numericalDerivative21<Vector6, Pose3,
-      OptionalJacobian<6, 6> >(&Pose3::Logmap, p, boost::none, 1e-2);
+      OptionalJacobian<6, 6> >(&Pose3::Logmap, p, boost::none);
-  EXPECT(assert_equal(expectedH, actualH, 1e-5));
+  EXPECT(assert_equal(expectedH, actualH));
 }
 
 /* ************************************************************************* */
@@ -745,7 +749,7 @@ TEST( Pose3, stream)
 TEST(Pose3 , Traits) {
   check_group_invariants(T2,T3);
   check_manifold_invariants(T2,T3);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
+  CHECK_LIE_GROUP_DERIVATIVES(T2,T3,false);
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -18,11 +18,10 @@
 
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
+#include <gtsam/base/testLie.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/lieProxies.h>
-#include <gtsam/base/testLie.h>
-//#include <gtsam/base/chartTesting.h>
 
 #include <boost/math/constants/constants.hpp>
 
@@ -220,6 +219,30 @@ TEST(Rot3, log)
   CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
 }
 
+/* ************************************************************************* */
+TEST(Rot3, retract_localCoordinates)
+{
+  Vector3 d12 = repeat(3,0.1);
+  Rot3 R2 = R.retract(d12);
+  EXPECT(assert_equal(d12, R.localCoordinates(R2)));
+}
+/* ************************************************************************* */
+TEST(Rot3, expmap_logmap)
+{
+  Vector3 d12 = repeat(3,0.1);
+  Rot3 R2 = R.expmap(d12);
+  EXPECT(assert_equal(d12, R.logmap(R2)));
+}
+
+/* ************************************************************************* */
+TEST(Rot3, retract_localCoordinates2)
+{
+  Rot3 t1 = R, t2 = R*R, origin;
+  Vector d12 = t1.localCoordinates(t2);
+  EXPECT(assert_equal(t2, t1.retract(d12)));
+  Vector d21 = t2.localCoordinates(t1);
+  EXPECT(assert_equal(t1, t2.retract(d21)));
+}
 /* ************************************************************************* */
 Vector w = Vector3(0.1, 0.27, -0.2);
 
@@ -634,39 +657,7 @@ TEST(Rot3 , Traits) {
   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 expected, actual;
-  Matrix actualH1, actualH2;
-  Matrix numericalH1, numericalH2;
-
-  expected = R1 * R2;
-  actual = traits_x<Rot3>::Compose(R1, R2, actualH1, actualH2);
-  EXPECT(assert_equal(expected,actual));
-
-  numericalH1 = numericalDerivative21(traits_x<Rot3>::Compose, R1, R2);
-  EXPECT(assert_equal(numericalH1,actualH1));
-
-  numericalH2 = numericalDerivative22(traits_x<Rot3>::Compose, R1, R2);
-  EXPECT(assert_equal(numericalH2,actualH2));
-
-  expected = R1.inverse() * R2;
-  actual = traits_x<Rot3>::Between(R1, R2, actualH1, actualH2);
-  EXPECT(assert_equal(expected,actual));
-
-  numericalH1 = numericalDerivative21(traits_x<Rot3>::Between, R1, R2);
-  EXPECT(assert_equal(numericalH1,actualH1));
-
-  numericalH2 = numericalDerivative22(traits_x<Rot3>::Between, R1, R2);
-  EXPECT(assert_equal(numericalH2,actualH2));
-
-  expected = R1.inverse();
-  actual = traits_x<Rot3>::Inverse(R1, actualH1);
-  EXPECT(assert_equal(expected,actual));
-
-  numericalH1 = numericalDerivative11(traits_x<Rot3>::Inverse, R1);
-  EXPECT(assert_equal(numericalH1,actualH1));
-
-  CHECK_LIE_GROUP_DERIVATIVES(R1,R2);
 }
 
 /* ************************************************************************* */