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

Add switch between inline add and JacobianMap as a new class.

release/4.3a0
dellaert 2014-10-05 21:49:30 +02:00
parent 5b13306104
commit 33c1d072a4
1 changed files with 38 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -28,16 +28,21 @@ namespace gtsam {
template<typename T> template<typename T>
class Expression; class Expression;
//#define NEW_CLASS
#ifdef NEW_CLASS
class JacobianMap: public std::map<Key, Matrix> { class JacobianMap: public std::map<Key, Matrix> {
public: public:
void add(Key key, const Matrix& H) { void add(Key key, const Matrix& H) {
iterator it = find(key); iterator it = find(key);
if (it != end()) if (it != end())
it->second += H; it->second += H;
else else
insert(std::make_pair(key, H)); insert(std::make_pair(key, H));
} }
}; };
#else
typedef std::map<Key, Matrix> JacobianMap;
#endif
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
@ -56,13 +61,33 @@ private:
/// Insert terms into jacobians_, premultiplying by H, adding if already exists /// Insert terms into jacobians_, premultiplying by H, adding if already exists
void add(const JacobianMap& terms) { void add(const JacobianMap& terms) {
BOOST_FOREACH(const Pair& term, terms) BOOST_FOREACH(const Pair& term, terms)
jacobians_.add(term.first, term.second); #ifdef NEW_CLASS
jacobians_.add(term.first, term.second);
#else
{
JacobianMap::iterator it = jacobians_.find(term.first);
if (it != jacobians_.end())
it->second += term.second;
else
jacobians_[term.first] = term.second;
}
#endif
} }
/// Insert terms into jacobians_, premultiplying by H, adding if already exists /// Insert terms into jacobians_, premultiplying by H, adding if already exists
void add(const Matrix& H, const JacobianMap& terms) { void add(const Matrix& H, const JacobianMap& terms) {
BOOST_FOREACH(const Pair& term, terms) BOOST_FOREACH(const Pair& term, terms)
jacobians_.add(term.first, H * term.second); #ifdef NEW_CLASS
jacobians_.add(term.first, H * term.second);
#else
{
JacobianMap::iterator it = jacobians_.find(term.first);
if (it != jacobians_.end())
it->second += H * term.second;
else
jacobians_[term.first] = H * term.second;
}
#endif
} }
public: public:
@ -275,11 +300,19 @@ public:
/// If the expression is just a leaf, we just insert an identity matrix /// If the expression is just a leaf, we just insert an identity matrix
virtual void reverseAD(JacobianMap& jacobians) const { virtual void reverseAD(JacobianMap& jacobians) const {
size_t n = T::Dim(); 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 /// Base case: given df/dT, add it jacobians with correct key and we are done
virtual void reverseAD(const Matrix& H, JacobianMap& jacobians) const { virtual void reverseAD(const Matrix& H, JacobianMap& jacobians) const {
#ifdef NEW_CLASS
jacobians.add(key, H); jacobians.add(key, H);
#else
JacobianMap::iterator it = jacobians.find(key);
if (it != jacobians.end())
it->second += H;
else
jacobians[key] = H;
#endif
} }
}; };