add update and exists_

cpp/LieConfig-inl.h

@@ -83,6 +83,14 @@ namespace gtsam {
 		 insert(v.first, v.second);
   }
 
+  template<class J, class T>
+  void LieConfig<J,T>::update(const LieConfig<J,T>& cfg) {
+	  BOOST_FOREACH(const typename Values::value_type& v, values_) {
+	  	boost::optional<T> t = cfg.exists_(v.first);
+	  	if (t) insert(v.first, *t);
+	  }
+  }
+
   template<class J, class T>
   std::list<J> LieConfig<J,T>::keys() const {
 	  std::list<J> ret;

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<T> 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);
 

cpp/TupleConfig.h

@@ -67,6 +67,14 @@ namespace gtsam {
 		  second_.insert(config.second_);
 	  }
 
+	  // update function for whole configs
+	  template<class Cfg1, class Cfg2>
+	  void update(const TupleConfig<Cfg1, Cfg2>& config) { second_.update(config); }
+	  void update(const TupleConfig<Config1, Config2>& config) {
+	  	first_.update(config.first_);
+	  	second_.update(config.second_);
+	  }
+
 	  // insert a subconfig
 	  template<class Cfg>
 	  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<class Key>
+	  boost::optional<typename Key::Value_t> exists_(const Key& j)  const { return second_.exists_(j); }
+	  boost::optional<Value1>                exists_(const Key1& j) const { return first_.exists_(j); }
+
 	  // access operator
 	  template<class Key>
 	  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>& config) {first_.insert(config.first_); }
 
+	  // update function for whole configs
+	  void update(const TupleConfigEnd<Config>& 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<Value1> 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<X1> exists_(const J1& j) const { return first_.exists_(j); }
+    boost::optional<X2> exists_(const J2& j) const { return second_.exists_(j); }
 
   private:
   	/** Serialization function */