diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 8298e813a..d64d3d5bb 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -96,9 +96,11 @@ void HybridSmoother::removeFixedValues( } /* ************************************************************************* */ -void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors, +void HybridSmoother::update(const HybridNonlinearFactorGraph &newFactors, + const Values &initial, std::optional maxNrLeaves, const std::optional given_ordering) { + HybridGaussianFactorGraph linearizedFactors = *newFactors.linearize(initial); const KeySet originalNewFactorKeys = newFactors.keys(); #ifdef DEBUG_SMOOTHER std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() @@ -108,7 +110,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors, HybridGaussianFactorGraph updatedGraph; // Add the necessary conditionals from the previous timestep(s). std::tie(updatedGraph, hybridBayesNet_) = - addConditionals(newFactors, hybridBayesNet_); + addConditionals(linearizedFactors, hybridBayesNet_); #ifdef DEBUG_SMOOTHER // print size of newFactors, updatedGraph, hybridBayesNet_ std::cout << "updatedGraph size: " << updatedGraph.size() << std::endl; diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index ef75b06b5..e4f97b130 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -19,6 +19,7 @@ #include #include #include +#include #include @@ -26,8 +27,9 @@ namespace gtsam { class GTSAM_EXPORT HybridSmoother { private: - HybridBayesNet hybridBayesNet_; + HybridNonlinearFactorGraph allFactors_; + HybridBayesNet hybridBayesNet_; /// The threshold above which we make a decision about a mode. std::optional marginalThreshold_; DiscreteValues fixedValues_; @@ -73,12 +75,12 @@ class GTSAM_EXPORT HybridSmoother { * @param graph The new factors, should be linear only * @param maxNrLeaves The maximum number of leaves in the new discrete factor, * if applicable - * @param given_ordering The (optional) ordering for elimination, only + * @param givenOrdering The (optional) ordering for elimination, only * continuous variables are allowed */ - void update(const HybridGaussianFactorGraph& graph, + void update(const HybridNonlinearFactorGraph& graph, const Values& initial, std::optional maxNrLeaves = {}, - const std::optional given_ordering = {}); + const std::optional givenOrdering = {}); /** * @brief Get an elimination ordering which eliminates continuous