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

Merge branch 'feature/BAD_JacobianMap' into feature/BAD

release/4.3a0
dellaert 2014-10-05 21:54:02 +02:00
parent 5b13306104 632810ff9a
commit 16d402f8c9
1 changed files with 21 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -28,16 +28,7 @@ namespace gtsam {
template<typename T>
class Expression;
class JacobianMap: public std::map<Key, Matrix> {
public:
void add(Key key, const Matrix& H) {
iterator it = find(key);
if (it != end())
it->second += H;
else
insert(std::make_pair(key, H));
}
};
typedef std::map<Key, Matrix> JacobianMap;
//-----------------------------------------------------------------------------
/**
@ -55,14 +46,24 @@ private:
/// Insert terms into jacobians_, premultiplying by H, adding if already exists
void add(const JacobianMap& terms) {
BOOST_FOREACH(const Pair& term, terms)
jacobians_.add(term.first, term.second);
BOOST_FOREACH(const Pair& term, terms) {
JacobianMap::iterator it = jacobians_.find(term.first);
if (it != jacobians_.end())
it->second += term.second;
else
jacobians_[term.first] = term.second;
}
}
/// Insert terms into jacobians_, premultiplying by H, adding if already exists
void add(const Matrix& H, const JacobianMap& terms) {
BOOST_FOREACH(const Pair& term, terms)
jacobians_.add(term.first, H * term.second);
BOOST_FOREACH(const Pair& term, terms) {
JacobianMap::iterator it = jacobians_.find(term.first);
if (it != jacobians_.end())
it->second += H * term.second;
else
jacobians_[term.first] = H * term.second;
}
}
public:
@ -275,11 +276,15 @@ public:
/// If the expression is just a leaf, we just insert an identity matrix
virtual void reverseAD(JacobianMap& jacobians) const {
size_t n = T::Dim();
jacobians.add(key, Eigen::MatrixXd::Identity(n, n));
jacobians[key] = Eigen::MatrixXd::Identity(n, n);
}
/// Base case: given df/dT, add it jacobians with correct key and we are done
virtual void reverseAD(const Matrix& H, JacobianMap& jacobians) const {
jacobians.add(key, H);
JacobianMap::iterator it = jacobians.find(key);
if (it != jacobians.end())
it->second += H;
else
jacobians[key] = H;
}
};