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

separate function to collect discrete factors

release/4.3a0
Varun Agrawal 2025-01-02 16:07:46 -05:00
parent 02d9959033
commit 113492f8b5
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -296,14 +296,14 @@ static TableFactor TableProduct(const DiscreteFactorGraph &factors) {
} }
/* ************************************************************************ */ /* ************************************************************************ */
static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>> static DiscreteFactorGraph CollectDiscreteFactors(
discreteElimination(const HybridGaussianFactorGraph &factors, const HybridGaussianFactorGraph &factors) {
const Ordering &frontalKeys) {
DiscreteFactorGraph dfg; DiscreteFactorGraph dfg;
for (auto &f : factors) { for (auto &f : factors) {
if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) { if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
dfg.push_back(df); dfg.push_back(df);
} else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) { } else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
// Case where we have a HybridGaussianFactor with no continuous keys. // Case where we have a HybridGaussianFactor with no continuous keys.
// In this case, compute a discrete factor from the remaining error. // In this case, compute a discrete factor from the remaining error.
@ -336,6 +336,15 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
} }
} }
return dfg;
}
/* ************************************************************************ */
static std::pair<HybridConditional::shared_ptr, std::shared_ptr<Factor>>
discreteElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) {
DiscreteFactorGraph dfg = CollectDiscreteFactors(factors);
#if GTSAM_HYBRID_TIMING #if GTSAM_HYBRID_TIMING
gttic_(EliminateDiscrete); gttic_(EliminateDiscrete);
#endif #endif