More implementation for DynamicValues - switched to ptr_map

gtsam/base/DerivedValue.h

@@ -17,15 +17,16 @@ private:
 	/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
 	struct PoolTag { };
 
-public:
+protected:
-
 	DerivedValue() {}
 
+public:
+
 	virtual ~DerivedValue() {}
 
   /**
    * Create a duplicate object returned as a pointer to the generic Value interface.
-   * For the shake of performance, this function use singleton pool allocator instead of the normal heap allocator
+   * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator
    */
   virtual Value* clone_() const {
   	void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
@@ -34,9 +35,10 @@ public:
   }
 
   /**
-   * Create a duplicate object returned as a pointer to the generic Value interface
+   * Destroy and deallocate this object, only if it was originally allocated using clone_().
    */
   virtual void deallocate_() const {
+    this->~Value();
   	boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
   }
 
@@ -47,16 +49,16 @@ public:
 
     // Return the result of calling equals on the derived class
     return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
-//  	return CallDerivedEquals(this, p, tol);
   }
 
   /// Generic Value interface version of retract
-	virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
+	virtual Value* retract_(const Vector& delta) const {
     // Call retract on the derived class
     const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
 
     // Create a Value pointer copy of the result
-    std::auto_ptr<Value> resultAsValue(new DERIVED(retractResult));
+    void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
+    Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);
 
     // Return the pointer to the Value base class
     return resultAsValue;

gtsam/base/Value.h

@@ -126,7 +126,7 @@ namespace gtsam {
      * @param delta The delta vector in the tangent space of this value, by
      * which to increment this value.
      */
-    virtual std::auto_ptr<Value> retract_(const Vector& delta) const = 0;
+    virtual Value* retract_(const Vector& delta) const = 0;
 
     /** Compute the coordinates in the tangent space of this value that
      * retract() would map to \c value.

gtsam/nonlinear/DynamicValues-inl.h

@@ -29,17 +29,17 @@
 namespace gtsam {
 
   /* ************************************************************************* */
-  const char* DynamicValuesIncorrectType::what() const throw() {
+  class ValueCloneAllocator {
-    if(message_.empty())
+  public:
-      message_ =
+    static Value* allocate_clone(const Value& a) { return a.clone_(); }
-          "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " +
+    static void deallocate_clone(const Value* a) { a->deallocate_(); }
-          std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
+  private:
-    return message_.c_str();
+    ValueCloneAllocator() {}
-  }
+  };
 
   /* ************************************************************************* */
-  template<typename Value>
+  template<typename ValueType>
-  const Value& DynamicValues::at(const Symbol& j) const {
+  const ValueType& DynamicValues::at(const Symbol& j) const {
     // Find the item
     const_iterator item = values_.find(j);
 
@@ -48,11 +48,11 @@ namespace gtsam {
       throw DynamicValuesKeyDoesNotExist("retrieve", j);
 
     // Check the type and throw exception if incorrect
-    if(typeid(*item->second) != typeid(Value))
+    if(typeid(*item->second) != typeid(ValueType))
-      throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
+      throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
 
     // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
-    return static_cast<const Value&>(*item->second);
+    return static_cast<const ValueType&>(*item->second);
   }
 
   /* ************************************************************************* */
@@ -66,18 +66,18 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template<typename Value>
+  template<typename ValueType>
-  boost::optional<const Value&> DynamicValues::exists(const Symbol& j) const {
+  boost::optional<const ValueType&> DynamicValues::exists(const Symbol& j) const {
     // Find the item
     const_iterator item = values_.find(j);
 
     if(item != values_.end()) {
       // Check the type and throw exception if incorrect
-      if(typeid(*item->second) != typeid(Value))
+      if(typeid(*item->second) != typeid(ValueType))
-        throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
+        throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
 
       // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
-      return static_cast<const Value&>(*item->second);
+      return static_cast<const ValueType&>(*item->second);
     } else {
       return boost::none;
     }
@@ -96,7 +96,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class ValueType>
   void DynamicValues::insert(const Symbol& j, const ValueType& val) {
-    std::pair<iterator,bool> insertResult = values_.insert(make_pair(j, new ValueType(val)));
+    std::pair<iterator,bool> insertResult = values_.insert(j, val);
     if(!insertResult.second)
       throw DynamicValuesKeyAlreadyExists(j);
   }
@@ -104,9 +104,17 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class ValueType>
   void DynamicValues::update(const Symbol& j, const ValueType& val) {
+    // Find the value to update
     iterator item = values_.find(j);
     if(item == values_.end())
       throw DynamicValuesKeyDoesNotExist("update", j);
-    item->second = val.clone_();
+
+    // Cast to the derived type
+    if(typeid(*item->second) != typeid(Value))
+      throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
+    ValueType& valueAsDerived = static_cast<ValueType&>(*item->second);
+
+    // Assign
+    valueAsDerived = val;
   }
 }

gtsam/nonlinear/DynamicValues.cpp

@@ -41,10 +41,10 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void DynamicValues::print(const string& str) const {
-    cout << str << "DynamicValues with " << size() << " values:\n" << endl;
+    cout << str << "Values with " << size() << " values:\n" << endl;
-    BOOST_FOREACH(const KeyValueMap::value_type& key_value, values_) {
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
-      cout << "  " << (string)key_value.first << ": ";
+      cout << "  " << (string)key_value->first << ": ";
-      key_value.second->print("");
+      key_value->second->print("");
     }
   }
 
@@ -77,10 +77,12 @@ namespace gtsam {
   DynamicValues DynamicValues::retract(const VectorValues& delta, const Ordering& ordering) const {
     DynamicValues result;
 
-    BOOST_FOREACH(const KeyValuePair& key_value, values_) {
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
-      const SubVector& singleDelta = delta[ordering[key_value.first]]; // Delta for this value
+      const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
-      ValuePtr retractedValue(key_value.second->retract_(singleDelta)); // Retract
+      Symbol key = key_value->first;  // Non-const duplicate to deal with non-const insert argument
-      result.values_[key_value.first] = retractedValue; // Add retracted result directly to result values
+      Value* retractedValue(key_value->second->retract_(singleDelta)); // Retract
+      result.values_.insert(key, retractedValue); // Add retracted result directly to result values
+      retractedValue->deallocate_();
     }
 
     return result;
@@ -100,21 +102,23 @@ namespace gtsam {
     for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
       if(it1->first != it2->first)
         throw DynamicValuesMismatched(); // If keys do not match
-      result[ordering[it1->first]] = it1->second->localCoordinates_(*it2->second); // Will throw a dynamic_cast exception if types do not match
+      // Will throw a dynamic_cast exception if types do not match
+      result.insert(ordering[it1->first], it1->second->localCoordinates_(*it2->second));
     }
   }
 
   /* ************************************************************************* */
   void DynamicValues::insert(const DynamicValues& values) {
-    BOOST_FOREACH(const KeyValuePair& key_value, values.values_) {
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
-      values_.insert(make_pair(key_value.first, key_value.second->clone_()));
+      Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
+      values_.insert(key, key_value->second->clone_());
     }
   }
 
   /* ************************************************************************* */
   void DynamicValues::update(const DynamicValues& values) {
-    BOOST_FOREACH(const KeyValuePair& key_value, values) {
+    for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
-      this->update(key_value.first, *key_value.second);
+      this->update(key_value->first, *key_value->second);
     }
   }
 
@@ -128,9 +132,10 @@ namespace gtsam {
 
   /* ************************************************************************* */
   FastList<Symbol> DynamicValues::keys() const {
-    return FastList<Symbol>(
+    FastList<Symbol> result;
-        boost::make_transform_iterator(values_.begin(), boost::bind(&KeyValuePair::first, _1)),
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value)
-        boost::make_transform_iterator(values_.end(), boost::bind(&KeyValuePair::first, _1)));
+      result.push_back(key_value->first);
+    return result;
   }
 
   /* ************************************************************************* */
@@ -146,18 +151,45 @@ namespace gtsam {
     // 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, _1)))),
+            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))),
         boost::make_transform_iterator(values_.begin(),
-            boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second, _1)))));
+            boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))));
     return result;
   }
 
   /* ************************************************************************* */
   Ordering::shared_ptr DynamicValues::orderingArbitrary(Index firstVar) const {
     Ordering::shared_ptr ordering(new Ordering);
-    BOOST_FOREACH(const KeyValuePair& key_value, values_) {
+    for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
-      ordering->insert(key_value.first, firstVar++);
+      ordering->insert(key_value->first, firstVar++);
     }
     return ordering;
   }
+
+  /* ************************************************************************* */
+  const char* DynamicValuesKeyAlreadyExists::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
+    return message_.c_str();
+  }
+
+  /* ************************************************************************* */
+  const char* DynamicValuesKeyDoesNotExist::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to " + std::string(operation_) + " the key \"" +
+          (std::string)key_ + "\", which does not exist in the Values.";
+    return message_.c_str();
+  }
+
+  /* ************************************************************************* */
+  const char* DynamicValuesIncorrectType::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " +
+          std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
+    return message_.c_str();
+  }
+
 }

gtsam/nonlinear/DynamicValues.h

@@ -27,6 +27,9 @@
 #include <string>
 #include <utility>
 
+#include <boost/pool/pool_alloc.hpp>
+#include <boost/ptr_container/ptr_map.hpp>
+
 #include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/linear/VectorValues.h>
@@ -35,15 +38,39 @@
 
 namespace gtsam {
 
+  // Forward declarations
+  class ValueCloneAllocator;
+
+/**
+  * A non-templated config holding any types of Manifold-group elements.  A
+  * values structure is a map from keys to values. It is used to specify the
+  * value of a bunch of variables in a factor graph. A Values is a values
+  * structure which can hold variables that are elements on manifolds, not just
+  * vectors. It then, as a whole, implements a aggregate type which is also a
+  * manifold element, and hence supports operations dim, retract, and
+  * localCoordinates.
+  */
   class DynamicValues {
 
   private:
 
-    typedef std::auto_ptr<const Value> ValuePtr;
+    // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
-    typedef FastMap<Symbol, std::auto_ptr<const Value> > KeyValueMap;
+    // below) to clone and deallocate the Value objects, and a boost
-    typedef KeyValueMap::value_type KeyValuePair;
+    // fast_pool_allocator to allocate map nodes.  In this way, all memory is
+    // allocated in a boost memory pool.
+    typedef boost::ptr_map<
+        Symbol,
+        Value,
+        std::less<Symbol>,
+        ValueCloneAllocator,
+        boost::fast_pool_allocator<std::pair<const Symbol, void*> > > KeyValueMap;
+
+    // The member to store the values, see just above
     KeyValueMap values_;
 
+    // Type obtained by iterating
+    typedef KeyValueMap::const_iterator::value_type KeyValuePair;
+
   public:
 
     typedef KeyValueMap::iterator iterator;
@@ -75,8 +102,8 @@ namespace gtsam {
      * throws DynamicValuesIncorrectType if this requested type is not correct.
      * @return A const reference to the stored value
      */
-    template<typename Value>
+    template<typename ValueType>
-    const Value& at(const Symbol& j) const;
+    const ValueType& at(const Symbol& j) const;
 
     /** Retrieve a variable using a special key (typically TypedSymbol), which
      * contains the type of the value associated with the key, and which must
@@ -102,8 +129,8 @@ namespace gtsam {
      * \c Value if the key does exist, or boost::none if it does not exist.
      * Throws DynamicValuesIncorrectType if the value type associated with the
      * requested key does not match the stored value type. */
-    template<typename Value>
+    template<typename ValueType>
-    boost::optional<const Value&> exists(const Symbol& j) const;
+    boost::optional<const ValueType&> exists(const Symbol& j) const;
 
     /** Check if a value with key \c j exists, returns the value with type
      * \c Value if the key does exist, or boost::none if it does not exist.

gtsam/nonlinear/tests/testDynamicValues.cpp

@@ -29,7 +29,6 @@ using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
 
 typedef TypedSymbol<LieVector, 'v'> VecKey;
-typedef DynamicValues<VecKey> DynamicValues;
 
 VecKey key1(1), key2(2), key3(3), key4(4);
 
@@ -102,7 +101,7 @@ TEST( DynamicValues, insert_bad )
   cfg2.insert(key2, v3);
   cfg2.insert(key3, v4);
 
-  CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists<VecKey>);
+  CHECK_EXCEPTION(cfg1.insert(cfg2), DynamicValuesKeyAlreadyExists);
 }
 
 /* ************************************************************************* */
@@ -237,7 +236,7 @@ TEST(DynamicValues, exists_)
 	config0.insert(key1, Vector_(1, 1.));
 	config0.insert(key2, Vector_(1, 2.));
 
-	boost::optional<LieVector> v = config0.exists_(key1);
+	boost::optional<const LieVector&> v = config0.exists(key1);
 	CHECK(assert_equal(Vector_(1, 1.),*v));
 }