diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index d5c5f1279..9327d0803 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -26,6 +26,7 @@ #include #include +#include // template meta-programming headers #include @@ -48,7 +49,8 @@ namespace gtsam { template class Expression; -typedef std::map > JacobianMap; +typedef std::pair > JacobianPair; +typedef std::vector JacobianMap; //----------------------------------------------------------------------------- /** @@ -78,15 +80,19 @@ struct CallRecord { template void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { - JacobianMap::iterator it = jacobians.find(key); - it->second.block(0, 0) += dTdA; // block makes HUGE difference + JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(), + boost::bind(&JacobianPair::first, _1) == key); + assert(it!=jacobians.end()); + it->second.block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference } /// Handle Leaf Case for Dynamic Matrix type (slower) template<> void handleLeafCase( const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { - JacobianMap::iterator it = jacobians.find(key); + JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(), + boost::bind(&JacobianPair::first, _1) == key); + assert(it!=jacobians.end()); it->second += dTdA; } diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 84456c934..c9f3fae86 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -87,12 +87,13 @@ public: // Create and zero out blocks to be passed to expression_ JacobianMap blocks; + blocks.reserve(size()); for (DenseIndex i = 0; i < size(); i++) { Matrix& Hi = H->at(i); Hi.resize(Dim, dimensions_[i]); Hi.setZero(); // zero out Eigen::Block block = Hi.block(0, 0, Dim, dimensions_[i]); - blocks.insert(std::make_pair(keys_[i], block)); + blocks.push_back(std::make_pair(keys_[i], block)); } T value = expression_.value(x, blocks); @@ -121,8 +122,9 @@ public: // Create blocks into Ab_ to be passed to expression_ JacobianMap blocks; + blocks.reserve(size()); for (DenseIndex i = 0; i < size(); i++) - blocks.insert(std::make_pair(keys_[i], Ab(i))); + blocks.push_back(std::make_pair(keys_[i], Ab(i))); // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, blocks); // <<< Reverse AD happens here ! diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp index dc07c4b46..f26a1e61d 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -202,7 +202,7 @@ TEST(Expression, Snavely) { #ifdef GTSAM_USE_QUATERNIONS EXPECT_LONGS_EQUAL(480,expression.traceSize()); // Todo, should be zero #else - EXPECT_LONGS_EQUAL(528,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(448,expression.traceSize()); // Todo, should be zero #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 1997bdb53..90469d8c5 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -63,10 +63,10 @@ TEST(Expression, Leaf) { JacobianMap expected; Matrix H = eye(3); - expected.insert(make_pair(100, H.block(0, 0, 3, 3))); + expected.push_back(make_pair(100, H.block(0, 0, 3, 3))); JacobianMap actualMap2; - actualMap2.insert(make_pair(100, H.block(0, 0, 3, 3))); + actualMap2.push_back(make_pair(100, H.block(0, 0, 3, 3))); Rot3 actual2 = R.reverse(values, actualMap2); EXPECT(assert_equal(someR, actual2)); EXPECT(actualMap2 == expected);