From 6664699c4c124d5c82bb72c491b7d9d2401b42ab Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 12 Jun 2015 09:33:55 -0700 Subject: [PATCH 1/2] getSlots method --- gtsam/linear/HessianFactor.cpp | 12 +++-------- gtsam/linear/JacobianFactor.cpp | 20 +++++++----------- gtsam/linear/Scatter.cpp | 37 +++++++++++++++++++++++---------- gtsam/linear/Scatter.h | 22 +++++++++++++------- gtsam/slam/GeneralSFMFactor.h | 5 +++-- 5 files changed, 54 insertions(+), 42 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 27bd61fbd..a038a7ff8 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -347,19 +347,13 @@ double HessianFactor::error(const VectorValues& c) const { /* ************************************************************************* */ void HessianFactor::updateHessian(const Scatter& scatter, - SymmetricBlockMatrix* info) const { + SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); - // N is number of variables in information matrix, n in HessianFactor - DenseIndex N = info->nBlocks() - 1, n = size(); - // First build an array of slots - FastVector slots(n + 1); - DenseIndex slot = 0; - BOOST_FOREACH (Key key, *this) - slots[slot++] = scatter.at(key).slot; - slots[n] = N; + FastVector slots = scatter.getSlotsForKeys(keys_); // Apply updates to the upper triangle + DenseIndex n = size(); for (DenseIndex j = 0; j <= n; ++j) { DenseIndex J = slots[j]; for (DenseIndex i = 0; i <= j; ++i) { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index ff5431432..3d42db1ca 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -499,7 +499,7 @@ map JacobianFactor::hessianBlockDiagonal() const { /* ************************************************************************* */ void JacobianFactor::updateHessian(const Scatter& scatter, - SymmetricBlockMatrix* info) const { + SymmetricBlockMatrix* info) const { gttic(updateHessian_JacobianFactor); if (rows() == 0) return; @@ -514,24 +514,20 @@ void JacobianFactor::updateHessian(const Scatter& scatter, JacobianFactor whitenedFactor = whiten(); whitenedFactor.updateHessian(scatter, info); } else { - // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below - // N is number of variables in information matrix, n in JacobianFactor - DenseIndex N = info->nBlocks() - 1, n = Ab_.nBlocks() - 1; - // First build an array of slots - FastVector slots(n + 1); - DenseIndex slot = 0; - BOOST_FOREACH (Key key, *this) - slots[slot++] = scatter.at(key).slot; - slots[n] = N; + FastVector slots = scatter.getSlotsForKeys(keys_); + + // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below + DenseIndex n = Ab_.nBlocks() - 1; // Apply updates to the upper triangle // Loop over blocks of A, including RHS with j==n + // BOOST_FOREACH(DenseIndex J, slots) for (DenseIndex j = 0; j <= n; ++j) { - DenseIndex J = slots[j]; + const DenseIndex J = slots[j]; // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { - DenseIndex I = slots[i]; + const DenseIndex I = slots[i]; (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j); } // Fill diagonal block with Aj'*Aj diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 3efbc2004..21d20c14c 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -34,16 +34,17 @@ string SlotEntry::toString() const { /* ************************************************************************* */ Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { + boost::optional ordering) { gttic(Scatter_Constructor); - static const size_t none = std::numeric_limits::max(); + static const DenseIndex none = std::numeric_limits::max(); // First do the set union. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { + BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { if (factor) { for (GaussianFactor::const_iterator variable = factor->begin(); - variable != factor->end(); ++variable) { - // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers + variable != factor->end(); ++variable) { + // TODO: Fix this hack to cope with zero-row Jacobians that come from + // BayesTreeOrphanWrappers const JacobianFactor* asJacobian = dynamic_cast(factor.get()); if (!asJacobian || asJacobian->cols() > 1) @@ -56,22 +57,36 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, // If we have an ordering, pre-fill the ordered variables first size_t slot = 0; if (ordering) { - BOOST_FOREACH(Key key, *ordering) { + BOOST_FOREACH (Key key, *ordering) { const_iterator entry = find(key); if (entry == end()) throw std::invalid_argument( "The ordering provided to the HessianFactor Scatter constructor\n" - "contained extra variables that did not appear in the factors to combine."); + "contained extra variables that did not appear in the factors to " + "combine."); at(key).slot = (slot++); } } // Next fill in the slot indices (we can only get these after doing the set // union. - BOOST_FOREACH(value_type& var_slot, *this) { - if (var_slot.second.slot == none) - var_slot.second.slot = (slot++); + BOOST_FOREACH (value_type& var_slot, *this) { + if (var_slot.second.slot == none) var_slot.second.slot = (slot++); } } -} // gtsam +/* ************************************************************************* */ +FastVector Scatter::getSlotsForKeys( + const FastVector& keys) const { + gttic(getSlotsForKeys); + FastVector slots(keys.size() + 1); + DenseIndex slot = 0; + BOOST_FOREACH (Key key, keys) + slots[slot++] = at(key).slot; + slots.back() = size(); + return slots; +} + +/* ************************************************************************* */ + +} // gtsam diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index e212c5867..1d6c546b8 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -30,12 +30,11 @@ namespace gtsam { class GaussianFactorGraph; class Ordering; -/** - * One SlotEntry stores the slot index for a variable, as well its dimension. - */ +/// One SlotEntry stores the slot index for a variable, as well its dimension. struct GTSAM_EXPORT SlotEntry { - size_t slot, dimension; - SlotEntry(size_t _slot, size_t _dimension) + DenseIndex slot; + size_t dimension; + SlotEntry(DenseIndex _slot, size_t _dimension) : slot(_slot), dimension(_dimension) {} std::string toString() const; }; @@ -43,14 +42,21 @@ struct GTSAM_EXPORT SlotEntry { /** * Scatter is an intermediate data structure used when building a HessianFactor * incrementally, to get the keys in the right order. The "scatter" is a map - * from - * global variable indices to slot indices in the union of involved variables. - * We also include the dimensionality of the variable. + * from global variable indices to slot indices in the union of involved + * variables. We also include the dimensionality of the variable. */ class Scatter : public FastMap { public: Scatter(const GaussianFactorGraph& gfg, boost::optional ordering = boost::none); + + DenseIndex slot(Key key) const { return at(key).slot; } + + /** + * For the subset of keys given, return the slots in the same order, + * terminated by the a RHS slot equal to N, the size of the Scatter + */ + FastVector getSlotsForKeys(const FastVector& keys) const; }; } // \ namespace gtsam diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index ec779cbd4..c469bcada 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -166,8 +166,9 @@ namespace gtsam { DenseIndex N = info->nBlocks() - 1; // First build an array of slots - DenseIndex slotC = scatter.at(this->keys().front()).slot; - DenseIndex slotL = scatter.at(this->keys().back()).slot; + FastVector slots = scatter.getSlotsForKeys(keys_); + DenseIndex slotC = scatter.slot(keys_.front()); + DenseIndex slotL = scatter.slot(keys_.back()); DenseIndex slotB = N; // We perform I += A'*A to the upper triangle From c8cff296fbe24606811b567c65004a8552811e28 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Jun 2015 09:01:13 -0700 Subject: [PATCH 2/2] Don't bother making an array --- gtsam/linear/HessianFactor.cpp | 9 +++------ gtsam/linear/JacobianFactor.cpp | 10 +++------- gtsam/slam/GeneralSFMFactor.h | 1 - 3 files changed, 6 insertions(+), 14 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index a038a7ff8..c74d53476 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -349,15 +349,12 @@ double HessianFactor::error(const VectorValues& c) const { void HessianFactor::updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); - // First build an array of slots - FastVector slots = scatter.getSlotsForKeys(keys_); - // Apply updates to the upper triangle - DenseIndex n = size(); + DenseIndex n = size(), N = info->nBlocks()-1; for (DenseIndex j = 0; j <= n; ++j) { - DenseIndex J = slots[j]; + const DenseIndex J = j==n ? N : scatter.slot(keys_[j]); for (DenseIndex i = 0; i <= j; ++i) { - DenseIndex I = slots[i]; + const DenseIndex I = i==n ? N : scatter.slot(keys_[i]); (*info)(I, J) += info_(i, j); } } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 3d42db1ca..1e3433268 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -514,20 +514,16 @@ void JacobianFactor::updateHessian(const Scatter& scatter, JacobianFactor whitenedFactor = whiten(); whitenedFactor.updateHessian(scatter, info); } else { - // First build an array of slots - FastVector slots = scatter.getSlotsForKeys(keys_); - // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below - DenseIndex n = Ab_.nBlocks() - 1; + DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1; // Apply updates to the upper triangle // Loop over blocks of A, including RHS with j==n - // BOOST_FOREACH(DenseIndex J, slots) for (DenseIndex j = 0; j <= n; ++j) { - const DenseIndex J = slots[j]; + const DenseIndex J = j==n ? N : scatter.slot(keys_[j]); // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { - const DenseIndex I = slots[i]; + const DenseIndex I = scatter.slot(keys_[i]); (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j); } // Fill diagonal block with Aj'*Aj diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index c469bcada..4425d63d0 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -166,7 +166,6 @@ namespace gtsam { DenseIndex N = info->nBlocks() - 1; // First build an array of slots - FastVector slots = scatter.getSlotsForKeys(keys_); DenseIndex slotC = scatter.slot(keys_.front()); DenseIndex slotL = scatter.slot(keys_.back()); DenseIndex slotB = N;