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

Merged in enriquefernandezperdomo/gtsam/fix_recalculate_batch_step_when_variables_removed_onto_develop_04_aug_2016 (pull request #272) 

Fix recalculate batch step when variables removed
release/4.3a0
Enrique Fernández Perdomo 2018-09-17 15:04:47 +00:00 committed by Frank Dellaert
parent 6b6a8495bb 2fdb6ce48f
commit 19707b88a1
2 changed files with 26 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/inference/Ordering.cpp
View File

@ -53,8 +53,19 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
gttic(Ordering_COLAMDConstrained); gttic(Ordering_COLAMDConstrained);
gttic(Prepare); gttic(Prepare);
const size_t nVars = variableIndex.size();
if (nVars == 0)
{
return Ordering();
}
if (nVars == 1)
{
return Ordering(std::vector<Key>(1, variableIndex.begin()->first));
}
const size_t nEntries = variableIndex.nEntries(), nFactors = const size_t nEntries = variableIndex.nEntries(), nFactors =
variableIndex.nFactors(), nVars = variableIndex.size(); variableIndex.nFactors();
// Convert to compressed column major format colamd wants it in (== MATLAB format!) // Convert to compressed column major format colamd wants it in (== MATLAB format!)
const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors,
(int) nVars); /* colamd arg 3: size of the array A */ (int) nVars); /* colamd arg 3: size of the array A */

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);