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

Now only inline add, for performance

release/4.3a0
dellaert 2014-10-05 21:53:40 +02:00
parent 33c1d072a4
commit 632810ff9a
1 changed files with 2 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -28,21 +28,7 @@ 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> {
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));
}
};
#else
typedef std::map<Key, Matrix> JacobianMap; typedef std::map<Key, Matrix> JacobianMap;
#endif
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
@ -60,34 +46,24 @@ 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) {
#ifdef NEW_CLASS
jacobians_.add(term.first, term.second);
#else
{
JacobianMap::iterator it = jacobians_.find(term.first); JacobianMap::iterator it = jacobians_.find(term.first);
if (it != jacobians_.end()) if (it != jacobians_.end())
it->second += term.second; it->second += term.second;
else else
jacobians_[term.first] = term.second; 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) {
#ifdef NEW_CLASS
jacobians_.add(term.first, H * term.second);
#else
{
JacobianMap::iterator it = jacobians_.find(term.first); JacobianMap::iterator it = jacobians_.find(term.first);
if (it != jacobians_.end()) if (it != jacobians_.end())
it->second += H * term.second; it->second += H * term.second;
else else
jacobians_[term.first] = H * term.second; jacobians_[term.first] = H * term.second;
} }
#endif
} }
public: public:
@ -304,15 +280,11 @@ public:
} }
/// 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);
#else
JacobianMap::iterator it = jacobians.find(key); JacobianMap::iterator it = jacobians.find(key);
if (it != jacobians.end()) if (it != jacobians.end())
it->second += H; it->second += H;
else else
jacobians[key] = H; jacobians[key] = H;
#endif
} }
}; };