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