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

mu initialization test & minor formatting fixes

release/4.3a0
jingnanshi 2020-12-07 16:04:36 -05:00
parent 58e49fc0fc
commit d0a81f8441
1 changed files with 84 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
tests/testGncOptimizer.cpp
View File

@ -16,24 +16,27 @@
* @author Luca Carlone * @author Luca Carlone
* @author Frank Dellaert * @author Frank Dellaert
* *
* Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated
* From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) * Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to
* Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version:
* https://arxiv.org/pdf/1909.08605.pdf)
* *
* See also: * See also:
* Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications", * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness,
* arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020. * Minimally-Tuned Algorithms, and Applications", arxiv:
* https://arxiv.org/pdf/2007.15109.pdf, 2020.
*/ */
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/GncOptimizer.h>
#include <tests/smallExample.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/nonlinear/GncOptimizer.h>
#include <gtsam/slam/dataset.h>
#include <tests/smallExample.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
using symbol_shorthand::X;
static double tol = 1e-7; static double tol = 1e-7;
/* ************************************************************************* */ /* ************************************************************************* */
@ -69,7 +72,8 @@ TEST(GncOptimizer, gncParamsConstructor) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GncOptimizer, gncConstructor) { TEST(GncOptimizer, gncConstructor) {
// has to have Gaussian noise models ! // has to have Gaussian noise models !
auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor
// on a 2D point
Point2 p0(3, 3); Point2 p0(3, 3);
Values initial; Values initial;
@ -77,8 +81,8 @@ TEST(GncOptimizer, gncConstructor) {
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
GncParams<LevenbergMarquardtParams> gncParams(lmParams); GncParams<LevenbergMarquardtParams> gncParams(lmParams);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc =
gncParams); GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
CHECK(gnc.getFactors().equals(fg)); CHECK(gnc.getFactors().equals(fg));
CHECK(gnc.getState().equals(initial)); CHECK(gnc.getState().equals(initial));
@ -97,10 +101,11 @@ TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) {
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
GncParams<LevenbergMarquardtParams> gncParams(lmParams); GncParams<LevenbergMarquardtParams> gncParams(lmParams);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg_robust, auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(
initial, gncParams); fg_robust, initial, gncParams);
// make sure that when parsing the graph is transformed into one without robust loss // make sure that when parsing the graph is transformed into one without
// robust loss
CHECK(fg.equals(gnc.getFactors())); CHECK(fg.equals(gnc.getFactors()));
} }
@ -112,13 +117,25 @@ TEST(GncOptimizer, initializeMu) {
Values initial; Values initial;
initial.insert(X(1), p0); initial.insert(X(1), p0);
// testing GM mu initialization
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
GncParams<LevenbergMarquardtParams> gncParams(lmParams); GncParams<LevenbergMarquardtParams> gncParams(lmParams);
gncParams.setLossType( gncParams.setLossType(
GncParams<LevenbergMarquardtParams>::RobustLossType::GM); GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc_gm =
gncParams); GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
EXPECT_DOUBLES_EQUAL(gnc.initializeMu(), 2 * 198.999, 1e-3); // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq (barcSq=1 in this example) // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq
// (barcSq=1 in this example)
EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 2 * 198.999, 1e-3);
// testing TLS mu initialization
gncParams.setLossType(
GncParams<LevenbergMarquardtParams>::RobustLossType::TLS);
auto gnc_tls =
GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
// according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq)
// (barcSq=1 in this example)
EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 1 / (2 * 198.999 - 1), 1e-3);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -134,8 +151,8 @@ TEST(GncOptimizer, updateMu) {
GncParams<LevenbergMarquardtParams> gncParams(lmParams); GncParams<LevenbergMarquardtParams> gncParams(lmParams);
gncParams.setLossType( gncParams.setLossType(
GncParams<LevenbergMarquardtParams>::RobustLossType::GM); GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc =
gncParams); GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
double mu = 5.0; double mu = 5.0;
EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu / 1.4, tol); EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu / 1.4, tol);
@ -158,8 +175,8 @@ TEST(GncOptimizer, checkMuConvergence) {
GncParams<LevenbergMarquardtParams> gncParams(lmParams); GncParams<LevenbergMarquardtParams> gncParams(lmParams);
gncParams.setLossType( gncParams.setLossType(
GncParams<LevenbergMarquardtParams>::RobustLossType::GM); GncParams<LevenbergMarquardtParams>::RobustLossType::GM);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc =
gncParams); GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
double mu = 1.0; double mu = 1.0;
CHECK(gnc.checkMuConvergence(mu, 0)); CHECK(gnc.checkMuConvergence(mu, 0));
@ -175,7 +192,8 @@ TEST(GncOptimizer, calculateWeights) {
Values initial; Values initial;
initial.insert(X(1), p0); initial.insert(X(1), p0);
// we have 4 factors, 3 with zero errors (inliers), 1 with error 50 = 0.5 * 1/sigma^2 || [1;0] - [0;0] ||^2 (outlier) // we have 4 factors, 3 with zero errors (inliers), 1 with error 50 = 0.5 *
// 1/sigma^2 || [1;0] - [0;0] ||^2 (outlier)
Vector weights_expected = Vector::Zero(4); Vector weights_expected = Vector::Zero(4);
weights_expected[0] = 1.0; // zero error weights_expected[0] = 1.0; // zero error
weights_expected[1] = 1.0; // zero error weights_expected[1] = 1.0; // zero error
@ -191,10 +209,11 @@ TEST(GncOptimizer, calculateWeights) {
mu = 2.0; mu = 2.0;
double barcSq = 5.0; double barcSq = 5.0;
weights_expected[3] = std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50 weights_expected[3] =
std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50
gncParams.setInlierThreshold(barcSq); gncParams.setInlierThreshold(barcSq);
auto gnc2 = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, auto gnc2 =
gncParams); GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
weights_actual = gnc2.calculateWeights(initial, mu); weights_actual = gnc2.calculateWeights(initial, mu);
CHECK(assert_equal(weights_expected, weights_actual, tol)); CHECK(assert_equal(weights_expected, weights_actual, tol));
} }
@ -203,16 +222,17 @@ TEST(GncOptimizer, calculateWeights) {
TEST(GncOptimizer, makeWeightedGraph) { TEST(GncOptimizer, makeWeightedGraph) {
// create original factor // create original factor
double sigma1 = 0.1; double sigma1 = 0.1;
NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma( NonlinearFactorGraph nfg =
sigma1); example::nonlinearFactorGraphWithGivenSigma(sigma1);
// create expected // create expected
double sigma2 = 10; double sigma2 = 10;
NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma( NonlinearFactorGraph expected =
sigma2); example::nonlinearFactorGraphWithGivenSigma(sigma2);
// create weights // create weights
Vector weights = Vector::Ones(1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4 Vector weights = Vector::Ones(
1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4
weights[0] = 1e-4; weights[0] = 1e-4;
// create actual // create actual
@ -240,8 +260,8 @@ TEST(GncOptimizer, optimizeSimple) {
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
GncParams<LevenbergMarquardtParams> gncParams(lmParams); GncParams<LevenbergMarquardtParams> gncParams(lmParams);
auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc =
gncParams); GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
Values actual = gnc.optimize(); Values actual = gnc.optimize();
DOUBLES_EQUAL(0, fg.error(actual), tol); DOUBLES_EQUAL(0, fg.error(actual), tol);
@ -259,17 +279,23 @@ TEST(GncOptimizer, optimize) {
GaussNewtonParams gnParams; GaussNewtonParams gnParams;
GaussNewtonOptimizer gn(fg, initial, gnParams); GaussNewtonOptimizer gn(fg, initial, gnParams);
Values gn_results = gn.optimize(); Values gn_results = gn.optimize();
// converges to incorrect point due to lack of robustness to an outlier, ideal solution is Point2(0,0) // converges to incorrect point due to lack of robustness to an outlier, ideal
// solution is Point2(0,0)
CHECK(assert_equal(Point2(0.25, 0.0), gn_results.at<Point2>(X(1)), 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 // try with robust loss function and standard GN
auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with factors wrapped in Geman McClure losses auto fg_robust =
example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with
// factors wrapped in
// Geman McClure losses
GaussNewtonOptimizer gn2(fg_robust, initial, gnParams); GaussNewtonOptimizer gn2(fg_robust, initial, gnParams);
Values gn2_results = gn2.optimize(); Values gn2_results = gn2.optimize();
// converges to incorrect point, this time due to the nonconvexity of the loss // converges to incorrect point, this time due to the nonconvexity of the loss
CHECK(assert_equal(Point2(0.999706, 0.0), gn2_results.at<Point2>(X(1)), 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 // .. but graduated nonconvexity ensures both robustness and convergence in
// the face of nonconvexity
GncParams<GaussNewtonParams> gncParams(gnParams); GncParams<GaussNewtonParams> gncParams(gnParams);
// gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::SUMMARY); // gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::VerbosityGNC::SUMMARY);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams); auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
@ -315,31 +341,38 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
boost::tie(graph, initial) = load2D(filename); boost::tie(graph, initial) = load2D(filename);
// Add a Gaussian prior on first poses // Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); SharedDiagonal priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
graph->addPrior(0, priorMean, priorNoise); graph->addPrior(0, priorMean, priorNoise);
/// get expected values by optimizing outlier-free graph /// get expected values by optimizing outlier-free graph
Values expected = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); Values expected = LevenbergMarquardtOptimizer(*graph, *initial).optimize();
// add a few outliers // add a few outliers
SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.01)); SharedDiagonal betweenNoise =
graph->push_back( BetweenFactor<Pose2>(90 , 50 , Pose2(), betweenNoise) ); // some arbitrary and incorrect between factor noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.01));
graph->push_back(BetweenFactor<Pose2>(
90, 50, Pose2(),
betweenNoise)); // some arbitrary and incorrect between factor
/// get expected values by optimizing outlier-free graph /// get expected values by optimizing outlier-free graph
Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); Values expectedWithOutliers =
LevenbergMarquardtOptimizer(*graph, *initial).optimize();
// as expected, the following test fails due to the presence of an outlier! // as expected, the following test fails due to the presence of an outlier!
// CHECK(assert_equal(expected, expectedWithOutliers, 1e-3)); // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3));
// GNC // GNC
// Note: in difficult instances, we set the odometry measurements to be inliers, // Note: in difficult instances, we set the odometry measurements to be
// but this problem is simple enought to succeed even without that assumption // inliers, but this problem is simple enought to succeed even without that
// std::vector<size_t> knownInliers; // assumption std::vector<size_t> knownInliers;
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial, gncParams); auto gnc =
GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial, gncParams);
Values actual = gnc.optimize(); Values actual = gnc.optimize();
// compare // compare
CHECK(assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers! CHECK(
assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers!
} }
/* ************************************************************************* */ /* ************************************************************************* */