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

working unit tests: added chi2

release/4.3a0
lcarlone 2021-01-22 22:27:47 -05:00
parent fdced87b9a
commit 28b0f0ac8e
2 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/nonlinear/GncOptimizer.h
View File

@ -67,13 +67,6 @@ class GncOptimizer {
nfg_.resize(graph.size()); nfg_.resize(graph.size());
barcSq_ = Vector::Ones(graph.size()); barcSq_ = Vector::Ones(graph.size());
boost::math::chi_squared_distribution<double> chi2inv(3.0);
std::cout << "chi2inv.degrees_of_freedom() = " << chi2inv.degrees_of_freedom() << std::endl;
double a = boost::math::quantile(chi2inv, 0.997);
std::cout << " a " << a << std::endl;
double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_ double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
for (size_t i = 0; i < graph.size(); i++) { for (size_t i = 0; i < graph.size(); i++) {

5
tests/testGncOptimizer.cpp
View File

@ -128,6 +128,7 @@ TEST(GncOptimizer, initializeMu) {
gncParams.setLossType(GncLossType::GM); gncParams.setLossType(GncLossType::GM);
auto gnc_gm = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc_gm = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
gnc_gm.setInlierCostThresholds(1.0);
// according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq
// (barcSq=1 in this example) // (barcSq=1 in this example)
EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 2 * 198.999, 1e-3); EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 2 * 198.999, 1e-3);
@ -136,6 +137,7 @@ TEST(GncOptimizer, initializeMu) {
gncParams.setLossType(GncLossType::TLS); gncParams.setLossType(GncLossType::TLS);
auto gnc_tls = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, auto gnc_tls = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial,
gncParams); gncParams);
gnc_tls.setInlierCostThresholds(1.0);
// according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq) // according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq)
// (barcSq=1 in this example) // (barcSq=1 in this example)
EXPECT_DOUBLES_EQUAL(gnc_tls.initializeMu(), 1 / (2 * 198.999 - 1), 1e-3); EXPECT_DOUBLES_EQUAL(gnc_tls.initializeMu(), 1 / (2 * 198.999 - 1), 1e-3);
@ -339,6 +341,7 @@ TEST(GncOptimizer, calculateWeightsGM) {
GncParams<GaussNewtonParams> gncParams(gnParams); GncParams<GaussNewtonParams> gncParams(gnParams);
gncParams.setLossType(GncLossType::GM); gncParams.setLossType(GncLossType::GM);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams); auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
gnc.setInlierCostThresholds(1.0);
double mu = 1.0; double mu = 1.0;
Vector weights_actual = gnc.calculateWeights(initial, mu); Vector weights_actual = gnc.calculateWeights(initial, mu);
CHECK(assert_equal(weights_expected, weights_actual, tol)); CHECK(assert_equal(weights_expected, weights_actual, tol));
@ -550,7 +553,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
//gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY); //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams); gncParams);
gnc.setInlierCostThresholds(1.0);
Values gnc_result = gnc.optimize(); Values gnc_result = gnc.optimize();
CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3)); CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));