printErrors method for HybridNonlinearFactorGraph

gtsam/hybrid/HybridNonlinearFactorGraph.cpp

@@ -42,6 +42,98 @@ void HybridNonlinearFactorGraph::print(const std::string& s,
   }
 }
 
+/* ************************************************************************* */
+void HybridNonlinearFactorGraph::printErrors(
+    const HybridValues& values, const std::string& str,
+    const KeyFormatter& keyFormatter,
+    const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/,
+                             size_t /*index*/)>& printCondition) const {
+  std::cout << str << "size: " << size() << std::endl << std::endl;
+
+  std::stringstream ss;
+
+  for (size_t i = 0; i < factors_.size(); i++) {
+    auto&& factor = factors_[i];
+    std::cout << "Factor " << i << ": ";
+
+    // Clear the stringstream
+    ss.str(std::string());
+
+    if (auto mf = std::dynamic_pointer_cast<MixtureFactor>(factor)) {
+      if (factor == nullptr) {
+        std::cout << "nullptr"
+                  << "\n";
+      } else {
+        factor->print(ss.str(), keyFormatter);
+        std::cout << "error = ";
+        mf->error(values.nonlinear()).print("", DefaultKeyFormatter);
+        std::cout << std::endl;
+      }
+    } else if (auto gmf =
+                   std::dynamic_pointer_cast<GaussianMixtureFactor>(factor)) {
+      if (factor == nullptr) {
+        std::cout << "nullptr"
+                  << "\n";
+      } else {
+        factor->print(ss.str(), keyFormatter);
+        std::cout << "error = ";
+        gmf->error(values.continuous()).print("", DefaultKeyFormatter);
+        std::cout << std::endl;
+      }
+    } else if (auto gm = std::dynamic_pointer_cast<GaussianMixture>(factor)) {
+      if (factor == nullptr) {
+        std::cout << "nullptr"
+                  << "\n";
+      } else {
+        factor->print(ss.str(), keyFormatter);
+        std::cout << "error = ";
+        gm->error(values.continuous()).print("", DefaultKeyFormatter);
+        std::cout << std::endl;
+      }
+    } else if (auto nf = std::dynamic_pointer_cast<NonlinearFactor>(factor)) {
+      const double errorValue = (factor != nullptr ? nf->error(values) : .0);
+      if (!printCondition(factor.get(), errorValue, i))
+        continue;  // User-provided filter did not pass
+
+      if (factor == nullptr) {
+        std::cout << "nullptr"
+                  << "\n";
+      } else {
+        factor->print(ss.str(), keyFormatter);
+        std::cout << "error = " << errorValue << "\n";
+      }
+    } else if (auto gf = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
+      const double errorValue = (factor != nullptr ? gf->error(values) : .0);
+      if (!printCondition(factor.get(), errorValue, i))
+        continue;  // User-provided filter did not pass
+
+      if (factor == nullptr) {
+        std::cout << "nullptr"
+                  << "\n";
+      } else {
+        factor->print(ss.str(), keyFormatter);
+        std::cout << "error = " << errorValue << "\n";
+      }
+    } else if (auto df = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
+      if (factor == nullptr) {
+        std::cout << "nullptr"
+                  << "\n";
+      } else {
+        factor->print(ss.str(), keyFormatter);
+        std::cout << "error = ";
+        df->error().print("", DefaultKeyFormatter);
+        std::cout << std::endl;
+      }
+
+    } else {
+      continue;
+    }
+
+    std::cout << "\n";
+  }
+  std::cout.flush();
+}
+
 /* ************************************************************************* */
 HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
     const Values& continuousValues) const {

gtsam/hybrid/HybridNonlinearFactorGraph.h

@@ -63,6 +63,16 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
       const std::string& s = "HybridNonlinearFactorGraph",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
+  /** print errors along with factors*/
+  void printErrors(
+      const HybridValues& values,
+      const std::string& str = "HybridNonlinearFactorGraph: ",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter,
+      const std::function<bool(const Factor* /*factor*/,
+                               double /*whitenedError*/, size_t /*index*/)>&
+          printCondition =
+              [](const Factor*, double, size_t) { return true; }) const;
+
   /// @}
   /// @name Standard Interface
   /// @{

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -327,8 +327,8 @@ GaussianFactorGraph::shared_ptr batchGFG(double between,
   NonlinearFactorGraph graph;
   graph.addPrior<double>(X(0), 0, Isotropic::Sigma(1, 0.1));
 
-  auto between_x0_x1 = std::make_shared<MotionModel>(
+  auto between_x0_x1 = std::make_shared<MotionModel>(X(0), X(1), between,
-      X(0), X(1), between, Isotropic::Sigma(1, 1.0));
+                                                     Isotropic::Sigma(1, 1.0));
 
   graph.push_back(between_x0_x1);
 
@@ -397,6 +397,25 @@ TEST(HybridFactorGraph, Partial_Elimination) {
   EXPECT(remainingFactorGraph->at(2)->keys() == KeyVector({M(0), M(1)}));
 }
 
+TEST(HybridFactorGraph, PrintErrors) {
+  Switching self(3);
+
+  // Get nonlinear factor graph and add linear factors to be holistic
+  HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph;
+  fg.add(self.linearizedFactorGraph);
+
+  // Optimize to get HybridValues
+  HybridBayesNet::shared_ptr bn =
+      self.linearizedFactorGraph.eliminateSequential();
+  HybridValues hv = bn->optimize();
+
+  // Print and verify
+  fg.print();
+  std::cout << "\n\n\n" << std::endl;
+  fg.printErrors(
+      HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint));
+}
+
 /****************************************************************************
  * Test full elimination
  */