12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Don't bother making an array

release/4.3a0
Frank Dellaert 2015-06-13 09:01:13 -07:00
parent 6664699c4c
commit c8cff296fb
3 changed files with 6 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/linear/HessianFactor.cpp
View File

@ -349,15 +349,12 @@ double HessianFactor::error(const VectorValues& c) const {
void HessianFactor::updateHessian(const Scatter& scatter, void HessianFactor::updateHessian(const Scatter& scatter,
SymmetricBlockMatrix* info) const { SymmetricBlockMatrix* info) const {
gttic(updateHessian_HessianFactor); gttic(updateHessian_HessianFactor);
// First build an array of slots
FastVector<DenseIndex> slots = scatter.getSlotsForKeys(keys_);
// Apply updates to the upper triangle // Apply updates to the upper triangle
DenseIndex n = size(); DenseIndex n = size(), N = info->nBlocks()-1;
for (DenseIndex j = 0; j <= n; ++j) { 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) { 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); (*info)(I, J) += info_(i, j);
} }
} }

10
gtsam/linear/JacobianFactor.cpp
View File

@ -514,20 +514,16 @@ void JacobianFactor::updateHessian(const Scatter& scatter,
JacobianFactor whitenedFactor = whiten(); JacobianFactor whitenedFactor = whiten();
whitenedFactor.updateHessian(scatter, info); whitenedFactor.updateHessian(scatter, info);
} else { } else {
// First build an array of slots
FastVector<DenseIndex> slots = scatter.getSlotsForKeys(keys_);
// Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below // 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 // Apply updates to the upper triangle
// Loop over blocks of A, including RHS with j==n // Loop over blocks of A, including RHS with j==n
// BOOST_FOREACH(DenseIndex J, slots)
for (DenseIndex j = 0; j <= n; ++j) { 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 // Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) { 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); (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j);
} }
// Fill diagonal block with Aj'*Aj // Fill diagonal block with Aj'*Aj

1
gtsam/slam/GeneralSFMFactor.h
View File

@ -166,7 +166,6 @@ namespace gtsam {
DenseIndex N = info->nBlocks() - 1; DenseIndex N = info->nBlocks() - 1;
// First build an array of slots // First build an array of slots
FastVector<DenseIndex> slots = scatter.getSlotsForKeys(keys_);
DenseIndex slotC = scatter.slot(keys_.front()); DenseIndex slotC = scatter.slot(keys_.front());
DenseIndex slotL = scatter.slot(keys_.back()); DenseIndex slotL = scatter.slot(keys_.back());
DenseIndex slotB = N; DenseIndex slotB = N;