From b622262acfdf1d89b5490d0611b0ca1dd5c233f0 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Tue, 7 Apr 2020 01:31:40 +0200
Subject: [PATCH] printError(): allow custom factor filter

---
 gtsam/nonlinear/NonlinearFactorGraph.cpp | 19 ++++++++++++------
 gtsam/nonlinear/NonlinearFactorGraph.h   |  4 +++-
 tests/testNonlinearFactorGraph.cpp       | 25 ++++++++++++++++++++++++
 3 files changed, 41 insertions(+), 7 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 42b2d07f6..5f6cdcc98 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -61,19 +61,26 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key
 
 /* ************************************************************************* */
 void NonlinearFactorGraph::printErrors(const Values& values, const std::string& str,
-                                       const KeyFormatter& keyFormatter) const {
+    const KeyFormatter& keyFormatter,
+    const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>& printCondition) const
+{
   cout << str << "size: " << size() << endl
        << endl;
   for (size_t i = 0; i < factors_.size(); i++) {
+    const sharedFactor& factor = factors_[i];
+    const double errorValue = (factor != nullptr ? factors_[i]->error(values) : .0);
+    if (!printCondition(factor.get(),errorValue,i))
+      continue; // User-provided filter did not pass
+
     stringstream ss;
     ss << "Factor " << i << ": ";
-    if (factors_[i] == nullptr) {
-      cout << "nullptr" << endl;
+    if (factor == nullptr) {
+      cout << "nullptr" << "\n";
     } else {
-      factors_[i]->print(ss.str(), keyFormatter);
-      cout << "error = " << factors_[i]->error(values) << endl;
+      factor->print(ss.str(), keyFormatter);
+      cout << "error = " << errorValue << "\n";
     }
-    cout << endl;
+    cout << endl; // only one "endl" at end might be faster, \n for each factor
   }
 }
 
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 902a47a0f..232693a9e 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -103,7 +103,9 @@ namespace gtsam {
 
     /** print errors along with factors*/
     void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
-                     const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+      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;
 
     /** Test equality */
     bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 6a7a963bc..e403edaab 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -242,6 +242,31 @@ TEST(testNonlinearFactorGraph, eliminate) {
   EXPECT_LONGS_EQUAL(4, bn->size());
 }
 
+/* ************************************************************************* */
+TEST(NonlinearFactorGraph, printErrors)
+{
+  const NonlinearFactorGraph fg = createNonlinearFactorGraph();
+  const Values c = createValues();
+
+  // Test that it builds with default parameters.
+  // We cannot check the output since (at present) output is fixed to std::cout.
+  fg.printErrors(c);
+
+  // Second round: using callback filter to check that we actually visit all factors:
+  std::vector<bool> visited;
+  visited.assign(fg.size(), false);
+  const auto testFilter =
+      [&](const gtsam::Factor *f, double error, size_t index) {
+        EXPECT(f!=nullptr);
+        EXPECT(error>=.0);
+        visited.at(index)=true;
+        return false; // do not print
+      };
+  fg.printErrors(c,"Test graph: ", gtsam::DefaultKeyFormatter,testFilter);
+
+  for (bool visit : visited) EXPECT(visit==true);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */