12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #235 from borglab/feature/robust_unit_tests 

Add More Unit Tests for Robust Noise Models
release/4.3a0
Frank Dellaert 2020-04-04 14:03:28 -04:00 committed by GitHub
parent 87b478cd4e 4babfe2491
commit 669ffd0a05
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 117 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
tests/testNonlinearOptimizer.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -339,31 +340,136 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
}
/* ************************************************************************* */
TEST(NonlinearOptimizer, MoreOptimizationWithHuber) {
TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) {
NonlinearFactorGraph fg;
fg += PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2),
fg += BetweenFactor<Pose2>(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<Pose2>(1, 2, Pose2(1,0,M_PI/2),
fg += BetweenFactor<Pose2>(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;
auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize();
auto dl_result = DoglegOptimizer(fg, init).optimize();
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<Point2>(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01));
fg += BetweenFactor<Point2>(0, 1, Point2(1,1.8),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
noiseModel::Isotropic::Sigma(2,1)));
fg += BetweenFactor<Point2>(0, 1, Point2(1,0.9),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
noiseModel::Isotropic::Sigma(2,1)));
fg += BetweenFactor<Point2>(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()));
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, lm_result, 1e-4));
EXPECT(assert_equal(expected, dl_result, 1e-4));
}
/* ************************************************************************* */
TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) {
NonlinearFactorGraph fg;
fg += PriorFactor<Pose2>(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 11, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 10, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
fg += BetweenFactor<Pose2>(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;
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, 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<double> pts{-10,-3,-1,1,3,10,1000};
for(auto pt : pts) {
fg += PriorFactor<double>(0, pt, huber);
}
init.insert(0, 100.0);
expected.insert(0, 3.33333333);
LevenbergMarquardtParams params;
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, lm_result, tol));
EXPECT(assert_equal(expected, dl_result, tol));
}
/* ************************************************************************* */