diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 90ede7a2d..05ead7fc3 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -275,8 +275,12 @@ namespace gtsam { Eigen::Matrix operator()(Key j, const gtsam::Value * const pointer) { try { +//<<<<<<< HEAD // value returns a const Vector&, and the return makes a copy !!!!! return dynamic_cast >&>(*pointer).value(); +//======= +// return reinterpret_cast >&>(*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 + struct handle_wrap { + inline gtsam::GenericValue operator()(Key j, const ValueType& val) { + return gtsam::GenericValue(val); + } + }; + + // when insert/update a fixed size vector: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + // when insert/update a fixed size matrix: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + } + // insert a templated value template - void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(GenericValue(val))); - } + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(internal::handle_wrap()(j, val))); + } // update with templated value template void Values::update(Key j, const ValueType& val) { - update(j, static_cast(GenericValue(val))); + update(j, static_cast(internal::handle_wrap()(j, val))); } } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index b63bc0196..8c1c85038 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/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(key1); - //CHECK(assert_equal(expected, actual)); - CHECK_EXCEPTION(values.at(key1), exception); + Vector actual = values.at(key1); + LONGS_EQUAL(3, actual.rows()); + 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(key1); - //CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(expected, actual)); CHECK_EXCEPTION(values.at(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(key1); + LONGS_EQUAL(3, actual.rows()); + LONGS_EQUAL(1, actual.cols()); + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST(Values, MatrixDynamicInsertFixedRead) { Values values;