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

Pre-compute slots outside of loop.

release/4.3a0
Frank Dellaert 2025-01-28 19:33:48 -05:00
parent 02c8f02a10
commit 003730f844
2 changed files with 22 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam/linear/HessianFactor.cpp
View File

@ -348,28 +348,26 @@ double HessianFactor::error(const VectorValues& c) const {
/* ************************************************************************* */
void HessianFactor::updateHessian(const KeyVector& infoKeys,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_HessianFactor);
assert(info);
// Apply updates to the upper triangle
DenseIndex nrVariablesInThisFactor = size(), nrBlocksInInfo = info->nBlocks() - 1;
gttic(updateHessian_HessianFactor);
const DenseIndex nrVariablesInThisFactor = size();
vector<DenseIndex> slots(nrVariablesInThisFactor + 1);
for (DenseIndex j = 0; j < nrVariablesInThisFactor; ++j)
slots[j] = Slot(infoKeys, keys_[j]);
slots[nrVariablesInThisFactor] = info->nBlocks() - 1;
// Apply updates to the upper triangle
// Loop over this factor's blocks with indices (i,j)
// For every block (i,j), we determine the block (I,J) in info.
for (DenseIndex j = 0; j <= nrVariablesInThisFactor; ++j) {
const bool rhs = (j == nrVariablesInThisFactor);
const DenseIndex J = rhs ? nrBlocksInInfo : Slot(infoKeys, keys_[j]);
slots[j] = J;
for (DenseIndex i = 0; i <= j; ++i) {
const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid.
if (i == j) {
assert(I == J);
info->updateDiagonalBlock(I, info_.diagonalBlock(i));
} else {
assert(i < j);
assert(I != J);
info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j));
}
const DenseIndex J = slots[j];
info->updateDiagonalBlock(J, info_.diagonalBlock(j));
for (DenseIndex i = 0; i < j; ++i) {
const DenseIndex I = slots[i];
assert(i < j);
assert(I != J);
info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j));
}
}
}

11
gtsam/linear/JacobianFactor.cpp
View File

@ -602,16 +602,19 @@ void JacobianFactor::updateHessian(const KeyVector& infoKeys,
// Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below
DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1;
// Pre-calculate slots
vector<DenseIndex> slots(n + 1);
for (DenseIndex j = 0; j < n; ++j) slots[j] = Slot(infoKeys, keys_[j]);
slots[n] = N;
// 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) {
Eigen::Block<const Matrix> Ab_j = Ab_(j);
const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
slots[j] = J;
const DenseIndex J = slots[j];
// Fill off-diagonal blocks with Ai'*Aj
for (DenseIndex i = 0; i < j; ++i) {
const DenseIndex I = slots[i]; // because i<j, slots[i] is valid.
const DenseIndex I = slots[i];
info->updateOffDiagonalBlock(I, J, Ab_(i).transpose() * Ab_j);
}
// Fill diagonal block with Aj'*Aj