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

Save slots to bring cost down from O(n^3) to O(n^2) - again, in theory. In practice, it did seem to help for larger HessianFactors (as expected).

release/4.3a0
Frank Dellaert 2015-06-13 12:26:10 -07:00
parent f6575323d6
commit d0775faeba
2 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/linear/HessianFactor.cpp
View File

@ -350,11 +350,13 @@ void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_HessianFactor);
// Apply updates to the upper triangle
DenseIndex n = size(), N = info->nBlocks()-1;
DenseIndex n = size(), N = info->nBlocks() - 1;
vector<DenseIndex> slots(n + 1);
for (DenseIndex j = 0; j <= n; ++j) {
const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]);
const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
slots[j] = J;
for (DenseIndex i = 0; i <= j; ++i) {
const DenseIndex I = (i==n) ? N : Slot(infoKeys, keys_[i]);
const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid.
(*info)(I, J) += info_(i, j);
}
}

6
gtsam/linear/JacobianFactor.cpp
View File

@ -519,11 +519,13 @@ void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
// Apply updates to the upper triangle
// Loop over blocks of A, including RHS with j==n
vector<DenseIndex> slots(n+1);
for (DenseIndex j = 0; j <= n; ++j) {
const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]);
const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
slots[j] = J;
// Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) {
const DenseIndex I = Slot(infoKeys, keys_[i]);
const DenseIndex I = slots[i]; // because i<j, slots[i] is valid.
(*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j);
}
// Fill diagonal block with Aj'*Aj