Fix call sites for HybridNonlinearFactor

gtsam/hybrid/tests/Switching.h

@@ -159,9 +159,8 @@ struct Switching {
 
     // Add "motion models".
     for (size_t k = 0; k < K - 1; k++) {
-      KeyVector keys = {X(k), X(k + 1)};
       auto motion_models = motionModels(k, between_sigma);
-      nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
+      nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(modes[k],
                                                                  motion_models);
     }
 

gtsam/hybrid/tests/testHybridBayesNet.cpp

@@ -388,7 +388,7 @@ TEST(HybridBayesNet, Sampling) {
   auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   nfg.emplace_shared<HybridNonlinearFactor>(
-      KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2),
+      DiscreteKey(M(0), 2),
       std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
 
   DiscreteKey mode(M(0), 2);

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -437,8 +437,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   std::vector<NonlinearFactor::shared_ptr> components = {zero_motion,
                                                          one_motion};
-  nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
-                                            components);
+  nfg.emplace_shared<HybridNonlinearFactor>(m, components);
 
   return nfg;
 }
@@ -591,9 +590,7 @@ TEST(HybridEstimation, ModeSelection) {
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
   std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
 
-  KeyVector keys = {X(0), X(1)};
-  DiscreteKey modes(M(0), 2);
-  HybridNonlinearFactor mf(keys, modes, components);
+  HybridNonlinearFactor mf({M(0), 2}, components);
 
   initial.insert(X(0), 0.0);
   initial.insert(X(1), 0.0);
@@ -681,9 +678,7 @@ TEST(HybridEstimation, ModeSelection2) {
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
   std::vector<NonlinearFactor::shared_ptr> components = {model0, model1};
 
-  KeyVector keys = {X(0), X(1)};
-  DiscreteKey modes(M(0), 2);
-  HybridNonlinearFactor mf(keys, modes, components);
+  HybridNonlinearFactor mf({M(0), 2}, components);
 
   initial.insert<Vector3>(X(0), Z_3x1);
   initial.insert<Vector3>(X(1), Z_3x1);

gtsam/hybrid/tests/testHybridGaussianISAM.cpp

@@ -414,15 +414,13 @@ TEST(HybridGaussianISAM, NonTrivial) {
 
   // Add odometry factor with discrete modes.
   Pose2 odometry(1.0, 0.0, 0.0);
-  KeyVector contKeys = {W(0), W(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
   std::vector<NonlinearFactor::shared_ptr> components;
   components.emplace_back(
       new PlanarMotionModel(W(0), W(1), odometry, noise_model));  // moving
   components.emplace_back(
       new PlanarMotionModel(W(0), W(1), Pose2(0, 0, 0), noise_model));  // still
-  fg.emplace_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@@ -454,14 +452,12 @@ TEST(HybridGaussianISAM, NonTrivial) {
 
   /*************** Run Round 3 ***************/
   // Add odometry factor with discrete modes.
-  contKeys = {W(1), W(2)};
   components.clear();
   components.emplace_back(
       new PlanarMotionModel(W(1), W(2), odometry, noise_model));  // moving
   components.emplace_back(
       new PlanarMotionModel(W(1), W(2), Pose2(0, 0, 0), noise_model));  // still
-  fg.emplace_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(2), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(2), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@@ -496,14 +492,12 @@ TEST(HybridGaussianISAM, NonTrivial) {
 
   /*************** Run Round 4 ***************/
   // Add odometry factor with discrete modes.
-  contKeys = {W(2), W(3)};
   components.clear();
   components.emplace_back(
       new PlanarMotionModel(W(2), W(3), odometry, noise_model));  // moving
   components.emplace_back(
       new PlanarMotionModel(W(2), W(3), Pose2(0, 0, 0), noise_model));  // still
-  fg.emplace_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(3), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),

gtsam/hybrid/tests/testHybridNonlinearFactor.cpp

@@ -60,22 +60,22 @@ auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
 // Test simple to complex constructors...
 TEST(HybridGaussianFactor, ConstructorVariants) {
   using namespace test_constructor;
-  HybridNonlinearFactor fromFactors({X(1), X(2)}, m1, {f0, f1});
+  HybridNonlinearFactor fromFactors(m1, {f0, f1});
 
   std::vector<NonlinearFactorValuePair> pairs{{f0, 0.0}, {f1, 0.0}};
-  HybridNonlinearFactor fromPairs({X(1), X(2)}, m1, pairs);
+  HybridNonlinearFactor fromPairs(m1, pairs);
   assert_equal(fromFactors, fromPairs);
 
   HybridNonlinearFactor::FactorValuePairs decisionTree({m1}, pairs);
-  HybridNonlinearFactor fromDecisionTree({X(1), X(2)}, {m1}, decisionTree);
+  HybridNonlinearFactor fromDecisionTree({m1}, decisionTree);
   assert_equal(fromDecisionTree, fromPairs);
 }
 
 /* ************************************************************************* */
 // Test .print() output.
 TEST(HybridNonlinearFactor, Printing) {
   using namespace test_constructor;
-  HybridNonlinearFactor hybridFactor({X(1), X(2)}, {m1}, {f0, f1});
+  HybridNonlinearFactor hybridFactor({m1}, {f0, f1});
 
   std::string expected =
       R"(Hybrid [x1 x2; 1]
@@ -101,7 +101,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() {
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
   auto f1 =
       std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
-  return HybridNonlinearFactor({X(1), X(2)}, m1, {f0, f1});
+  return HybridNonlinearFactor(m1, {f0, f1});
 }
 
 /* ************************************************************************* */

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -117,7 +117,6 @@ TEST(HybridNonlinearFactorGraph, Resize) {
 
 /***************************************************************************/
 namespace test_motion {
-KeyVector contKeys = {X(0), X(1)};
 gtsam::DiscreteKey m1(M(1), 2);
 auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
 std::vector<NonlinearFactor::shared_ptr> components = {
@@ -139,8 +138,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
   auto discreteFactor = std::make_shared<DecisionTreeFactor>();
   hnfg.push_back(discreteFactor);
 
-  auto dcFactor =
-      std::make_shared<HybridNonlinearFactor>(contKeys, m1, components);
+  auto dcFactor = std::make_shared<HybridNonlinearFactor>(m1, components);
   hnfg.push_back(dcFactor);
 
   Values linearizationPoint;
@@ -156,26 +154,6 @@ TEST(HybridGaussianFactorGraph, Resize) {
   EXPECT_LONGS_EQUAL(gfg.size(), 0);
 }
 
-/***************************************************************************
- * Test that the HybridNonlinearFactor reports correctly if the number of
- * continuous keys provided do not match the keys in the factors.
- */
-TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
-  using namespace test_motion;
-
-  auto nonlinearFactor = std::make_shared<BetweenFactor<double>>(
-      X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
-  auto discreteFactor = std::make_shared<DecisionTreeFactor>();
-
-  // Check for exception when number of continuous keys are under-specified.
-  THROWS_EXCEPTION(
-      std::make_shared<HybridNonlinearFactor>(KeyVector{X(0)}, m1, components));
-
-  // Check for exception when number of continuous keys are too many.
-  THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
-      KeyVector{X(0), X(1), X(2)}, m1, components));
-}
-
 /*****************************************************************************
  * Test push_back on HFG makes the correct distinction.
  */
@@ -828,14 +806,12 @@ TEST(HybridNonlinearFactorGraph, DefaultDecisionTree) {
 
   // Add odometry factor
   Pose2 odometry(2.0, 0.0, 0.0);
-  KeyVector contKeys = {X(0), X(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
   std::vector<NonlinearFactor::shared_ptr> motion_models = {
       std::make_shared<PlanarMotionModel>(X(0), X(1), Pose2(0, 0, 0),
                                           noise_model),
       std::make_shared<PlanarMotionModel>(X(0), X(1), odometry, noise_model)};
-  fg.emplace_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), motion_models);
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey{M(1), 2}, motion_models);
 
   // Add Range-Bearing measurements to from X0 to L0 and X1 to L1.
   // create a noise model for the landmark measurements
@@ -901,7 +877,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
   std::vector<NonlinearFactorValuePair> factors{{f0, model0->negLogConstant()},
                                                 {f1, model1->negLogConstant()}};
 
-  HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors);
+  HybridNonlinearFactor mixtureFactor(m1, factors);
 
   HybridNonlinearFactorGraph hfg;
   hfg.push_back(mixtureFactor);

gtsam/hybrid/tests/testHybridNonlinearISAM.cpp

@@ -433,15 +433,13 @@ TEST(HybridNonlinearISAM, NonTrivial) {
   /*************** Run Round 2 ***************/
   // Add odometry factor with discrete modes.
   Pose2 odometry(1.0, 0.0, 0.0);
-  KeyVector contKeys = {W(0), W(1)};
   auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
   auto still = std::make_shared<PlanarMotionModel>(W(0), W(1), Pose2(0, 0, 0),
                                                    noise_model),
        moving = std::make_shared<PlanarMotionModel>(W(0), W(1), odometry,
                                                     noise_model);
   std::vector<NonlinearFactor::shared_ptr> components{moving, still};
-  fg.emplace_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(1), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(1), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
@@ -473,14 +471,12 @@ TEST(HybridNonlinearISAM, NonTrivial) {
 
   /*************** Run Round 3 ***************/
   // Add odometry factor with discrete modes.
-  contKeys = {W(1), W(2)};
   still = std::make_shared<PlanarMotionModel>(W(1), W(2), Pose2(0, 0, 0),
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
   components = {moving, still};
-  fg.emplace_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(2), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(2), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
@@ -515,14 +511,12 @@ TEST(HybridNonlinearISAM, NonTrivial) {
 
   /*************** Run Round 4 ***************/
   // Add odometry factor with discrete modes.
-  contKeys = {W(2), W(3)};
   still = std::make_shared<PlanarMotionModel>(W(2), W(3), Pose2(0, 0, 0),
                                               noise_model);
   moving =
       std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
   components = {moving, still};
-  fg.emplace_shared<HybridNonlinearFactor>(
-      contKeys, gtsam::DiscreteKey(M(3), 2), components);
+  fg.emplace_shared<HybridNonlinearFactor>(DiscreteKey(M(3), 2), components);
 
   // Add equivalent of ImuFactor
   fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),