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

Fixed typo in LieConfig expmap(Vector)

release/4.3a0
Richard Roberts 2010-01-10 17:48:55 +00:00
parent 33ba34a1f6
commit edb68b3b60
1 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
cpp/LieConfig-inl.h
View File

@ -61,6 +61,7 @@ namespace gtsam {
dim_ += dim(val); dim_ += dim(val);
} }
// todo: insert for every element is inefficient
template<class T> template<class T>
LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta) { LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta) {
LieConfig<T> newConfig; LieConfig<T> newConfig;
@ -75,19 +76,22 @@ namespace gtsam {
return newConfig; return newConfig;
} }
// This version just creates a VectorConfig then calls function above // todo: insert for every element is inefficient
template<class T> template<class T>
LieConfig<T> expmap(const LieConfig<T>& c, const Vector& delta) { LieConfig<T> expmap(const LieConfig<T>& c, const Vector& delta) {
VectorConfig deltaConfig; if(delta.size() != dim(c))
int delta_offset = 0; throw invalid_argument("Delta vector length does not match config dimensionality.");
string j; T pj; LieConfig<T> newConfig;
FOREACH_PAIR(j, pj, c) { pair<string, T> value;
int cur_dim = dim(pj); int delta_offset = 0;
Vector dj = sub(delta, delta_offset, delta_offset+cur_dim); BOOST_FOREACH(value, c) {
deltaConfig.insert(j,dj); int cur_dim = dim(value.second);
delta_offset += cur_dim; newConfig.insert(value.first,
} expmap(value.second,
return expmap(c,deltaConfig); sub(delta, delta_offset, delta_offset+cur_dim)));
delta_offset += cur_dim;
}
return newConfig;
} }
} }