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

Fix issue with HybridOrdering

release/4.3a0
Frank Dellaert 2023-01-08 00:21:51 -08:00
parent 2e233364b6
commit 691baa56de
1 changed files with 2 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -70,17 +70,10 @@ static void throwRuntimeError(const std::string &s,
/* ************************************************************************ */ /* ************************************************************************ */
const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) { const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) {
KeySet discrete_keys = graph.discreteKeys(); KeySet discrete_keys = graph.discreteKeySet();
for (auto &factor : graph) {
for (const DiscreteKey &k : factor->discreteKeys()) {
discrete_keys.insert(k.first);
}
}
const VariableIndex index(graph); const VariableIndex index(graph);
Ordering ordering = Ordering::ColamdConstrainedLast( return Ordering::ColamdConstrainedLast(
index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true); index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true);
return ordering;
} }
/* ************************************************************************ */ /* ************************************************************************ */