12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

GenericValue based on defined traits to replace DerivedValue, first implementation

release/4.3a0
Mike Bosse 2014-10-24 16:59:37 +02:00
parent 95827dd4d8
commit 0681212084
3 changed files with 143 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
gtsam/base/GenericValue.h
View File

@ -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>
class DerivedValue : public Value {
namespace traits {
// 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();
DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
void *place = boost::singleton_pool<PoolTag, sizeof(This)>::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<PoolTag, sizeof(DERIVED)>::free((void*)this); // Release memory from pool
this->~GenericValue(); // Virtual destructor cleans up the derived object
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
const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(p);
// Cast the base class Value pointer to a templated generic class pointer
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
const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
// Call retract on the derived class using the retract trait function
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();
Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);
void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(This)>::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<const DERIVED&>(value2);
// Cast the base class Value pointer to a templated generic class pointer
const This& genericValue2 = dynamic_cast<const This&>(value2);
// Return the result of calling localCoordinates on the derived class
return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
// Return the result of calling localCoordinates trait on the derived class
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 {
return static_cast<const DERIVED&>(*this);
operator const T& () const {
return value_;
}
/// Conversion to the derived class
operator DERIVED& () {
return static_cast<DERIVED&>(*this);
operator T& () {
return value_;
}
protected:
/// Assignment operator, protected because only the Value or DERIVED
/// assignment operators should be used.
DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
// Nothing to do, do not call base class assignment operator
return *this;
}
// DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& 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 */

63
gtsam/nonlinear/Values-inl.h
View File

@ -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)));
}
}

18
gtsam/nonlinear/Values.h
View File

@ -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));
}
// 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));
return filter(key_value.key) && (typeid(GenericValue<ValueType>) == typeid(key_value.value) );
}
/** Serialization function */