GenericValue based on defined traits to replace DerivedValue, first implementation

gtsam/base/GenericValue.h

@@ -10,9 +10,10 @@
  * -------------------------------------------------------------------------- */
 
 /*
- * @file DerivedValue.h
+ * @file GenericValue.h
  * @date Jan 26, 2012
  * @author Duy Nguyen Ta
+ * @author Mike Bosse, Abel Gawel, Renaud Dube
  */
 
 #pragma once
@@ -40,15 +41,52 @@
 
 namespace gtsam {
 
-template<class DERIVED>
+namespace traits {
-class DerivedValue : public Value {
 
+// trait to wrap the default equals of types
+template<typename T>
+  bool equals(const T& a, const T& b, double tol) {
+    return a.equals(b,tol);
+  }
+
+// trait to compute the local coordinates of other with respect to origin
+template<typename T>
+Vector localCoordinates(const T& origin, const T& other) {
+  return origin.localCoordinates(other);
+}
+
+template<typename T>
+T retract(const T& origin, const Vector& delta) {
+  return origin.retract(delta);
+}
+
+template<typename T>
+void print(const T& obj, const std::string& str) {
+  obj.print(str);
+}
+
+template<typename T>
+size_t getDimension(const T& obj) {
+  return obj.dim();
+}
+}
+
+template<class T>
+class GenericValue : public Value {
+public:
+  typedef T ValueType;
+  typedef GenericValue This;
 protected:
-  DerivedValue() {}
+  T value_;
 
 public:
+  GenericValue() {}
+  GenericValue(const T& value) : value_(value) {}
 
-  virtual ~DerivedValue() {}
+  T& value() { return value_; }
+  const T& value() const { return value_; }
+
+  virtual ~GenericValue() {}
 
   /**
    * Create a duplicate object returned as a pointer to the generic Value interface.
@@ -56,8 +94,8 @@ public:
    * The result must be deleted with Value::deallocate_, not with the 'delete' operator.
    */
   virtual Value* clone_() const {
-    void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
+    void *place = boost::singleton_pool<PoolTag, sizeof(This)>::malloc();
-    DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
+    This* ptr = new(place) This(*this);
     return ptr;
   }
 
@@ -65,34 +103,35 @@ public:
    * Destroy and deallocate this object, only if it was originally allocated using clone_().
    */
   virtual void deallocate_() const {
-    this->~DerivedValue(); // Virtual destructor cleans up the derived object
+    this->~GenericValue(); // Virtual destructor cleans up the derived object
-    boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this); // Release memory from pool
+    boost::singleton_pool<PoolTag, sizeof(This)>::free((void*)this); // Release memory from pool
   }
 
   /**
    * Clone this value (normal clone on the heap, delete with 'delete' operator)
    */
   virtual boost::shared_ptr<Value> clone() const {
-    return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
+    return boost::make_shared<This>(*this);
   }
 
   /// equals implementing generic Value interface
   virtual bool equals_(const Value& p, double tol = 1e-9) const {
-    // Cast the base class Value pointer to a derived class pointer
+    // Cast the base class Value pointer to a templated generic class pointer
-    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(p);
+    const This& genericValue2 = dynamic_cast<const This&>(p);
+
+    // Return the result of using the equals traits for the derived class
+    return traits::equals<T>(this->value_, genericValue2.value_, tol);
 
-    // Return the result of calling equals on the derived class
-    return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
   }
 
   /// Generic Value interface version of retract
   virtual Value* retract_(const Vector& delta) const {
-    // Call retract on the derived class
+    // Call retract on the derived class using the retract trait function
-    const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
+    const T retractResult = traits::retract<T>(value_,delta);
 
     // Create a Value pointer copy of the result
-    void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
+    void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(This)>::malloc();
-    Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);
+    Value* resultAsValue = new(resultAsValuePlace) This(retractResult);
 
     // Return the pointer to the Value base class
     return resultAsValue;
@@ -100,44 +139,52 @@ public:
 
   /// Generic Value interface version of localCoordinates
   virtual Vector localCoordinates_(const Value& value2) const {
-    // Cast the base class Value pointer to a derived class pointer
+    // Cast the base class Value pointer to a templated generic class pointer
-    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(value2);
+    const This& genericValue2 = dynamic_cast<const This&>(value2);
 
-    // Return the result of calling localCoordinates on the derived class
+    // Return the result of calling localCoordinates trait on the derived class
-    return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
+    return traits::localCoordinates<T>(value_,genericValue2.value_);
+  }
+
+  virtual void print(const std::string& str) const {
+    traits::print<T>(value_,str);
+  }
+  virtual size_t dim() const {
+    return traits::getDimension<T>(value_); // need functional form here since the dimension may be dynamic
   }
 
   /// Assignment operator
   virtual Value& operator=(const Value& rhs) {
     // Cast the base class Value pointer to a derived class pointer
-    const DERIVED& derivedRhs = dynamic_cast<const DERIVED&>(rhs);
+    const This& derivedRhs = dynamic_cast<const This&>(rhs);
 
     // Do the assignment and return the result
-    return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
+    this->value_ = derivedRhs.value_;
+    return *this;
   }
 
   /// Conversion to the derived class
-  operator const DERIVED& () const {
+  operator const T& () const {
-    return static_cast<const DERIVED&>(*this);
+    return value_;
   }
 
   /// Conversion to the derived class
-  operator DERIVED& () {
+  operator T& () {
-    return static_cast<DERIVED&>(*this);
+    return value_;
   }
 
+
 protected:
   /// Assignment operator, protected because only the Value or DERIVED
   /// assignment operators should be used.
-  DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
+//  DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
-    // Nothing to do, do not call base class assignment operator
+//    // Nothing to do, do not call base class assignment operator
-    return *this;
+//    return *this;
-  }
+//  }
 
 private:
   /// Fake Tag struct for singleton pool allocator. In fact, it is never used!
   struct PoolTag { };
-
 };
 
 } /* namespace gtsam */

gtsam/nonlinear/Values-inl.h

@@ -52,6 +52,36 @@ namespace gtsam {
     _ValuesConstKeyValuePair(const _ValuesKeyValuePair<ValueType>& rhs) : key(rhs.key), value(rhs.value) {}
   };
 
+  /* ************************************************************************* */
+
+  // Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair
+  // need to use a struct here for later partial specialization
+  template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
+  struct ValuesCastHelper {
+    static CastedKeyValuePairType cast(KeyValuePairType key_value) {
+      // Static cast because we already checked the type during filtering
+      return CastedKeyValuePairType(key_value.key, const_cast<GenericValue<ValueType>&>(static_cast<const GenericValue<ValueType>&>(key_value.value)).value());
+    }
+  };
+  // partial specialized version for ValueType == Value
+  template<class CastedKeyValuePairType, class KeyValuePairType>
+  struct ValuesCastHelper<Value,CastedKeyValuePairType,KeyValuePairType> {
+    static CastedKeyValuePairType cast(KeyValuePairType key_value) {
+      // Static cast because we already checked the type during filtering
+      // in this case the casted and keyvalue pair are essentially the same type (key,Value&) so perhaps this could be done with just a cast of the key_value?
+      return CastedKeyValuePairType(key_value.key, key_value.value);
+    }
+  };
+  // partial specialized version for ValueType == Value
+  template<class CastedKeyValuePairType, class KeyValuePairType>
+  struct ValuesCastHelper<const Value,CastedKeyValuePairType,KeyValuePairType> {
+    static CastedKeyValuePairType cast(KeyValuePairType key_value) {
+      // Static cast because we already checked the type during filtering
+      // in this case the casted and keyvalue pair are essentially the same type (key,Value&) so perhaps this could be done with just a cast of the key_value?
+      return CastedKeyValuePairType(key_value.key, key_value.value);
+    }
+  };
+
   /* ************************************************************************* */
   template<class ValueType>
   class Values::Filtered {
@@ -99,19 +129,19 @@ namespace gtsam {
       begin_(boost::make_transform_iterator(
       boost::make_filter_iterator(
       filter, values.begin(), values.end()),
-      &castHelper<ValueType, KeyValuePair, Values::KeyValuePair>)),
+      &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)),
       end_(boost::make_transform_iterator(
       boost::make_filter_iterator(
       filter, values.end(), values.end()),
-      &castHelper<ValueType, KeyValuePair, Values::KeyValuePair>)),
+      &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)),
       constBegin_(boost::make_transform_iterator(
       boost::make_filter_iterator(
       filter, ((const Values&)values).begin(), ((const Values&)values).end()),
-      &castHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>)),
+      &ValuesCastHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>::cast)),
       constEnd_(boost::make_transform_iterator(
       boost::make_filter_iterator(
       filter, ((const Values&)values).end(), ((const Values&)values).end()),
-      &castHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>)) {}
+      &ValuesCastHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>::cast)) {}
 
     friend class Values;
     iterator begin_;
@@ -175,7 +205,7 @@ namespace gtsam {
   Values::Values(const Values::Filtered<ValueType>& view) {
     BOOST_FOREACH(const typename Filtered<ValueType>::KeyValuePair& key_value, view) {
       Key key = key_value.key;
-      insert(key, key_value.value);
+      insert<ValueType>(key, key_value.value);
     }
   }
 
@@ -184,7 +214,7 @@ namespace gtsam {
   Values::Values(const Values::ConstFiltered<ValueType>& view) {
     BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) {
       Key key = key_value.key;
-      insert(key, key_value.value);
+      insert<ValueType>(key, key_value.value);
     }
   }
 
@@ -214,6 +244,13 @@ namespace gtsam {
     return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
   }
 
+  /* ************************************************************************* */
+   template<>
+   inline bool Values::filterHelper<Value>(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
+     // Filter and check the type
+     return filter(key_value.key);
+   }
+
   /* ************************************************************************* */
   template<typename ValueType>
   const ValueType& Values::at(Key j) const {
@@ -225,11 +262,11 @@ namespace gtsam {
       throw ValuesKeyDoesNotExist("retrieve", j);
 
     // Check the type and throw exception if incorrect
-    if(typeid(*item->second) != typeid(ValueType))
+    if(typeid(*item->second) != typeid(GenericValue<ValueType>))
       throw ValuesIncorrectType(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 ValueType&>(*item->second);
+    return static_cast<const GenericValue<ValueType>&>(*item->second).value();
   }
 
   /* ************************************************************************* */
@@ -240,14 +277,20 @@ namespace gtsam {
 
     if(item != values_.end()) {
       // Check the type and throw exception if incorrect
-      if(typeid(*item->second) != typeid(ValueType))
+      if(typeid(*item->second) != typeid(GenericValue<ValueType>))
         throw ValuesIncorrectType(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 ValueType&>(*item->second);
+      return static_cast<const GenericValue<ValueType>&>(*item->second).value_;
     } else {
       return boost::none;
     }
   }
 
+  /* ************************************************************************* */
+  template<typename ValueType>
+  void Values::insert(Key j, const ValueType& val) {
+    insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
+  }
+
 }

gtsam/nonlinear/Values.h

@@ -45,6 +45,7 @@
 #include <utility>
 
 #include <gtsam/base/Value.h>
+#include <gtsam/base/GenericValue.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/inference/Key.h>
 
@@ -248,6 +249,12 @@ namespace gtsam {
     /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
     void insert(Key j, const Value& val);
 
+    /** Templated verion to add a variable with the given j,
+     * throws KeyAlreadyExists<J> if j is already present
+     * will wrap the val into a GenericValue<ValueType> to insert*/
+    template <typename ValueType> void insert(Key j, const ValueType& val);
+
+
     /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
     void insert(const Values& values);
 
@@ -259,6 +266,7 @@ namespace gtsam {
 
     /** single element change of existing element */
     void update(Key j, const Value& val);
+    template <typename T> void update(Key j, const T& val);
 
     /** update the current available values without adding new ones */
     void update(const Values& values);
@@ -369,15 +377,9 @@ namespace gtsam {
     // supplied \c filter function.
     template<class ValueType>
     static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
+      BOOST_STATIC_ASSERT((!std::is_same<ValueType,Value>::value));
       // Filter and check the type
-      return filter(key_value.key) && (typeid(ValueType) == typeid(key_value.value) || typeid(ValueType) == typeid(Value));
+      return filter(key_value.key) && (typeid(GenericValue<ValueType>) == typeid(key_value.value) );
-    }
-
-    // Cast to the derived ValueType
-    template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
-    static CastedKeyValuePairType castHelper(KeyValuePairType key_value) {
-      // Static cast because we already checked the type during filtering
-      return CastedKeyValuePairType(key_value.key, static_cast<ValueType&>(key_value.value));
     }
 
     /** Serialization function */