printError(): allow custom factor filter

release/4.3a0
Jose Luis Blanco Claraco 2020-04-07 01:31:40 +02:00
parent 5c9c4bed93
commit b622262acf
No known key found for this signature in database
GPG Key ID: D443304FBD70A641
3 changed files with 41 additions and 7 deletions

View File

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

View File

@ -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;

View File

@ -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); }
/* ************************************************************************* */