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 <utility>
#include <vector> #include <vector>
#define GTSAM_HYBRID_WITH_TABLEFACTOR 0 #define GTSAM_HYBRID_WITH_TABLEFACTOR 1
namespace gtsam { namespace gtsam {

13
gtsam/hybrid/HybridSmoother.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
const KeySet &continuousKeys) { const KeySet &lastKeysToEliminate) {
// Get all the discrete keys from the factors // Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeySet(); KeySet allDiscrete = factors.discreteKeySet();
@ -34,7 +34,7 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
KeyVector lastKeys; KeyVector lastKeys;
// Insert continuous keys first. // Insert continuous keys first.
for (auto &k : continuousKeys) { for (auto &k : lastKeysToEliminate) {
if (!allDiscrete.exists(k)) { if (!allDiscrete.exists(k)) {
lastKeys.push_back(k); lastKeys.push_back(k);
} }
@ -44,11 +44,10 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
std::copy(allDiscrete.begin(), allDiscrete.end(), std::copy(allDiscrete.begin(), allDiscrete.end(),
std::back_inserter(lastKeys)); std::back_inserter(lastKeys));
const VariableIndex index(factors);
// Get an ordering where the new keys are eliminated last // Get an ordering where the new keys are eliminated last
Ordering ordering = Ordering::ColamdConstrainedLast( Ordering ordering = Ordering::ColamdConstrainedLast(
index, KeyVector(lastKeys.begin(), lastKeys.end()), true); factors, KeyVector(lastKeys.begin(), lastKeys.end()), true);
return ordering; return ordering;
} }
@ -79,7 +78,9 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
// If no ordering provided, then we compute one // If no ordering provided, then we compute one
if (!given_ordering.has_value()) { if (!given_ordering.has_value()) {
// Get the keys from the new factors // 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, // Since updatedGraph now has all the connected conditionals,
// we can get the correct ordering. // we can get the correct ordering.