Avoid double traversal

gtsam/hybrid/HybridFactor.h

@@ -56,11 +56,9 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /// Enum to help with categorizing hybrid factors.
   enum class Category { None, Discrete, Continuous, Hybrid };
 
- private:
+ protected:
   /// Record what category of HybridFactor this is.
   Category category_ = Category::None;
-
- protected:
   // Set of DiscreteKeys for this factor.
   DiscreteKeys discreteKeys_;
   /// Record continuous keys for book-keeping

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -61,17 +61,22 @@ HybridGaussianConditional::HybridGaussianConditional(
     }
     return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
   };
-  BaseFactor::factors_ = HybridGaussianFactor::Factors(conditionals, func);
+  HybridGaussianFactor::FactorValuePairs pairs(conditionals, func);
 
-  // Initialize base classes
-  KeyVector continuousKeys = frontals;
-  continuousKeys.insert(continuousKeys.end(), parents.begin(), parents.end());
-  BaseFactor::keys_ = continuousKeys;
-  BaseFactor::discreteKeys_ = discreteParents;
+  // Adjust frontals size
   BaseConditional::nrFrontals_ = frontals.size();
 
-  // Assign conditionals
-  conditionals_ = conditionals;  // TODO(frank): a duplicate of factors_ !!!
+  // Initialize HybridFactor
+  HybridFactor::category_ = HybridFactor::Category::Hybrid;
+  HybridFactor::discreteKeys_ = discreteParents;
+  HybridFactor::keys_ = frontals;
+  keys_.insert(keys_.end(), parents.begin(), parents.end());
+
+  // Initialize BaseFactor
+  BaseFactor::factors_ = BaseFactor::augment(pairs);  // TODO(frank): expensive
+
+  // Assign local conditionals. TODO(frank): a duplicate of factors_ !!!
+  conditionals_ = conditionals;
 }
 
 /* *******************************************************************************/

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -38,11 +38,11 @@ namespace gtsam {
  * Gaussian factor in factors.
  * @return HybridGaussianFactor::Factors
  */
-static HybridGaussianFactor::Factors augment(
-    const HybridGaussianFactor::FactorValuePairs &factors) {
+HybridGaussianFactor::Factors HybridGaussianFactor::augment(
+    const FactorValuePairs &factors) {
   // Find the minimum value so we can "proselytize" to positive values.
   // Done because we can't have sqrt of negative numbers.
-  HybridGaussianFactor::Factors gaussianFactors;
+  Factors gaussianFactors;
   AlgebraicDecisionTree<Key> valueTree;
   std::tie(gaussianFactors, valueTree) = unzip(factors);
 
@@ -73,7 +73,7 @@ static HybridGaussianFactor::Factors augment(
     return std::dynamic_pointer_cast<GaussianFactor>(
         std::make_shared<JacobianFactor>(gfg));
   };
-  return HybridGaussianFactor::Factors(factors, update);
+  return Factors(factors, update);
 }
 
 /* *******************************************************************************/

gtsam/hybrid/HybridGaussianFactor.h

@@ -73,6 +73,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// Decision tree of Gaussian factors indexed by discrete keys.
   Factors factors_;
 
+  /// Helper function to "hide" the constants in the Jacobian factors.
+  static Factors augment(const FactorValuePairs &factors);
+
   /**
    * @brief Helper function to return factors and functional to create a
    * DecisionTree of Gaussian Factor Graphs.