From e312abdbf0a77dcbba6fd255cd51896531d7401f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Mar 2020 15:22:19 -0500 Subject: [PATCH 1/3] Add More Unit Tests for Robust Noise Models --- tests/testNonlinearOptimizer.cpp | 191 +++++++++++++++++++++++++++++-- 1 file changed, 180 insertions(+), 11 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 0e7793552..7cf5e1e2d 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -339,31 +340,199 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { } /* ************************************************************************* */ -TEST(NonlinearOptimizer, MoreOptimizationWithHuber) { +TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { NonlinearFactorGraph fg; fg += PriorFactor(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); - fg += BetweenFactor(0, 1, Pose2(1,0,M_PI/2), + fg += BetweenFactor(0, 1, Pose2(1,1.1,M_PI/4), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(1, 2, Pose2(1,0,M_PI/2), + fg += BetweenFactor(0, 1, Pose2(1,0.9,M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0), noiseModel::Isotropic::Sigma(3,1))); Values init; - init.insert(0, Pose2(10,10,0)); - init.insert(1, Pose2(1,0,M_PI)); - init.insert(2, Pose2(1,1,-M_PI)); + init.insert(0, Pose2(0,0,0)); + init.insert(1, Pose2(0.961187, 0.99965, 1.1781)); Values expected; expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,0,M_PI/2)); - expected.insert(2, Pose2(1,1,M_PI)); + expected.insert(1, Pose2(0.961187, 0.99965, 1.1781)); + + LevenbergMarquardtParams lm_params; + lm_params.setRelativeErrorTol(0); + lm_params.setAbsoluteErrorTol(0); + lm_params.setMaxIterations(100); + lm_params.setlambdaUpperBound(1e10); + // lm_params.setVerbosityLM("TRYLAMBDA"); + + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(100000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + + DoglegParams dl_params; + dl_params.setRelativeErrorTol(0); + dl_params.setAbsoluteErrorTol(0); + dl_params.setMaxIterations(100); + + // cg_params.setVerbosity("ERROR"); + + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); + auto dl_result = DoglegOptimizer(fg, init, dl_params).optimize(); + + EXPECT(assert_equal(expected, cg_result, 3e-2)); + EXPECT(assert_equal(expected, gn_result, 3e-2)); + EXPECT(assert_equal(expected, lm_result, 3e-2)); + EXPECT(assert_equal(expected, dl_result, 3e-2)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { + + NonlinearFactorGraph fg; + fg += PriorFactor(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01)); + fg += BetweenFactor(0, 1, Point2(1,1.8), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + fg += BetweenFactor(0, 1, Point2(1,0.9), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + fg += BetweenFactor(0, 1, Point2(1,90), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + + Values init; + init.insert(0, Point2(1,1)); + init.insert(1, Point2(1,0)); + + Values expected; + expected.insert(0, Point2(0,0)); + expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize())); - EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init, params).optimize())); - EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(100000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + + // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); + + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + EXPECT(assert_equal(expected, gn_result, 1e-4)); + EXPECT(assert_equal(expected, gn_result, 1e-4)); + EXPECT(assert_equal(expected, lm_result, 1e-4)); + EXPECT(assert_equal(expected, dl_result, 1e-4)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { + + NonlinearFactorGraph fg; + fg += PriorFactor(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1)); + fg += BetweenFactor(0, 1, Pose2(0,9, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0, 11, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0, 10, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0,9, 0), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + + Values init; + init.insert(0, Pose2(0, 0, 0)); + init.insert(1, Pose2(0, 10, M_PI/4)); + + Values expected; + expected.insert(0, Pose2(0, 0, 0)); + expected.insert(1, Pose2(0, 10, 1.45212)); + + LevenbergMarquardtParams params; + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(100000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + + // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); + fg.printErrors(cg_result); + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); + + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + EXPECT(assert_equal(expected, gn_result, 1e-1)); + EXPECT(assert_equal(expected, gn_result, 1e-1)); + EXPECT(assert_equal(expected, lm_result, 1e-1)); + EXPECT(assert_equal(expected, dl_result, 1e-1)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, RobustMeanCalculation) { + + NonlinearFactorGraph fg; + + Values init; + + Values expected; + + auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(20), + noiseModel::Isotropic::Sigma(1, 1)); + + vector pts{-10,-3,-1,1,3,10,1000}; + for(auto pt : pts) { + fg += PriorFactor(0, pt, huber); + } + + init.insert(0, 100.0); + expected.insert(0, 3.33333333); + + LevenbergMarquardtParams params; + // params.setVerbosityLM("TRYLAMBDA"); + params.setAbsoluteErrorTol(1e-20); + params.setRelativeErrorTol(1e-20); + + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(10000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); +// cg_result.print("CG: "); +// cout << fg.error(cg_result) << endl << endl << endl; + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); +// gn_result.print("GN: "); +// cout << fg.error(gn_result) << endl << endl << endl; + + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); +// lm_result.print("LM: "); +// cout << fg.error(lm_result) << endl << endl << endl; + + auto dl_result = DoglegOptimizer(fg, init).optimize(); +// dl_result.print("DL: "); +// cout << fg.error(dl_result) << endl << endl << endl; + EXPECT(assert_equal(expected, gn_result, tol)); + EXPECT(assert_equal(expected, gn_result, tol)); + EXPECT(assert_equal(expected, lm_result, tol)); + EXPECT(assert_equal(expected, dl_result, tol)); } /* ************************************************************************* */ From 3c0671ba8de1a5fa9b115fc8cf6e5ac3b768ce41 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Mar 2020 19:38:57 -0500 Subject: [PATCH 2/3] Removed commentted out and print-s --- tests/testNonlinearOptimizer.cpp | 34 ++++---------------------------- 1 file changed, 4 insertions(+), 30 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 7cf5e1e2d..4b7ca6a03 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -364,9 +364,8 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { lm_params.setAbsoluteErrorTol(0); lm_params.setMaxIterations(100); lm_params.setlambdaUpperBound(1e10); - // lm_params.setVerbosityLM("TRYLAMBDA"); - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + NonlinearConjugateGradientOptimizer::Parameters cg_params; cg_params.setErrorTol(0); cg_params.setMaxIterations(100000); cg_params.setRelativeErrorTol(0); @@ -377,8 +376,6 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { dl_params.setAbsoluteErrorTol(0); dl_params.setMaxIterations(100); - // cg_params.setVerbosity("ERROR"); - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); @@ -414,19 +411,11 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(100000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); + NonlinearConjugateGradientOptimizer::Parameters cg_params; - // cg_params.setVerbosity("ERROR"); auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); - auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); - auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); - auto dl_result = DoglegOptimizer(fg, init).optimize(); EXPECT(assert_equal(expected, gn_result, 1e-4)); @@ -468,13 +457,9 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { cg_params.setRelativeErrorTol(0); cg_params.setAbsoluteErrorTol(0); - // cg_params.setVerbosity("ERROR"); auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); - fg.printErrors(cg_result); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); - auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); - auto dl_result = DoglegOptimizer(fg, init).optimize(); EXPECT(assert_equal(expected, gn_result, 1e-1)); @@ -504,7 +489,6 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { expected.insert(0, 3.33333333); LevenbergMarquardtParams params; - // params.setVerbosityLM("TRYLAMBDA"); params.setAbsoluteErrorTol(1e-20); params.setRelativeErrorTol(1e-20); @@ -513,22 +497,12 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { cg_params.setMaxIterations(10000); cg_params.setRelativeErrorTol(0); cg_params.setAbsoluteErrorTol(0); - // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); -// cg_result.print("CG: "); -// cout << fg.error(cg_result) << endl << endl << endl; - auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); -// gn_result.print("GN: "); -// cout << fg.error(gn_result) << endl << endl << endl; - auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); -// lm_result.print("LM: "); -// cout << fg.error(lm_result) << endl << endl << endl; - auto dl_result = DoglegOptimizer(fg, init).optimize(); -// dl_result.print("DL: "); -// cout << fg.error(dl_result) << endl << endl << endl; + EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, lm_result, tol)); From 4babfe24911eba8348ce5ff857e68637be524862 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 Mar 2020 17:39:28 -0500 Subject: [PATCH 3/3] Remove redundant params --- tests/testNonlinearOptimizer.cpp | 39 +------------------------------- 1 file changed, 1 insertion(+), 38 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 4b7ca6a03..7b5e7a0e0 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -360,28 +360,11 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { expected.insert(1, Pose2(0.961187, 0.99965, 1.1781)); LevenbergMarquardtParams lm_params; - lm_params.setRelativeErrorTol(0); - lm_params.setAbsoluteErrorTol(0); - lm_params.setMaxIterations(100); - lm_params.setlambdaUpperBound(1e10); - NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(100000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); - - DoglegParams dl_params; - dl_params.setRelativeErrorTol(0); - dl_params.setAbsoluteErrorTol(0); - dl_params.setMaxIterations(100); - - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); - auto dl_result = DoglegOptimizer(fg, init, dl_params).optimize(); + auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, cg_result, 3e-2)); EXPECT(assert_equal(expected, gn_result, 3e-2)); EXPECT(assert_equal(expected, lm_result, 3e-2)); EXPECT(assert_equal(expected, dl_result, 3e-2)); @@ -411,14 +394,11 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - NonlinearConjugateGradientOptimizer::Parameters cg_params; - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, gn_result, 1e-4)); EXPECT(assert_equal(expected, gn_result, 1e-4)); EXPECT(assert_equal(expected, lm_result, 1e-4)); EXPECT(assert_equal(expected, dl_result, 1e-4)); @@ -451,18 +431,11 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { expected.insert(1, Pose2(0, 10, 1.45212)); LevenbergMarquardtParams params; - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(100000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, gn_result, 1e-1)); EXPECT(assert_equal(expected, gn_result, 1e-1)); EXPECT(assert_equal(expected, lm_result, 1e-1)); EXPECT(assert_equal(expected, dl_result, 1e-1)); @@ -489,21 +462,11 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { expected.insert(0, 3.33333333); LevenbergMarquardtParams params; - params.setAbsoluteErrorTol(1e-20); - params.setRelativeErrorTol(1e-20); - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(10000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); - - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, lm_result, tol)); EXPECT(assert_equal(expected, dl_result, tol));