diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index ef5d62185..f8ad04ba1 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -28,16 +28,21 @@ namespace gtsam { template class Expression; +//#define NEW_CLASS +#ifdef NEW_CLASS class JacobianMap: public std::map { public: void add(Key key, const Matrix& H) { iterator it = find(key); if (it != end()) - it->second += H; + it->second += H; else - insert(std::make_pair(key, H)); + insert(std::make_pair(key, H)); } }; +#else +typedef std::map JacobianMap; +#endif //----------------------------------------------------------------------------- /** @@ -56,13 +61,33 @@ 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); +#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 void add(const Matrix& H, const JacobianMap& 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: @@ -275,11 +300,19 @@ 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 { +#ifdef NEW_CLASS jacobians.add(key, H); +#else + JacobianMap::iterator it = jacobians.find(key); + if (it != jacobians.end()) + it->second += H; + else + jacobians[key] = H; +#endif } };