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;