diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index b6622980b..c95cc686f 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -186,6 +186,7 @@ DiscreteValues HybridBayesNet::mpe() const { } } } + return discrete_fg.optimize(); } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 4943f91cb..bc36ec94d 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -327,6 +327,8 @@ discreteElimination(const HybridGaussianFactorGraph &factors, gttic_(EliminateDiscreteFormDiscreteConditional); #endif // Check type of product, and get as TableFactor for efficiency. + // Use object instead of pointer since we need it + // for the TableDistribution constructor. TableFactor p; if (auto tf = std::dynamic_pointer_cast(product)) { p = *tf; @@ -334,11 +336,12 @@ discreteElimination(const HybridGaussianFactorGraph &factors, p = TableFactor(product->toDecisionTreeFactor()); } auto conditional = std::make_shared(p); + #if GTSAM_HYBRID_TIMING gttoc_(EliminateDiscreteFormDiscreteConditional); #endif - DiscreteFactor::shared_ptr sum = product->sum(frontalKeys); + DiscreteFactor::shared_ptr sum = p.sum(frontalKeys); return {std::make_shared(conditional), sum};