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

Fixed 2nd expmap template, which now creates a VectorConfig and calls the first template

release/4.3a0
Frank Dellaert 2010-01-10 16:16:03 +00:00
parent 65e4dc1342
commit 4a21fb9387
1 changed files with 23 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cpp/LieConfig-inl.h
View File

@ -39,10 +39,9 @@ namespace gtsam {
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;
string j; T pj; string j; T pj;
FOREACH_PAIR(j, pj, c.values_) { FOREACH_PAIR(j, pj, c) {
if (delta.contains(j)) { if (delta.contains(j)) {
const Vector& dj = delta[j]; const Vector& dj = delta[j];
//check_size(j,vj,dj);
newConfig.insert(j, expmap(pj,dj)); newConfig.insert(j, expmap(pj,dj));
} else } else
newConfig.insert(j, pj); newConfig.insert(j, pj);
@ -50,19 +49,19 @@ namespace gtsam {
return newConfig; return newConfig;
} }
// This version just creates a VectorConfig then calls function above
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) {
LieConfig<T> newConfig; VectorConfig deltaConfig;
pair<string, Vector> value;
int delta_offset = 0; int delta_offset = 0;
BOOST_FOREACH(value, c) { string j; T pj;
int cur_dim = dim(value.second); FOREACH_PAIR(j, pj, c) {
newConfig.insert(value.first, int cur_dim = dim(pj);
expmap(value.second, Vector dj = sub(delta, delta_offset, delta_offset+cur_dim);
sub(delta, delta_offset, delta_offset+cur_dim))); deltaConfig.insert(j,dj);
delta_offset += cur_dim; delta_offset += cur_dim;
} }
return newConfig; return expmap(c,deltaConfig);
} }
} }