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

Remove unused indices in batch step 

In the batch step (in recalculate) we need to remove the unused indices
from the variable index, otherwise the elimination would throw an
exception saying:

  "Requested to eliminate a key that is not in the factors"
release/4.3a0
Enrique Fernandez 2016-08-03 15:41:27 -04:00
parent 86d1d42c36
commit 2fdb6ce48f
1 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/nonlinear/ISAM2.cpp
View File

@ -334,13 +334,23 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
gttic(add_keys); gttic(add_keys);
br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end())); br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end()));
// Removed unused keys:
VariableIndex affectedFactorsVarIndex = variableIndex_;
affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), unusedIndices.end());
for (const Key key: unusedIndices)
{
affectedKeysSet->erase(key);
}
gttoc(add_keys); gttoc(add_keys);
gttic(ordering); gttic(ordering);
Ordering order; Ordering order;
if(constrainKeys) if(constrainKeys)
{ {
order = Ordering::ColamdConstrained(variableIndex_, *constrainKeys); order = Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys);
} }
else else
{ {
@ -350,11 +360,11 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
FastMap<Key, int> constraintGroups; FastMap<Key, int> constraintGroups;
for(Key var: observedKeys) for(Key var: observedKeys)
constraintGroups[var] = 1; constraintGroups[var] = 1;
order = Ordering::ColamdConstrained(variableIndex_, constraintGroups); order = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups);
} }
else else
{ {
order = Ordering::Colamd(variableIndex_); order = Ordering::Colamd(affectedFactorsVarIndex);
} }
} }
gttoc(ordering); gttoc(ordering);
@ -366,7 +376,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
gttoc(linearize); gttoc(linearize);
gttic(eliminate); gttic(eliminate);
ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, variableIndex_, order)) ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, affectedFactorsVarIndex, order))
.eliminate(params_.getEliminationFunction()).first; .eliminate(params_.getEliminationFunction()).first;
gttoc(eliminate); gttoc(eliminate);