Merge branch 'hybrid-error-scalars' into hybrid-enum

borglab · Sep 17, 2024 · 340fae9 · 340fae9
2 parents 8cb95d5 + d4923db
commit 340fae9
Show file tree

Hide file tree

Showing 19 changed files with 151 additions and 137 deletions.
diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -55,24 +55,6 @@ HybridGaussianConditional::conditionals() const {
   return conditionals_;
 }
 
-/* *******************************************************************************/
-HybridGaussianConditional::HybridGaussianConditional(
-    KeyVector &&continuousFrontals, KeyVector &&continuousParents,
-    DiscreteKeys &&discreteParents,
-    std::vector<GaussianConditional::shared_ptr> &&conditionals)
-    : HybridGaussianConditional(continuousFrontals, continuousParents,
-                                discreteParents,
-                                Conditionals(discreteParents, conditionals)) {}
-
-/* *******************************************************************************/
-HybridGaussianConditional::HybridGaussianConditional(
-    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
-    const DiscreteKeys &discreteParents,
-    const std::vector<GaussianConditional::shared_ptr> &conditionals)
-    : HybridGaussianConditional(continuousFrontals, continuousParents,
-                                discreteParents,
-                                Conditionals(discreteParents, conditionals)) {}
-
 /* *******************************************************************************/
 // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be
 // derived from HybridGaussianFactor, no?

diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
@@ -106,32 +106,6 @@ class GTSAM_EXPORT HybridGaussianConditional
                             const DiscreteKeys &discreteParents,
                             const Conditionals &conditionals);
 
-  /**
-   * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
-   *
-   * @param continuousFrontals The continuous frontal variables
-   * @param continuousParents The continuous parent variables
-   * @param discreteParents Discrete parents variables
-   * @param conditionals List of conditionals
-   */
-  HybridGaussianConditional(
-      KeyVector &&continuousFrontals, KeyVector &&continuousParents,
-      DiscreteKeys &&discreteParents,
-      std::vector<GaussianConditional::shared_ptr> &&conditionals);
-
-  /**
-   * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
-   *
-   * @param continuousFrontals The continuous frontal variables
-   * @param continuousParents The continuous parent variables
-   * @param discreteParents Discrete parents variables
-   * @param conditionals List of conditionals
-   */
-  HybridGaussianConditional(
-      const KeyVector &continuousFrontals, const KeyVector &continuousParents,
-      const DiscreteKeys &discreteParents,
-      const std::vector<GaussianConditional::shared_ptr> &conditionals);
-
   /// @}
   /// @name Testable
   /// @{
@@ -273,7 +247,7 @@ class GTSAM_EXPORT HybridGaussianConditional
 #endif
 };
 
-/// Return the DiscreteKey vector as a set.
+/// Return the DiscreteKeys vector as a set.
 std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys);
 
 // traits

diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -42,9 +42,11 @@ HybridGaussianFactor::Factors augment(
     const HybridGaussianFactor::FactorValuePairs &factors) {
   // Find the minimum value so we can "proselytize" to positive values.
   // Done because we can't have sqrt of negative numbers.
-  auto unzipped_pair = unzip(factors);
-  const HybridGaussianFactor::Factors gaussianFactors = unzipped_pair.first;
-  const AlgebraicDecisionTree<Key> valueTree = unzipped_pair.second;
+  HybridGaussianFactor::Factors gaussianFactors;
+  AlgebraicDecisionTree<Key> valueTree;
+  std::tie(gaussianFactors, valueTree) = unzip(factors);
+
+  // Normalize
   double min_value = valueTree.min();
   AlgebraicDecisionTree<Key> values =
       valueTree.apply([&min_value](double n) { return n - min_value; });

diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/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,
-                             FactorValuePairs(discreteKeys, factors)) {}
+      : HybridGaussianFactor(continuousKeys, {discreteKey},
+                             FactorValuePairs({discreteKey}, factors)) {}
 
   /// @}
   /// @name Testable

diff --git a/gtsam/hybrid/HybridNonlinearFactor.h b/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(
@@ -140,7 +141,7 @@ class HybridNonlinearFactor : public HybridFactor {
     auto errorFunc =
         [continuousValues](const std::pair<sharedFactor, double>& f) {
           auto [factor, val] = f;
-          return factor->error(continuousValues) + (0.5 * val * val);
+          return factor->error(continuousValues) + (0.5 * val);
         };
     DecisionTree<Key, double> result(factors_, errorFunc);
     return result;
@@ -159,7 +160,7 @@ class HybridNonlinearFactor : public HybridFactor {
     auto [factor, val] = factors_(discreteValues);
     // Compute the error for the selected factor
     const double factorError = factor->error(continuousValues);
-    return factorError + (0.5 * val * val);
+    return factorError + (0.5 * val);
   }
 
   /**

diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
@@ -57,15 +57,16 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
 
   // keyFunc(1) to keyFunc(n+1)
   for (size_t t = 1; t < n; t++) {
-    std::vector<GaussianFactorValuePair> components = {
-        {std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
-                                          I_3x3, Z_3x1),
-         0.0},
-        {std::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
-                                          I_3x3, Vector3::Ones()),
-         0.0}};
-    hfg.add(HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)},
-                                 {{dKeyFunc(t), 2}}, components));
+    DiscreteKeys dKeys{{dKeyFunc(t), 2}};
+    HybridGaussianFactor::FactorValuePairs components(
+        dKeys, {{std::make_shared<JacobianFactor>(keyFunc(t), I_3x3,
+                                                  keyFunc(t + 1), I_3x3, Z_3x1),
+                 0.0},
+                {std::make_shared<JacobianFactor>(
+                     keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Vector3::Ones()),
+                 0.0}});
+    hfg.add(
+        HybridGaussianFactor({keyFunc(t), keyFunc(t + 1)}, dKeys, components));
 
     if (t > 1) {
       hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}},
@@ -167,8 +168,8 @@ struct Switching {
         components.push_back(
             {std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0});
       }
-      nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(
-          keys, DiscreteKeys{modes[k]}, components);
+      nonlinearFactorGraph.emplace_shared<HybridNonlinearFactor>(keys, modes[k],
+                                                                 components);
     }
 
     // Add measurement factors

diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h
@@ -43,12 +43,13 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
   // Create Gaussian mixture z_i = x0 + noise for each measurement.
   for (size_t i = 0; i < num_measurements; i++) {
     const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
+    DiscreteKeys modes{mode_i};
+    std::vector<GaussianConditional::shared_ptr> conditionals{
+        GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5),
+        GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)};
     bayesNet.emplace_shared<HybridGaussianConditional>(
         KeyVector{Z(i)}, KeyVector{X(0)}, DiscreteKeys{mode_i},
-        std::vector{GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0),
-                                                             Z_1x1, 0.5),
-                    GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0),
-                                                             Z_1x1, 3)});
+        HybridGaussianConditional::Conditionals(modes, conditionals));
   }
 
   // Create prior on X(0).

diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -107,9 +107,11 @@ TEST(HybridBayesNet, evaluateHybrid) {
   // Create hybrid Bayes net.
   HybridBayesNet bayesNet;
   bayesNet.push_back(continuousConditional);
+  DiscreteKeys discreteParents{Asia};
   bayesNet.emplace_shared<HybridGaussianConditional>(
-      KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia},
-      std::vector{conditional0, conditional1});
+      KeyVector{X(1)}, KeyVector{}, discreteParents,
+      HybridGaussianConditional::Conditionals(
+          discreteParents, std::vector{conditional0, conditional1}));
   bayesNet.emplace_shared<DiscreteConditional>(Asia, "99/1");
 
   // Create values at which to evaluate.
@@ -168,9 +170,11 @@ TEST(HybridBayesNet, Error) {
              conditional1 = std::make_shared<GaussianConditional>(
                  X(1), Vector1::Constant(2), I_1x1, model1);
 
+  DiscreteKeys discreteParents{Asia};
   auto gm = std::make_shared<HybridGaussianConditional>(
-      KeyVector{X(1)}, KeyVector{}, DiscreteKeys{Asia},
-      std::vector{conditional0, conditional1});
+      KeyVector{X(1)}, KeyVector{}, discreteParents,
+      HybridGaussianConditional::Conditionals(
+          discreteParents, std::vector{conditional0, conditional1}));
   // Create hybrid Bayes net.
   HybridBayesNet bayesNet;
   bayesNet.push_back(continuousConditional);
@@ -383,15 +387,15 @@ TEST(HybridBayesNet, Sampling) {
   HybridNonlinearFactorGraph nfg;
 
   auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0));
+  nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
+
   auto zero_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
   auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
-
   DiscreteKeys discreteKeys{DiscreteKey(M(0), 2)};
   HybridNonlinearFactor::Factors factors(
       discreteKeys, {{zero_motion, 0.0}, {one_motion, 0.0}});
-  nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
   nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, discreteKeys,
                                             factors);
 

diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/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)},
-                                            DiscreteKeys{m}, components);
+  nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, m,
+                                            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,18 +614,22 @@ 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));
   bn.push_back(
       GaussianConditional::sharedMeanAndStddev(Z(0), -I_1x1, X(1), Z_1x1, 0.1));
+
+  std::vector<GaussianConditional::shared_ptr> conditionals{
+      GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
+                                               Z_1x1, noise_loose),
+      GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), -I_1x1, X(1),
+                                               Z_1x1, noise_tight)};
   bn.emplace_shared<HybridGaussianConditional>(
       KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
-      std::vector{GaussianConditional::sharedMeanAndStddev(
-                      Z(0), I_1x1, X(0), -I_1x1, X(1), Z_1x1, noise_loose),
-                  GaussianConditional::sharedMeanAndStddev(
-                      Z(0), I_1x1, X(0), -I_1x1, X(1), Z_1x1, noise_tight)});
+      HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
+                                              conditionals));
 
   VectorValues vv;
   vv.insert(Z(0), Z_1x1);
@@ -648,18 +649,22 @@ 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));
   bn.push_back(
       GaussianConditional::sharedMeanAndStddev(Z(0), -I_3x3, X(1), Z_3x1, 0.1));
+
+  std::vector<GaussianConditional::shared_ptr> conditionals{
+      GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
+                                               Z_3x1, noise_loose),
+      GaussianConditional::sharedMeanAndStddev(Z(0), I_3x3, X(0), -I_3x3, X(1),
+                                               Z_3x1, noise_tight)};
   bn.emplace_shared<HybridGaussianConditional>(
       KeyVector{Z(0)}, KeyVector{X(0), X(1)}, DiscreteKeys{mode},
-      std::vector{GaussianConditional::sharedMeanAndStddev(
-                      Z(0), I_3x3, X(0), -I_3x3, X(1), Z_3x1, noise_loose),
-                  GaussianConditional::sharedMeanAndStddev(
-                      Z(0), I_3x3, X(0), -I_3x3, X(1), Z_3x1, noise_tight)});
+      HybridGaussianConditional::Conditionals(DiscreteKeys{mode},
+                                              conditionals));
 
   VectorValues vv;
   vv.insert(Z(0), Z_3x1);
@@ -679,18 +684,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);

diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp
@@ -52,7 +52,9 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
                                              commonSigma),
     GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
                                              commonSigma)};
-const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals);
+const HybridGaussianConditional mixture(
+    {Z(0)}, {X(0)}, {mode},
+    HybridGaussianConditional::Conditionals({mode}, conditionals));
 }  // namespace equal_constants
 
 /* ************************************************************************* */
@@ -153,7 +155,9 @@ const std::vector<GaussianConditional::shared_ptr> conditionals{
                                              0.5),
     GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
                                              3.0)};
-const HybridGaussianConditional mixture({Z(0)}, {X(0)}, {mode}, conditionals);
+const HybridGaussianConditional mixture(
+    {Z(0)}, {X(0)}, {mode},
+    HybridGaussianConditional::Conditionals({mode}, conditionals));
 }  // namespace mode_dependent_constants
 
 /* ************************************************************************* */