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