diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index d64d3d5bb..2d486ab2f 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -101,6 +101,12 @@ void HybridSmoother::update(const HybridNonlinearFactorGraph &newFactors, std::optional maxNrLeaves, const std::optional given_ordering) { HybridGaussianFactorGraph linearizedFactors = *newFactors.linearize(initial); + + // Record the new nonlinear factors and + // linearization point for relinearization + allFactors_.push_back(newFactors); + linearizationPoint_.insert_or_assign(initial); + const KeySet originalNewFactorKeys = newFactors.keys(); #ifdef DEBUG_SMOOTHER std::cout << "hybridBayesNet_ size before: " << hybridBayesNet_.size() diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index e4f97b130..f973573ea 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -28,6 +28,7 @@ namespace gtsam { class GTSAM_EXPORT HybridSmoother { private: HybridNonlinearFactorGraph allFactors_; + Values linearizationPoint_; HybridBayesNet hybridBayesNet_; /// The threshold above which we make a decision about a mode. @@ -125,6 +126,16 @@ 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); + } + private: /// Helper to compute the ordering if ordering is not given. Ordering maybeComputeOrdering(const HybridGaussianFactorGraph& updatedGraph, diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 610062345..9ea7ea0e9 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -287,6 +287,8 @@ class HybridSmoother { std::optional maxNrLeaves = std::nullopt, const std::optional given_ordering = std::nullopt); + void relinearize(); + gtsam::Ordering getOrdering(const gtsam::HybridGaussianFactorGraph& factors, const gtsam::KeySet& newFactorKeys);