From 649478f18608d220f2e3b86095ee44506dfbca16 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 15 Oct 2014 01:19:14 +0200 Subject: [PATCH 01/23] Should work but seg-faults --- gtsam/linear/NoiseModel.h | 10 ++++++++ gtsam_unstable/nonlinear/ExpressionFactor.h | 27 ++++++++++++--------- 2 files changed, 25 insertions(+), 12 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 4b0e4848d..597ebe1dd 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -61,6 +61,11 @@ namespace gtsam { Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} + /** true if a constrained noise mode, saves slow/clumsy dynamic casting */ + virtual bool is_constrained() const { + return false; + } + /// Dimensionality inline size_t dim() const { return dim_;} @@ -385,6 +390,11 @@ namespace gtsam { virtual ~Constrained() {} + /** true if a constrained noise mode, saves slow/clumsy dynamic casting */ + virtual bool is_constrained() const { + return true; + } + /// Access mu as a vector const Vector& mu() const { return mu_; } diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index b6bfba27f..a0e843935 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -63,11 +63,11 @@ public: // Create and zero out blocks to be passed to expression_ JacobianMap blocks; - for(DenseIndex i=0;iat(i); Hi.resize(T::dimension, dims[i]); Hi.setZero(); // zero out - Eigen::Block block = Hi.block(0,0,T::dimension, dims[i]); + Eigen::Block block = Hi.block(0, 0, T::dimension, dims[i]); blocks.insert(std::make_pair(keys_[i], block)); } @@ -87,17 +87,18 @@ public: std::vector dims = expression_.dimensions(); // Allocate memory on stack and create a view on it (saves a malloc) - size_t m1 = std::accumulate(dims.begin(),dims.end(),1); - double memory[T::dimension*m1]; - Eigen::Map > matrix(memory,T::dimension,m1); + size_t m1 = std::accumulate(dims.begin(), dims.end(), 1); + double memory[T::dimension * m1]; + Eigen::Map > matrix( + memory, T::dimension, m1); matrix.setZero(); // zero out - // Construct block matrix, is of right size but un-initialized + // Construct block matrix, is then of right and initialized to zero VerticalBlockMatrix Ab(dims, matrix, true); // Create blocks to be passed to expression_ JacobianMap blocks; - for(DenseIndex i=0;i(this->noiseModel_); - if (constrained) { - return boost::make_shared(this->keys(), Ab, - constrained->unit()); + if (noiseModel_->is_constrained()) { + noiseModel::Constrained::shared_ptr p = // + boost::dynamic_pointer_cast(noiseModel_); + if (!p) + throw std::invalid_argument( + "ExpressionFactor: constrained NoiseModel cast failed."); + return boost::make_shared(this->keys(), Ab, p->unit()); } else return boost::make_shared(this->keys(), Ab); } From 97d4120858f85e78ab0a0cf0541b765c2c34065a Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 31 Oct 2014 07:10:53 -0400 Subject: [PATCH 02/23] 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 03/23] 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 04/23] 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 f5c6ccca17e4650a8da1d2b823f2fe22efe20361 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 31 Oct 2014 13:48:39 +0100 Subject: [PATCH 05/23] Changed size (because transpose_) was removed --- gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 a5b8d0fd353ebcb64325602c7200fb6b25c391c5 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 31 Oct 2014 11:06:26 -0400 Subject: [PATCH 06/23] 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 07/23] 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 08/23] 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 09/23] 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; } From 7b539fbb5cc6082a698c27a6604cc8627fcbbd42 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Nov 2014 11:35:49 +0100 Subject: [PATCH 10/23] Make JacobianMap a wrapper around a VerticalBlockMatrix, which avoids us having to make a vector of references into it --- gtsam_unstable/nonlinear/Expression-inl.h | 32 +++++++++----- gtsam_unstable/nonlinear/Expression.h | 7 +-- gtsam_unstable/nonlinear/ExpressionFactor.h | 47 ++++++++++----------- 3 files changed, 45 insertions(+), 41 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 9327d0803..8da65ffda 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -49,8 +50,25 @@ namespace gtsam { template class Expression; -typedef std::pair > JacobianPair; -typedef std::vector JacobianMap; +/** + * Expressions are designed to write their derivatives into an already allocated + * Jacobian of the correct size, of type VerticalBlockMatrix. + * The JacobianMap provides a mapping from keys to the underlying blocks. + */ +class JacobianMap { + const FastVector& keys_; + VerticalBlockMatrix& Ab_; +public: + JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { + } + /** Access a single block in the underlying matrix with read/write access */ + VerticalBlockMatrix::Block operator()(Key key) { + FastVector::const_iterator it = std::find(keys_.begin(),keys_.end(),key); + DenseIndex block = it - keys_.begin(); + return Ab_(block); + } +}; //----------------------------------------------------------------------------- /** @@ -80,20 +98,14 @@ struct CallRecord { 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 < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference + jacobians(key).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 = std::find_if(jacobians.begin(), jacobians.end(), - boost::bind(&JacobianPair::first, _1) == key); - assert(it!=jacobians.end()); - it->second += dTdA; + jacobians(key) += dTdA; } //----------------------------------------------------------------------------- diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 6ac7d9ce8..a1f617b8f 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -123,7 +123,7 @@ public: } /// Return value and derivatives, reverse AD version - T reverse(const Values& values, JacobianMap& jacobians) const { + T value(const Values& values, JacobianMap& jacobians) const { // The following piece of code is absolutely crucial for performance. // We allocate a block of memory on the stack, which can be done at runtime // with modern C++ compilers. The traceExecution then fills this memory @@ -142,11 +142,6 @@ public: return root_->value(values); } - /// Return value and derivatives - T value(const Values& values, JacobianMap& jacobians) const { - return reverse(values, jacobians); - } - const boost::shared_ptr >& root() const { return root_; } diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index c9f3fae86..89a3ab5fc 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -81,27 +81,27 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if (H) { - // H should be pre-allocated - assert(H->size()==size()); - - // 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.push_back(std::make_pair(keys_[i], block)); - } - - T value = expression_.value(x, blocks); - return measurement_.localCoordinates(value); - } else { +// if (H) { +// // H should be pre-allocated +// assert(H->size()==size()); +// +// // 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.push_back(std::make_pair(keys_[i], block)); +// } +// +// T value = expression_.value(x, blocks); +// return measurement_.localCoordinates(value); +// } else { const T& value = expression_.value(x); return measurement_.localCoordinates(value); - } +// } } virtual boost::shared_ptr linearize(const Values& x) const { @@ -120,14 +120,11 @@ public: // Construct block matrix, is of right size but un-initialized VerticalBlockMatrix Ab(dimensions_, matrix, true); - // Create blocks into Ab_ to be passed to expression_ - JacobianMap blocks; - blocks.reserve(size()); - for (DenseIndex i = 0; i < size(); i++) - blocks.push_back(std::make_pair(keys_[i], Ab(i))); + // Wrap keys and VerticalBlockMatrix into structure passed to expression_ + JacobianMap map(keys_,Ab); // Evaluate error to get Jacobians and RHS vector b - T value = expression_.value(x, blocks); // <<< Reverse AD happens here ! + T value = expression_.value(x, map); // <<< Reverse AD happens here ! Ab(size()).col(0) = -measurement_.localCoordinates(value); // Whiten the corresponding system now From f38b0b0eed768c29ada4b8593f54cb2fcbf98172 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Nov 2014 11:50:28 +0100 Subject: [PATCH 11/23] Fixed unwhitenedError --- gtsam_unstable/nonlinear/Expression-inl.h | 2 +- gtsam_unstable/nonlinear/ExpressionFactor.h | 47 +++++++++++---------- 2 files changed, 25 insertions(+), 24 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 8da65ffda..b4cbb8728 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -62,7 +62,7 @@ public: JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : keys_(keys), Ab_(Ab) { } - /** Access a single block in the underlying matrix with read/write access */ + /// Access via key VerticalBlockMatrix::Block operator()(Key key) { FastVector::const_iterator it = std::find(keys_.begin(),keys_.end(),key); DenseIndex block = it - keys_.begin(); diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 89a3ab5fc..e74d07b29 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -75,41 +75,42 @@ public: } /** - * Error function *without* the NoiseModel, \f$ z-h(x) \f$. + * Error function *without* the NoiseModel, \f$ h(x)-z \f$. * We override this method to provide * both the function evaluation and its derivative(s) in H. */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { -// if (H) { -// // H should be pre-allocated -// assert(H->size()==size()); -// -// // 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.push_back(std::make_pair(keys_[i], block)); -// } -// -// T value = expression_.value(x, blocks); -// return measurement_.localCoordinates(value); -// } else { + if (H) { + // H should be pre-allocated + assert(H->size()==size()); + + VerticalBlockMatrix Ab(dimensions_, Dim); + + // Wrap keys and VerticalBlockMatrix into structure passed to expression_ + JacobianMap map(keys_, Ab); + Ab.matrix().setZero(); + + // Evaluate error to get Jacobians and RHS vector b + T value = expression_.value(x, map); // <<< Reverse AD happens here ! + + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < size(); i++) + H->at(i) = Ab(i); + + return measurement_.localCoordinates(value); + } else { const T& value = expression_.value(x); return measurement_.localCoordinates(value); -// } + } } virtual boost::shared_ptr linearize(const Values& x) const { // This method has been heavily optimized for maximum performance. // We allocate a VerticalBlockMatrix on the stack first, and then create - // Eigen::Block views on this piece of memory which is then passed - // to [expression_.value] below, which writes directly into Ab_. + // a JacobianMap view onto it, which is then passed + // to [expression_.value] to allow it to write directly into Ab_. // Another malloc saved by creating a Matrix on the stack double memory[Dim * augmentedCols_]; @@ -121,7 +122,7 @@ public: VerticalBlockMatrix Ab(dimensions_, matrix, true); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ - JacobianMap map(keys_,Ab); + JacobianMap map(keys_, Ab); // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! From a4fa61a7a4f0517392fbad3d8acc1c05822ddb44 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Nov 2014 11:56:38 +0100 Subject: [PATCH 12/23] Removed JacobianMap tests --- gtsam_unstable/nonlinear/tests/testExpression.cpp | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 90469d8c5..d660d28b5 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -46,11 +46,8 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); TEST(Expression, constant) { Expression R(someR); Values values; - JacobianMap actualMap; - Rot3 actual = R.value(values, actualMap); + Rot3 actual = R.value(values); EXPECT(assert_equal(someR, actual)); - JacobianMap expected; - EXPECT(actualMap == expected); EXPECT_LONGS_EQUAL(0, R.traceSize()) } @@ -61,15 +58,8 @@ TEST(Expression, Leaf) { Values values; values.insert(100, someR); - JacobianMap expected; - Matrix H = eye(3); - expected.push_back(make_pair(100, H.block(0, 0, 3, 3))); - - JacobianMap actualMap2; - actualMap2.push_back(make_pair(100, H.block(0, 0, 3, 3))); - Rot3 actual2 = R.reverse(values, actualMap2); + Rot3 actual2 = R.value(values); EXPECT(assert_equal(someR, actual2)); - EXPECT(actualMap2 == expected); } /* ************************************************************************* */ From 12e38a44e4f502abce324e886daef494c60b195d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Nov 2014 14:13:08 +0100 Subject: [PATCH 13/23] WriteableJacobianFactor will allow ExpressionFactor to write into the factor directly, (hopefull) eliminating huge overhead. --- .../nonlinear/tests/testExpressionFactor.cpp | 55 +++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b0900a43b..b5476bee9 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -28,6 +28,9 @@ #include +#include +using boost::assign::list_of; + using namespace std; using namespace gtsam; @@ -420,6 +423,58 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } +/* ************************************************************************* */ + +/** + * Special version of JacobianFactor that allows Jacobians to be written + */ +class WriteableJacobianFactor: public JacobianFactor { + + /** + * Constructor + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + WriteableJacobianFactor(const KEYS& keys, const DIMENSIONS& dims, + DenseIndex m, const SharedDiagonal& model = SharedDiagonal()) { + + // Check noise model dimension + if (model && (DenseIndex) model->dim() != m) + throw InvalidNoiseModel(m, model->dim()); + + // copy the keys + keys_.resize(keys.size()); + std::copy(keys.begin(), keys.end(), keys_.begin()); + + // Check number of variables + if (dims.size() != keys_.size()) + throw std::invalid_argument( + "WriteableJacobianFactor: size of dimensions and keys do not agree."); + + Ab_ = VerticalBlockMatrix(dims.begin(), dims.end(), m, true); + Ab_.matrix().setZero(); + model_ = model; + } + friend class ExpressionFactorWriteableJacobianFactorTest; + template + friend class ExpressionFactor; +}; + +// Test Writeable JacobianFactor +TEST(ExpressionFactor, WriteableJacobianFactor) { + std::list keys = list_of(1)(2); + vector dimensions(2); + dimensions[0] = 6; + dimensions[1] = 3; + SharedDiagonal model; + WriteableJacobianFactor actual(keys, dimensions, 2, model); + JacobianFactor expected(1, zeros(2, 6), 2, zeros(2, 3), zero(2)); + EXPECT( assert_equal(expected, *(JacobianFactor*)(&actual),1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 7debde75184d3215e6552da48138dce173611553 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Nov 2014 15:12:06 +0100 Subject: [PATCH 14/23] Moved to ExpressionFactor that now uses it - timing seems worse ? --- gtsam_unstable/nonlinear/ExpressionFactor.h | 85 ++++++++++++++----- .../nonlinear/tests/testExpressionFactor.cpp | 39 --------- 2 files changed, 62 insertions(+), 62 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index c9f3fae86..e94e2bec4 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -24,8 +24,57 @@ #include #include +class ExpressionFactorWriteableJacobianFactorTest; + namespace gtsam { +/** + * Special version of JacobianFactor that allows Jacobians to be written + * Eliminates a large proportion of overhead + * Note all ExpressionFactor are friends, not for general consumption. + */ +class WriteableJacobianFactor: public JacobianFactor { + +public: + + /** + * Constructor + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + WriteableJacobianFactor(const KEYS& keys, const DIMENSIONS& dims, + DenseIndex m, const SharedDiagonal& model = SharedDiagonal()) { + + // Check noise model dimension + if (model && (DenseIndex) model->dim() != m) + throw InvalidNoiseModel(m, model->dim()); + + // copy the keys + keys_.resize(keys.size()); + std::copy(keys.begin(), keys.end(), keys_.begin()); + + // Check number of variables + if (dims.size() != keys_.size()) + throw std::invalid_argument( + "WriteableJacobianFactor: size of dimensions and keys do not agree."); + + Ab_ = VerticalBlockMatrix(dims.begin(), dims.end(), m, true); + Ab_.matrix().setZero(); + model_ = model; + } + + VerticalBlockMatrix& Ab() { + return Ab_; + } + +// friend class ::ExpressionFactorWriteableJacobianFactorTest; +// template +// friend class ExpressionFactor; +}; + /** * Factor that supports arbitrary expressions via AD */ @@ -106,42 +155,32 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { - // This method has been heavily optimized for maximum performance. - // We allocate a VerticalBlockMatrix on the stack first, and then create - // Eigen::Block views on this piece of memory which is then passed - // to [expression_.value] below, which writes directly into Ab_. + // Create noise model + SharedDiagonal model; + noiseModel::Constrained::shared_ptr constrained = // + boost::dynamic_pointer_cast(this->noiseModel_); + if (constrained) + model = constrained->unit(); - // Another malloc saved by creating a Matrix on the stack - double memory[Dim * augmentedCols_]; - Eigen::Map > // - matrix(memory, Dim, augmentedCols_); - matrix.setZero(); // zero out - - // Construct block matrix, is of right size but un-initialized - VerticalBlockMatrix Ab(dimensions_, matrix, true); + // Create a writeable JacobianFactor in advance + boost::shared_ptr factor = boost::make_shared< + WriteableJacobianFactor>(keys_, dimensions_, + traits::dimension::value, model); // Create blocks into Ab_ to be passed to expression_ JacobianMap blocks; blocks.reserve(size()); for (DenseIndex i = 0; i < size(); i++) - blocks.push_back(std::make_pair(keys_[i], Ab(i))); + blocks.push_back(std::make_pair(keys_[i], factor->Ab()(i))); // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, blocks); // <<< Reverse AD happens here ! - Ab(size()).col(0) = -measurement_.localCoordinates(value); + factor->Ab()(size()).col(0) = -measurement_.localCoordinates(value); // Whiten the corresponding system now // TODO ! this->noiseModel_->WhitenSystem(Ab); - // TODO pass unwhitened + noise model to Gaussian factor - // For now, only linearized constrained factors have noise model at linear level!!! - noiseModel::Constrained::shared_ptr constrained = // - boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained) { - return boost::make_shared(this->keys(), Ab, - constrained->unit()); - } else - return boost::make_shared(this->keys(), Ab); + return factor; } }; // ExpressionFactor diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b5476bee9..f9a4bc532 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -424,45 +424,6 @@ TEST(ExpressionFactor, composeTernary) { } /* ************************************************************************* */ - -/** - * Special version of JacobianFactor that allows Jacobians to be written - */ -class WriteableJacobianFactor: public JacobianFactor { - - /** - * Constructor - * @param keys in some order - * @param diemnsions of the variables in same order - * @param m output dimension - * @param model noise model (default NULL) - */ - template - WriteableJacobianFactor(const KEYS& keys, const DIMENSIONS& dims, - DenseIndex m, const SharedDiagonal& model = SharedDiagonal()) { - - // Check noise model dimension - if (model && (DenseIndex) model->dim() != m) - throw InvalidNoiseModel(m, model->dim()); - - // copy the keys - keys_.resize(keys.size()); - std::copy(keys.begin(), keys.end(), keys_.begin()); - - // Check number of variables - if (dims.size() != keys_.size()) - throw std::invalid_argument( - "WriteableJacobianFactor: size of dimensions and keys do not agree."); - - Ab_ = VerticalBlockMatrix(dims.begin(), dims.end(), m, true); - Ab_.matrix().setZero(); - model_ = model; - } - friend class ExpressionFactorWriteableJacobianFactorTest; - template - friend class ExpressionFactor; -}; - // Test Writeable JacobianFactor TEST(ExpressionFactor, WriteableJacobianFactor) { std::list keys = list_of(1)(2); From cb69f2cb8285b78d4f2b382a5fca5449f909891a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 11:40:48 +0100 Subject: [PATCH 15/23] Fastest linearize so far. Putting 'unsafe' constructor in JacobianFactor itself makes a *huge* difference. --- gtsam/linear/JacobianFactor.h | 19 +++++++ gtsam_unstable/nonlinear/ExpressionFactor.h | 55 +------------------ .../nonlinear/tests/testExpressionFactor.cpp | 4 +- 3 files changed, 24 insertions(+), 54 deletions(-) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b90012822..d98596a9f 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -140,6 +140,25 @@ namespace gtsam { JacobianFactor( const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); + /** + * Constructor + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, + const SharedDiagonal& model = SharedDiagonal()) : + Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { + Ab_.matrix().setZero(); + } + + /// Direct access to VerticalBlockMatrix + VerticalBlockMatrix& Ab() { + return Ab_; + } + /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots * structure computed for \c graph is already available, providing it will reduce the amount of diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index aaf10dc98..0394fbe5c 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -24,57 +24,8 @@ #include #include -class ExpressionFactorWriteableJacobianFactorTest; - namespace gtsam { -/** - * Special version of JacobianFactor that allows Jacobians to be written - * Eliminates a large proportion of overhead - * Note all ExpressionFactor are friends, not for general consumption. - */ -class WriteableJacobianFactor: public JacobianFactor { - -public: - - /** - * Constructor - * @param keys in some order - * @param diemnsions of the variables in same order - * @param m output dimension - * @param model noise model (default NULL) - */ - template - WriteableJacobianFactor(const KEYS& keys, const DIMENSIONS& dims, - DenseIndex m, const SharedDiagonal& model = SharedDiagonal()) { - - // Check noise model dimension - if (model && (DenseIndex) model->dim() != m) - throw InvalidNoiseModel(m, model->dim()); - - // copy the keys - keys_.resize(keys.size()); - std::copy(keys.begin(), keys.end(), keys_.begin()); - - // Check number of variables - if (dims.size() != keys_.size()) - throw std::invalid_argument( - "WriteableJacobianFactor: size of dimensions and keys do not agree."); - - Ab_ = VerticalBlockMatrix(dims.begin(), dims.end(), m, true); - Ab_.matrix().setZero(); - model_ = model; - } - - VerticalBlockMatrix& Ab() { - return Ab_; - } - -// friend class ::ExpressionFactorWriteableJacobianFactorTest; -// template -// friend class ExpressionFactor; -}; - /** * Factor that supports arbitrary expressions via AD */ @@ -164,9 +115,9 @@ public: model = constrained->unit(); // Create a writeable JacobianFactor in advance - boost::shared_ptr factor = boost::make_shared< - WriteableJacobianFactor>(keys_, dimensions_, - traits::dimension::value, model); + boost::shared_ptr factor( + new JacobianFactor(keys_, dimensions_, traits::dimension::value, + model)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ JacobianMap map(keys_, factor->Ab()); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index f9a4bc532..08496e5ff 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -431,9 +431,9 @@ TEST(ExpressionFactor, WriteableJacobianFactor) { dimensions[0] = 6; dimensions[1] = 3; SharedDiagonal model; - WriteableJacobianFactor actual(keys, dimensions, 2, model); + JacobianFactor actual(keys, dimensions, 2, model); JacobianFactor expected(1, zeros(2, 6), 2, zeros(2, 3), zero(2)); - EXPECT( assert_equal(expected, *(JacobianFactor*)(&actual),1e-9)); + EXPECT( assert_equal(expected, actual,1e-9)); } /* ************************************************************************* */ From b9e3c3b11609860cc031929503dc6173e7092e23 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 12:01:52 +0100 Subject: [PATCH 16/23] Made unsafe constructor private, but made ExpressionFactor a friend. --- gtsam/linear/JacobianFactor.h | 46 +++++++++---------- gtsam_unstable/nonlinear/ExpressionFactor.h | 18 ++++---- .../nonlinear/tests/testExpressionFactor.cpp | 13 ------ 3 files changed, 32 insertions(+), 45 deletions(-) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d98596a9f..d1dc7625c 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -82,20 +82,22 @@ namespace gtsam { class GTSAM_EXPORT JacobianFactor : public GaussianFactor { public: + typedef JacobianFactor This; ///< Typedef to this class typedef GaussianFactor Base; ///< Typedef to base class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - protected: - VerticalBlockMatrix Ab_; // the block view of the full matrix - noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix - - public: typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::constBlock constABlock; typedef ABlock::ColXpr BVector; typedef constABlock::ConstColXpr constBVector; + protected: + + VerticalBlockMatrix Ab_; // the block view of the full matrix + noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix + + public: /** Convert from other GaussianFactor */ explicit JacobianFactor(const GaussianFactor& gf); @@ -140,25 +142,6 @@ namespace gtsam { JacobianFactor( const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); - /** - * Constructor - * @param keys in some order - * @param diemnsions of the variables in same order - * @param m output dimension - * @param model noise model (default NULL) - */ - template - JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, - const SharedDiagonal& model = SharedDiagonal()) : - Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { - Ab_.matrix().setZero(); - } - - /// Direct access to VerticalBlockMatrix - VerticalBlockMatrix& Ab() { - return Ab_; - } - /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots * structure computed for \c graph is already available, providing it will reduce the amount of @@ -347,6 +330,21 @@ namespace gtsam { private: + /** Unsafe Constructor that creates an uninitialized Jacobian of right size + * @param keys in some order + * @param diemnsions of the variables in same order + * @param m output dimension + * @param model noise model (default NULL) + */ + template + JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, + const SharedDiagonal& model = SharedDiagonal()) : + Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { + } + + // be very selective on who can access these private methods: + template friend class ExpressionFactor; + /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 0394fbe5c..1d5d0cb12 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -107,24 +107,26 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { - // Create noise model - SharedDiagonal model; + // Check whether noise model is constrained or not noiseModel::Constrained::shared_ptr constrained = // boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained) - model = constrained->unit(); // Create a writeable JacobianFactor in advance boost::shared_ptr factor( - new JacobianFactor(keys_, dimensions_, traits::dimension::value, - model)); + constrained ? new JacobianFactor(keys_, dimensions_, Dim, + constrained->unit()) : + new JacobianFactor(keys_, dimensions_, Dim)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ - JacobianMap map(keys_, factor->Ab()); + VerticalBlockMatrix& Ab = factor->matrixObject(); + JacobianMap map(keys_, Ab); + + // Zero out Jacobian so we can simply add to it + Ab.matrix().setZero(); // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! - factor->Ab()(size()).col(0) = -measurement_.localCoordinates(value); + Ab(size()).col(0) = -measurement_.localCoordinates(value); // Whiten the corresponding system now // TODO ! this->noiseModel_->WhitenSystem(Ab); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 08496e5ff..23c2fa1ce 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -423,19 +423,6 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(expected, *jf,1e-9)); } -/* ************************************************************************* */ -// Test Writeable JacobianFactor -TEST(ExpressionFactor, WriteableJacobianFactor) { - std::list keys = list_of(1)(2); - vector dimensions(2); - dimensions[0] = 6; - dimensions[1] = 3; - SharedDiagonal model; - JacobianFactor actual(keys, dimensions, 2, model); - JacobianFactor expected(1, zeros(2, 6), 2, zeros(2, 3), zero(2)); - EXPECT( assert_equal(expected, actual,1e-9)); -} - /* ************************************************************************* */ int main() { TestResult tr; From 049631e530b1bb2219375e906a752bd582b7d337 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 12:57:13 +0100 Subject: [PATCH 17/23] Avoid re-allocating vertical offsets --- .cproject | 106 +++++++++---------- gtsam/base/VerticalBlockMatrix.h | 39 ++++--- gtsam/base/tests/testVerticalBlockMatrix.cpp | 23 +++- 3 files changed, 91 insertions(+), 77 deletions(-) diff --git a/.cproject b/.cproject index ce5ac0778..a596e90bf 100644 --- a/.cproject +++ b/.cproject @@ -600,7 +600,6 @@ make - tests/testBayesTree.run true false @@ -608,7 +607,6 @@ make - testBinaryBayesNet.run true false @@ -656,7 +654,6 @@ make - testSymbolicBayesNet.run true false @@ -664,7 +661,6 @@ make - tests/testSymbolicFactor.run true false @@ -672,7 +668,6 @@ make - testSymbolicFactorGraph.run true false @@ -688,7 +683,6 @@ make - tests/testBayesTree true false @@ -1120,7 +1114,6 @@ make - testErrors.run true false @@ -1350,46 +1343,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1472,6 +1425,7 @@ make + testSimulated2DOriented.run true false @@ -1511,6 +1465,7 @@ make + testSimulated2D.run true false @@ -1518,6 +1473,7 @@ make + testSimulated3D.run true false @@ -1531,6 +1487,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1788,7 +1784,6 @@ cpack - -G DEB true false @@ -1796,7 +1791,6 @@ cpack - -G RPM true false @@ -1804,7 +1798,6 @@ cpack - -G TGZ true false @@ -1812,7 +1805,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2441,6 +2433,14 @@ true true + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + make -j5 @@ -2579,7 +2579,6 @@ make - testGraph.run true false @@ -2587,7 +2586,6 @@ make - testJunctionTree.run true false @@ -2595,7 +2593,6 @@ make - testSymbolicBayesNetB.run true false @@ -3115,6 +3112,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index c09cc7577..b075d73b3 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -65,9 +65,10 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block. */ template - VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) : - rowStart_(0), rowEnd_(height), blockStart_(0) - { + VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, + bool appendOneDimension = false) : + variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); @@ -75,26 +76,28 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ template - VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : - matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) - { + VerticalBlockMatrix(const CONTAINER& dimensions, + const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : + matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); - if(variableColOffsets_.back() != matrix_.cols()) - throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); + if (variableColOffsets_.back() != matrix_.cols()) + throw std::invalid_argument( + "Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); assertInvariants(); } - /** - * Construct from iterator over the sizes of each vertical block. */ + /** Construct from iterator over the sizes of each vertical block. */ template - VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) : - rowStart_(0), rowEnd_(height), blockStart_(0) - { + VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, + DenseIndex height, bool appendOneDimension = false) : + variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)), + rowStart_(0), rowEnd_(height), blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); } - + /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data. * If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and @@ -203,18 +206,12 @@ namespace gtsam { template void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); variableColOffsets_[0] = 0; DenseIndex j=0; - for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { + for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j) variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; - ++ j; - } if(appendOneDimension) - { variableColOffsets_[j+1] = variableColOffsets_[j] + 1; - ++ j; - } } friend class SymmetricBlockMatrix; diff --git a/gtsam/base/tests/testVerticalBlockMatrix.cpp b/gtsam/base/tests/testVerticalBlockMatrix.cpp index fad23fa7d..c504752aa 100644 --- a/gtsam/base/tests/testVerticalBlockMatrix.cpp +++ b/gtsam/base/tests/testVerticalBlockMatrix.cpp @@ -24,9 +24,20 @@ using namespace std; using namespace gtsam; using boost::assign::list_of; +list L = list_of(3)(2)(1); +vector dimensions(L.begin(),L.end()); + //***************************************************************************** -TEST(VerticalBlockMatrix, constructor) { - VerticalBlockMatrix actual(list_of(3)(2)(1), +TEST(VerticalBlockMatrix, Constructor1) { + VerticalBlockMatrix actual(dimensions,6); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + +//***************************************************************************** +TEST(VerticalBlockMatrix, Constructor2) { + VerticalBlockMatrix actual(dimensions, (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, // 2, 8, 9, 10, 11, 12, // 3, 9, 15, 16, 17, 18, // @@ -38,6 +49,14 @@ TEST(VerticalBlockMatrix, constructor) { EXPECT_LONGS_EQUAL(3,actual.nBlocks()); } +//***************************************************************************** +TEST(VerticalBlockMatrix, Constructor3) { + VerticalBlockMatrix actual(dimensions.begin(),dimensions.end(),6); + EXPECT_LONGS_EQUAL(6,actual.rows()); + EXPECT_LONGS_EQUAL(6,actual.cols()); + EXPECT_LONGS_EQUAL(3,actual.nBlocks()); +} + //***************************************************************************** int main() { TestResult tr; From b21db08ec12b430f3b64ab25b050131b6f929e0c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 13:03:36 +0100 Subject: [PATCH 18/23] Fixed small issue, should not assign to reference in case of Quaternions. --- gtsam/geometry/Pose3.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index faf5d4bbb..7c4ac34e2 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -243,8 +243,8 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { if (Dpose) { - const Matrix R = R_.matrix(); - Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + const Matrix3 R = R_.matrix(); + Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Dpose->resize(3, 6); (*Dpose) << DR, R; } @@ -263,7 +263,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case - const Matrix3& Rt = R_.transpose(); + const Matrix3 Rt = R_.transpose(); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); @@ -280,7 +280,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, /* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { - const Matrix3& Rt = R_.transpose(); + const Matrix3 Rt = R_.transpose(); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); From 90a0fa6e455b79476c5caf350339df6b081203f1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 13:53:22 +0100 Subject: [PATCH 19/23] Check if active --- gtsam_unstable/nonlinear/ExpressionFactor.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 68abedd02..69056e6ef 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -107,6 +107,10 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { + // Only linearize if the factor is active + if (!this->active(x)) + return boost::shared_ptr(); + // Create a writeable JacobianFactor in advance // In case noise model is constrained, we need to provide a noise model bool constrained = noiseModel_->is_constrained(); From d0e004c05f01446a2fa79eb4bcb9b176da291500 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 14:35:22 +0100 Subject: [PATCH 20/23] No this-> needed --- gtsam/nonlinear/NonlinearFactor.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 19e8e2b00..f6f43b062 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -25,7 +25,7 @@ namespace gtsam { void NonlinearFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s << " keys = { "; - BOOST_FOREACH(Key key, this->keys()) { + BOOST_FOREACH(Key key, keys()) { std::cout << keyFormatter(key) << " "; } std::cout << "}" << std::endl; @@ -52,7 +52,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey( /* ************************************************************************* */ NonlinearFactor::shared_ptr NonlinearFactor::rekey( const std::vector& new_keys) const { - assert(new_keys.size() == this->keys().size()); + assert(new_keys.size() == keys().size()); shared_ptr new_factor = clone(); new_factor->keys() = new_keys; return new_factor; @@ -62,7 +62,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey( void NoiseModelFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { Base::print(s, keyFormatter); - this->noiseModel_->print(" noise model: "); + noiseModel_->print(" noise model: "); } /* ************************************************************************* */ @@ -92,7 +92,7 @@ Vector NoiseModelFactor::whitenedError(const Values& c) const { /* ************************************************************************* */ double NoiseModelFactor::error(const Values& c) const { - if (this->active(c)) { + if (active(c)) { const Vector b = unwhitenedError(c); check(noiseModel_, b.size()); return 0.5 * noiseModel_->distance(b); @@ -106,21 +106,21 @@ boost::shared_ptr NoiseModelFactor::linearize( const Values& x) const { // Only linearize if the factor is active - if (!this->active(x)) + if (!active(x)) return boost::shared_ptr(); // Call evaluate error to get Jacobians and RHS vector b - std::vector A(this->size()); + std::vector A(size()); Vector b = -unwhitenedError(x, A); check(noiseModel_, b.size()); // Whiten the corresponding system now - this->noiseModel_->WhitenSystem(A, b); + noiseModel_->WhitenSystem(A, b); // Fill in terms, needed to create JacobianFactor below - std::vector > terms(this->size()); - for (size_t j = 0; j < this->size(); ++j) { - terms[j].first = this->keys()[j]; + std::vector > terms(size()); + for (size_t j = 0; j < size(); ++j) { + terms[j].first = keys()[j]; terms[j].second.swap(A[j]); } From 8a6d8bfc82dcc41c6e731246a9d27b5c4f008640 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 14:35:32 +0100 Subject: [PATCH 21/23] Back to single --- gtsam_unstable/timing/timeOneCameraExpression.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/gtsam_unstable/timing/timeOneCameraExpression.cpp index 67b83b78b..4cb655825 100644 --- a/gtsam_unstable/timing/timeOneCameraExpression.cpp +++ b/gtsam_unstable/timing/timeOneCameraExpression.cpp @@ -23,7 +23,7 @@ using namespace std; using namespace gtsam; -#define time timeMultiThreaded +#define time timeSingleThreaded int main() { From d2f56b13ed455d9f8220472093b3709480e9b38d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 14:37:52 +0100 Subject: [PATCH 22/23] Non-trivial noise models now correctly handled (at a small performance penalty, due to malloc of Vector b). --- gtsam_unstable/nonlinear/ExpressionFactor.h | 10 +-- .../nonlinear/tests/testExpressionFactor.cpp | 70 +++++++++---------- 2 files changed, 41 insertions(+), 39 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 69056e6ef..f7b61120a 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -108,7 +108,7 @@ public: virtual boost::shared_ptr linearize(const Values& x) const { // Only linearize if the factor is active - if (!this->active(x)) + if (!active(x)) return boost::shared_ptr(); // Create a writeable JacobianFactor in advance @@ -128,11 +128,13 @@ public: // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! - Ab(size()).col(0) = -measurement_.localCoordinates(value); + Vector b(-measurement_.localCoordinates(value)); - // Whiten the corresponding system now - // TODO ! this->noiseModel_->WhitenSystem(Ab); + // Whiten the corresponding system + // Note the Ab.matrix() includes uninitialized RHS, but this beats taking it apart + noiseModel_->WhitenSystem(Ab.matrix(),b); + Ab(size()).col(0) = b; return factor; } }; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 23c2fa1ce..fa2e5cadb 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -65,41 +65,41 @@ TEST(ExpressionFactor, Leaf) { EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); } -///* ************************************************************************* */ -//// non-zero noise model -//TEST(ExpressionFactor, Model) { -// using namespace leaf; -// -// SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); -// -// // Create old-style factor to create expected value and derivatives -// PriorFactor old(2, Point2(0, 0), model); -// -// // Concise version -// ExpressionFactor f(model, Point2(0, 0), p); -// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); -// EXPECT_LONGS_EQUAL(2, f.dim()); -// boost::shared_ptr gf2 = f.linearize(values); -// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); -//} -// -///* ************************************************************************* */ -//// Constrained noise model -//TEST(ExpressionFactor, Constrained) { -// using namespace leaf; -// -// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); -// -// // Create old-style factor to create expected value and derivatives -// PriorFactor old(2, Point2(0, 0), model); -// -// // Concise version -// ExpressionFactor f(model, Point2(0, 0), p); -// EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); -// EXPECT_LONGS_EQUAL(2, f.dim()); -// boost::shared_ptr gf2 = f.linearize(values); -// EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); -//} +/* ************************************************************************* */ +// non-zero noise model +TEST(ExpressionFactor, Model) { + using namespace leaf; + + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01)); + + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); + + // Concise version + ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +} + +/* ************************************************************************* */ +// Constrained noise model +TEST(ExpressionFactor, Constrained) { + using namespace leaf; + + SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0)); + + // Create old-style factor to create expected value and derivatives + PriorFactor old(2, Point2(0, 0), model); + + // Concise version + ExpressionFactor f(model, Point2(0, 0), p); + EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf2 = f.linearize(values); + EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); +} /* ************************************************************************* */ // Unary(Leaf)) From 17d352bab4cf5784d502c7865acdc96c40bcbc41 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 14:42:59 +0100 Subject: [PATCH 23/23] Slight re-factor --- gtsam_unstable/nonlinear/ExpressionFactor.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index f7b61120a..37a89af6b 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -128,13 +128,12 @@ public: // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! - Vector b(-measurement_.localCoordinates(value)); + Ab(size()).col(0) = -measurement_.localCoordinates(value); - // Whiten the corresponding system - // Note the Ab.matrix() includes uninitialized RHS, but this beats taking it apart - noiseModel_->WhitenSystem(Ab.matrix(),b); + // Whiten the corresponding system, Ab already contains RHS + Vector dummy(Dim); + noiseModel_->WhitenSystem(Ab.matrix(),dummy); - Ab(size()).col(0) = b; return factor; } };