add HybridNonlinearFactor and nonlinear HybridFactorGraph

gtsam/hybrid/HybridFactorGraph.h

@@ -0,0 +1,108 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   HybridFactorGraph.h
+ * @brief  Nonlinear hybrid factor graph that uses type erasure
+ * @author Varun Agrawal
+ * @date   May 28, 2022
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/Ordering.h>
+
+namespace gtsam {
+
+/**
+ * Hybrid Factor Graph
+ * -----------------------
+ * This is the non-linear version of a hybrid factor graph.
+ * Everything inside needs to be hybrid factor or hybrid conditional.
+ */
+class HybridFactorGraph : public FactorGraph<HybridFactor> {
+ public:
+  using Base = FactorGraph<HybridFactor>;
+  using This = HybridFactorGraph;              ///< this class
+  using shared_ptr = boost::shared_ptr<This>;  ///< shared_ptr to This
+
+  using Values = gtsam::Values;  ///< backwards compatibility
+  using Indices = KeyVector;     ///> map from keys to values
+
+  /// @name Constructors
+  /// @{
+
+  HybridFactorGraph() = default;
+
+  /**
+   * Implicit copy/downcast constructor to override explicit template container
+   * constructor. In BayesTree this is used for:
+   * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
+   * */
+  template <class DERIVEDFACTOR>
+  HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
+
+  /// @}
+
+  // Allow use of selected FactorGraph methods:
+  using Base::empty;
+  using Base::reserve;
+  using Base::size;
+  using FactorGraph::add;
+  using Base::operator[];
+
+  /// Add a Jacobian factor to the factor graph.
+  void add(JacobianFactor&& factor);
+
+  /// Add a Jacobian factor as a shared ptr.
+  void add(boost::shared_ptr<JacobianFactor> factor);
+
+  /// Add a DecisionTreeFactor to the factor graph.
+  void add(DecisionTreeFactor&& factor);
+
+  /// Add a DecisionTreeFactor as a shared ptr.
+  void add(boost::shared_ptr<DecisionTreeFactor> factor);
+
+  /**
+   * @brief Linearize all the continuous factors in the
+   * NonlinearHybridFactorGraph.
+   *
+   * @param continuousValues: Dictionary of continuous values.
+   * @return GaussianHybridFactorGraph::shared_ptr
+   */
+  GaussianHybridFactorGraph::shared_ptr linearize(
+      const Values& continuousValues) const {
+    // create an empty linear FG
+    GaussianHybridFactorGraph::shared_ptr linearFG =
+        boost::make_shared<GaussianHybridFactorGraph>();
+
+    linearFG->reserve(size());
+
+    // linearize all factors
+    for (const sharedFactor& factor : factors_) {
+      if (factor) {
+        if (auto nf = boost::dynamic_pointer_cast<NonlinearFactor>) {
+          (*linearFG) += factor->linearize(linearizationPoint);
+        } else if (auto hf = boost::dynamic_pointer_cast<HybridFactor>) {
+          if (hf->isContinuous()) {
+          }
+        }
+
+      } else
+        (*linearFG) += GaussianFactor::shared_ptr();
+    }
+  }
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -0,0 +1,45 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file HybridNonlinearFactor.cpp
+ *  @date May 28, 2022
+ *  @author Varun Agrawal
+ */
+
+#include <gtsam/hybrid/HybridNonlinearFactor.h>
+
+#include <boost/make_shared.hpp>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+HybridNonlinearFactor::HybridNonlinearFactor(NonlinearFactor::shared_ptr other)
+    : Base(other->keys()), inner_(other) {}
+
+/* ************************************************************************* */
+HybridNonlinearFactor::HybridNonlinearFactor(NonlinearFactor &&nf)
+    : Base(nf.keys()),
+      inner_(boost::make_shared<NonlinearFactor>(std::move(nf))) {}
+
+/* ************************************************************************* */
+bool HybridNonlinearFactor::equals(const HybridFactor &lf, double tol) const {
+  return Base(lf, tol);
+}
+
+/* ************************************************************************* */
+void HybridNonlinearFactor::print(const std::string &s,
+                                  const KeyFormatter &formatter) const {
+  HybridFactor::print(s, formatter);
+  inner_->print("inner: ", formatter);
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridNonlinearFactor.h

@@ -0,0 +1,60 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file HybridNonlinearFactor.h
+ *  @date May 28, 2022
+ *  @author Varun Agrawal
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * A HybridNonlinearFactor is a layer over NonlinearFactor so that we do not
+ * have a diamond inheritance.
+ */
+class HybridNonlinearFactor : public HybridFactor {
+ private:
+  NonlinearFactor::shared_ptr inner_;
+
+ public:
+  using Base = HybridFactor;
+  using This = HybridNonlinearFactor;
+  using shared_ptr = boost::shared_ptr<This>;
+
+  // Explicit conversion from a shared ptr of GF
+  explicit HybridNonlinearFactor(NonlinearFactor::shared_ptr other);
+
+  // Forwarding constructor from concrete NonlinearFactor
+  explicit HybridNonlinearFactor(NonlinearFactor &&jf);
+
+ public:
+  /// @name Testable
+  /// @{
+
+  /// Check equality.
+  virtual bool equals(const HybridFactor &lf, double tol) const override;
+
+  /// GTSAM print utility.
+  void print(
+      const std::string &s = "HybridNonlinearFactor\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+
+  /// @}
+
+  NonlinearFactor::shared_ptr inner() const { return inner_; }
+};
+}  // namespace gtsam

gtsam/hybrid/tests/testHybridFactorGraph.h

@@ -0,0 +1,50 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testHybridFactorGraph.cpp
+ * @brief   Unit tests for HybridFactorGraph
+ * @author  Varun Agrawal
+ * @date    May 2021
+ */
+
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/utilities.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+
+/* ****************************************************************************
+ * Test that any linearizedFactorGraph gaussian factors are appended to the
+ * existing gaussian factor graph in the hybrid factor graph.
+ */
+TEST(HybridFactorGraph, GaussianFactorGraph) {
+  // Initialize the hybrid factor graph
+  HybridFactorGraph fg;
+
+  // Add a simple prior factor to the nonlinear factor graph
+  fg.emplace_shared<PriorFactor<double>>(X(0), 0, Isotropic::Sigma(1, 0.1));
+
+  // Add a linear factor to the nonlinear factor graph
+  fg.add(X(0), I_1x1, Vector1(5));
+
+  // Linearization point
+  Values linearizationPoint;
+  linearizationPoint.insert<double>(X(0), 0);
+
+  GaussianHybridFactorGraph ghfg = fg.linearize(linearizationPoint);
+
+  // ghfg.push_back(ghfg.gaussianGraph().begin(), ghfg.gaussianGraph().end());
+
+  // EXPECT_LONGS_EQUAL(2, dcmfg.gaussianGraph().size());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */