From 0681212084d2452e4cb99600f9d5e7dd9ed883ce Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Fri, 24 Oct 2014 16:59:37 +0200 Subject: [PATCH] GenericValue based on defined traits to replace DerivedValue, first implementation --- gtsam/base/GenericValue.h | 113 +++++++++++++++++++++++++---------- gtsam/nonlinear/Values-inl.h | 63 +++++++++++++++---- gtsam/nonlinear/Values.h | 18 +++--- 3 files changed, 143 insertions(+), 51 deletions(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 78155d308..f7b5e985a 100644 --- a/gtsam/base/GenericValue.h +++ b/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 DerivedValue : public Value { +namespace traits { +// trait to wrap the default equals of types +template + 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 +Vector localCoordinates(const T& origin, const T& other) { + return origin.localCoordinates(other); +} + +template +T retract(const T& origin, const Vector& delta) { + return origin.retract(delta); +} + +template +void print(const T& obj, const std::string& str) { + obj.print(str); +} + +template +size_t getDimension(const T& obj) { + return obj.dim(); +} +} + +template +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::malloc(); - DERIVED* ptr = new(place) DERIVED(static_cast(*this)); + void *place = boost::singleton_pool::malloc(); + 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 - boost::singleton_pool::free((void*)this); // Release memory from pool + this->~GenericValue(); // Virtual destructor cleans up the derived object + boost::singleton_pool::free((void*)this); // Release memory from pool } /** * Clone this value (normal clone on the heap, delete with 'delete' operator) */ virtual boost::shared_ptr clone() const { - return boost::make_shared(static_cast(*this)); + return boost::make_shared(*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 - const DERIVED& derivedValue2 = dynamic_cast(p); + // Cast the base class Value pointer to a templated generic class pointer + const This& genericValue2 = dynamic_cast(p); + + // Return the result of using the equals traits for the derived class + return traits::equals(this->value_, genericValue2.value_, tol); - // Return the result of calling equals on the derived class - return (static_cast(this))->equals(derivedValue2, tol); } /// Generic Value interface version of retract virtual Value* retract_(const Vector& delta) const { - // Call retract on the derived class - const DERIVED retractResult = (static_cast(this))->retract(delta); + // Call retract on the derived class using the retract trait function + const T retractResult = traits::retract(value_,delta); // Create a Value pointer copy of the result - void* resultAsValuePlace = boost::singleton_pool::malloc(); - Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult); + void* resultAsValuePlace = boost::singleton_pool::malloc(); + 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 - const DERIVED& derivedValue2 = dynamic_cast(value2); + // Cast the base class Value pointer to a templated generic class pointer + const This& genericValue2 = dynamic_cast(value2); - // Return the result of calling localCoordinates on the derived class - return (static_cast(this))->localCoordinates(derivedValue2); + // Return the result of calling localCoordinates trait on the derived class + return traits::localCoordinates(value_,genericValue2.value_); + } + + virtual void print(const std::string& str) const { + traits::print(value_,str); + } + virtual size_t dim() const { + return traits::getDimension(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(rhs); + const This& derivedRhs = dynamic_cast(rhs); // Do the assignment and return the result - return (static_cast(this))->operator=(derivedRhs); + this->value_ = derivedRhs.value_; + return *this; } /// Conversion to the derived class - operator const DERIVED& () const { - return static_cast(*this); + operator const T& () const { + return value_; } /// Conversion to the derived class - operator DERIVED& () { - return static_cast(*this); + operator T& () { + return value_; } + protected: /// Assignment operator, protected because only the Value or DERIVED /// assignment operators should be used. - DerivedValue& operator=(const DerivedValue& rhs) { - // Nothing to do, do not call base class assignment operator - return *this; - } +// DerivedValue& operator=(const DerivedValue& rhs) { +// // Nothing to do, do not call base class assignment operator +// return *this; +// } private: /// Fake Tag struct for singleton pool allocator. In fact, it is never used! struct PoolTag { }; - }; } /* namespace gtsam */ diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 7b812551e..11c44cad4 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -52,6 +52,36 @@ namespace gtsam { _ValuesConstKeyValuePair(const _ValuesKeyValuePair& 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 + 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&>(static_cast&>(key_value.value)).value()); + } + }; + // partial specialized version for ValueType == Value + template + struct ValuesCastHelper { + 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 + struct ValuesCastHelper { + 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 Values::Filtered { @@ -99,19 +129,19 @@ namespace gtsam { begin_(boost::make_transform_iterator( boost::make_filter_iterator( filter, values.begin(), values.end()), - &castHelper)), + &ValuesCastHelper::cast)), end_(boost::make_transform_iterator( boost::make_filter_iterator( filter, values.end(), values.end()), - &castHelper)), + &ValuesCastHelper::cast)), constBegin_(boost::make_transform_iterator( boost::make_filter_iterator( filter, ((const Values&)values).begin(), ((const Values&)values).end()), - &castHelper)), + &ValuesCastHelper::cast)), constEnd_(boost::make_transform_iterator( boost::make_filter_iterator( filter, ((const Values&)values).end(), ((const Values&)values).end()), - &castHelper)) {} + &ValuesCastHelper::cast)) {} friend class Values; iterator begin_; @@ -175,7 +205,7 @@ namespace gtsam { Values::Values(const Values::Filtered& view) { BOOST_FOREACH(const typename Filtered::KeyValuePair& key_value, view) { Key key = key_value.key; - insert(key, key_value.value); + insert(key, key_value.value); } } @@ -184,7 +214,7 @@ namespace gtsam { Values::Values(const Values::ConstFiltered& view) { BOOST_FOREACH(const typename ConstFiltered::KeyValuePair& key_value, view) { Key key = key_value.key; - insert(key, key_value.value); + insert(key, key_value.value); } } @@ -214,6 +244,13 @@ namespace gtsam { return ConstFiltered(boost::bind(&filterHelper, filterFcn, _1), *this); } + /* ************************************************************************* */ + template<> + inline bool Values::filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + // Filter and check the type + return filter(key_value.key); + } + /* ************************************************************************* */ template 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)) 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(*item->second); + return static_cast&>(*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)) 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(*item->second); + return static_cast&>(*item->second).value_; } else { return boost::none; } } + /* ************************************************************************* */ + template + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(GenericValue(val))); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 811846f79..5caa735f1 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -45,6 +45,7 @@ #include #include +#include #include #include @@ -248,6 +249,12 @@ namespace gtsam { /** Add a variable with the given j, throws KeyAlreadyExists if j is already present */ void insert(Key j, const Value& val); + /** Templated verion to add a variable with the given j, + * throws KeyAlreadyExists if j is already present + * will wrap the val into a GenericValue to insert*/ + template void insert(Key j, const ValueType& val); + + /** Add a set of variables, throws KeyAlreadyExists 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 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 static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + BOOST_STATIC_ASSERT((!std::is_same::value)); // Filter and check the type - return filter(key_value.key) && (typeid(ValueType) == typeid(key_value.value) || typeid(ValueType) == typeid(Value)); - } - - // Cast to the derived ValueType - template - static CastedKeyValuePairType castHelper(KeyValuePairType key_value) { - // Static cast because we already checked the type during filtering - return CastedKeyValuePairType(key_value.key, static_cast(key_value.value)); + return filter(key_value.key) && (typeid(GenericValue) == typeid(key_value.value) ); } /** Serialization function */