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

In iSAM2, added a check in the Batch-mode path that checks if linearized factors should be stored for performance reasons. Without this, factors were cached in batch-mode but not in the normal incremental-mode, leading to inconsistency

release/4.3a0
Stephen Williams 2012-07-05 18:59:10 +00:00
parent f55612d381
commit 134951f21c
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/nonlinear/ISAM2.cpp
View File

@ -329,11 +329,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
toc(1,"reorder"); toc(1,"reorder");
tic(2,"linearize"); tic(2,"linearize");
linearFactors_ = *nonlinearFactors_.linearize(theta_, ordering_); GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_, ordering_);
if(params_.cacheLinearizedFactors)
linearFactors_ = linearized;
toc(2,"linearize"); toc(2,"linearize");
tic(5,"eliminate"); tic(5,"eliminate");
JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearFactors_, variableIndex_); JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearized, variableIndex_);
sharedClique newRoot; sharedClique newRoot;
if(params_.factorization == ISAM2Params::CHOLESKY) if(params_.factorization == ISAM2Params::CHOLESKY)
newRoot = jt.eliminate(EliminatePreferCholesky); newRoot = jt.eliminate(EliminatePreferCholesky);
@ -352,7 +354,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
lastAffectedMarkedCount = markedKeys.size(); lastAffectedMarkedCount = markedKeys.size();
lastAffectedVariableCount = affectedKeysSet->size(); lastAffectedVariableCount = affectedKeysSet->size();
lastAffectedFactorCount = linearFactors_.size(); lastAffectedFactorCount = linearized.size();
// Reeliminated keys for detailed results // Reeliminated keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {