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,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_HessianFactor);
// First build an array of slots
FastVector<DenseIndex> 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);
}
}

10
gtsam/linear/JacobianFactor.cpp
View File

@ -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<DenseIndex> 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

1
gtsam/slam/GeneralSFMFactor.h
View File

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