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

Only execute the RemoveVariables code in iSAM2 if there actually are variables to remove.

release/4.3a0
Stephen Williams 2012-08-01 21:31:19 +00:00
parent 98aa3b0627
commit 1a8dc9bdcc
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/nonlinear/ISAM2.cpp
View File

@ -747,17 +747,20 @@ ISAM2Result ISAM2::update(
// After the top of the tree has been redone and may have index gaps from
// unused keys, condense the indices to remove gaps by rearranging indices
// in all data structures.
Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
if(!unusedKeys.empty()) {
tic(10,"remove variables");
Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
toc(10,"remove variables");
}
result.cliques = this->nodes().size();
deltaDoglegUptodate_ = false;
deltaUptodate_ = false;
tic(10,"evaluate error after");
tic(11,"evaluate error after");
if(params_.evaluateNonlinearError)
result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate()));
toc(10,"evaluate error after");
toc(11,"evaluate error after");
return result;
}