(in branch) more implementation for DynamicValues

gtsam/base/Value.h

@@ -101,6 +101,9 @@ namespace gtsam {
   class Value {
   public:
 
+    /** Allocate and construct a clone of this value */
+    virtual std::auto_ptr<Value> clone_() const = 0;
+
     /** Print this value, for debugging and unit tests */
     virtual void print(const std::string& str = "") const = 0;
 

gtsam/geometry/Rot3.h

@@ -178,6 +178,11 @@ namespace gtsam {
     static Rot3 rodriguez(double wx, double wy, double wz)
       { return rodriguez(Vector_(3,wx,wy,wz));}
 
+    /**
+     * Create a duplicate object returned as a pointer to the generic Value interface
+     */
+    virtual std::auto_ptr<Value> clone_() const { return std::auto_ptr<Value>(new Rot3(*this)); }
+
     /// @}
     /// @name Testable
     /// @{

gtsam/nonlinear/DynamicValues-inl.h

@@ -100,9 +100,11 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void DynamicValues::insert(const DynamicValues& values) {
+  template<class ValueType>
-    BOOST_FOREACH(const KeyValuePair& key_value, values) {
+  void DynamicValues::update(const Symbol& j, const ValueType& val) {
-      insert(key_value.first, key_value.)
+    iterator item = values_.find(j);
-    }
+    if(item == values_end)
+      throw DynamicValuesKeyDoesNotExist("update", j);
+    item->second = val.clone_();
   }
 }

gtsam/nonlinear/DynamicValues.cpp

@@ -24,7 +24,10 @@
 
 #include <gtsam/nonlinear/DynamicValues.h>
 
+#include <list>
+
 #include <boost/foreach.hpp>
+#include <boost/iterator/transform_iterator.hpp>
 
 using namespace std;
 
@@ -32,7 +35,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   DynamicValues::DynamicValues(const DynamicValues& other) {
-
+    this->insert(other);
   }
 
   /* ************************************************************************* */
@@ -100,5 +103,60 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  void DynamicValues::insert(const DynamicValues& values) {
+    BOOST_FOREACH(const KeyValuePair& key_value, values) {
+      insert(key_value.first, key_value.second);
+    }
+  }
 
+  /* ************************************************************************* */
+  void DynamicValues::update(const DynamicValues& values) {
+    BOOST_FOREACH(const KeyValuePair& key_value, values) {
+      this->update(key_value.first, *key_value.second);
+    }
+  }
+
+  /* ************************************************************************* */
+  void DynamicValues::erase(const Symbol& j) {
+    iterator item = values_.find(j);
+    if(item == values_end)
+      throw DynamicValuesKeyDoesNotExist("erase", j);
+    values_.erase(item);
+  }
+
+  /* ************************************************************************* */
+  FastList<Symbol> DynamicValues::keys() const {
+    return list<Symbol>(
+        boost::make_transform_iterator(values_.begin(), &KeyValuePair::first),
+        boost::make_transform_iterator(values_.end(), &KeyValuePair::first));
+  }
+
+  /* ************************************************************************* */
+  DynamicValues& DynamicValues::operator=(const DynamicValues& rhs) {
+    this->clear();
+    this->insert(rhs);
+    return *this;
+  }
+
+  /* ************************************************************************* */
+  vector<size_t> DynamicValues::dims(const Ordering& ordering) const {
+    vector<size_t> result(values_.size());
+    // Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
+    result.assign(
+        boost::make_transform_iterator(values_.begin(),
+            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second)))),
+        boost::make_transform_iterator(values_.begin(),
+            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second)))));
+    return result;
+  }
+
+  /* ************************************************************************* */
+  Ordering::shared_ptr DynamicValues::orderingArbitrary(Index firstVar = 0) const {
+    Ordering::shared_ptr ordering(new Ordering);
+    BOOST_FOREACH(const KeyValuePair& key_value, values_) {
+      ordering->insert(key_value.first, firstVar++);
+    }
+    return ordering;
+  }
 }

gtsam/nonlinear/DynamicValues.h

@@ -154,10 +154,10 @@ namespace gtsam {
     void insert(const Symbol& j, const ValueType& val);
 
     /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
-    void insert(const DynamicValues& cfg);
+    void insert(const DynamicValues& values);
 
     /** update the current available values without adding new ones */
-    void update(const DynamicValues& cfg);
+    void update(const DynamicValues& values);
 
     /** single element change of existing element */
     template<class ValueType>
@@ -176,34 +176,13 @@ namespace gtsam {
      * Returns a set of keys in the config
      * Note: by construction, the list is ordered
      */
-    std::list<Symbol> keys() const;
+    FastList<Symbol> keys() const;
 
     /** Replace all keys and variables */
-    DynamicValues& operator=(const DynamicValues& rhs) {
+    DynamicValues& operator=(const DynamicValues& rhs);
-      values_ = rhs.values_;
-      return (*this);
-    }
 
     /** Remove all variables from the config */
-    void clear() {
+    void clear() { values_.clear(); }
-      values_.clear();
-    }
-
-    /**
-     * Apply a class with an application operator() to a const_iterator over
-     * every <key,value> pair.  The operator must be able to handle such an
-     * iterator for every type in the Values, (i.e. through templating).
-     */
-    template<typename A>
-    void apply(A& operation) {
-      for(iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
-    template<typename A>
-    void apply(A& operation) const {
-      for(const_iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
 
     /** Create an array of variable dimensions using the given ordering */
     std::vector<size_t> dims(const Ordering& ordering) const;

gtsam/nonlinear/Key.h

@@ -31,6 +31,9 @@
 
 namespace gtsam {
 
+  // Forward declarations
+  class Symbol;
+
 /**
  * TypedSymbol key class is templated on
  * 1) the type T it is supposed to retrieve, for extra type checking