new HybridSmoother::relinearize method

gtsam/hybrid/HybridSmoother.cpp

@@ -101,6 +101,12 @@ void HybridSmoother::update(const HybridNonlinearFactorGraph &newFactors,
                             std::optional<size_t> maxNrLeaves,
                             const std::optional<Ordering> 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()

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,

gtsam/hybrid/hybrid.i

@@ -287,6 +287,8 @@ class HybridSmoother {
       std::optional<size_t> maxNrLeaves = std::nullopt,
       const std::optional<gtsam::Ordering> given_ordering = std::nullopt);
 
+  void relinearize();
+
   gtsam::Ordering getOrdering(const gtsam::HybridGaussianFactorGraph& factors,
                               const gtsam::KeySet& newFactorKeys);