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

Made LieValues allow for values to not be on the linear side during expmap

release/4.3a0
Richard Roberts 2011-01-18 02:40:22 +00:00
parent 7bfbb6b829
commit 9ac20eea8a
3 changed files with 18 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/nonlinear/LieValues-inl.h
View File

@ -148,8 +148,11 @@ namespace gtsam {
BOOST_FOREACH(const KeyValue& value, this->values_) { BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first; const J& j = value.first;
const typename J::Value& pj = value.second; const typename J::Value& pj = value.second;
const Vector& dj = delta[ordering[j]]; Index index;
newValues.insert(j, pj.expmap(dj)); if(ordering.tryAt(j,index)) {
newValues.insert(j, pj.expmap(delta[index]));
} else
newValues.insert(j, pj);
} }
return newValues; return newValues;
} }

4
gtsam/nonlinear/LieValues.h
View File

@ -206,10 +206,12 @@ namespace gtsam {
std::vector<size_t> dimensions; std::vector<size_t> dimensions;
_ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {} _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
template<typename I> void operator()(const I& key_value) { template<typename I> void operator()(const I& key_value) {
Index var = ordering[key_value->first]; Index var;
if(ordering.tryAt(key_value->first, var)) {
assert(var < dimensions.size()); assert(var < dimensions.size());
dimensions[var] = key_value->second.dim(); dimensions[var] = key_value->second.dim();
} }
}
}; };
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam/nonlinear/Ordering.h
View File

@ -64,6 +64,14 @@ public:
Index& at(const Symbol& key) { return operator[](key); } Index& at(const Symbol& key) { return operator[](key); }
Index at(const Symbol& key) const { return operator[](key); } Index at(const Symbol& key) const { return operator[](key); }
bool tryAt(const Symbol& key, Index& index) const {
const_iterator i = order_.find(key);
if(i != order_.end()) {
index = i->second;
return true;
} else
return false;
}
Index& operator[](const Symbol& key) { Index& operator[](const Symbol& key) {
iterator i=order_.find(key); assert(i != order_.end()); return i->second; } iterator i=order_.find(key); assert(i != order_.end()); return i->second; }
Index operator[](const Symbol& key) const { Index operator[](const Symbol& key) const {