diff --git a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o b/gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o deleted file mode 100644 index 0e2eddee2..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o b/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o deleted file mode 100644 index ea1e61b09..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o and /dev/null differ diff --git a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o b/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o deleted file mode 100644 index 213b1cc0c..000000000 Binary files a/gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o and /dev/null differ diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 9327d0803..b4cbb8728 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 via key + 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 e94e2bec4..aaf10dc98 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -124,7 +124,7 @@ 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. */ @@ -134,18 +134,19 @@ public: // 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)); - } + 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); - T value = expression_.value(x, blocks); return measurement_.localCoordinates(value); } else { const T& value = expression_.value(x); @@ -167,14 +168,11 @@ public: 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], factor->Ab()(i))); + // Wrap keys and VerticalBlockMatrix into structure passed to expression_ + JacobianMap map(keys_, factor->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 ! factor->Ab()(size()).col(0) = -measurement_.localCoordinates(value); // Whiten the corresponding system now 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); } /* ************************************************************************* */ diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp index 04908f129..92522c440 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -27,7 +27,7 @@ using namespace std; using namespace gtsam; -#define time timeMultiThreaded +#define time timeSingleThreaded boost::shared_ptr fixedK(new Cal3_S2());