(in branch) more implementation for DynamicValues

gtsam/nonlinear/DynamicValues-inl.h

@@ -35,4 +35,74 @@ namespace gtsam {
     return message_.c_str();
   }
 
+  /* ************************************************************************* */
+  template<typename Value>
+  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<const Value&>(*item->second);
+  }
+
+  /* ************************************************************************* */
+  template<typename TypedKey>
+  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<typename TypedKey::Value>(symbol);
+  }
+
+  /* ************************************************************************* */
+  template<typename Value>
+  boost::optional<Value> 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<const Value&>(*item->second);
+    } else {
+      return boost::none;
+    }
+  }
+
+  /* ************************************************************************* */
+  template<class TypedKey>
+  boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const {
+    // Convert to Symbol
+    const Symbol symbol(j.symbol());
+
+    // Call exists with the Value type from the key
+    return exists<typename TypedKey::Value>(symbol);
+  }
+
+  /* ************************************************************************* */
+  template<class ValueType>
+  void DynamicValues::insert(const Symbol& j, const ValueType& val) {
+    pair<iterator,bool> 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.)
+    }
+  }
 }

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<const Value> 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
+    }
+  }
+
+
 }

gtsam/nonlinear/DynamicValues.h

@@ -38,7 +38,9 @@ namespace gtsam {
 
   private:
 
-    typedef FastMap<Symbol, std::auto_ptr<Value> > KeyValueMap;
+    typedef std::auto_ptr<const Value> ValuePtr;
+    typedef FastMap<Symbol, std::auto_ptr<const Value> > 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<typename Value>
-    boost::optional<Value> exists(const Symbol& j) const;
+    boost::optional<const Value&> 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<class TypedKey>
-    boost::optional<typename TypedKey::Value> exists(const TypedKey& j) const;
+    boost::optional<const typename TypedKey::Value&> 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 ValueType>
   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() :
+    DynamicValuesKeyAlreadyExists(const Symbol& key) throw() :
-      key_(key), value_(value) {}
+      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 <gtsam/nonlinear/DynamicValues-inl.h>

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<class T, char C>
+Symbol TypedSymbol<T,C>::symbol() const {
+  return Symbol(*this);
+}
+
 } // namespace gtsam