Merge branch 'develop' into direct-hybrid-fg

Author: varunagrawal

gtsam/hybrid/HybridConditional.cpp

@@ -28,14 +28,9 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals,
                                      const DiscreteKeys &discreteFrontals,
                                      const KeyVector &continuousParents,
                                      const DiscreteKeys &discreteParents)
-    : HybridConditional(
-          CollectKeys(
-              {continuousFrontals.begin(), continuousFrontals.end()},
-              KeyVector{continuousParents.begin(), continuousParents.end()}),
-          CollectDiscreteKeys(
-              {discreteFrontals.begin(), discreteFrontals.end()},
-              {discreteParents.begin(), discreteParents.end()}),
-          continuousFrontals.size() + discreteFrontals.size()) {}
+    : HybridConditional(CollectKeys(continuousFrontals, continuousParents),
+                        CollectDiscreteKeys(discreteFrontals, discreteParents),
+                        continuousFrontals.size() + discreteFrontals.size()) {}
 
 /* ************************************************************************ */
 HybridConditional::HybridConditional(
@@ -56,9 +51,7 @@ HybridConditional::HybridConditional(
 /* ************************************************************************ */
 HybridConditional::HybridConditional(
     const std::shared_ptr<HybridGaussianConditional> &gaussianMixture)
-    : BaseFactor(KeyVector(gaussianMixture->keys().begin(),
-                           gaussianMixture->keys().begin() +
-                               gaussianMixture->nrContinuous()),
+    : BaseFactor(gaussianMixture->continuousKeys(),
                  gaussianMixture->discreteKeys()),
       BaseConditional(gaussianMixture->nrFrontals()) {
   inner_ = gaussianMixture;

gtsam/hybrid/HybridFactor.cpp

@@ -50,41 +50,65 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &keys)
-    : Base(keys), isContinuous_(true), continuousKeys_(keys) {}
+    : Base(keys), category_(Category::Continuous), continuousKeys_(keys) {}
+
+/* ************************************************************************ */
+HybridFactor::Category GetCategory(const KeyVector &continuousKeys,
+                                   const DiscreteKeys &discreteKeys) {
+  if ((continuousKeys.size() == 0) && (discreteKeys.size() != 0)) {
+    return HybridFactor::Category::Discrete;
+  } else if ((continuousKeys.size() != 0) && (discreteKeys.size() == 0)) {
+    return HybridFactor::Category::Continuous;
+  } else if ((continuousKeys.size() != 0) && (discreteKeys.size() != 0)) {
+    return HybridFactor::Category::Hybrid;
+  } else {
+    // Case where we have no keys. Should never happen.
+    return HybridFactor::Category::None;
+  }
+}
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &continuousKeys,
                            const DiscreteKeys &discreteKeys)
     : Base(CollectKeys(continuousKeys, discreteKeys)),
-      isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)),
-      isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)),
-      isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)),
+      category_(GetCategory(continuousKeys, discreteKeys)),
       discreteKeys_(discreteKeys),
       continuousKeys_(continuousKeys) {}
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
     : Base(CollectKeys({}, discreteKeys)),
-      isDiscrete_(true),
+      category_(Category::Discrete),
       discreteKeys_(discreteKeys),
       continuousKeys_({}) {}
 
 /* ************************************************************************ */
 bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
-  return e != nullptr && Base::equals(*e, tol) &&
-         isDiscrete_ == e->isDiscrete_ && isContinuous_ == e->isContinuous_ &&
-         isHybrid_ == e->isHybrid_ && continuousKeys_ == e->continuousKeys_ &&
+  return e != nullptr && Base::equals(*e, tol) && category_ == e->category_ &&
+         continuousKeys_ == e->continuousKeys_ &&
          discreteKeys_ == e->discreteKeys_;
 }
 
 /* ************************************************************************ */
 void HybridFactor::print(const std::string &s,
                          const KeyFormatter &formatter) const {
   std::cout << (s.empty() ? "" : s + "\n");
-  if (isContinuous_) std::cout << "Continuous ";
-  if (isDiscrete_) std::cout << "Discrete ";
-  if (isHybrid_) std::cout << "Hybrid ";
+  switch (category_) {
+    case Category::Continuous:
+      std::cout << "Continuous ";
+      break;
+    case Category::Discrete:
+      std::cout << "Discrete ";
+      break;
+    case Category::Hybrid:
+      std::cout << "Hybrid ";
+      break;
+    case Category::None:
+      std::cout << "None ";
+      break;
+  }
+
   std::cout << "[";
   for (size_t c = 0; c < continuousKeys_.size(); c++) {
     std::cout << formatter(continuousKeys_.at(c));

gtsam/hybrid/HybridFactor.h

@@ -52,10 +52,13 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
  * @ingroup hybrid
  */
 class GTSAM_EXPORT HybridFactor : public Factor {
+ public:
+  /// Enum to help with categorizing hybrid factors.
+  enum class Category { None, Discrete, Continuous, Hybrid };
+
  private:
-  bool isDiscrete_ = false;
-  bool isContinuous_ = false;
-  bool isHybrid_ = false;
+  /// Record what category of HybridFactor this is.
+  Category category_ = Category::None;
 
  protected:
   // Set of DiscreteKeys for this factor.
@@ -116,13 +119,13 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /// @{
 
   /// True if this is a factor of discrete variables only.
-  bool isDiscrete() const { return isDiscrete_; }
+  bool isDiscrete() const { return category_ == Category::Discrete; }
 
   /// True if this is a factor of continuous variables only.
-  bool isContinuous() const { return isContinuous_; }
+  bool isContinuous() const { return category_ == Category::Continuous; }
 
   /// True is this is a Discrete-Continuous factor.
-  bool isHybrid() const { return isHybrid_; }
+  bool isHybrid() const { return category_ == Category::Hybrid; }
 
   /// Return the number of continuous variables in this factor.
   size_t nrContinuous() const { return continuousKeys_.size(); }
@@ -142,9 +145,7 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   template <class ARCHIVE>
   void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
     ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-    ar &BOOST_SERIALIZATION_NVP(isDiscrete_);
-    ar &BOOST_SERIALIZATION_NVP(isContinuous_);
-    ar &BOOST_SERIALIZATION_NVP(isHybrid_);
+    ar &BOOST_SERIALIZATION_NVP(category_);
     ar &BOOST_SERIALIZATION_NVP(discreteKeys_);
     ar &BOOST_SERIALIZATION_NVP(continuousKeys_);
   }

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -55,23 +55,14 @@ 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 DiscreteKey &discreteParent,
     const std::vector<GaussianConditional::shared_ptr> &conditionals)
     : HybridGaussianConditional(continuousFrontals, continuousParents,
-                                discreteParents,
-                                Conditionals(discreteParents, conditionals)) {}
+                                DiscreteKeys{discreteParent},
+                                Conditionals({discreteParent}, conditionals)) {}
 
 /* *******************************************************************************/
 // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be

gtsam/hybrid/HybridGaussianConditional.h

@@ -107,29 +107,18 @@ class GTSAM_EXPORT HybridGaussianConditional
                             const Conditionals &conditionals);
 
   /**
-   * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
+   * @brief Make a Gaussian Mixture from a vector of Gaussian conditionals.
+   * The DecisionTree-based constructor is preferred over this one.
    *
    * @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
+   * @param discreteParent Single discrete parent variable
+   * @param conditionals Vector of conditionals with the same size as the
+   * cardinality of the discrete parent.
    */
   HybridGaussianConditional(
       const KeyVector &continuousFrontals, const KeyVector &continuousParents,
-      const DiscreteKeys &discreteParents,
+      const DiscreteKey &discreteParent,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
   /// @}

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; });

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

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -114,10 +114,11 @@ void HybridGaussianFactorGraph::printErrors(
                     << "\n";
         } else {
           // Is hybrid
-          auto mixtureComponent =
+          auto conditionalComponent =
               hc->asMixture()->operator()(values.discrete());
-          mixtureComponent->print(ss.str(), keyFormatter);
-          std::cout << "error = " << mixtureComponent->error(values) << "\n";
+          conditionalComponent->print(ss.str(), keyFormatter);
+          std::cout << "error = " << conditionalComponent->error(values)
+                    << "\n";
         }
       }
     } else if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
@@ -411,10 +412,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
   // Create the HybridGaussianConditional from the conditionals
   HybridGaussianConditional::Conditionals conditionals(
       eliminationResults, [](const Result &pair) { return pair.first; });
-  auto gaussianMixture = std::make_shared<HybridGaussianConditional>(
+  auto hybridGaussian = std::make_shared<HybridGaussianConditional>(
       frontalKeys, continuousSeparator, discreteSeparator, conditionals);
 
-  return {std::make_shared<HybridConditional>(gaussianMixture), newFactor};
+  return {std::make_shared<HybridConditional>(hybridGaussian), newFactor};
 }
 
 /* ************************************************************************
@@ -465,7 +466,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
   // Now we will need to know how to retrieve the corresponding continuous
   // densities for the assignment (c1,c2,c3) (OR (c2,c3,c1), note there is NO
   // defined order!). We also need to consider when there is pruning. Two
-  // mixture factors could have different pruning patterns - one could have
+  // hybrid factors could have different pruning patterns - one could have
   // (c1=0,c2=1) pruned, and another could have (c2=0,c3=1) pruned, and this
   // creates a big problem in how to identify the intersection of non-pruned
   // branches.

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);
   }
 
   /**

gtsam/hybrid/hybrid.i

@@ -76,7 +76,7 @@ virtual class HybridConditional {
 class HybridGaussianFactor : gtsam::HybridFactor {
   HybridGaussianFactor(
       const gtsam::KeyVector& continuousKeys,
-      const gtsam::DiscreteKeys& discreteKeys,
+      const gtsam::DiscreteKey& discreteKey,
       const std::vector<std::pair<gtsam::GaussianFactor::shared_ptr, double>>&
           factorsList);
 
@@ -91,8 +91,12 @@ class HybridGaussianConditional : gtsam::HybridFactor {
       const gtsam::KeyVector& continuousFrontals,
       const gtsam::KeyVector& continuousParents,
       const gtsam::DiscreteKeys& discreteParents,
-      const std::vector<gtsam::GaussianConditional::shared_ptr>&
-          conditionalsList);
+      const gtsam::HybridGaussianConditional::Conditionals& conditionals);
+  HybridGaussianConditional(
+      const gtsam::KeyVector& continuousFrontals,
+      const gtsam::KeyVector& continuousParents,
+      const gtsam::DiscreteKey& discreteParent,
+      const std::vector<gtsam::GaussianConditional::shared_ptr>& conditionals);
 
   gtsam::HybridGaussianFactor* likelihood(
       const gtsam::VectorValues& frontals) const;
@@ -248,7 +252,7 @@ class HybridNonlinearFactor : gtsam::HybridFactor {
       bool normalized = false);
 
   HybridNonlinearFactor(
-      const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
+      const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
       const std::vector<std::pair<gtsam::NonlinearFactor*, double>>& factors,
       bool normalized = false);