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

Play with different elimination schemes

release/4.3a0
Frank Dellaert 2025-01-30 16:55:50 -05:00
parent 16b2710c3f
commit a24246ab63
2 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -50,7 +50,7 @@
#include <utility>
#include <vector>
#define GTSAM_HYBRID_WITH_TABLEFACTOR 0
#define GTSAM_HYBRID_WITH_TABLEFACTOR 1
namespace gtsam {

13
gtsam/hybrid/HybridSmoother.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
/* ************************************************************************* */
Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
const KeySet &continuousKeys) {
const KeySet &lastKeysToEliminate) {
// Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeySet();
@ -34,7 +34,7 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
KeyVector lastKeys;
// Insert continuous keys first.
for (auto &k : continuousKeys) {
for (auto &k : lastKeysToEliminate) {
if (!allDiscrete.exists(k)) {
lastKeys.push_back(k);
}
@ -44,11 +44,10 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
std::copy(allDiscrete.begin(), allDiscrete.end(),
std::back_inserter(lastKeys));
const VariableIndex index(factors);
// Get an ordering where the new keys are eliminated last
Ordering ordering = Ordering::ColamdConstrainedLast(
index, KeyVector(lastKeys.begin(), lastKeys.end()), true);
factors, KeyVector(lastKeys.begin(), lastKeys.end()), true);
return ordering;
}
@ -79,7 +78,9 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
// If no ordering provided, then we compute one
if (!given_ordering.has_value()) {
// Get the keys from the new factors
const KeySet continuousKeysToInclude;// = newFactors.keys();
KeySet continuousKeysToInclude; // Scheme 1: empty, 15sec/2000, 64sec/3000 (69s without TF)
// continuousKeysToInclude = newFactors.keys(); // Scheme 2: all, 8sec/2000, 160sec/3000
// continuousKeysToInclude = updatedGraph.keys(); // Scheme 3: all, stopped after 80sec/2000
// Since updatedGraph now has all the connected conditionals,
// we can get the correct ordering.