diff --git a/cpp/LieConfig-inl.h b/cpp/LieConfig-inl.h index fdffc2c4d..6bea2d98b 100644 --- a/cpp/LieConfig-inl.h +++ b/cpp/LieConfig-inl.h @@ -91,6 +91,11 @@ namespace gtsam { } } + template + void LieConfig::update(const J& j, const T& val) { + values_[j] = val; + } + template std::list LieConfig::keys() const { std::list ret; diff --git a/cpp/LieConfig.h b/cpp/LieConfig.h index db650ec7c..acc9bdde7 100644 --- a/cpp/LieConfig.h +++ b/cpp/LieConfig.h @@ -100,6 +100,9 @@ namespace gtsam { /** update the current available values without adding new ones */ void update(const LieConfig& cfg); + /** single element change of existing element */ + void update(const J& j, const T& val); + /** Remove a variable from the config */ void erase(const J& j); diff --git a/cpp/testLieConfig.cpp b/cpp/testLieConfig.cpp index 60c1c5a67..0395fa8bb 100644 --- a/cpp/testLieConfig.cpp +++ b/cpp/testLieConfig.cpp @@ -79,6 +79,22 @@ TEST( LieConfig, insert_config ) CHECK(assert_equal(cfg1, expected)); } +/* ************************************************************************* */ +TEST( LieConfig, insert_overlap ) +{ + LieConfig cfg; + Vector v1 = Vector_(3, 5.0, 6.0, 7.0); + Vector v2 = Vector_(3, 8.0, 9.0, 1.0); + + cfg.insert("x1", v1); + CHECK(cfg.size() == 1); + CHECK(assert_equal(v1, cfg.at("x1"))); + + cfg.update("x1", v2); + CHECK(cfg.size() == 1); + CHECK(assert_equal(v2, cfg.at("x1"))); // fails - need to change behavior +} + /* ************************************************************************* */ TEST(LieConfig, dim_zero) {