From 08fab8a9362d4cc12365afb76233eae85fa2db6c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 29 May 2022 13:33:04 -0400
Subject: [PATCH] HybridNonlinearFactor linearize method

---
 gtsam/hybrid/HybridNonlinearFactor.h | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/gtsam/hybrid/HybridNonlinearFactor.h
index 53d7bfbf4..504992e22 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.h
+++ b/gtsam/hybrid/HybridNonlinearFactor.h
@@ -18,6 +18,7 @@
 #pragma once
 
 #include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
@@ -53,5 +54,10 @@ class HybridNonlinearFactor : public HybridFactor {
   /// @}
 
   NonlinearFactor::shared_ptr inner() const { return inner_; }
+
+  /// Linearize to a HybridGaussianFactor at the linearization point `c`.
+  boost::shared_ptr<HybridGaussianFactor> linearize(const Values &c) const {
+    return boost::make_shared<HybridGaussianFactor>(inner_->linearize(c));
+  }
 };
 }  // namespace gtsam