diff --git a/cpp/LieConfig-inl.h b/cpp/LieConfig-inl.h index 862dc76fb..9bf9bd63a 100644 --- a/cpp/LieConfig-inl.h +++ b/cpp/LieConfig-inl.h @@ -83,6 +83,14 @@ namespace gtsam { insert(v.first, v.second); } + template + void LieConfig::update(const LieConfig& cfg) { + BOOST_FOREACH(const typename Values::value_type& v, values_) { + boost::optional t = cfg.exists_(v.first); + if (t) insert(v.first, *t); + } + } + template std::list LieConfig::keys() const { std::list ret; diff --git a/cpp/LieConfig.h b/cpp/LieConfig.h index d13cd09c9..db650ec7c 100644 --- a/cpp/LieConfig.h +++ b/cpp/LieConfig.h @@ -66,6 +66,12 @@ namespace gtsam { /** Check if a variable exists */ bool exists(const J& i) const { return values_.find(i)!=values_.end(); } + /** Check if a variable exists and return it if so */ + boost::optional exists_(const J& i) const { + const_iterator it = values_.find(i); + if (it==values_.end()) return boost::none; else return it->second; + } + /** The number of variables in this config */ size_t size() const { return values_.size(); } @@ -91,6 +97,9 @@ namespace gtsam { /** Add a set of variables */ void insert(const LieConfig& cfg); + /** update the current available values without adding new ones */ + void update(const LieConfig& cfg); + /** Remove a variable from the config */ void erase(const J& j); diff --git a/cpp/TupleConfig.h b/cpp/TupleConfig.h index 478e59029..aa8322244 100644 --- a/cpp/TupleConfig.h +++ b/cpp/TupleConfig.h @@ -67,6 +67,14 @@ namespace gtsam { second_.insert(config.second_); } + // update function for whole configs + template + void update(const TupleConfig& config) { second_.update(config); } + void update(const TupleConfig& config) { + first_.update(config.first_); + second_.update(config.second_); + } + // insert a subconfig template void insertSub(const Cfg& config) { second_.insertSub(config); } @@ -82,6 +90,11 @@ namespace gtsam { bool exists(const Key& j) const { return second_.exists(j); } bool exists(const Key1& j) const { return first_.exists(j); } + // a variant of exists + template + boost::optional exists_(const Key& j) const { return second_.exists_(j); } + boost::optional exists_(const Key1& j) const { return first_.exists_(j); } + // access operator template const typename Key::Value_t & operator[](const Key& j) const { return second_[j]; } @@ -156,6 +169,9 @@ namespace gtsam { // insert function for whole configs void insert(const TupleConfigEnd& config) {first_.insert(config.first_); } + // update function for whole configs + void update(const TupleConfigEnd& config) {first_.update(config.first_); } + // insert function for sub configs void insertSub(const Config& config) {first_.insert(config); } @@ -167,6 +183,8 @@ namespace gtsam { bool exists(const Key1& j) const { return first_.exists(j); } + boost::optional exists_(const Key1& j) const { return first_.exists_(j); } + const Value1& at(const Key1& j) const { return first_.at(j); } size_t size() const { return first_.size(); } @@ -479,6 +497,8 @@ namespace gtsam { */ bool exists(const J1& j) const { return first_.exists(j); } bool exists(const J2& j) const { return second_.exists(j); } + boost::optional exists_(const J1& j) const { return first_.exists_(j); } + boost::optional exists_(const J2& j) const { return second_.exists_(j); } private: /** Serialization function */