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

@@ -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) {
+  void Values::insert(Key j, const ValueType& val) {
-     insert(j, static_cast<const Value&>(GenericValue<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)));
   }
 
 }

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);
+  Vector actual = values.at<Vector>(key1);
-  //CHECK(assert_equal(expected, actual));
+  LONGS_EQUAL(3, actual.rows());
-  CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
+  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;