Get Error From Hessians

gtsam/discrete/AlgebraicDecisionTree.h

@@ -163,7 +163,7 @@ namespace gtsam {
                const typename Base::LabelFormatter& labelFormatter =
                    &DefaultFormatter) const {
       auto valueFormatter = [](const double& v) {
-        return (boost::format("%4.2g") % v).str();
+        return (boost::format("%4.4g") % v).str();
       };
       Base::print(s, labelFormatter, valueFormatter);
     }

gtsam/hybrid/HybridFactorGraph.cpp

@@ -90,9 +90,9 @@ DiscreteKeys HybridFactorGraph::discreteKeys() const {
   result = discreteGraph_.discreteKeys();
   // Discrete keys from the DC factor graph.
   auto dcKeys = dcGraph_.discreteKeys();
-  for(auto&& key: dcKeys) {
+  for (auto&& key : dcKeys) {
     // Only insert unique keys
-    if(std::find(result.begin(), result.end(), key) == result.end()) {
+    if (std::find(result.begin(), result.end(), key) == result.end()) {
       result.push_back(key);
     }
   }
@@ -175,27 +175,30 @@ pair<GaussianMixture::shared_ptr, boost::shared_ptr<Factor>> EliminateHybrid(
   DecisionTree<Key, Pair> eliminationResults(sum, eliminate);
 
   // STEP 3: Create result
-  // TODO(Frank): auto pair = eliminationResults.unzip();
+  auto pair = unzip(eliminationResults);
+  GaussianMixture::Conditionals conditionals = pair.first;
+  DCGaussianMixtureFactor::Factors separatorFactors = pair.second;
 
   const DiscreteKeys discreteKeys = factors.discreteKeys();
 
-  // Grab the conditionals and create the GaussianMixture
-  auto first = [](const Pair& result) { return result.first; };
-  GaussianMixture::Conditionals conditionals(eliminationResults, first);
+  // Create the GaussianMixture from the conditionals
   auto conditional =
       boost::make_shared<GaussianMixture>(keys, discreteKeys, conditionals);
 
   // If there are no more continuous parents, then we should create here a
   // DiscreteFactor, with the error for each discrete choice.
   if (separatorKeys.size() == 0) {
-    auto discreteFactor = factors.toDecisionTreeFactor();
+    auto factorError = [&](const GaussianFactor::shared_ptr& factor) {
+      return exp(-factor->error(VectorValues()));
+    };
+    DecisionTree<Key, double> fdt(separatorFactors, factorError);
+    auto discreteFactor =
+        boost::make_shared<DecisionTreeFactor>(factors.discreteKeys(), fdt);
+
     return {conditional, discreteFactor};
 
   } else {
     // Create a resulting DCGaussianMixture on the separator.
-    auto second = [](const Pair& result) { return result.second; };
-    DCGaussianMixtureFactor::Factors separatorFactors(eliminationResults,
-                                                      second);
     auto factor = boost::make_shared<DCGaussianMixtureFactor>(
         separatorKeys, discreteKeys, separatorFactors);
     return {conditional, factor};

gtsam/hybrid/HybridFactorGraph.h

@@ -224,35 +224,6 @@ class HybridFactorGraph : protected FactorGraph<Factor>,
     }
   }
 
-  // DEPRECATED below:.
-
-  // /**
-  //  * Add a nonlinear factor to the internal nonlinear factor graph
-  //  * @param nonlinearFactor - the factor to add
-  //  */
-  // template <typename FACTOR>
-  // IsNonlinear<FACTOR> push_nonlinear(const FACTOR& nonlinearFactor) {
-  //   emplace_shared<FACTOR>(nonlinearFactor);
-  // }
-
-  // /**
-  //  * Add a discrete factor to the internal discrete graph
-  //  * @param discreteFactor - the factor to add
-  //  */
-  // template <typename FACTOR>
-  // IsDiscrete<FACTOR> push_discrete(const FACTOR& discreteFactor) {
-  //   emplace_shared<FACTOR>(discreteFactor);
-  // }
-
-  // /**
-  //  * Add a discrete-continuous (DC) factor to the internal DC graph
-  //  * @param dcFactor - the factor to add
-  //  */
-  // template <typename FACTOR>
-  // IsDC<FACTOR> push_dc(const FACTOR& dcFactor) {
-  //   emplace_shared<FACTOR>(dcFactor);
-  // }
-
   /**
    * Simply prints the factor graph.
    */

gtsam/hybrid/tests/testHybridFactorGraph.cpp

@@ -334,7 +334,7 @@ TEST(DCGaussianElimination, Eliminate_fully) {
   auto factors = self.linearizedFactorGraph;
 
   // Add measurement factors
-  auto measurement_noise = noiseModel::Isotropic::Sigmas(Vector1(0.01));
+  auto measurement_noise = noiseModel::Isotropic::Sigma(1, 0.1);
   factors.emplace_gaussian<JacobianFactor>(X(1), I_1x1, 1 * Vector1::Ones(), measurement_noise);
   factors.emplace_gaussian<JacobianFactor>(X(2), I_1x1, 2 * Vector1::Ones(), measurement_noise);