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

Extra asserts

release/4.3a0
Frank Dellaert 2013-11-01 01:03:32 +00:00
parent 4c47e6fcc4
commit b123c5dda2
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/linear/HessianFactor.cpp
View File

@ -392,10 +392,11 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
// Apply updates to the upper triangle // Apply updates to the upper triangle
gttic(update); gttic(update);
size_t nrInfoBlocks = this->info_.nBlocks();
for(DenseIndex j2=0; j2<update.info_.nBlocks(); ++j2) { for(DenseIndex j2=0; j2<update.info_.nBlocks(); ++j2) {
DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? this->info_.nBlocks()-1 : slots[j2]; DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j2];
for(DenseIndex j1=0; j1<=j2; ++j1) { for(DenseIndex j1=0; j1<=j2; ++j1) {
DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? this->info_.nBlocks()-1 : slots[j1]; DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? nrInfoBlocks-1 : slots[j1];
if(slot2 > slot1) if(slot2 > slot1)
info_(slot1, slot2).noalias() += update.info_(j1, j2); info_(slot1, slot2).noalias() += update.info_(j1, j2);
else if(slot1 > slot2) else if(slot1 > slot2)
@ -450,10 +451,13 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
// Apply updates to the upper triangle // Apply updates to the upper triangle
gttic(update); gttic(update);
for(DenseIndex j2=0; j2<updateBlocks.nBlocks(); ++j2) { // Horizontal block of Hessian size_t nrInfoBlocks = this->info_.nBlocks(), nrUpdateBlocks = updateBlocks.nBlocks();
DenseIndex slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2]; for(DenseIndex j2=0; j2<nrUpdateBlocks; ++j2) { // Horizontal block of Hessian
DenseIndex slot2 = (j2 == update.size()) ? nrInfoBlocks-1 : slots[j2];
assert(slot2>=0 && slot2<=nrInfoBlocks);
for(DenseIndex j1=0; j1<=j2; ++j1) { // Vertical block of Hessian for(DenseIndex j1=0; j1<=j2; ++j1) { // Vertical block of Hessian
DenseIndex slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; DenseIndex slot1 = (j1 == update.size()) ? nrInfoBlocks-1 : slots[j1];
assert(slot1>=0 && slot1<nrInfoBlocks);
updateBlocks.offset(0); updateBlocks.offset(0);
if(slot2 > slot1) if(slot2 > slot1)
info_(slot1, slot2).noalias() += updateBlocks(j1).transpose() * updateBlocks(j2); info_(slot1, slot2).noalias() += updateBlocks(j1).transpose() * updateBlocks(j2);