fix continuousKeySet method

gtsam/hybrid/HybridFactorGraph.cpp

@@ -67,6 +67,8 @@ const KeySet HybridFactorGraph::continuousKeySet() const {
       for (const Key& key : p->continuousKeys()) {
         keys.insert(key);
       }
+    } else if (auto p = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
+      keys.insert(p->keys().begin(), p->keys().end());
     }
   }
   return keys;

gtsam/hybrid/tests/testHybridFactorGraph.cpp

@@ -18,7 +18,9 @@
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/utilities.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/nonlinear/PriorFactor.h>
 
 using namespace std;
@@ -37,6 +39,32 @@ TEST(HybridFactorGraph, Constructor) {
   HybridFactorGraph fg;
 }
 
+/* ************************************************************************* */
+// Test if methods to get keys work as expected.
+TEST(HybridFactorGraph, Keys) {
+  HybridGaussianFactorGraph hfg;
+
+  // Add prior on x0
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+
+  // Add factor between x0 and x1
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+
+  // Add a gaussian mixture factor ϕ(x1, c1)
+  DiscreteKey m1(M(1), 2);
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+      M(1), std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
+      std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
+  hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt));
+
+  KeySet expected_continuous{X(0), X(1)};
+  EXPECT(
+      assert_container_equality(expected_continuous, hfg.continuousKeySet()));
+
+  KeySet expected_discrete{M(1)};
+  EXPECT(assert_container_equality(expected_discrete, hfg.discreteKeySet()));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp

@@ -902,7 +902,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) {
   // Test resulting posterior Bayes net has correct size:
   EXPECT_LONGS_EQUAL(8, posterior->size());
 
-  // TODO(dellaert): this test fails - no idea why.
+  // Ratio test
   EXPECT(ratioTest(bn, measurements, *posterior));
 }