From e1faf8874e4569ad02234534e6866ac0e587c5e4 Mon Sep 17 00:00:00 2001
From: Jing Dong <jdong@gatech.edu>
Date: Sun, 5 Apr 2015 18:24:17 -0400
Subject: [PATCH] try to fix FixedValues branch by wrap all Eigen type in
 Dynamic size in Values, since fixed size Eigen type cannot read out by
 GenericValue if size is not explictly given

---
 gtsam/nonlinear/Values-inl.h         | 42 ++++++++++++++++++++++++----
 gtsam/nonlinear/tests/testValues.cpp | 25 +++++++++++++----
 2 files changed, 56 insertions(+), 11 deletions(-)

diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 8a905a0ac..c2e7ac8fe 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -274,7 +274,7 @@ namespace gtsam {
        Eigen::Matrix<double, -1, 1> operator()(Key j,
           const gtsam::Value * const pointer) {
         try {
-          return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
+          return reinterpret_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
         } catch (std::bad_cast &) {
           throw ValuesIncorrectType(j, typeid(*pointer),
               typeid(Eigen::Matrix<double, -1, 1>));
@@ -367,16 +367,48 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
+
+  // wrap all fixed in dynamics when insert and update
+  namespace internal {
+
+  // general type
+  template<typename ValueType>
+  struct handle_wrap {
+    inline gtsam::GenericValue<ValueType> operator()(Key j, const ValueType& val) {
+      return gtsam::GenericValue<ValueType>(val);
+    }
+  };
+
+  // when insert/update a fixed size vector: convert to dynamic size
+  template<int M>
+  struct handle_wrap<Eigen::Matrix<double, M, 1> > {
+    inline gtsam::GenericValue<Eigen::Matrix<double, -1, 1> > operator()(
+        Key j, const Eigen::Matrix<double, M, 1>& val) {
+      return gtsam::GenericValue<Eigen::Matrix<double, -1, 1> >(val);
+    }
+  };
+
+  // when insert/update a fixed size matrix: convert to dynamic size
+  template<int M, int N>
+  struct handle_wrap<Eigen::Matrix<double, M, N> > {
+    inline gtsam::GenericValue<Eigen::Matrix<double, -1, -1> > operator()(
+        Key j, const Eigen::Matrix<double, M, N>& val) {
+      return gtsam::GenericValue<Eigen::Matrix<double, -1, -1> >(val);
+    }
+  };
+
+  }
+
   // insert a templated value
   template<typename ValueType>
-   void Values::insert(Key j, const ValueType& val) {
-     insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
-   }
+  void Values::insert(Key j, const ValueType& val) {
+    insert(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
+  }
 
   // update with templated value
   template <typename ValueType>
   void Values::update(Key j, const ValueType& val) {
-    update(j, static_cast<const Value&>(GenericValue<ValueType >(val)));
+    update(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
   }
 
 }
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index ecc01c95b..b283f5355 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -487,14 +487,15 @@ TEST(Values, VectorDynamicInsertFixedRead) {
 }
 
 /* ************************************************************************* */
-TEST(Values, VectorFixedInsertDynamicRead) {
+TEST(Values, VectorDynamicInsertDynamicRead) {
   Values values;
-  Vector3 v; v << 5.0, 6.0, 7.0;
+  Vector v(3); v << 5.0, 6.0, 7.0;
   values.insert(key1, v);
   Vector expected(3); expected << 5.0, 6.0, 7.0;
-  Vector actual(3); actual = values.at<Vector>(key1);
-  //CHECK(assert_equal(expected, actual));
-  CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
+  Vector actual = values.at<Vector>(key1);
+  LONGS_EQUAL(3, actual.rows());
+  LONGS_EQUAL(1, actual.cols());
+  CHECK(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */
@@ -504,10 +505,22 @@ TEST(Values, VectorFixedInsertFixedRead) {
   values.insert(key1, v);
   Vector3 expected; expected << 5.0, 6.0, 7.0;
   Vector3 actual = values.at<Vector3>(key1);
-  //CHECK(assert_equal(expected, actual));
+  CHECK(assert_equal(expected, actual));
   CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
 }
 
+/* ************************************************************************* */
+TEST(Values, VectorFixedInsertDynamicRead) {
+  Values values;
+  Vector3 v; v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  Vector expected(3); expected << 5.0, 6.0, 7.0;
+  Vector actual = values.at<Vector>(key1);
+  LONGS_EQUAL(3, actual.rows());
+  LONGS_EQUAL(1, actual.cols());
+  CHECK(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 TEST(Values, MatrixDynamicInsertFixedRead) {
   Values values;