From 7b539fbb5cc6082a698c27a6604cc8627fcbbd42 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Nov 2014 11:35:49 +0100 Subject: [PATCH 1/3] 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 2/3] 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 3/3] 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); } /* ************************************************************************* */