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

(in branch) more implementation for DynamicValues

release/4.3a0
Richard Roberts 2012-01-15 23:06:01 +00:00
parent ef8a82c8d7
commit d323d5b963
4 changed files with 161 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -35,4 +35,74 @@ namespace gtsam {
return message_.c_str();
}
/* ************************************************************************* */
template<typename Value>
const Value& DynamicValues::at(const Symbol& j) const {
// Find the item
const_iterator item = values_.find(j);
// Throw exception if it does not exist
if(item == values_.end())
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));
// We have already checked the type, so do a "blind" static_cast, not dynamic_cast
return static_cast<const Value&>(*item->second);
}
/* ************************************************************************* */
template<typename TypedKey>
const typename TypedKey::Value& DynamicValues::at(const TypedKey& j) const {
// Convert to Symbol
const Symbol symbol(j.symbol());
// Call at with the Value type from the key
return at<typename TypedKey::Value>(symbol);
}
/* ************************************************************************* */
template<typename Value>
boost::optional<Value> 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));
// We have already checked the type, so do a "blind" static_cast, not dynamic_cast
return static_cast<const Value&>(*item->second);
} else {
return boost::none;
}
}
/* ************************************************************************* */
template<class TypedKey>
boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const {
// Convert to Symbol
const Symbol symbol(j.symbol());
// Call exists with the Value type from the key
return exists<typename TypedKey::Value>(symbol);
}
/* ************************************************************************* */
template<class ValueType>
void DynamicValues::insert(const Symbol& j, const ValueType& val) {
pair<iterator,bool> insertResult = values_.insert(make_pair(j, ValuePtr(new ValueType(val))));
if(!insertResult.second)
throw DynamicValuesKeyAlreadyExists(j);
}
/* ************************************************************************* */
void DynamicValues::insert(const DynamicValues& values) {
BOOST_FOREACH(const KeyValuePair& key_value, values) {
insert(key_value.first, key_value.)
}
}
}

59
gtsam/nonlinear/DynamicValues.cpp
View File

@ -30,6 +30,12 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
DynamicValues::DynamicValues(const DynamicValues& other) {
}
/* ************************************************************************* */
void DynamicValues::print(const string& str) const {
cout << str << "DynamicValues with " << size() << " values:\n" << endl;
BOOST_FOREACH(const KeyValueMap::value_type& key_value, values_) {
@ -38,10 +44,61 @@ namespace gtsam {
}
}
/* ************************************************************************* */
bool DynamicValues::equals(const DynamicValues& other, double tol) const {
if(this->size() != other.size())
return false;
for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2)
for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
if(typeid(*it1->second) != typeid(*it2->second))
return false;
if(it1->first != it2->first)
return false;
if(!it1->second->equals_(*it2->second, tol))
return false;
}
return true; // We return false earlier if we find anything that does not match
}
/* ************************************************************************* */
bool DynamicValues::exists(const Symbol& j) const {
return values_.find(j) != values_.end();
}
/* ************************************************************************* */
VectorValues DynamicValues::zeroVectors(const Ordering& ordering) const {
return VectorValues::Zero(this->dims(ordering));
}
/* ************************************************************************* */
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
const std::auto_ptr<const Value> retractedValue = key_value.second->retract_(singleDelta); // Retract
result.values_.insert(make_pair(key_value.first, retractedValue)); // Add retracted result directly to result values
}
return result;
}
/* ************************************************************************* */
VectorValues DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering) const {
VectorValues result(this->dims(ordering));
localCoordinates(cp, ordering, result);
return result;
}
/* ************************************************************************* */
void DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& result) const {
if(this->size() != cp.size())
throw DynamicValuesMismatched();
for(const_iterator it1=this->begin(), it2=other.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
}
}
}

39
gtsam/nonlinear/DynamicValues.h
View File

@ -38,7 +38,9 @@ namespace gtsam {
private:
typedef FastMap<Symbol, std::auto_ptr<Value> > KeyValueMap;
typedef std::auto_ptr<const Value> ValuePtr;
typedef FastMap<Symbol, std::auto_ptr<const Value> > KeyValueMap;
typedef KeyValueMap::value_type KeyValuePair;
KeyValueMap values_;
public:
@ -52,7 +54,7 @@ namespace gtsam {
DynamicValues() {}
/** Copy constructor duplicates all keys and values */
DynamicValues(const DynamicValues& other) {}
DynamicValues(const DynamicValues& other);
/// @name Testable
/// @{
@ -93,14 +95,14 @@ namespace gtsam {
/** Check if a value exists with key \c j. See exists<>(const Symbol& j)
* and exists(const TypedKey& j) for versions that return the value if it
* exists. */
bool exists(const Symbol& i) const;
bool 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.
* Throws DynamicValuesIncorrectType if the value type associated with the
* requested key does not match the stored value type. */
template<typename Value>
boost::optional<Value> exists(const Symbol& j) const;
boost::optional<const Value&> 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.
@ -111,7 +113,7 @@ namespace gtsam {
* requested key does not match the stored value type.
*/
template<class TypedKey>
boost::optional<typename TypedKey::Value> exists(const TypedKey& j) const;
boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const;
/** The number of variables in this config */
size_t size() const { return values_.size(); }
@ -120,7 +122,7 @@ namespace gtsam {
bool empty() const { return values_.empty(); }
/** Get a zero VectorValues of the correct structure */
VectorValues zero(const Ordering& ordering) const;
VectorValues zeroVectors(const Ordering& ordering) const;
const_iterator begin() const { return values_.begin(); }
const_iterator end() const { return values_.end(); }
@ -134,9 +136,6 @@ namespace gtsam {
/// @name Manifold Operations
/// @{
/** The dimensionality of the tangent space */
size_t dim() const;
/** Add a delta config to current config and returns a new config */
DynamicValues retract(const VectorValues& delta, const Ordering& ordering) const;
@ -220,28 +219,23 @@ namespace gtsam {
};
/* ************************************************************************* */
template<class ValueType>
class DynamicValuesKeyAlreadyExists : public std::exception {
protected:
const Symbol key_; ///< The key that already existed
const Value value_; ///< The value attempted to be inserted
private:
mutable std::string message_;
public:
/// Construct with the key-value pair attemped to be added
DynamicValuesKeyAlreadyExists(const Symbol& key, const Value& value) throw() :
key_(key), value_(value) {}
DynamicValuesKeyAlreadyExists(const Symbol& key) throw() :
key_(key) {}
virtual ~DynamicValuesKeyAlreadyExists() throw() {}
/// The duplicate key that was attemped to be added
const Symbol& key() const throw() { return key_; }
/// The value that was attempted to be added
const Value& value() const throw() { return value_; }
/// The message to be displayed to the user
virtual const char* what() const throw();
};
@ -300,6 +294,19 @@ namespace gtsam {
virtual const char* what() const throw();
};
/* ************************************************************************* */
class DynamicValuesMismatched : public std::exception {
public:
DynamicValuesMismatched() throw() {}
virtual ~DynamicValuesMismatched() throw() {}
virtual const char* what() const throw() {
return "The Values 'this' and the argument passed to DynamicValues::localCoordinates have mismatched keys and values";
}
};
}
#include <gtsam/nonlinear/DynamicValues-inl.h>

10
gtsam/nonlinear/Key.h
View File

@ -76,6 +76,7 @@ public:
std::string latex() const {
return (boost::format("%c_{%d}") % C % j_).str();
}
Symbol symbol() const;
// logic:
@ -298,6 +299,9 @@ public:
std::string label_s = (boost::format("%1%") % label_).str();
return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str();
}
Symbol symbol() const {
return Symbol(*this);
}
// Needed for conversion to LabeledSymbol
size_t convertLabel() const {
@ -354,5 +358,11 @@ private:
}
};
/* ************************************************************************* */
template<class T, char C>
Symbol TypedSymbol<T,C>::symbol() const {
return Symbol(*this);
}
} // namespace gtsam