printError(): allow custom factor filter
parent
5c9c4bed93
commit
b622262acf
|
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
Loading…
Reference in New Issue