Merge pull request #2120 from borglab/docs-hybrid

Author: varunagrawal

gtsam/discrete/DiscreteValues.cpp

@@ -152,12 +152,6 @@ string DiscreteValues::html(const KeyFormatter& keyFormatter,
   return ss.str();
 }
 
-/* ************************************************************************ */
-void PrintDiscreteValues(const DiscreteValues& values, const std::string& s,
-                         const KeyFormatter& keyFormatter) {
-  values.print(s, keyFormatter);
-}
-
 string markdown(const DiscreteValues& values, const KeyFormatter& keyFormatter,
                 const DiscreteValues::Names& names) {
   return values.markdown(keyFormatter, names);

gtsam/discrete/DiscreteValues.h

@@ -197,11 +197,6 @@ inline std::vector<DiscreteValues> cartesianProduct(const DiscreteKeys& keys) {
   return DiscreteValues::CartesianProduct(keys);
 }
 
-/// Free version of print for wrapper
-void GTSAM_EXPORT
-PrintDiscreteValues(const DiscreteValues& values, const std::string& s = "",
-                    const KeyFormatter& keyFormatter = DefaultKeyFormatter);
-
 /// Free version of markdown.
 std::string GTSAM_EXPORT
 markdown(const DiscreteValues& values,

gtsam/discrete/discrete.i

@@ -23,10 +23,6 @@ class DiscreteKeys {
 std::vector<gtsam::DiscreteValues> cartesianProduct(
     const gtsam::DiscreteKeys& keys);
 
-void PrintDiscreteValues(
-    const gtsam::DiscreteValues& values, const std::string& s = "",
-    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
-
 string markdown(
     const gtsam::DiscreteValues& values,
     const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);

gtsam/discrete/tests/testDiscreteBayesTree.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/discrete/DiscreteBayesNet.h>
 #include <gtsam/discrete/DiscreteBayesTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/base/TestableAssertions.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -290,30 +291,31 @@ TEST(DiscreteBayesTree, Dot) {
   std::string actual = self.bayesTree->dot();
   // print actual:
   if (debug) std::cout << actual << std::endl;
-  EXPECT(actual ==
-         "digraph G{\n"
-         "0[label=\"13, 11, 6, 7\"];\n"
-         "0->1\n"
-         "1[label=\"14 : 11, 13\"];\n"
-         "1->2\n"
-         "2[label=\"9, 12 : 14\"];\n"
-         "2->3\n"
-         "3[label=\"3 : 9, 12\"];\n"
-         "2->4\n"
-         "4[label=\"2 : 9, 12\"];\n"
-         "2->5\n"
-         "5[label=\"8 : 12, 14\"];\n"
-         "5->6\n"
-         "6[label=\"1 : 8, 12\"];\n"
-         "5->7\n"
-         "7[label=\"0 : 8, 12\"];\n"
-         "1->8\n"
-         "8[label=\"10 : 13, 14\"];\n"
-         "8->9\n"
-         "9[label=\"5 : 10, 13\"];\n"
-         "8->10\n"
-         "10[label=\"4 : 10, 13\"];\n"
-         "}");
+  std::string expected =
+    R"(digraph G{
+13[label="13, 11, 6, 7"];
+13->14
+14[label="14 : 11, 13"];
+14->9
+9[label="9, 12 : 14"];
+9->3
+3[label="3 : 9, 12"];
+9->2
+2[label="2 : 9, 12"];
+9->8
+8[label="8 : 12, 14"];
+8->1
+1[label="1 : 8, 12"];
+8->0
+0[label="0 : 8, 12"];
+14->10
+10[label="10 : 13, 14"];
+10->5
+5[label="5 : 10, 13"];
+10->4
+4[label="4 : 10, 13"];
+})";
+  EXPECT(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */

gtsam/hybrid/HybridEliminationTree.h

@@ -43,21 +43,22 @@ class GTSAM_EXPORT HybridEliminationTree
   /// @{
 
   /**
-   * Build the elimination tree of a factor graph using pre-computed column
+   * Construct the elimination tree of a factor graph using pre-computed column
    * structure.
    * @param factorGraph The factor graph for which to build the elimination tree
    * @param structure The set of factors involving each variable.  If this is
    * not precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
    * named constructor instead.
-   * @return The elimination tree
+   * @param order The ordering of the variables.
    */
   HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
                         const VariableIndex& structure, const Ordering& order);
 
-  /** Build the elimination tree of a factor graph.  Note that this has to
+  /** Construct the elimination tree of a factor graph.  Note that this has to
    * compute the column structure as a VariableIndex, so if you already have
    * this precomputed, use the other constructor instead.
    * @param factorGraph The factor graph for which to build the elimination tree
+   * @param order The ordering of the variables.
    */
   HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
                         const Ordering& order);

gtsam/hybrid/HybridFactor.h

@@ -135,7 +135,7 @@ class GTSAM_EXPORT HybridFactor : public Factor {
 
   /// Compute tree of linear errors.
   virtual AlgebraicDecisionTree<Key> errorTree(
-      const VectorValues &values) const = 0;
+      const VectorValues &continuousValues) const = 0;
 
   /// Restrict the factor to the given discrete values.
   virtual std::shared_ptr<Factor> restrict(

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -22,6 +22,7 @@
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
 #include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridGaussianProductFactor.h>
@@ -193,16 +194,41 @@ AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
 }
 
 /* *******************************************************************************/
-double HybridGaussianFactor::error(const HybridValues& values) const {
+double HybridGaussianFactor::error(const HybridValues& hybridValues) const {
   // Directly index to get the component, no need to build the whole tree.
-  const GaussianFactorValuePair pair = factors_(values.discrete());
-  return PotentiallyPrunedComponentError(pair, values.continuous());
+  const GaussianFactorValuePair pair = factors_(hybridValues.discrete());
+  return PotentiallyPrunedComponentError(pair, hybridValues.continuous());
 }
 
 /* ************************************************************************ */
 std::shared_ptr<Factor> HybridGaussianFactor::restrict(
-    const DiscreteValues& assignment) const {
-  throw std::runtime_error("HybridGaussianFactor::restrict not implemented");
+  const DiscreteValues& assignment) const {
+  FactorValuePairs restrictedTree = this->factors_;  // Start with the original tree
+
+  const DiscreteKeys& currentFactorDiscreteKeys = this->discreteKeys();
+  DiscreteKeys newFactorDiscreteKeys;  // For the new, restricted factor
+
+  // Iterate over the discrete keys of the current factor
+  for (const DiscreteKey& discreteKey : currentFactorDiscreteKeys) {
+    const Key& key = discreteKey.first;
+
+    // Check if this key is specified in the assignment
+    if (assignment.find(key) != assignment.end()) {
+      // Key is in assignment: restrict the tree by choosing the branch
+      restrictedTree = restrictedTree.choose(key, assignment.at(key));
+      // This key is now fixed, so it's not a discrete key for the new factor
+    }
+    else {
+      // Key is not in assignment: it remains a discrete key for the new factor
+      newFactorDiscreteKeys.push_back(discreteKey);
+    }
+  }
+
+  // Create and return the new HybridGaussianFactor.
+  // Its constructor will derive continuous keys from the GaussianFactor
+  // shared_ptrs within the restrictedTree.
+  return std::make_shared<HybridGaussianFactor>(newFactorDiscreteKeys,
+    restrictedTree);
 }
 
 /* ************************************************************************ */

gtsam/hybrid/HybridGaussianFactor.h

@@ -144,7 +144,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
    * @brief Compute the log-likelihood, including the log-normalizing constant.
    * @return double
    */
-  double error(const HybridValues &values) const override;
+  double error(const HybridValues &hybridValues) const override;
 
   /// Getter for GaussianFactor decision tree
   const FactorValuePairs &factors() const { return factors_; }

gtsam/hybrid/HybridJunctionTree.h

@@ -57,15 +57,7 @@ class GTSAM_EXPORT HybridJunctionTree
   typedef HybridJunctionTree This;           ///< This class
   typedef std::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
 
-  /**
-   * Build the elimination tree of a factor graph using precomputed column
-   * structure.
-   * @param factorGraph The factor graph for which to build the elimination tree
-   * @param structure The set of factors involving each variable.  If this is
-   * not precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
-   * named constructor instead.
-   * @return The elimination tree
-   */
+  /// Construct the junction tree from an elimination tree
   HybridJunctionTree(const HybridEliminationTree& eliminationTree);
 };
 

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -117,8 +117,8 @@ double HybridNonlinearFactor::error(
 }
 
 /* *******************************************************************************/
-double HybridNonlinearFactor::error(const HybridValues& values) const {
-  return error(values.nonlinear(), values.discrete());
+double HybridNonlinearFactor::error(const HybridValues& hybridValues) const {
+  return error(hybridValues.nonlinear(), hybridValues.discrete());
 }
 
 /* *******************************************************************************/
@@ -138,6 +138,7 @@ void HybridNonlinearFactor::print(const std::string& s,
     auto [factor, val] = v;
     if (factor) {
       RedirectCout rd;
+      std::cout << "(val=" << val << ") ";
       factor->print("", keyFormatter);
       return rd.str();
     } else {