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

Merged gtborg/gtsam into develop

release/4.3a0
Mike Sheffler 2018-09-27 03:57:09 -07:00
parent 3b8985fb6e 9aad5084e0
commit 1a1cfdd0ad
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(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 =
variableIndex.nFactors(), nVars = variableIndex.size();
variableIndex.nFactors();
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors,
(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);
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);
gttic(ordering);
Ordering order;
if(constrainKeys)
{
order = Ordering::ColamdConstrained(variableIndex_, *constrainKeys);
order = Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys);
}
else
{
@ -350,11 +360,11 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
FastMap<Key, int> constraintGroups;
for(Key var: observedKeys)
constraintGroups[var] = 1;
order = Ordering::ColamdConstrained(variableIndex_, constraintGroups);
order = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups);
}
else
{
order = Ordering::Colamd(variableIndex_);
order = Ordering::Colamd(affectedFactorsVarIndex);
}
}
gttoc(ordering);
@ -366,7 +376,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
gttoc(linearize);
gttic(eliminate);
ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, variableIndex_, order))
ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, affectedFactorsVarIndex, order))
.eliminate(params_.getEliminationFunction()).first;
gttoc(eliminate);