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& 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 663415bca..0e17700d0 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/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& + printCondition = [](const Factor *,double, size_t) {return true;}) const; /** Test equality */ bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index e48b11b32..041aa3441 100644 --- a/gtsam/nonlinear/Values.h +++ b/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 if j is already present - * if no chart is specified, the DefaultChart is used + * throws KeyDoesNotExist if j is not present. + * If no chart is specified, the DefaultChart is used. */ template void update(Key j, const T& val); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index a4ddf50f2..fdb080a63 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/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 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); } /* ************************************************************************* */