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

Fixed Values iterators and removed apply function

release/4.3a0
Richard Roberts 2012-02-02 22:06:33 +00:00
parent 26cdf28421
commit 4b2b9d8158
4 changed files with 94 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -117,22 +117,6 @@ struct ISAM2<CONDITIONAL, GRAPH>::Impl {
};
/* ************************************************************************* */
struct _VariableAdder {
Ordering& ordering;
Permuted<VectorValues>& vconfig;
Index nextVar;
_VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){}
template<typename I>
void operator()(I xIt) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
vconfig.permutation()[nextVar] = nextVar;
ordering.insert(xIt->first, nextVar);
if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl;
++ nextVar;
}
};
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
@ -151,8 +135,13 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
delta.permutation().resize(originalnVars + newTheta.size());
{
_VariableAdder vadder(ordering, delta, originalnVars);
newTheta.apply(vadder);
Index nextVar = originalnVars;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
delta.permutation()[nextVar] = nextVar;
ordering.insert(key_value.first, nextVar);
if(debug) cout << "Adding variable " << (string)key_value.first << " with order " << nextVar << endl;
++ nextVar;
}
assert(delta.permutation().size() == delta.container().size());
assert(ordering.nVars() == delta.size());
assert(ordering.size() == delta.size());
@ -222,41 +211,6 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::
}
}
/* ************************************************************************* */
// Internal class used in ExpmapMasked()
// This debug version sets delta entries that are applied to "Inf". The
// idea is that if a delta is applied, the variable is being relinearized,
// so the same delta should not be re-applied because it will be recalc-
// ulated. This is a debug check to prevent against a mix-up of indices
// or not keeping track of recalculated variables.
struct _SelectiveExpmapAndClear {
const Permuted<VectorValues>& delta;
const Ordering& ordering;
const vector<bool>& mask;
const boost::optional<Permuted<VectorValues>&> invalidate;
_SelectiveExpmapAndClear(const Permuted<VectorValues>& _delta, const Ordering& _ordering, const vector<bool>& _mask, boost::optional<Permuted<VectorValues>&> _invalidate) :
delta(_delta), ordering(_ordering), mask(_mask), invalidate(_invalidate) {}
template<typename I>
void operator()(I it_x) {
Index var = ordering[it_x->first];
if(ISDEBUG("ISAM2 update verbose")) {
if(mask[var])
cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
else
cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
}
assert(delta[var].size() == (int)it_x->second->dim());
assert(delta[var].unaryExpr(&isfinite<double>).all());
if(mask[var]) {
Value* retracted = it_x->second->retract_(delta[var]);
*it_x->second = *retracted;
retracted->deallocate_();
if(invalidate)
(*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
}
}
};
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
@ -268,8 +222,29 @@ void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permute
invalidateIfDebug = boost::optional<Permuted<VectorValues>&>();
#endif
_SelectiveExpmapAndClear selectiveExpmapper(delta, ordering, mask, invalidateIfDebug);
values.apply(selectiveExpmapper);
assert(values.size() == ordering.size());
Values::iterator key_value;
Ordering::const_iterator key_index;
for(key_value = values.begin(), key_index = ordering.begin();
key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) {
assert(key_value->first == key_index->first);
const Index var = key_index->second;
if(ISDEBUG("ISAM2 update verbose")) {
if(mask[var])
cout << "expmap " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
else
cout << " " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
}
assert(delta[var].size() == (int)key_value->second.dim());
assert(delta[var].unaryExpr(&isfinite<double>).all());
if(mask[var]) {
Value* retracted = key_value->second.retract_(delta[var]);
key_value->second = *retracted;
retracted->deallocate_();
if(invalidateIfDebug)
(*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
}
}
}
/* ************************************************************************* */

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

@ -41,7 +41,7 @@ namespace gtsam {
template<typename ValueType>
const ValueType& Values::at(const Symbol& j) const {
// Find the item
const_iterator item = values_.find(j);
KeyValueMap::const_iterator item = values_.find(j);
// Throw exception if it does not exist
if(item == values_.end())
@ -69,7 +69,7 @@ namespace gtsam {
template<typename ValueType>
boost::optional<const ValueType&> Values::exists(const Symbol& j) const {
// Find the item
const_iterator item = values_.find(j);
KeyValueMap::const_iterator item = values_.find(j);
if(item != values_.end()) {
// Check the type and throw exception if incorrect

46
gtsam/nonlinear/Values.cpp
View File

@ -42,9 +42,9 @@ namespace gtsam {
/* ************************************************************************* */
void Values::print(const string& str) const {
cout << str << "Values with " << size() << " values:\n" << endl;
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
cout << " " << (string)key_value->first << ": ";
key_value->second->print("");
key_value->second.print("");
}
}
@ -53,11 +53,11 @@ namespace gtsam {
if(this->size() != other.size())
return false;
for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
if(typeid(*it1->second) != typeid(*it2->second))
if(typeid(it1->second) != typeid(it2->second))
return false;
if(it1->first != it2->first)
return false;
if(!it1->second->equals_(*it2->second, tol))
if(!it1->second.equals_(it2->second, tol))
return false;
}
return true; // We return false earlier if we find anything that does not match
@ -77,10 +77,10 @@ namespace gtsam {
Values Values::retract(const VectorValues& delta, const Ordering& ordering) const {
Values result;
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
for(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
Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values
}
@ -102,7 +102,7 @@ namespace gtsam {
if(it1->first != it2->first)
throw DynamicValuesMismatched(); // If keys do not match
// Will throw a dynamic_cast exception if types do not match
result.insert(ordering[it1->first], it1->second->localCoordinates_(*it2->second));
result.insert(ordering[it1->first], it1->second.localCoordinates_(it2->second));
}
}
@ -116,16 +116,16 @@ namespace gtsam {
/* ************************************************************************* */
void Values::insert(const Values& values) {
for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
insert(key, *key_value->second);
insert(key, key_value->second);
}
}
/* ************************************************************************* */
void Values::update(const Symbol& j, const Value& val) {
// Find the value to update
iterator item = values_.find(j);
KeyValueMap::iterator item = values_.find(j);
if(item == values_.end())
throw ValuesKeyDoesNotExist("update", j);
@ -138,14 +138,14 @@ namespace gtsam {
/* ************************************************************************* */
void Values::update(const Values& values) {
for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
this->update(key_value->first, *key_value->second);
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
this->update(key_value->first, key_value->second);
}
}
/* ************************************************************************* */
void Values::erase(const Symbol& j) {
iterator item = values_.find(j);
KeyValueMap::iterator item = values_.find(j);
if(item == values_.end())
throw ValuesKeyDoesNotExist("erase", j);
values_.erase(item);
@ -154,7 +154,7 @@ namespace gtsam {
/* ************************************************************************* */
FastList<Symbol> Values::keys() const {
FastList<Symbol> result;
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value)
for(const_iterator key_value = begin(); key_value != end(); ++key_value)
result.push_back(key_value->first);
return result;
}
@ -168,23 +168,17 @@ namespace gtsam {
/* ************************************************************************* */
vector<size_t> Values::dims(const Ordering& ordering) const {
// vector<size_t> result(values_.size());
// // Transform with Value::dim(auto_ptr::get(KeyValuePair::second))
// result.assign(
// boost::make_transform_iterator(values_.begin(),
// boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))),
// boost::make_transform_iterator(values_.end(),
// boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))));
// return result;
_ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
vector<size_t> result(values_.size());
BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) {
result[ordering[key_value.first]] = key_value.second.dim();
}
return result;
}
/* ************************************************************************* */
Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const {
Ordering::shared_ptr ordering(new Ordering);
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
ordering->insert(key_value->first, firstVar++);
}
return ordering;

94
gtsam/nonlinear/Values.h
View File

@ -29,6 +29,9 @@
#include <boost/pool/pool_alloc.hpp>
#include <boost/ptr_container/ptr_map.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <gtsam/base/Value.h>
#include <gtsam/base/FastMap.h>
@ -41,19 +44,6 @@ namespace gtsam {
// Forward declarations
class ValueCloneAllocator;
struct _ValuesDimensionCollector {
const Ordering& ordering;
std::vector<size_t> dimensions;
_ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
template<typename I> void operator()(const I& key_value) {
Index var;
if(ordering.tryAt(key_value->first, var)) {
assert(var < dimensions.size());
dimensions[var] = key_value->second->dim();
}
}
};
/**
* 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
@ -81,15 +71,33 @@ namespace gtsam {
// The member to store the values, see just above
KeyValueMap values_;
// Type obtained by iterating
typedef KeyValueMap::const_iterator::value_type KeyValuePair;
// Types obtained by iterating
typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
public:
typedef KeyValueMap::iterator iterator;
typedef KeyValueMap::const_iterator const_iterator;
typedef KeyValueMap::reverse_iterator reverse_iterator;
typedef KeyValueMap::const_reverse_iterator const_reverse_iterator;
/// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
typedef std::pair<const Symbol&, const Value&> ConstKeyValuePair;
/// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
typedef std::pair<const Symbol&, Value&> KeyValuePair;
/// Mutable forward iterator, with value type KeyValuePair
typedef boost::transform_iterator<
boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
/// Const forward iterator, with value type ConstKeyValuePair
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_iterator> const_iterator;
/// Mutable reverse iterator, with value type KeyValuePair
typedef boost::transform_iterator<
boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::reverse_iterator> reverse_iterator;
/// Const reverse iterator, with value type ConstKeyValuePair
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
/** Default constructor creates an empty Values class */
Values() {}
@ -165,14 +173,21 @@ namespace gtsam {
/** Get a zero VectorValues of the correct structure */
VectorValues zeroVectors(const Ordering& ordering) const;
const_iterator begin() const { return values_.begin(); }
const_iterator end() const { return values_.end(); }
iterator begin() { return values_.begin(); }
iterator end() { return values_.end(); }
const_reverse_iterator rbegin() const { return values_.rbegin(); }
const_reverse_iterator rend() const { return values_.rend(); }
reverse_iterator rbegin() { return values_.rbegin(); }
reverse_iterator rend() { return values_.rend(); }
private:
static std::pair<const Symbol&, const Value&> make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
return std::make_pair<const Symbol&, const Value&>(key_value.first, *key_value.second); }
static std::pair<const Symbol&, Value&> make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
return std::make_pair<const Symbol&, Value&>(key_value.first, *key_value.second); }
public:
const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }
const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); }
iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); }
iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); }
const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); }
const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); }
reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
/// @name Manifold Operations
/// @{
@ -188,8 +203,6 @@ namespace gtsam {
///@}
// imperative methods:
/** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
void insert(const Symbol& j, const Value& val);
@ -205,12 +218,6 @@ namespace gtsam {
/** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
void erase(const Symbol& j);
/** Remove a variable from the config while returning the dimensionality of
* the removed element (normally not needed by user code), throws
* KeyDoesNotExist<J> if j is not present.
*/
//void erase(const J& j, size_t& dim);
/**
* Returns a set of keys in the config
* Note: by construction, the list is ordered
@ -226,23 +233,6 @@ namespace gtsam {
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const;
/**
* Apply a class with an application operator() to a const_iterator over
* every <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Values, (i.e. through templating).
*/
template<typename A>
void apply(A& operation) {
for(iterator it = begin(); it != end(); ++it)
operation(it);
}
template<typename A>
void apply(A& operation) const {
for(const_iterator it = begin(); it != end(); ++it)
operation(it);
}
/**
* Generate a default ordering, simply in key sort order. To instead
* create a fill-reducing ordering, use