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

More implementation for DynamicValues - switched to ptr_map

release/4.3a0
Richard Roberts 2012-01-27 15:34:54 +00:00
parent 9e00963d54
commit 6dde91f636
6 changed files with 125 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/base/DerivedValue.h
View File

@ -17,15 +17,16 @@ private:
/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
struct PoolTag { };
public:
protected:
DerivedValue() {}
public:
virtual ~DerivedValue() {}
/**
* Create a duplicate object returned as a pointer to the generic Value interface.
* For the shake of performance, this function use singleton pool allocator instead of the normal heap allocator
* For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator
*/
virtual Value* clone_() const {
void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
@ -34,9 +35,10 @@ public:
}
/**
* Create a duplicate object returned as a pointer to the generic Value interface
* Destroy and deallocate this object, only if it was originally allocated using clone_().
*/
virtual void deallocate_() const {
this->~Value();
boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
}
@ -47,16 +49,16 @@ public:
// Return the result of calling equals on the derived class
return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
// return CallDerivedEquals(this, p, tol);
}
/// Generic Value interface version of retract
virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
virtual Value* retract_(const Vector& delta) const {
// Call retract on the derived class
const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
// Create a Value pointer copy of the result
std::auto_ptr<Value> resultAsValue(new DERIVED(retractResult));
void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);
// Return the pointer to the Value base class
return resultAsValue;

2
gtsam/base/Value.h
View File

@ -126,7 +126,7 @@ namespace gtsam {
* @param delta The delta vector in the tangent space of this value, by
* which to increment this value.
*/
virtual std::auto_ptr<Value> retract_(const Vector& delta) const = 0;
virtual Value* retract_(const Vector& delta) const = 0;
/** Compute the coordinates in the tangent space of this value that
* retract() would map to \c value.

46
gtsam/nonlinear/DynamicValues-inl.h
View File

@ -29,17 +29,17 @@
namespace gtsam {
/* ************************************************************************* */
const char* DynamicValuesIncorrectType::what() const throw() {
if(message_.empty())
message_ =
"Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " +
std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
return message_.c_str();
}
class ValueCloneAllocator {
public:
static Value* allocate_clone(const Value& a) { return a.clone_(); }
static void deallocate_clone(const Value* a) { a->deallocate_(); }
private:
ValueCloneAllocator() {}
};
/* ************************************************************************* */
template<typename Value>
const Value& DynamicValues::at(const Symbol& j) const {
template<typename ValueType>
const ValueType& DynamicValues::at(const Symbol& j) const {
// Find the item
const_iterator item = values_.find(j);
@ -48,11 +48,11 @@ namespace gtsam {
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));
if(typeid(*item->second) != typeid(ValueType))
throw DynamicValuesIncorrectType(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 Value&>(*item->second);
return static_cast<const ValueType&>(*item->second);
}
/* ************************************************************************* */
@ -66,18 +66,18 @@ namespace gtsam {
}
/* ************************************************************************* */
template<typename Value>
boost::optional<const Value&> DynamicValues::exists(const Symbol& j) const {
template<typename ValueType>
boost::optional<const ValueType&> 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));
if(typeid(*item->second) != typeid(ValueType))
throw DynamicValuesIncorrectType(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 Value&>(*item->second);
return static_cast<const ValueType&>(*item->second);
} else {
return boost::none;
}
@ -96,7 +96,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class ValueType>
void DynamicValues::insert(const Symbol& j, const ValueType& val) {
std::pair<iterator,bool> insertResult = values_.insert(make_pair(j, new ValueType(val)));
std::pair<iterator,bool> insertResult = values_.insert(j, val);
if(!insertResult.second)
throw DynamicValuesKeyAlreadyExists(j);
}
@ -104,9 +104,17 @@ namespace gtsam {
/* ************************************************************************* */
template<class ValueType>
void DynamicValues::update(const Symbol& j, const ValueType& val) {
// Find the value to update
iterator item = values_.find(j);
if(item == values_.end())
throw DynamicValuesKeyDoesNotExist("update", j);
item->second = val.clone_();
// Cast to the derived type
if(typeid(*item->second) != typeid(Value))
throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
ValueType& valueAsDerived = static_cast<ValueType&>(*item->second);
// Assign
valueAsDerived = val;
}
}

72
gtsam/nonlinear/DynamicValues.cpp
View File

@ -41,10 +41,10 @@ namespace gtsam {
/* ************************************************************************* */
void DynamicValues::print(const string& str) const {
cout << str << "DynamicValues with " << size() << " values:\n" << endl;
BOOST_FOREACH(const KeyValueMap::value_type& key_value, values_) {
cout << " " << (string)key_value.first << ": ";
key_value.second->print("");
cout << str << "Values with " << size() << " values:\n" << endl;
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
cout << " " << (string)key_value->first << ": ";
key_value->second->print("");
}
}
@ -77,10 +77,12 @@ namespace gtsam {
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
ValuePtr retractedValue(key_value.second->retract_(singleDelta)); // Retract
result.values_[key_value.first] = retractedValue; // Add retracted result directly to result values
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->second->retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values
retractedValue->deallocate_();
}
return result;
@ -100,21 +102,23 @@ namespace gtsam {
for(const_iterator it1=this->begin(), it2=cp.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
// Will throw a dynamic_cast exception if types do not match
result.insert(ordering[it1->first], it1->second->localCoordinates_(*it2->second));
}
}
/* ************************************************************************* */
void DynamicValues::insert(const DynamicValues& values) {
BOOST_FOREACH(const KeyValuePair& key_value, values.values_) {
values_.insert(make_pair(key_value.first, key_value.second->clone_()));
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
values_.insert(key, key_value->second->clone_());
}
}
/* ************************************************************************* */
void DynamicValues::update(const DynamicValues& values) {
BOOST_FOREACH(const KeyValuePair& key_value, values) {
this->update(key_value.first, *key_value.second);
for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
this->update(key_value->first, *key_value->second);
}
}
@ -128,9 +132,10 @@ namespace gtsam {
/* ************************************************************************* */
FastList<Symbol> DynamicValues::keys() const {
return FastList<Symbol>(
boost::make_transform_iterator(values_.begin(), boost::bind(&KeyValuePair::first, _1)),
boost::make_transform_iterator(values_.end(), boost::bind(&KeyValuePair::first, _1)));
FastList<Symbol> result;
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value)
result.push_back(key_value->first);
return result;
}
/* ************************************************************************* */
@ -146,18 +151,45 @@ namespace gtsam {
// Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
result.assign(
boost::make_transform_iterator(values_.begin(),
boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second, _1)))),
boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))),
boost::make_transform_iterator(values_.begin(),
boost::bind(&Value::dim, boost::bind(&ValuePtr::get, boost::bind(&KeyValuePair::second, _1)))));
boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))));
return result;
}
/* ************************************************************************* */
Ordering::shared_ptr DynamicValues::orderingArbitrary(Index firstVar) const {
Ordering::shared_ptr ordering(new Ordering);
BOOST_FOREACH(const KeyValuePair& key_value, values_) {
ordering->insert(key_value.first, firstVar++);
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
ordering->insert(key_value->first, firstVar++);
}
return ordering;
}
/* ************************************************************************* */
const char* DynamicValuesKeyAlreadyExists::what() const throw() {
if(message_.empty())
message_ =
"Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
return message_.c_str();
}
/* ************************************************************************* */
const char* DynamicValuesKeyDoesNotExist::what() const throw() {
if(message_.empty())
message_ =
"Attempting to " + std::string(operation_) + " the key \"" +
(std::string)key_ + "\", which does not exist in the Values.";
return message_.c_str();
}
/* ************************************************************************* */
const char* DynamicValuesIncorrectType::what() const throw() {
if(message_.empty())
message_ =
"Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " +
std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
return message_.c_str();
}
}

41
gtsam/nonlinear/DynamicValues.h
View File

@ -27,6 +27,9 @@
#include <string>
#include <utility>
#include <boost/pool/pool_alloc.hpp>
#include <boost/ptr_container/ptr_map.hpp>
#include <gtsam/base/Value.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/linear/VectorValues.h>
@ -35,15 +38,39 @@
namespace gtsam {
// Forward declarations
class ValueCloneAllocator;
/**
* A non-templated config holding any types of Manifold-group elements. A
* values structure is a map from keys to values. It is used to specify the
* value of a bunch of variables in a factor graph. A Values is a values
* structure which can hold variables that are elements on manifolds, not just
* vectors. It then, as a whole, implements a aggregate type which is also a
* manifold element, and hence supports operations dim, retract, and
* localCoordinates.
*/
class DynamicValues {
private:
typedef std::auto_ptr<const Value> ValuePtr;
typedef FastMap<Symbol, std::auto_ptr<const Value> > KeyValueMap;
typedef KeyValueMap::value_type KeyValuePair;
// Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
// below) to clone and deallocate the Value objects, and a boost
// fast_pool_allocator to allocate map nodes. In this way, all memory is
// allocated in a boost memory pool.
typedef boost::ptr_map<
Symbol,
Value,
std::less<Symbol>,
ValueCloneAllocator,
boost::fast_pool_allocator<std::pair<const Symbol, void*> > > KeyValueMap;
// The member to store the values, see just above
KeyValueMap values_;
// Type obtained by iterating
typedef KeyValueMap::const_iterator::value_type KeyValuePair;
public:
typedef KeyValueMap::iterator iterator;
@ -75,8 +102,8 @@ namespace gtsam {
* throws DynamicValuesIncorrectType if this requested type is not correct.
* @return A const reference to the stored value
*/
template<typename Value>
const Value& at(const Symbol& j) const;
template<typename ValueType>
const ValueType& at(const Symbol& j) const;
/** Retrieve a variable using a special key (typically TypedSymbol), which
* contains the type of the value associated with the key, and which must
@ -102,8 +129,8 @@ namespace gtsam {
* \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<const Value&> exists(const Symbol& j) const;
template<typename ValueType>
boost::optional<const ValueType&> 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.

5
gtsam/nonlinear/tests/testDynamicValues.cpp
View File

@ -29,7 +29,6 @@ using namespace std;
static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef DynamicValues<VecKey> DynamicValues;
VecKey key1(1), key2(2), key3(3), key4(4);
@ -102,7 +101,7 @@ TEST( DynamicValues, insert_bad )
cfg2.insert(key2, v3);
cfg2.insert(key3, v4);
CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists<VecKey>);
CHECK_EXCEPTION(cfg1.insert(cfg2), DynamicValuesKeyAlreadyExists);
}
/* ************************************************************************* */
@ -237,7 +236,7 @@ TEST(DynamicValues, exists_)
config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.));
boost::optional<LieVector> v = config0.exists_(key1);
boost::optional<const LieVector&> v = config0.exists(key1);
CHECK(assert_equal(Vector_(1, 1.),*v));
}