diff --git a/tests/smallExample.h b/tests/smallExample.h index 0c933d106..271fb0581 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -363,6 +363,53 @@ inline NonlinearFactorGraph createReallyNonlinearFactorGraph() { return *sharedReallyNonlinearFactorGraph(); } +/* ************************************************************************* */ +inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() { + using symbol_shorthand::X; + boost::shared_ptr fg(new NonlinearFactorGraph); + Point2 z(0.0, 0.0); + double sigma = 0.1; + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + // 3 noiseless inliers + fg->push_back(factor); + fg->push_back(factor); + fg->push_back(factor); + + // 1 outlier + Point2 z_out(1.0, 0.0); + boost::shared_ptr factor_out( + new smallOptimize::UnaryFactor(z_out, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + fg->push_back(factor_out); + + return *fg; +} + +/* ************************************************************************* */ +inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() { + using symbol_shorthand::X; + boost::shared_ptr fg(new NonlinearFactorGraph); + Point2 z(0.0, 0.0); + double sigma = 0.1; + auto gmNoise = noiseModel::Robust::Create( + noiseModel::mEstimator::GemanMcClure::Create(1.0), noiseModel::Isotropic::Sigma(2,sigma)); + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, gmNoise, X(1))); + // 3 noiseless inliers + fg->push_back(factor); + fg->push_back(factor); + fg->push_back(factor); + + // 1 outlier + Point2 z_out(1.0, 0.0); + boost::shared_ptr factor_out( + new smallOptimize::UnaryFactor(z_out, gmNoise, X(1))); + fg->push_back(factor_out); + + return *fg; +} + + /* ************************************************************************* */ inline std::pair createNonlinearSmoother(int T) { using namespace impl; diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 87ac26841..1951e51f1 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -253,9 +253,6 @@ TEST(GncOptimizer, gncConstructor) { //TEST(GncOptimizer, calculateWeights) { //} // -///* ************************************************************************* */ -//TEST(GncOptimizer, copyGraph) { -//} /* ************************************************************************* * TEST(GncOptimizer, makeGraph) { @@ -274,7 +271,7 @@ TEST(GncOptimizer, makeGraph) { } /* ************************************************************************* */ -TEST(GncOptimizer, optimize) { +TEST(GncOptimizer, optimizeSimple) { // has to have Gaussian noise models ! auto fg = example::createReallyNonlinearFactorGraph(); @@ -286,12 +283,40 @@ TEST(GncOptimizer, optimize) { GncParams gncParams(lmParams); auto gnc = GncOptimizer>(fg, initial, gncParams); - gncParams.print(""); - Values actual = gnc.optimize(); DOUBLES_EQUAL(0, fg.error(actual), tol); } +/* ************************************************************************* */ +TEST(GncOptimizer, optimize) { + // has to have Gaussian noise models ! + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(1, 0); + Values initial; + initial.insert(X(1), p0); + + // try with nonrobust cost function and standard GN + GaussNewtonParams gnParams; + GaussNewtonOptimizer gn(fg, initial, gnParams); + Values gn_results = gn.optimize(); + // converges to incorrect point due to lack of robustness to an outlier, ideal solution is Point2(0,0) + CHECK(assert_equal(gn_results.at(X(1)), Point2(1.31812,0.0), 1e-3)); + + // try with robust loss function and standard GN + auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with factors wrapped in Geman McClure losses + GaussNewtonOptimizer gn2(fg_robust, initial, gnParams); + Values gn2_results = gn2.optimize(); + // converges to incorrect point, this time due to the nonconvexity of the loss + CHECK(assert_equal(gn2_results.at(X(1)), Point2(1.18712,0.0), 1e-3)); + + // .. but graduated nonconvexity ensures both robustness and convergence in the face of nonconvexity + GncParams gncParams(gnParams); + auto gnc = GncOptimizer>(fg, initial, gncParams); + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(gnc_result.at(X(1)), Point2(0.0,0.0), 1e-3)); +} + /* ************************************************************************* */ int main() { TestResult tr;