More mock-ups added

gtsam/hybrid/CGMixtureFactor.cpp

@@ -2,4 +2,16 @@
 // Created by Fan Jiang on 3/11/22.
 //
 
-#include "CGMixtureFactor.h"
+#include <gtsam/hybrid/CGMixtureFactor.h>
+
+namespace gtsam {
+
+CGMixtureFactor::CGMixtureFactor(const KeyVector &continuousKeys,
+                                 const DiscreteKeys &discreteKeys,
+                                 const Factors &factors) : Base(continuousKeys, discreteKeys),
+                                                           factors_(factors) {}
+bool CGMixtureFactor::equals(const HybridFactor &lf, double tol) const {
+  return false;
+}
+
+}

gtsam/hybrid/CGMixtureFactor.h

@@ -1,15 +1,21 @@
 /* ----------------------------------------------------------------------------
- * Copyright 2021 The Ambitious Folks of the MRG
+
+ * 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   CGMixtureFactor.h
  * @brief  A set of Gaussian factors indexed by a set of discrete keys.
- * @author Varun Agrawal
  * @author Fan Jiang
+ * @author Varun Agrawal
  * @author Frank Dellaert
- * @date   December 2021
+ * @date   Mar 12, 2022
  */
 
 #include <gtsam/hybrid/HybridFactor.h>
@@ -31,9 +37,11 @@ public:
 
   CGMixtureFactor() = default;
 
-  CGMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors) : Base(continuousKeys, discreteKeys) {
+  CGMixtureFactor(const KeyVector &continuousKeys,
+                  const DiscreteKeys &discreteKeys,
+                  const Factors &factors);
 
-  }
+  bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 };
 
 }

gtsam/hybrid/CLGaussianConditional.cpp

@@ -0,0 +1,20 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   CLGaussianConditional.cpp
+ * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
+ * @author Fan Jiang
+ * @date   Mar 12, 2022
+ */
+
+#include <gtsam/hybrid/CLGaussianConditional.h>
+

gtsam/hybrid/CLGaussianConditional.h

@@ -0,0 +1,31 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   CLGaussianConditional.h
+ * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
+ * @author Fan Jiang
+ * @date   Mar 12, 2022
+ */
+
+#include <gtsam/inference/Conditional.h>
+#include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+class CLGaussianConditional : public HybridFactor, public Conditional<HybridFactor, CLGaussianConditional> {
+public:
+  using This = CLGaussianConditional;
+  using shared_ptr = boost::shared_ptr<CLGaussianConditional>;
+  using BaseFactor = HybridFactor;
+
+
+};
+}

gtsam/hybrid/HybridConditional.h

@@ -34,7 +34,7 @@ namespace gtsam {
  *
  * As a type-erased variant of:
  * - DiscreteConditional
- * - GaussianMixture
+ * - CLGaussianConditional
  * - GaussianConditional
  */
 class GTSAM_EXPORT HybridConditional

gtsam/hybrid/HybridFactorGraph.cpp

@@ -32,20 +32,24 @@ EliminateHybrid(const HybridFactorGraph &factors,
   // In the case of multifrontal, we will need to use a constrained ordering
   // so that the discrete parts will be guaranteed to be eliminated last!
 
-  // PRODUCT: multiply all factors
+  // PREPROCESS: Identify the nature of the current elimination
-  gttic(product);
   KeySet allKeys;
   // TODO: we do a mock by just doing the correct key thing
-  std::cout << "Begin Eliminate\n";
+  std::cout << "Begin Eliminate: ";
+  frontalKeys.print();
+
   for (auto &&factor : factors) {
-    std::cout << ">>> Eliminating: ";
+    std::cout << ">>> Adding factor: ";
-    factor->printKeys();
+    factor->print();
     allKeys.insert(factor->begin(), factor->end());
   }
   for (auto &k : frontalKeys) {
     allKeys.erase(k);
   }
 
+  // PRODUCT: multiply all factors
+  gttic(product);
+
   HybridConditional sum(allKeys.size(), Ordering(allKeys));
 //  HybridDiscreteFactor product(DiscreteConditional());
 //  for (auto&& factor : factors) product = (*factor) * product;

gtsam/hybrid/HybridGaussianFactor.h

@@ -29,11 +29,11 @@ class HybridGaussianFactor : public HybridFactor {
 
   GaussianFactor::shared_ptr inner;
 
-  // Implicit conversion from a shared ptr of GF
+  // Explicit conversion from a shared ptr of GF
-  HybridGaussianFactor(GaussianFactor::shared_ptr other);
+  explicit HybridGaussianFactor(GaussianFactor::shared_ptr other);
 
   // Forwarding constructor from concrete JacobianFactor
-  HybridGaussianFactor(JacobianFactor &&jf);
+  explicit HybridGaussianFactor(JacobianFactor &&jf);
 
  public:
   virtual bool equals(const HybridFactor& lf, double tol) const override;

gtsam/hybrid/tests/testHybridConditional.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
+#include <gtsam/hybrid/CGMixtureFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 
@@ -37,16 +38,15 @@ using namespace std;
 using namespace gtsam;
 
 using gtsam::symbol_shorthand::X;
+using gtsam::symbol_shorthand::C;
 
 /* ************************************************************************* */
-TEST_UNSAFE(HybridFactorGraph, test) {
+TEST_UNSAFE(HybridFactorGraph, creation) {
   HybridConditional test;
-  GTSAM_PRINT(test);
 
   HybridFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
-  GTSAM_PRINT(hfg);
 }
 
 TEST_UNSAFE(HybridFactorGraph, eliminate) {
@@ -56,7 +56,7 @@ TEST_UNSAFE(HybridFactorGraph, eliminate) {
 
   auto result = hfg.eliminatePartialSequential({0});
 
-  GTSAM_PRINT(*result.first);
+  EXPECT_LONGS_EQUAL(result.first->size(), 1);
 }
 
 TEST(HybridFactorGraph, eliminateMultifrontal) {
@@ -69,21 +69,30 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
 
   auto result = hfg.eliminatePartialMultifrontal({X(0)});
 
-  GTSAM_PRINT(*result.first);
+  EXPECT_LONGS_EQUAL(result.first->size(), 1);
-  GTSAM_PRINT(*result.second);
+  EXPECT_LONGS_EQUAL(result.second->size(), 1);
 }
 
 TEST(HybridFactorGraph, eliminateFullMultifrontal) {
+
+  std::cout << ">>>>>>>>>>>>>>\n";
+
   HybridFactorGraph hfg;
 
-  DiscreteKey x(X(1), 2);
+  DiscreteKey x(C(1), 2);
 
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt;
+
+  hfg.add(CGMixtureFactor({X(1)}, { x }, dt));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
 
-  auto result = hfg.eliminateMultifrontal();
+  auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
 
   GTSAM_PRINT(*result);
+  GTSAM_PRINT(*result->marginalFactor(C(1)));
 }
 
 /* ************************************************************************* */