diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index d0608059e..c30258919 100644 --- a/gtsam/base/DerivedValue.h +++ b/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::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::free((void*)this); } @@ -47,16 +49,16 @@ public: // Return the result of calling equals on the derived class return (static_cast(this))->equals(derivedValue2, tol); -// return CallDerivedEquals(this, p, tol); } /// Generic Value interface version of retract - virtual std::auto_ptr retract_(const Vector& delta) const { + virtual Value* retract_(const Vector& delta) const { // Call retract on the derived class const DERIVED retractResult = (static_cast(this))->retract(delta); // Create a Value pointer copy of the result - std::auto_ptr resultAsValue(new DERIVED(retractResult)); + void* resultAsValuePlace = boost::singleton_pool::malloc(); + Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult); // Return the pointer to the Value base class return resultAsValue; diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index dcb4a9c16..1e3c809c3 100644 --- a/gtsam/base/Value.h +++ b/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 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. diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h index b1d67de08..3c30b7c41 100644 --- a/gtsam/nonlinear/DynamicValues-inl.h +++ b/gtsam/nonlinear/DynamicValues-inl.h @@ -29,17 +29,17 @@ namespace gtsam { /* ************************************************************************* */ - 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(); - } + class ValueCloneAllocator { + public: + static Value* allocate_clone(const Value& a) { return a.clone_(); } + static void deallocate_clone(const Value* a) { a->deallocate_(); } + private: + ValueCloneAllocator() {} + }; /* ************************************************************************* */ - template - const Value& DynamicValues::at(const Symbol& j) const { + template + 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)) - throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value)); + if(typeid(*item->second) != typeid(ValueType)) + 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(*item->second); + return static_cast(*item->second); } /* ************************************************************************* */ @@ -66,18 +66,18 @@ namespace gtsam { } /* ************************************************************************* */ - template - boost::optional DynamicValues::exists(const Symbol& j) const { + template + boost::optional 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)) - throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value)); + if(typeid(*item->second) != typeid(ValueType)) + 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(*item->second); + return static_cast(*item->second); } else { return boost::none; } @@ -96,7 +96,7 @@ namespace gtsam { /* ************************************************************************* */ template void DynamicValues::insert(const Symbol& j, const ValueType& val) { - std::pair insertResult = values_.insert(make_pair(j, new ValueType(val))); + std::pair insertResult = values_.insert(j, val); if(!insertResult.second) throw DynamicValuesKeyAlreadyExists(j); } @@ -104,9 +104,17 @@ namespace gtsam { /* ************************************************************************* */ template 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(*item->second); + + // Assign + valueAsDerived = val; } } diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp index 1e7996788..c77894666 100644 --- a/gtsam/nonlinear/DynamicValues.cpp +++ b/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; - BOOST_FOREACH(const KeyValueMap::value_type& key_value, values_) { - cout << " " << (string)key_value.first << ": "; - key_value.second->print(""); + cout << str << "Values with " << size() << " values:\n" << endl; + for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) { + cout << " " << (string)key_value->first << ": "; + 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_) { - const SubVector& singleDelta = delta[ordering[key_value.first]]; // Delta for this value - ValuePtr retractedValue(key_value.second->retract_(singleDelta)); // Retract - result.values_[key_value.first] = retractedValue; // Add retracted result directly to result 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 + Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument + 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_) { - values_.insert(make_pair(key_value.first, key_value.second->clone_())); + for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) { + 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) { - this->update(key_value.first, *key_value.second); + for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { + this->update(key_value->first, *key_value->second); } } @@ -128,9 +132,10 @@ namespace gtsam { /* ************************************************************************* */ FastList DynamicValues::keys() const { - return FastList( - boost::make_transform_iterator(values_.begin(), boost::bind(&KeyValuePair::first, _1)), - boost::make_transform_iterator(values_.end(), boost::bind(&KeyValuePair::first, _1))); + FastList result; + for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) + 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_) { - ordering->insert(key_value.first, firstVar++); + for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) { + 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(); + } + } diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h index 745384cd7..de9a42b84 100644 --- a/gtsam/nonlinear/DynamicValues.h +++ b/gtsam/nonlinear/DynamicValues.h @@ -27,6 +27,9 @@ #include #include +#include +#include + #include #include #include @@ -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 ValuePtr; - typedef FastMap > KeyValueMap; - typedef KeyValueMap::value_type KeyValuePair; + // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined + // below) to clone and deallocate the Value objects, and a boost + // 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, + ValueCloneAllocator, + boost::fast_pool_allocator > > 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 - const Value& at(const Symbol& j) const; + template + 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 - boost::optional exists(const Symbol& j) const; + template + boost::optional 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. diff --git a/gtsam/nonlinear/tests/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp index 4cd9e8068..cd76389ec 100644 --- a/gtsam/nonlinear/tests/testDynamicValues.cpp +++ b/gtsam/nonlinear/tests/testDynamicValues.cpp @@ -29,7 +29,6 @@ using namespace std; static double inf = std::numeric_limits::infinity(); typedef TypedSymbol VecKey; -typedef DynamicValues 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); + 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 v = config0.exists_(key1); + boost::optional v = config0.exists(key1); CHECK(assert_equal(Vector_(1, 1.),*v)); }