diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 2d486ab2f..aa3bf0657 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -291,4 +291,20 @@ HybridValues HybridSmoother::optimize() const { return HybridValues(continuous, mpe); } +/* ************************************************************************* */ +void HybridSmoother::relinearize() { + allFactors_ = allFactors_.restrict(fixedValues_); + HybridGaussianFactorGraph::shared_ptr linearized = + allFactors_.linearize(linearizationPoint_); + HybridBayesNet::shared_ptr bayesNet = linearized->eliminateSequential(); + HybridValues delta = bayesNet->optimize(); + linearizationPoint_ = linearizationPoint_.retract(delta.continuous()); + reInitialize(*bayesNet); +} + +/* ************************************************************************* */ +Values HybridSmoother::linearizationPoint() const { + return linearizationPoint_; +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index f973573ea..7ef7f4ebb 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -126,15 +126,12 @@ class GTSAM_EXPORT HybridSmoother { /// Optimize the hybrid Bayes Net, taking into accound fixed values. HybridValues optimize() const; - void relinearize() { - allFactors_ = allFactors_.restrict(fixedValues_); - HybridGaussianFactorGraph::shared_ptr linearized = - allFactors_.linearize(linearizationPoint_); - HybridBayesNet::shared_ptr bayesNet = linearized->eliminateSequential(); - HybridValues delta = bayesNet->optimize(); - linearizationPoint_ = linearizationPoint_.retract(delta.continuous()); - reInitialize(*bayesNet); - } + /// Relinearize the nonlinear factor graph + /// with the latest linearization point. + void relinearize(); + + /// Return the current linearization point. + Values linearizationPoint() const; private: /// Helper to compute the ordering if ordering is not given. diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 9ea7ea0e9..fa58caea7 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -283,12 +283,15 @@ class HybridSmoother { void reInitialize(gtsam::HybridBayesNet& hybridBayesNet); void update( - const gtsam::HybridNonlinearFactorGraph& graph, const Values& initial, + const gtsam::HybridNonlinearFactorGraph& graph, + const gtsam::Values& initial, std::optional maxNrLeaves = std::nullopt, const std::optional given_ordering = std::nullopt); void relinearize(); + gtsam::Values linearizationPoint() const; + gtsam::Ordering getOrdering(const gtsam::HybridGaussianFactorGraph& factors, const gtsam::KeySet& newFactorKeys);