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

break into smaller methods

release/4.3a0
Varun Agrawal 2025-03-17 16:00:13 -04:00
parent 88fb8f7c65
commit 2dc45bac96
2 changed files with 44 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
gtsam/hybrid/HybridSmoother.cpp
View File

@ -61,6 +61,40 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
return ordering;
}
/* ************************************************************************* */
Ordering HybridSmoother::maybeComputeOrdering(
const HybridGaussianFactorGraph &updatedGraph,
const std::optional<Ordering> givenOrdering) {
Ordering ordering;
// If no ordering provided, then we compute one
if (!givenOrdering.has_value()) {
// Get the keys from the new factors
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.
ordering = this->getOrdering(updatedGraph, continuousKeysToInclude);
} else {
ordering = *givenOrdering;
}
return ordering;
}
/* ************************************************************************* */
void HybridSmoother::removeFixedValues(
const HybridGaussianFactorGraph &newFactors) {
for (Key key : newFactors.discreteKeySet()) {
if (fixedValues_.find(key) != fixedValues_.end()) {
fixedValues_.erase(key);
}
}
}
/* ************************************************************************* */
void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
std::optional<size_t> maxNrLeaves,
@ -84,22 +118,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
<< std::endl;
#endif
Ordering ordering;
// If no ordering provided, then we compute one
if (!given_ordering.has_value()) {
// Get the keys from the new factors
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.
ordering = this->getOrdering(updatedGraph, continuousKeysToInclude);
} else {
ordering = *given_ordering;
}
Ordering ordering = this->maybeComputeOrdering(updatedGraph, given_ordering);
#if GTSAM_HYBRID_TIMING
gttic_(HybridSmootherEliminate);
@ -119,11 +138,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
#endif
// Remove fixed values for discrete keys which are introduced in newFactors
for (Key key : newFactors.discreteKeySet()) {
if (fixedValues_.find(key) != fixedValues_.end()) {
fixedValues_.erase(key);
}
}
removeFixedValues(newFactors);
#ifdef DEBUG_SMOOTHER
// Print discrete keys in the bayesNetFragment:

8
gtsam/hybrid/HybridSmoother.h
View File

@ -122,6 +122,14 @@ class GTSAM_EXPORT HybridSmoother {
/// Optimize the hybrid Bayes Net, taking into accound fixed values.
HybridValues optimize() const;
private:
/// Helper to compute the ordering if ordering is not given.
Ordering maybeComputeOrdering(const HybridGaussianFactorGraph& updatedGraph,
const std::optional<Ordering> givenOrdering);
/// Remove fixed discrete values for discrete keys introduced in `newFactors`.
void removeFixedValues(const HybridGaussianFactorGraph& newFactors);
};
} // namespace gtsam