From 97d4120858f85e78ab0a0cf0541b765c2c34065a Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 31 Oct 2014 07:10:53 -0400 Subject: [PATCH 1/7] Changed the type of JacobianMap as std::vector --- gtsam_unstable/nonlinear/Expression-inl.h | 23 ++++++++++++++++----- gtsam_unstable/nonlinear/ExpressionFactor.h | 6 ++++-- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index d5c5f1279..43d3071ce 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -48,7 +48,8 @@ namespace gtsam { template class Expression; -typedef std::map > JacobianMap; +//typedef std::map > JacobianMap; +typedef std::vector > > JacobianMap; //----------------------------------------------------------------------------- /** @@ -78,16 +79,28 @@ 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 + for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it) + { + if((*it).first == key) + { + (*it).second.block(0, 0) += dTdA; // block makes HUGE difference + break; + } + } } /// 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); - it->second += dTdA; + for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it) + { + if((*it).first == key) + { + (*it).second += dTdA; // block makes HUGE difference + break; + } + } } //----------------------------------------------------------------------------- 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 ! From 133fcf20e26f9a79e2a0e4c31f31099da754ced6 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 31 Oct 2014 07:22:19 -0400 Subject: [PATCH 2/7] Cleaned up some commented codes --- gtsam_unstable/nonlinear/Expression-inl.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 43d3071ce..9b1ad8fd3 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -48,7 +48,6 @@ namespace gtsam { template class Expression; -//typedef std::map > JacobianMap; typedef std::vector > > JacobianMap; //----------------------------------------------------------------------------- From 6a20d35a6010fe56fdf68056fcb468488440f89d Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 31 Oct 2014 07:28:07 -0400 Subject: [PATCH 3/7] Modified pointer expression --- gtsam_unstable/nonlinear/Expression-inl.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 9b1ad8fd3..215f19c1a 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -80,9 +80,9 @@ void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it) { - if((*it).first == key) + if(it->first == key) { - (*it).second.block(0, 0) += dTdA; // block makes HUGE difference + it->second.block(0, 0) += dTdA; // block makes HUGE difference break; } } @@ -94,9 +94,9 @@ void handleLeafCase( JacobianMap& jacobians, Key key) { for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it) { - if((*it).first == key) + if(it->first == key) { - (*it).second += dTdA; // block makes HUGE difference + it->second += dTdA; // block makes HUGE difference break; } } From a5b8d0fd353ebcb64325602c7200fb6b25c391c5 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 31 Oct 2014 11:06:26 -0400 Subject: [PATCH 4/7] Modified finding method --- gtsam_unstable/nonlinear/Expression-inl.h | 29 ++++++++++------------- 1 file changed, 12 insertions(+), 17 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 215f19c1a..ca8ff1bea 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -26,6 +26,8 @@ #include #include +#include +#include // template meta-programming headers #include @@ -48,7 +50,8 @@ namespace gtsam { template class Expression; -typedef std::vector > > JacobianMap; +typedef std::pair > JacobianPair; +typedef std::vector JacobianMap; //----------------------------------------------------------------------------- /** @@ -78,28 +81,20 @@ struct CallRecord { template void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { - for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it) - { - if(it->first == key) - { - it->second.block(0, 0) += dTdA; // block makes HUGE difference - break; - } - } + JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(), + boost::bind(&JacobianPair::first, _1)==key); + assert(it~= jacobians.end()); + it->second.block(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) { - for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it) - { - if(it->first == key) - { - it->second += dTdA; // block makes HUGE difference - break; - } - } + JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(), + boost::bind(&JacobianPair::first, _1)==key); + assert(it~= jacobians.end()); + it->second += dTdA; } //----------------------------------------------------------------------------- From 768a4abc058ed9641e2256a74058955c1661fee6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 31 Oct 2014 16:27:46 +0100 Subject: [PATCH 5/7] Does not need lambda --- gtsam_unstable/nonlinear/Expression-inl.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index ca8ff1bea..4a91c03da 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -27,7 +27,6 @@ #include #include #include -#include // template meta-programming headers #include From d0c3bc0c8e1d2dfa083bae534b904f136cf68e9b Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 31 Oct 2014 16:27:54 +0100 Subject: [PATCH 6/7] Fixed tests --- gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam_unstable/nonlinear/tests/testExpression.cpp | 4 ++-- gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) 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); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index c063f182f..b0900a43b 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -208,8 +208,8 @@ TEST(ExpressionFactor, Shallow) { EXPECT_LONGS_EQUAL(464, sizeof(Binary::Record)); LONGS_EQUAL(112+464, expectedTraceSize); #else - EXPECT_LONGS_EQUAL(496, sizeof(Binary::Record)); - LONGS_EQUAL(112+496, expectedTraceSize); + EXPECT_LONGS_EQUAL(432, sizeof(Binary::Record)); + LONGS_EQUAL(112+432, expectedTraceSize); #endif size_t size = expression.traceSize(); CHECK(size); From 593edd1e5cb53deb2f30635a3e21536e56eb7051 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 31 Oct 2014 16:29:15 +0100 Subject: [PATCH 7/7] Fixed asserts --- gtsam_unstable/nonlinear/Expression-inl.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 4a91c03da..9327d0803 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -81,9 +81,9 @@ template void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(), - boost::bind(&JacobianPair::first, _1)==key); - assert(it~= jacobians.end()); - it->second.block(0, 0) += dTdA; // block makes HUGE difference + 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<> @@ -91,8 +91,8 @@ void handleLeafCase( const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(), - boost::bind(&JacobianPair::first, _1)==key); - assert(it~= jacobians.end()); + boost::bind(&JacobianPair::first, _1) == key); + assert(it!=jacobians.end()); it->second += dTdA; }