From 9cdb1e08fe936c5bfdc58eddcd18a28eb7643808 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 16 Jan 2012 22:54:19 +0000 Subject: [PATCH] (in branch) more implementation for DynamicValues --- gtsam/base/Value.h | 3 ++ gtsam/geometry/Rot3.h | 5 +++ gtsam/nonlinear/DynamicValues-inl.h | 10 +++-- gtsam/nonlinear/DynamicValues.cpp | 60 ++++++++++++++++++++++++++++- gtsam/nonlinear/DynamicValues.h | 31 +++------------ gtsam/nonlinear/Key.h | 3 ++ 6 files changed, 81 insertions(+), 31 deletions(-) diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 3a4b56c72..eb86e69f1 100644 --- a/gtsam/base/Value.h +++ b/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 clone_() const = 0; + /** Print this value, for debugging and unit tests */ virtual void print(const std::string& str = "") const = 0; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 4f5322961..d2e72abeb 100644 --- a/gtsam/geometry/Rot3.h +++ b/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 clone_() const { return std::auto_ptr(new Rot3(*this)); } + /// @} /// @name Testable /// @{ diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h index d54cf8b47..9c81b0293 100644 --- a/gtsam/nonlinear/DynamicValues-inl.h +++ b/gtsam/nonlinear/DynamicValues-inl.h @@ -100,9 +100,11 @@ namespace gtsam { } /* ************************************************************************* */ - void DynamicValues::insert(const DynamicValues& values) { - BOOST_FOREACH(const KeyValuePair& key_value, values) { - insert(key_value.first, key_value.) - } + template + void DynamicValues::update(const Symbol& j, const ValueType& val) { + iterator item = values_.find(j); + if(item == values_end) + throw DynamicValuesKeyDoesNotExist("update", j); + item->second = val.clone_(); } } diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp index 90f41ddd8..6a93a29f5 100644 --- a/gtsam/nonlinear/DynamicValues.cpp +++ b/gtsam/nonlinear/DynamicValues.cpp @@ -24,7 +24,10 @@ #include +#include + #include +#include 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 DynamicValues::keys() const { + return list( + 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 DynamicValues::dims(const Ordering& ordering) const { + vector 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; + } } diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h index 27831fab9..7d7dbadd7 100644 --- a/gtsam/nonlinear/DynamicValues.h +++ b/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 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 @@ -176,34 +176,13 @@ namespace gtsam { * Returns a set of keys in the config * Note: by construction, the list is ordered */ - std::list keys() const; + FastList keys() const; /** Replace all keys and variables */ - DynamicValues& operator=(const DynamicValues& rhs) { - values_ = rhs.values_; - return (*this); - } + DynamicValues& operator=(const DynamicValues& rhs); /** Remove all variables from the config */ - void clear() { - values_.clear(); - } - - /** - * Apply a class with an application operator() to a const_iterator over - * every pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). - */ - template - void apply(A& operation) { - for(iterator it = begin(); it != end(); ++it) - operation(it); - } - template - void apply(A& operation) const { - for(const_iterator it = begin(); it != end(); ++it) - operation(it); - } + void clear() { values_.clear(); } /** Create an array of variable dimensions using the given ordering */ std::vector dims(const Ordering& ordering) const; diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h index a4624b640..dcccc1b44 100644 --- a/gtsam/nonlinear/Key.h +++ b/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