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-20 16:26:45 +00:00
parent 64703d09da
commit f76c7be5b2
2 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -22,6 +22,8 @@
* which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
*/ */
#include <utility>
#include <gtsam/nonlinear/DynamicValues.h> // Only so Eclipse finds class definition #include <gtsam/nonlinear/DynamicValues.h> // Only so Eclipse finds class definition
namespace gtsam { namespace gtsam {
@ -94,7 +96,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class ValueType> template<class ValueType>
void DynamicValues::insert(const Symbol& j, const ValueType& val) { void DynamicValues::insert(const Symbol& j, const ValueType& val) {
pair<iterator,bool> insertResult = values_.insert(make_pair(j, ValuePtr(new ValueType(val)))); std::pair<iterator,bool> insertResult = values_.insert(make_pair(j, ValuePtr(new ValueType(val))));
if(!insertResult.second) if(!insertResult.second)
throw DynamicValuesKeyAlreadyExists(j); throw DynamicValuesKeyAlreadyExists(j);
} }

8
gtsam/nonlinear/DynamicValues.cpp
View File

@ -78,7 +78,7 @@ namespace gtsam {
BOOST_FOREACH(const KeyValuePair& key_value, values_) { BOOST_FOREACH(const KeyValuePair& key_value, values_) {
const SubVector& singleDelta = delta[ordering[key_value.first]]; // Delta for this value 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 ValuePtr retractedValue(key_value.second->retract_(singleDelta)); // Retract
result.values_.insert(make_pair(key_value.first, retractedValue)); // Add retracted result directly to result values result.values_.insert(make_pair(key_value.first, retractedValue)); // Add retracted result directly to result values
} }
@ -96,7 +96,7 @@ namespace gtsam {
void DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& result) const { void DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& result) const {
if(this->size() != cp.size()) if(this->size() != cp.size())
throw DynamicValuesMismatched(); throw DynamicValuesMismatched();
for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) { for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
if(it1->first != it2->first) if(it1->first != it2->first)
throw DynamicValuesMismatched(); // If keys do not match 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 result[ordering[it1->first]] = it1->second->localCoordinates_(*it2->second); // Will throw a dynamic_cast exception if types do not match
@ -120,14 +120,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void DynamicValues::erase(const Symbol& j) { void DynamicValues::erase(const Symbol& j) {
iterator item = values_.find(j); iterator item = values_.find(j);
if(item == values_end) if(item == values_.end())
throw DynamicValuesKeyDoesNotExist("erase", j); throw DynamicValuesKeyDoesNotExist("erase", j);
values_.erase(item); values_.erase(item);
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastList<Symbol> DynamicValues::keys() const { FastList<Symbol> DynamicValues::keys() const {
return list<Symbol>( return FastList<Symbol>(
boost::make_transform_iterator(values_.begin(), &KeyValuePair::first), boost::make_transform_iterator(values_.begin(), &KeyValuePair::first),
boost::make_transform_iterator(values_.end(), &KeyValuePair::first)); boost::make_transform_iterator(values_.end(), &KeyValuePair::first));
} }