update API to only allow a single DiscreteKey and vector of size the same as the discrete key cardinality

gtsam/hybrid/HybridGaussianFactor.h

@@ -96,15 +96,15 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * GaussianFactor shared pointers.
    *
    * @param continuousKeys Vector of keys for continuous factors.
-   * @param discreteKeys Vector of discrete keys.
+   * @param discreteKey The discrete key to index each component.
    * @param factors Vector of gaussian factor shared pointers
-   *  and arbitrary scalars.
+   *  and arbitrary scalars. Same size as the cardinality of discreteKey.
    */
   HybridGaussianFactor(const KeyVector &continuousKeys,
-                       const DiscreteKeys &discreteKeys,
+                       const DiscreteKey &discreteKey,
                        const std::vector<GaussianFactorValuePair> &factors)
-      : HybridGaussianFactor(continuousKeys, discreteKeys,
+      : HybridGaussianFactor(continuousKeys, {discreteKey},
-                             FactorValuePairs(discreteKeys, factors)) {}
+                             FactorValuePairs({discreteKey}, factors)) {}
 
   /// @}
   /// @name Testable

gtsam/hybrid/HybridNonlinearFactor.h

@@ -92,17 +92,18 @@ class HybridNonlinearFactor : public HybridFactor {
    * @tparam FACTOR The type of the factor shared pointers being passed in.
    * Will be typecast to NonlinearFactor shared pointers.
    * @param keys Vector of keys for continuous factors.
-   * @param discreteKeys Vector of discrete keys.
+   * @param discreteKey The discrete key indexing each component factor.
    * @param factors Vector of nonlinear factor and scalar pairs.
+   * Same size as the cardinality of discreteKey.
    * @param normalized Flag indicating if the factor error is already
    * normalized.
    */
   template <typename FACTOR>
   HybridNonlinearFactor(
-      const KeyVector& keys, const DiscreteKeys& discreteKeys,
+      const KeyVector& keys, const DiscreteKey& discreteKey,
       const std::vector<std::pair<std::shared_ptr<FACTOR>, double>>& factors,
       bool normalized = false)
-      : Base(keys, discreteKeys), normalized_(normalized) {
+      : Base(keys, {discreteKey}), normalized_(normalized) {
     std::vector<NonlinearFactorValuePair> nonlinear_factors;
     KeySet continuous_keys_set(keys.begin(), keys.end());
     KeySet factor_keys_set;
@@ -118,7 +119,7 @@ class HybridNonlinearFactor : public HybridFactor {
             "Factors passed into HybridNonlinearFactor need to be nonlinear!");
       }
     }
-    factors_ = Factors(discreteKeys, nonlinear_factors);
+    factors_ = Factors({discreteKey}, nonlinear_factors);
 
     if (continuous_keys_set != factor_keys_set) {
       throw std::runtime_error(

gtsam/hybrid/tests/Switching.h

@@ -168,8 +168,8 @@ struct Switching {
         components.push_back(
             {std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0});
       }
-      nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(
+      nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
-          keys, DiscreteKeys{modes[k]}, components);
+                                                                 components);
     }
 
     // Add measurement factors

gtsam/hybrid/tests/testHybridEstimation.cpp

@@ -437,8 +437,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
   std::vector<NonlinearFactorValuePair> components = {{zero_motion, 0.0},
                                                       {one_motion, 0.0}};
-  nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)},
+  nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
-                                            DiscreteKeys{m}, components);
+                                            components);
 
   return nfg;
 }
@@ -583,9 +583,6 @@ TEST(HybridEstimation, ModeSelection) {
   graph.emplace_shared<PriorFactor<double>>(X(0), 0.0, measurement_model);
   graph.emplace_shared<PriorFactor<double>>(X(1), 0.0, measurement_model);
 
-  DiscreteKeys modes;
-  modes.emplace_back(M(0), 2);
-
   // The size of the noise model
   size_t d = 1;
   double noise_tight = 0.5, noise_loose = 5.0;
@@ -594,11 +591,11 @@ TEST(HybridEstimation, ModeSelection) {
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<MotionModel>(
            X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
-
   std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
                                                       {model1, 0.0}};
 
   KeyVector keys = {X(0), X(1)};
+  DiscreteKey modes(M(0), 2);
   HybridNonlinearFactor mf(keys, modes, components);
 
   initial.insert(X(0), 0.0);
@@ -617,7 +614,7 @@ TEST(HybridEstimation, ModeSelection) {
 
   /**************************************************************/
   HybridBayesNet bn;
-  const DiscreteKey mode{M(0), 2};
+  const DiscreteKey mode(M(0), 2);
 
   bn.push_back(
       GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(0), Z_1x1, 0.1));
@@ -648,7 +645,7 @@ TEST(HybridEstimation, ModeSelection2) {
   double noise_tight = 0.5, noise_loose = 5.0;
 
   HybridBayesNet bn;
-  const DiscreteKey mode{M(0), 2};
+  const DiscreteKey mode(M(0), 2);
 
   bn.push_back(
       GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(0), Z_3x1, 0.1));
@@ -679,18 +676,15 @@ TEST(HybridEstimation, ModeSelection2) {
   graph.emplace_shared<PriorFactor<Vector3>>(X(0), Z_3x1, measurement_model);
   graph.emplace_shared<PriorFactor<Vector3>>(X(1), Z_3x1, measurement_model);
 
-  DiscreteKeys modes;
-  modes.emplace_back(M(0), 2);
-
   auto model0 = std::make_shared<BetweenFactor<Vector3>>(
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_loose)),
        model1 = std::make_shared<BetweenFactor<Vector3>>(
            X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
-
   std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
                                                       {model1, 0.0}};
 
   KeyVector keys = {X(0), X(1)};
+  DiscreteKey modes(M(0), 2);
   HybridNonlinearFactor mf(keys, modes, components);
 
   initial.insert<Vector3>(X(0), Z_3x1);

gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp

@@ -181,7 +181,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
   std::vector<GaussianFactorValuePair> factors = {
       {std::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1), 0.0},
       {std::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()), 0.0}};
-  hfg.add(HybridGaussianFactor({X(1)}, {{M(1), 2}}, factors));
+  hfg.add(HybridGaussianFactor({X(1)}, {M(1), 2}, factors));
 
   hfg.add(DecisionTreeFactor(m1, {2, 8}));
   // TODO(Varun) Adding extra discrete variable not connected to continuous
@@ -241,7 +241,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
     std::vector<GaussianFactorValuePair> factors = {
         {std::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1), 0.0},
         {std::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()), 0.0}};
-    hfg.add(HybridGaussianFactor({X(0)}, {{M(0), 2}}, factors));
+    hfg.add(HybridGaussianFactor({X(0)}, {M(0), 2}, factors));
 
     DecisionTree<Key, GaussianFactorValuePair> dt1(
         M(1), {std::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1), 0.0},

gtsam/hybrid/tests/testHybridGaussianISAM.cpp

@@ -423,7 +423,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
   std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
       {moving, 0.0}, {still, 0.0}};
   auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
+      contKeys, gtsam::DiscreteKey(M(1), 2), components);
   fg.push_back(mixtureFactor);
 
   // Add equivalent of ImuFactor
@@ -463,7 +463,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
       std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
   components = {{moving, 0.0}, {still, 0.0}};
   mixtureFactor = std::make_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
+      contKeys, gtsam::DiscreteKey(M(2), 2), components);
   fg.push_back(mixtureFactor);
 
   // Add equivalent of ImuFactor
@@ -506,7 +506,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
       std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
   components = {{moving, 0.0}, {still, 0.0}};
   mixtureFactor = std::make_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
+      contKeys, gtsam::DiscreteKey(M(3), 2), components);
   fg.push_back(mixtureFactor);
 
   // Add equivalent of ImuFactor

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -135,7 +135,7 @@ TEST(HybridGaussianFactorGraph, Resize) {
   std::vector<std::pair<MotionModel::shared_ptr, double>> components = {
       {still, 0.0}, {moving, 0.0}};
   auto dcFactor = std::make_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
+      contKeys, gtsam::DiscreteKey(M(1), 2), components);
   nhfg.push_back(dcFactor);
 
   Values linearizationPoint;
@@ -170,12 +170,12 @@ TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) {
   // Check for exception when number of continuous keys are under-specified.
   KeyVector contKeys = {X(0)};
   THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
+      contKeys, gtsam::DiscreteKey(M(1), 2), components));
 
   // Check for exception when number of continuous keys are too many.
   contKeys = {X(0), X(1), X(2)};
   THROWS_EXCEPTION(std::make_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
+      contKeys, gtsam::DiscreteKey(M(1), 2), components));
 }
 
 /*****************************************************************************
@@ -807,7 +807,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
   std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> motion_models =
       {{still, 0.0}, {moving, 0.0}};
   fg.emplace_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models);
+      contKeys, gtsam::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

gtsam/hybrid/tests/testHybridNonlinearISAM.cpp

@@ -442,7 +442,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
   std::vector<std::pair<PlanarMotionModel::shared_ptr, double>> components = {
       {moving, 0.0}, {still, 0.0}};
   auto mixtureFactor = std::make_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components);
+      contKeys, gtsam::DiscreteKey(M(1), 2), components);
   fg.push_back(mixtureFactor);
 
   // Add equivalent of ImuFactor
@@ -482,7 +482,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
       std::make_shared<PlanarMotionModel>(W(1), W(2), odometry, noise_model);
   components = {{moving, 0.0}, {still, 0.0}};
   mixtureFactor = std::make_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components);
+      contKeys, gtsam::DiscreteKey(M(2), 2), components);
   fg.push_back(mixtureFactor);
 
   // Add equivalent of ImuFactor
@@ -525,7 +525,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
       std::make_shared<PlanarMotionModel>(W(2), W(3), odometry, noise_model);
   components = {{moving, 0.0}, {still, 0.0}};
   mixtureFactor = std::make_shared<HybridNonlinearFactor>(
-      contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components);
+      contKeys, gtsam::DiscreteKey(M(3), 2), components);
   fg.push_back(mixtureFactor);
 
   // Add equivalent of ImuFactor

gtsam/hybrid/tests/testSerializationHybrid.cpp

@@ -76,7 +76,7 @@ BOOST_CLASS_EXPORT_GUID(HybridBayesNet, "gtsam_HybridBayesNet");
 // Test HybridGaussianFactor serialization.
 TEST(HybridSerialization, HybridGaussianFactor) {
   KeyVector continuousKeys{X(0)};
-  DiscreteKeys discreteKeys{{M(0), 2}};
+  DiscreteKey discreteKey{M(0), 2};
 
   auto A = Matrix::Zero(2, 1);
   auto b0 = Matrix::Zero(2, 1);
@@ -85,7 +85,7 @@ TEST(HybridSerialization, HybridGaussianFactor) {
   auto f1 = std::make_shared<JacobianFactor>(X(0), A, b1);
   std::vector<GaussianFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
 
-  const HybridGaussianFactor factor(continuousKeys, discreteKeys, factors);
+  const HybridGaussianFactor factor(continuousKeys, discreteKey, factors);
 
   EXPECT(equalsObj<HybridGaussianFactor>(factor));
   EXPECT(equalsXML<HybridGaussianFactor>(factor));