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

simplified small test to make it more understandable

release/4.3a0
lcarlone 2020-11-27 16:18:36 -05:00
parent 52225998fe
commit 7c22c2c402
2 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
tests/smallExample.h
View File

@ -369,8 +369,9 @@ inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() {
boost::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
Point2 z(0.0, 0.0);
double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
boost::shared_ptr<PriorFactor<Point2>> factor(
new PriorFactor<Point2>(X(1), z, noiseModel::Isotropic::Sigma(2,sigma)));
// 3 noiseless inliers
fg->push_back(factor);
fg->push_back(factor);
@ -378,8 +379,8 @@ inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() {
// 1 outlier
Point2 z_out(1.0, 0.0);
boost::shared_ptr<smallOptimize::UnaryFactor> factor_out(
new smallOptimize::UnaryFactor(z_out, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
boost::shared_ptr<PriorFactor<Point2>> factor_out(
new PriorFactor<Point2>(X(1), z_out, noiseModel::Isotropic::Sigma(2,sigma)));
fg->push_back(factor_out);
return *fg;
@ -393,8 +394,8 @@ inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() {
double sigma = 0.1;
auto gmNoise = noiseModel::Robust::Create(
noiseModel::mEstimator::GemanMcClure::Create(1.0), noiseModel::Isotropic::Sigma(2,sigma));
boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, gmNoise, X(1)));
boost::shared_ptr<PriorFactor<Point2>> factor(
new PriorFactor<Point2>(X(1), z, gmNoise));
// 3 noiseless inliers
fg->push_back(factor);
fg->push_back(factor);
@ -402,8 +403,8 @@ inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() {
// 1 outlier
Point2 z_out(1.0, 0.0);
boost::shared_ptr<smallOptimize::UnaryFactor> factor_out(
new smallOptimize::UnaryFactor(z_out, gmNoise, X(1)));
boost::shared_ptr<PriorFactor<Point2>> factor_out(
new PriorFactor<Point2>(X(1), z_out, gmNoise));
fg->push_back(factor_out);
return *fg;

6
tests/testGncOptimizer.cpp
View File

@ -367,20 +367,20 @@ TEST(GncOptimizer, optimize) {
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<Point2>(X(1)), Point2(1.31812,0.0), 1e-3));
CHECK(assert_equal(Point2(0.25,0.0), gn_results.at<Point2>(X(1)), 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<Point2>(X(1)), Point2(1.18712,0.0), 1e-3));
CHECK(assert_equal(Point2(0.999706,0.0), gn2_results.at<Point2>(X(1)), 1e-3));
// .. but graduated nonconvexity ensures both robustness and convergence in the face of nonconvexity
GncParams<GaussNewtonParams> gncParams(gnParams);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
Values gnc_result = gnc.optimize();
CHECK(assert_equal(gnc_result.at<Point2>(X(1)), Point2(0.0,0.0), 1e-3));
CHECK(assert_equal(Point2(0.0,0.0), gnc_result.at<Point2>(X(1)), 1e-3));
}
/* ************************************************************************* */