From d323d5b963fdb8b7203b985bbaf6fe08ea14d0ec Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 15 Jan 2012 23:06:01 +0000 Subject: [PATCH] (in branch) more implementation for DynamicValues --- gtsam/nonlinear/DynamicValues-inl.h | 70 +++++++++++++++++++++++++++++ gtsam/nonlinear/DynamicValues.cpp | 59 +++++++++++++++++++++++- gtsam/nonlinear/DynamicValues.h | 39 +++++++++------- gtsam/nonlinear/Key.h | 10 +++++ 4 files changed, 161 insertions(+), 17 deletions(-) diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h index 451077d9c..d54cf8b47 100644 --- a/gtsam/nonlinear/DynamicValues-inl.h +++ b/gtsam/nonlinear/DynamicValues-inl.h @@ -35,4 +35,74 @@ namespace gtsam { return message_.c_str(); } + /* ************************************************************************* */ + template + const Value& DynamicValues::at(const Symbol& j) const { + // Find the item + const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + 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)); + + // We have already checked the type, so do a "blind" static_cast, not dynamic_cast + return static_cast(*item->second); + } + + /* ************************************************************************* */ + template + const typename TypedKey::Value& DynamicValues::at(const TypedKey& j) const { + // Convert to Symbol + const Symbol symbol(j.symbol()); + + // Call at with the Value type from the key + return at(symbol); + } + + /* ************************************************************************* */ + 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)); + + // We have already checked the type, so do a "blind" static_cast, not dynamic_cast + return static_cast(*item->second); + } else { + return boost::none; + } + } + + /* ************************************************************************* */ + template + boost::optional exists(const TypedKey& j) const { + // Convert to Symbol + const Symbol symbol(j.symbol()); + + // Call exists with the Value type from the key + return exists(symbol); + } + + /* ************************************************************************* */ + template + void DynamicValues::insert(const Symbol& j, const ValueType& val) { + pair insertResult = values_.insert(make_pair(j, ValuePtr(new ValueType(val)))); + if(!insertResult.second) + throw DynamicValuesKeyAlreadyExists(j); + } + + /* ************************************************************************* */ + void DynamicValues::insert(const DynamicValues& values) { + BOOST_FOREACH(const KeyValuePair& key_value, values) { + insert(key_value.first, key_value.) + } + } } diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp index 79270d060..90f41ddd8 100644 --- a/gtsam/nonlinear/DynamicValues.cpp +++ b/gtsam/nonlinear/DynamicValues.cpp @@ -30,6 +30,12 @@ using namespace std; namespace gtsam { + /* ************************************************************************* */ + DynamicValues::DynamicValues(const DynamicValues& other) { + + } + + /* ************************************************************************* */ void DynamicValues::print(const string& str) const { cout << str << "DynamicValues with " << size() << " values:\n" << endl; BOOST_FOREACH(const KeyValueMap::value_type& key_value, values_) { @@ -38,10 +44,61 @@ namespace gtsam { } } + /* ************************************************************************* */ bool DynamicValues::equals(const DynamicValues& other, double tol) const { if(this->size() != other.size()) return false; - for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) + for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) { + if(typeid(*it1->second) != typeid(*it2->second)) + return false; + if(it1->first != it2->first) + return false; + if(!it1->second->equals_(*it2->second, tol)) + return false; + } + return true; // We return false earlier if we find anything that does not match } + /* ************************************************************************* */ + bool DynamicValues::exists(const Symbol& j) const { + return values_.find(j) != values_.end(); + } + + /* ************************************************************************* */ + VectorValues DynamicValues::zeroVectors(const Ordering& ordering) const { + return VectorValues::Zero(this->dims(ordering)); + } + + /* ************************************************************************* */ + 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 + const std::auto_ptr retractedValue = key_value.second->retract_(singleDelta); // Retract + result.values_.insert(make_pair(key_value.first, retractedValue)); // Add retracted result directly to result values + } + + return result; + } + + /* ************************************************************************* */ + VectorValues DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering) const { + VectorValues result(this->dims(ordering)); + localCoordinates(cp, ordering, result); + return result; + } + + /* ************************************************************************* */ + void DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& result) const { + if(this->size() != cp.size()) + throw DynamicValuesMismatched(); + for(const_iterator it1=this->begin(), it2=other.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 + } + } + + } diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h index 747741bfb..27831fab9 100644 --- a/gtsam/nonlinear/DynamicValues.h +++ b/gtsam/nonlinear/DynamicValues.h @@ -38,7 +38,9 @@ namespace gtsam { private: - typedef FastMap > KeyValueMap; + typedef std::auto_ptr ValuePtr; + typedef FastMap > KeyValueMap; + typedef KeyValueMap::value_type KeyValuePair; KeyValueMap values_; public: @@ -52,7 +54,7 @@ namespace gtsam { DynamicValues() {} /** Copy constructor duplicates all keys and values */ - DynamicValues(const DynamicValues& other) {} + DynamicValues(const DynamicValues& other); /// @name Testable /// @{ @@ -93,14 +95,14 @@ namespace gtsam { /** Check if a value exists with key \c j. See exists<>(const Symbol& j) * and exists(const TypedKey& j) for versions that return the value if it * exists. */ - bool exists(const Symbol& i) const; + bool 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. * 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; + 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. @@ -111,7 +113,7 @@ namespace gtsam { * requested key does not match the stored value type. */ template - boost::optional exists(const TypedKey& j) const; + boost::optional exists(const TypedKey& j) const; /** The number of variables in this config */ size_t size() const { return values_.size(); } @@ -120,7 +122,7 @@ namespace gtsam { bool empty() const { return values_.empty(); } /** Get a zero VectorValues of the correct structure */ - VectorValues zero(const Ordering& ordering) const; + VectorValues zeroVectors(const Ordering& ordering) const; const_iterator begin() const { return values_.begin(); } const_iterator end() const { return values_.end(); } @@ -134,9 +136,6 @@ namespace gtsam { /// @name Manifold Operations /// @{ - /** The dimensionality of the tangent space */ - size_t dim() const; - /** Add a delta config to current config and returns a new config */ DynamicValues retract(const VectorValues& delta, const Ordering& ordering) const; @@ -220,28 +219,23 @@ namespace gtsam { }; /* ************************************************************************* */ - template class DynamicValuesKeyAlreadyExists : public std::exception { protected: const Symbol key_; ///< The key that already existed - const Value value_; ///< The value attempted to be inserted private: mutable std::string message_; public: /// Construct with the key-value pair attemped to be added - DynamicValuesKeyAlreadyExists(const Symbol& key, const Value& value) throw() : - key_(key), value_(value) {} + DynamicValuesKeyAlreadyExists(const Symbol& key) throw() : + key_(key) {} virtual ~DynamicValuesKeyAlreadyExists() throw() {} /// The duplicate key that was attemped to be added const Symbol& key() const throw() { return key_; } - /// The value that was attempted to be added - const Value& value() const throw() { return value_; } - /// The message to be displayed to the user virtual const char* what() const throw(); }; @@ -300,6 +294,19 @@ namespace gtsam { virtual const char* what() const throw(); }; + /* ************************************************************************* */ + class DynamicValuesMismatched : public std::exception { + + public: + DynamicValuesMismatched() throw() {} + + virtual ~DynamicValuesMismatched() throw() {} + + virtual const char* what() const throw() { + return "The Values 'this' and the argument passed to DynamicValues::localCoordinates have mismatched keys and values"; + } + }; + } #include diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h index de609d413..a4624b640 100644 --- a/gtsam/nonlinear/Key.h +++ b/gtsam/nonlinear/Key.h @@ -76,6 +76,7 @@ public: std::string latex() const { return (boost::format("%c_{%d}") % C % j_).str(); } + Symbol symbol() const; // logic: @@ -298,6 +299,9 @@ public: std::string label_s = (boost::format("%1%") % label_).str(); return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str(); } + Symbol symbol() const { + return Symbol(*this); + } // Needed for conversion to LabeledSymbol size_t convertLabel() const { @@ -354,5 +358,11 @@ private: } }; +/* ************************************************************************* */ +template +Symbol TypedSymbol::symbol() const { + return Symbol(*this); +} + } // namespace gtsam