Merge remote-tracking branch 'origin/feature/FixFixedValues' into feature/Feature/FixedValues

Conflicts:
	gtsam/nonlinear/Values-inl.h

gtsam/nonlinear/Values-inl.h

@@ -275,8 +275,12 @@ namespace gtsam {
        Eigen::Matrix<double, -1, 1> operator()(Key j,
           const gtsam::Value * const pointer) {
         try {
+//<<<<<<< HEAD
           // value returns a const Vector&, and the return makes a copy !!!!!
           return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
+//=======
+//          return reinterpret_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
+//>>>>>>> feature/FixFixedValues
         } catch (std::bad_cast &) {
           // If a fixed vector was stored, we end up here as well.
           throw ValuesIncorrectType(j, typeid(*pointer),
@@ -379,16 +383,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

@@ -488,14 +488,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));
 }
 
 /* ************************************************************************* */
@@ -505,10 +506,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;