add new methods for HybridSmoother relinearization

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

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.

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<size_t> maxNrLeaves = std::nullopt,
       const std::optional<gtsam::Ordering> given_ordering = std::nullopt);
 
   void relinearize();
 
+  gtsam::Values linearizationPoint() const;
+
   gtsam::Ordering getOrdering(const gtsam::HybridGaussianFactorGraph& factors,
                               const gtsam::KeySet& newFactorKeys);