Merge branch develop

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
   }
 }
 

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -104,7 +104,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;

gtsam/nonlinear/Values.h

@@ -276,8 +276,8 @@ namespace gtsam {
     void update(Key j, const Value& val);
 
     /** Templated version to update a variable with the given j,
-      * throws KeyAlreadyExists<J> if j is already present
-      * if no chart is specified, the DefaultChart<ValueType> is used
+      * throws KeyDoesNotExist<J> if j is not present.
+      * If no chart is specified, the DefaultChart<ValueType> is used.
       */
     template <typename T>
     void update(Key j, const T& val);

tests/testNonlinearFactorGraph.cpp

@@ -285,6 +285,30 @@ TEST(testNonlinearFactorGraph, addPrior) {
   EXPECT(0 != graph.error(values));
 }
 
+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); }
 /* ************************************************************************* */