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 operator()(Key j, const gtsam::Value * const pointer) { try { - return dynamic_cast >&>(*pointer).value(); + return reinterpret_cast >&>(*pointer).value(); } catch (std::bad_cast &) { throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); @@ -367,16 +367,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 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(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)); } /* ************************************************************************* */ @@ -504,10 +505,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;