Merge pull request #2056 from borglab/hybrid-updates

Hybrid Updates

gtsam/hybrid/HybridNonlinearFactor.cpp

@@ -134,10 +134,12 @@ void HybridNonlinearFactor::print(const std::string& s,
   std::cout << (s.empty() ? "" : s + " ");
   Base::print("", keyFormatter);
   std::cout << "\nHybridNonlinearFactor\n";
-  auto valueFormatter = [](const std::pair<sharedFactor, double>& v) {
+  auto valueFormatter = [&keyFormatter](const std::pair<sharedFactor, double>& v) {
     auto [factor, val] = v;
     if (factor) {
-      return "Nonlinear factor on " + std::to_string(factor->size()) + " keys";
+      RedirectCout rd;
+      factor->print("", keyFormatter);
+      return rd.str();
     } else {
       return std::string("nullptr");
     }

gtsam/hybrid/HybridSmoother.cpp

@@ -171,7 +171,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &newFactors,
   HybridBayesNet updatedHybridBayesNet(hybridBayesNet);
 
   KeySet involvedKeys = newFactors.keys();
-  auto involved = [&involvedKeys](const Key &key) {
+  auto involved = [](const KeySet &involvedKeys, const Key &key) {
     return involvedKeys.find(key) != involvedKeys.end();
   };
 
@@ -195,7 +195,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &newFactors,
       auto conditional = hybridBayesNet.at(i);
 
       for (auto &key : conditional->frontals()) {
-        if (involved(key)) {
+        if (involved(involvedKeys, key)) {
           // Add the conditional parents to involvedKeys
           // so we add those conditionals too.
           for (auto &&parentKey : conditional->parents()) {
@@ -214,7 +214,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &newFactors,
       auto conditional = hybridBayesNet.at(i);
 
       for (auto &key : conditional->frontals()) {
-        if (involved(key)) {
+        if (involved(involvedKeys, key)) {
           newConditionals.push_back(conditional);
 
           // Remove the conditional from the updated Bayes net

gtsam/hybrid/tests/testHybridNonlinearFactor.cpp

@@ -81,8 +81,14 @@ TEST(HybridNonlinearFactor, Printing) {
       R"(Hybrid [x1 x2; 1]
 HybridNonlinearFactor
  Choice(1) 
- 0 Leaf Nonlinear factor on 2 keys
- 1 Leaf Nonlinear factor on 2 keys
+ 0 Leaf BetweenFactor(x1,x2)
+  measured:  0
+  noise model: diagonal sigmas [1];
+
+ 1 Leaf BetweenFactor(x1,x2)
+  measured:  1
+  noise model: diagonal sigmas [1];
+
 )";
   EXPECT(assert_print_equal(expected, hybridFactor));
 }

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -535,6 +535,9 @@ TEST(HybridNonlinearFactorGraph, Printing) {
   const auto [hybridBayesNet, remainingFactorGraph] =
       linearizedFactorGraph.eliminatePartialSequential(ordering);
 
+  // Set precision so we are consistent on all platforms
+  std::cout << std::setprecision(6);
+
 #ifdef GTSAM_DT_MERGING
   string expected_hybridFactorGraph = R"(
 size: 7