From 0acffe5ae9b9533d587c00ffdaaad209a481ff85 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 21 Oct 2014 11:13:14 +0200
Subject: [PATCH] Fixed bug in DefaultChart: keeping a reference s never a good
 idea.

---
 gtsam/base/Manifold.h                         |  8 ++---
 .../nonlinear/tests/testAdaptAutoDiff.cpp     | 32 +++++++++++++++----
 2 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index 4ed371803..c4420bb7d 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -148,7 +148,7 @@ template<typename T>
 struct DefaultChart {
   BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
   typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
-  T const & t_;
+  T t_;
   DefaultChart(const T& t) :
       t_(t) {
   }
@@ -186,6 +186,7 @@ template<typename T> struct Canonical {
 template<>
 struct DefaultChart<double> {
   typedef Eigen::Matrix<double, 1, 1> vector;
+  double t_;
   DefaultChart(double t) :
       t_(t) {
   }
@@ -197,8 +198,6 @@ struct DefaultChart<double> {
   double retract(const vector& d) {
     return t_ + d[0];
   }
-private:
-  double t_;
 };
 
 // Fixed size Eigen::Matrix type
@@ -207,6 +206,7 @@ template<int M, int N, int Options>
 struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
   typedef Eigen::Matrix<double, M, N, Options> T;
   typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
+  T t_;
   DefaultChart(const T& t) :
       t_(t) {
   }
@@ -219,8 +219,6 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
     Eigen::Map<const T> map(d.data());
     return t_ + map;
   }
-private:
-  T const & t_;
 };
 
 /**
diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp
index 45267bf81..d697a382f 100644
--- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp
+++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp
@@ -202,7 +202,7 @@ TEST(Manifold, Canonical) {
   EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
 
   Canonical<Vector2> chart2;
-  EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0));
+  EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0)));
   EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0));
 
   Canonical<double> chart3;
@@ -211,12 +211,30 @@ TEST(Manifold, Canonical) {
   EXPECT(chart3.apply(1)==v1);
   EXPECT(chart3.retract(v1)==1);
 
-  // Dynamic does not work yet !
-//  Vector z = zero(2), v(2);
-//  v << 1, 0;
-//  Canonical<Vector> chart4(z);
-//  EXPECT(chart4.apply(v)==v);
-//  EXPECT(chart4.retract(v)==v);
+  Canonical<Point3> chart4;
+  Point3 point(1,2,3);
+  Vector3 v3(1,2,3);
+  EXPECT(assert_equal((Vector)chart4.apply(point),v3));
+  EXPECT(assert_equal(chart4.retract(v3),point));
+
+  Canonical<Pose3> chart5;
+  Pose3 pose(Rot3::identity(),point);
+  Vector6 v6; v6 << 0,0,0,1,2,3;
+  EXPECT(assert_equal((Vector)chart5.apply(pose),v6));
+  EXPECT(assert_equal(chart5.retract(v6),pose));
+
+  Canonical<Camera> chart6;
+  Cal3Bundler cal0(0,0,0);
+  Camera camera(Pose3(),cal0);
+  Vector9 z9 = Vector9::Zero();
+  EXPECT(assert_equal((Vector)chart6.apply(camera),z9));
+  EXPECT(assert_equal(chart6.retract(z9),camera));
+
+  Cal3Bundler cal; // Note !! Cal3Bundler() != zero<Cal3Bundler>::value()
+  Camera camera2(pose,cal);
+  Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0;
+  EXPECT(assert_equal((Vector)chart6.apply(camera2),v9));
+  EXPECT(assert_equal(chart6.retract(v9),camera2));
 }
 
 /* ************************************************************************* */