From 6aed1685ed3b8641f3c693f31734b21d07ee5719 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 24 Sep 2020 15:44:29 -0400 Subject: [PATCH 01/30] adding robust cost function - version 1 --- gtsam/sfm/BinaryMeasurement.h | 7 +++- gtsam/sfm/ShonanAveraging.cpp | 47 ++++++++++++++++++------- gtsam/sfm/ShonanAveraging.h | 13 +++++++ gtsam/sfm/tests/testShonanAveraging.cpp | 24 +++++++++++++ 4 files changed, 78 insertions(+), 13 deletions(-) diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index c525c1b9e6..9540564e0d 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -71,6 +71,11 @@ template class BinaryMeasurement : public Factor { this->noiseModel_->print(" noise model: "); } + void makeNoiseModelRobust(){ + this->noiseModel_ = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_); + } + bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const { const BinaryMeasurement *e = dynamic_cast *>(&expected); @@ -80,4 +85,4 @@ template class BinaryMeasurement : public Factor { } /// @} }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index df2d72c289..e993283023 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -53,7 +53,8 @@ ShonanAveragingParameters::ShonanAveragingParameters( optimalityThreshold(optimalityThreshold), alpha(alpha), beta(beta), - gamma(gamma) { + gamma(gamma), + useHuber(false){ // By default, we will do conjugate gradient lm.linearSolverType = LevenbergMarquardtParams::Iterative; @@ -819,9 +820,15 @@ template class ShonanAveraging<2>; ShonanAveraging2::ShonanAveraging2(const Measurements &measurements, const Parameters ¶meters) - : ShonanAveraging<2>(measurements, parameters) {} + : ShonanAveraging<2>(measurements, parameters) { + if (parameters.useHuber == true) + std::cout << "Parameter useHuber is disregarded when you pass measurements to ShonanAveraging2." + "Pass g2o file as input to enable this functionality" << std::endl; +} + ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<2>(parseMeasurements(g2oFile), parameters) {} + : ShonanAveraging<2>(parseMeasurements(g2oFile, + parameters.useHuber? nullptr : nullptr), parameters) {} /* ************************************************************************* */ // Explicit instantiation for d=3 @@ -829,17 +836,22 @@ template class ShonanAveraging<3>; ShonanAveraging3::ShonanAveraging3(const Measurements &measurements, const Parameters ¶meters) - : ShonanAveraging<3>(measurements, parameters) {} + : ShonanAveraging<3>(measurements, parameters) { + if (parameters.useHuber == true) + std::cout << "Parameter useHuber is disregarded when you pass measurements to ShonanAveraging2." + "Pass g2o file as input to enable this functionality" << std::endl; +} ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<3>(parseMeasurements(g2oFile), parameters) {} + : ShonanAveraging<3>(parseMeasurements(g2oFile, + parameters.useHuber? nullptr : nullptr), parameters) {} // TODO(frank): Deprecate after we land pybind wrapper // Extract Rot3 measurement from Pose3 betweenfactors // Modeled after similar function in dataset.cpp static BinaryMeasurement convert( - const BetweenFactor::shared_ptr &f) { + const BetweenFactor::shared_ptr &f, bool useHuber = false) { auto gaussian = boost::dynamic_pointer_cast(f->noiseModel()); if (!gaussian) @@ -847,22 +859,33 @@ static BinaryMeasurement convert( "parseMeasurements can only convert Pose3 measurements " "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); - return BinaryMeasurement( - f->key1(), f->key2(), f->measured().rotation(), - noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); + if(!useHuber){ + return BinaryMeasurement( + f->key1(), f->key2(), f->measured().rotation(), + noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); + }else{ // wrap noise mode in Huber loss + std::cout << "setting robust huber loss " << std::endl; + return BinaryMeasurement( + f->key1(), f->key2(), f->measured().rotation(), + noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), + noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true))); + } } static ShonanAveraging3::Measurements extractRot3Measurements( - const BetweenFactorPose3s &factors) { + const BetweenFactorPose3s &factors, bool useHuber = false) { ShonanAveraging3::Measurements result; result.reserve(factors.size()); - for (auto f : factors) result.push_back(convert(f)); + for (auto f : factors) result.push_back(convert(f,useHuber)); return result; } ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors, const Parameters ¶meters) - : ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {} + : ShonanAveraging<3>(parameters.useHuber? + extractRot3Measurements(factors) : + extractRot3Measurements(factors), parameters) {} /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index edd9f33a22..9718805b8a 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -53,6 +53,7 @@ struct GTSAM_EXPORT ShonanAveragingParameters { double alpha; // weight of anchor-based prior (default 0) double beta; // weight of Karcher-based prior (default 1) double gamma; // weight of gauge-fixing factors (default 0) + bool useHuber; // if enabled, the Huber loss is used in the optimization (default is false) ShonanAveragingParameters(const LevenbergMarquardtParams &lm = LevenbergMarquardtParams::CeresDefaults(), @@ -77,6 +78,18 @@ struct GTSAM_EXPORT ShonanAveragingParameters { void setGaugesWeight(double value) { gamma = value; } double getGaugesWeight() { return gamma; } + + void setUseHuber(bool value) { useHuber = value; } + bool getUseHuber() { return useHuber; } + + void print() const { + std::cout << " ShonanAveragingParameters: " << std::endl; + std::cout << " alpha: " << alpha << std::endl; + std::cout << " beta: " << beta << std::endl; + std::cout << " gamma: " << gamma << std::endl; + std::cout << " useHuber: " << useHuber << std::endl; + std::cout << " --------------------------" << std::endl; + } }; using ShonanAveragingParameters2 = ShonanAveragingParameters<2>; diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index 1200c8ebb8..e23f9e20b9 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -321,6 +321,30 @@ TEST(ShonanAveraging2, noisyToyGraph) { EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! } +/* ************************************************************************* */ +TEST(ShonanAveraging2, noisyToyGraphWithHuber) { + // Load 2D toy example + auto lmParams = LevenbergMarquardtParams::CeresDefaults(); + string g2oFile = findExampleDataFile("noisyToyGraph.txt"); + ShonanAveraging2::Parameters parameters(lmParams); + auto measurements = parseMeasurements(g2oFile); + std::cout << "----- changing huber before " << std::endl; + parameters.setUseHuber(true); + parameters.print(); + std::cout << "----- changing huber after " << std::endl; + ShonanAveraging2 shonan(measurements, parameters); + EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns()); + + // Check graph building + NonlinearFactorGraph graph = shonan.buildGraphAt(2); + graph.print(); + EXPECT_LONGS_EQUAL(6, graph.size()); + auto initial = shonan.initializeRandomly(kRandomNumberGenerator); + auto result = shonan.run(initial, 2); + EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); + EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! +} + /* ************************************************************************* */ // Test alpha/beta/gamma prior weighting. TEST(ShonanAveraging3, PriorWeights) { From 001a55ad3a53fcd97c07dae4fee66225b29012d1 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 24 Sep 2020 16:35:41 -0400 Subject: [PATCH 02/30] robust noise in place - test fails due to non-isotropic covariance? --- gtsam/sfm/ShonanAveraging.cpp | 47 +++++++++---------------- gtsam/sfm/ShonanAveraging.h | 9 +++++ gtsam/sfm/tests/testShonanAveraging.cpp | 2 -- 3 files changed, 26 insertions(+), 32 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index e993283023..1e6fcf6daf 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -820,15 +820,13 @@ template class ShonanAveraging<2>; ShonanAveraging2::ShonanAveraging2(const Measurements &measurements, const Parameters ¶meters) - : ShonanAveraging<2>(measurements, parameters) { - if (parameters.useHuber == true) - std::cout << "Parameter useHuber is disregarded when you pass measurements to ShonanAveraging2." - "Pass g2o file as input to enable this functionality" << std::endl; -} + : ShonanAveraging<2>(parameters.useHuber? + makeNoiseModelRobust(measurements) : measurements, parameters) {} ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<2>(parseMeasurements(g2oFile, - parameters.useHuber? nullptr : nullptr), parameters) {} + : ShonanAveraging<2>(parameters.useHuber? + makeNoiseModelRobust( parseMeasurements(g2oFile) ) : + parseMeasurements(g2oFile), parameters) {} /* ************************************************************************* */ // Explicit instantiation for d=3 @@ -836,22 +834,20 @@ template class ShonanAveraging<3>; ShonanAveraging3::ShonanAveraging3(const Measurements &measurements, const Parameters ¶meters) - : ShonanAveraging<3>(measurements, parameters) { - if (parameters.useHuber == true) - std::cout << "Parameter useHuber is disregarded when you pass measurements to ShonanAveraging2." - "Pass g2o file as input to enable this functionality" << std::endl; -} + : ShonanAveraging<3>(parameters.useHuber? + makeNoiseModelRobust(measurements) : measurements, parameters) {} ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<3>(parseMeasurements(g2oFile, - parameters.useHuber? nullptr : nullptr), parameters) {} + : ShonanAveraging<3>(parameters.useHuber? + makeNoiseModelRobust( parseMeasurements(g2oFile) ) : + parseMeasurements(g2oFile), parameters) {} // TODO(frank): Deprecate after we land pybind wrapper // Extract Rot3 measurement from Pose3 betweenfactors // Modeled after similar function in dataset.cpp static BinaryMeasurement convert( - const BetweenFactor::shared_ptr &f, bool useHuber = false) { + const BetweenFactor::shared_ptr &f) { auto gaussian = boost::dynamic_pointer_cast(f->noiseModel()); if (!gaussian) @@ -859,32 +855,23 @@ static BinaryMeasurement convert( "parseMeasurements can only convert Pose3 measurements " "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); - if(!useHuber){ - return BinaryMeasurement( - f->key1(), f->key2(), f->measured().rotation(), - noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); - }else{ // wrap noise mode in Huber loss - std::cout << "setting robust huber loss " << std::endl; - return BinaryMeasurement( - f->key1(), f->key2(), f->measured().rotation(), - noiseModel::Robust::Create( - noiseModel::mEstimator::Huber::Create(1.345), - noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true))); - } + return BinaryMeasurement( + f->key1(), f->key2(), f->measured().rotation(), + noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); } static ShonanAveraging3::Measurements extractRot3Measurements( - const BetweenFactorPose3s &factors, bool useHuber = false) { + const BetweenFactorPose3s &factors) { ShonanAveraging3::Measurements result; result.reserve(factors.size()); - for (auto f : factors) result.push_back(convert(f,useHuber)); + for (auto f : factors) result.push_back(convert(f)); return result; } ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors, const Parameters ¶meters) : ShonanAveraging<3>(parameters.useHuber? - extractRot3Measurements(factors) : + makeNoiseModelRobust( extractRot3Measurements(factors) ): extractRot3Measurements(factors), parameters) {} /* ************************************************************************* */ diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 9718805b8a..6efcb045b3 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -164,6 +164,15 @@ class GTSAM_EXPORT ShonanAveraging { return measurements_[k]; } + /// wrap factors with robust Huber loss + static Measurements makeNoiseModelRobust(Measurements measurements){ + Measurements robustMeasurements = measurements; + for (auto &measurement : robustMeasurements) { + measurement.makeNoiseModelRobust(); + } + return robustMeasurements; + } + /// k^th measurement, as a Rot. const Rot &measured(size_t k) const { return measurements_[k].measured(); } diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index e23f9e20b9..ae24094dee 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -328,10 +328,8 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { string g2oFile = findExampleDataFile("noisyToyGraph.txt"); ShonanAveraging2::Parameters parameters(lmParams); auto measurements = parseMeasurements(g2oFile); - std::cout << "----- changing huber before " << std::endl; parameters.setUseHuber(true); parameters.print(); - std::cout << "----- changing huber after " << std::endl; ShonanAveraging2 shonan(measurements, parameters); EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns()); From 73600c8faaa830d3ca5db3dbbd42ab2048028323 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 24 Sep 2020 17:06:20 -0400 Subject: [PATCH 03/30] solving issue with robust model --- gtsam/sfm/ShonanAveraging.cpp | 26 +++++++++++++++++-------- gtsam/sfm/tests/testShonanAveraging.cpp | 2 +- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 1e6fcf6daf..8fcfbeb269 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -337,14 +337,24 @@ double ShonanAveraging::cost(const Values &values) const { // Get kappa from noise model template static double Kappa(const BinaryMeasurement &measurement) { - const auto &isotropic = boost::dynamic_pointer_cast( - measurement.noiseModel()); - if (!isotropic) { - throw std::invalid_argument( - "Shonan averaging noise models must be isotropic."); - } - const double sigma = isotropic->sigma(); - return 1.0 / (sigma * sigma); + const auto &isotropic = boost::dynamic_pointer_cast( + measurement.noiseModel()); + double sigma; + if (isotropic) { + sigma = isotropic->sigma(); + } else{ + const auto &robust = boost::dynamic_pointer_cast( + measurement.noiseModel()); + if (robust) { + std::cout << "Verification of optimality does not work with robust cost function" << std::endl; + sigma = 1; // setting arbitrary value + }else{ + throw std::invalid_argument( + "Shonan averaging noise models must be isotropic (but robust losses are allowed)."); + + } + } + return 1.0 / (sigma * sigma); } /* ************************************************************************* */ diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index ae24094dee..e2fa9c22a9 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -338,7 +338,7 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { graph.print(); EXPECT_LONGS_EQUAL(6, graph.size()); auto initial = shonan.initializeRandomly(kRandomNumberGenerator); - auto result = shonan.run(initial, 2); + auto result = shonan.run(initial, 2, 3); EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! } From 564e623f44ff1f85fb45227a5f2765f09fef4673 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 24 Sep 2020 18:24:23 -0400 Subject: [PATCH 04/30] attempting robustification in Frobenius factor --- gtsam/sfm/ShonanAveraging.cpp | 34 +++++++++++++++++++++---- gtsam/sfm/tests/testShonanAveraging.cpp | 18 ++++++++----- gtsam/slam/FrobeniusFactor.cpp | 19 +++++++++++--- 3 files changed, 57 insertions(+), 14 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 8fcfbeb269..0ac893b7c4 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -139,13 +139,21 @@ ShonanAveraging::ShonanAveraging(const Measurements &measurements, /* ************************************************************************* */ template NonlinearFactorGraph ShonanAveraging::buildGraphAt(size_t p) const { - NonlinearFactorGraph graph; + std::cout << "zz0" << std::endl; + NonlinearFactorGraph graph; auto G = boost::make_shared(SO<-1>::VectorizedGenerators(p)); + + std::cout << "zz1" << std::endl; for (const auto &measurement : measurements_) { const auto &keys = measurement.keys(); const auto &Rij = measurement.measured(); const auto &model = measurement.noiseModel(); + measurement.print("measurement"); + std::cout << "zzzz1" << std::endl; + model->print(); + std::cout << "zzzz2" << std::endl; graph.emplace_shared>(keys[0], keys[1], Rij, p, model, G); + std::cout << "zzzz3" << std::endl; } // Possibly add Karcher prior @@ -153,13 +161,13 @@ NonlinearFactorGraph ShonanAveraging::buildGraphAt(size_t p) const { const size_t dim = SOn::Dimension(p); graph.emplace_shared>(graph.keys(), dim); } - + std::cout << "zz2" << std::endl; // Possibly add gauge factors - they are probably useless as gradient is zero if (parameters_.gamma > 0 && p > d + 1) { for (auto key : graph.keys()) graph.emplace_shared(key, p, d, parameters_.gamma); } - + std::cout << "z3" << std::endl; return graph; } @@ -177,6 +185,7 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { // Build graph NonlinearFactorGraph graph = buildGraphAt(p); + std::cout << "yy1" << std::endl; // Anchor prior is added here as depends on initial value (and cost is zero) if (parameters_.alpha > 0) { size_t i; @@ -187,7 +196,7 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { graph.emplace_shared>(i, SOn::Lift(p, value.matrix()), model); } - + std::cout << "yy2" << std::endl; // Optimize return boost::make_shared(graph, initial, parameters_.lm); @@ -197,7 +206,9 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { template Values ShonanAveraging::tryOptimizingAt(size_t p, const Values &initial) const { - auto lm = createOptimizerAt(p, initial); + std::cout << "xx1" << std::endl; + auto lm = createOptimizerAt(p, initial); + std::cout << "xx2" << std::endl; return lm->optimize(); } @@ -803,17 +814,29 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, Values initialSOp = LiftTo(pMin, initialEstimate); // lift to pMin! for (size_t p = pMin; p <= pMax; p++) { // Optimize until convergence at this level + std::cout << "4a" << std::endl; Qstar = tryOptimizingAt(p, initialSOp); + std::cout << "4aa" << std::endl; + if(parameters_.useHuber){ // in this case, there is no optimality verification + std::cout << "4aaa" << std::endl; + if(pMin!=pMax) + std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl; + const Values SO3Values = roundSolution(Qstar); + return std::make_pair(SO3Values, 0); + } + std::cout << "4b" << std::endl; // Check certificate of global optimzality Vector minEigenVector; double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); + std::cout << "4bb" << std::endl; if (minEigenValue > parameters_.optimalityThreshold) { // If at global optimum, round and return solution const Values SO3Values = roundSolution(Qstar); return std::make_pair(SO3Values, minEigenValue); } + std::cout << "4c" << std::endl; // Not at global optimimum yet, so check whether we will go to next level if (p != pMax) { // Calculate initial estimate for next level by following minEigenVector @@ -821,6 +844,7 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue); } } + std::cout << "4d" << std::endl; throw std::runtime_error("Shonan::run did not converge for given pMax"); } diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index e2fa9c22a9..fd4d5e34fb 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -330,17 +330,23 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { auto measurements = parseMeasurements(g2oFile); parameters.setUseHuber(true); parameters.print(); + std::cout << "1" << std::endl; ShonanAveraging2 shonan(measurements, parameters); EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns()); // Check graph building - NonlinearFactorGraph graph = shonan.buildGraphAt(2); - graph.print(); - EXPECT_LONGS_EQUAL(6, graph.size()); + std::cout << "2" << std::endl; +// NonlinearFactorGraph graph = shonan.buildGraphAt(2); +// graph.print(); +// EXPECT_LONGS_EQUAL(6, graph.size()); + std::cout << "3" << std::endl; + auto initial = shonan.initializeRandomly(kRandomNumberGenerator); - auto result = shonan.run(initial, 2, 3); - EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); - EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! + std::cout << "4" << std::endl; + auto result = shonan.run(initial, 2,3); + std::cout << "5" << std::endl; +// EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); +// EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! } /* ************************************************************************* */ diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index 5697a0cd61..7af4958e82 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -26,8 +26,15 @@ namespace gtsam { boost::shared_ptr ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { double sigma = 1.0; + std::cout << "111111" << std::endl; if (model != nullptr) { - auto sigmas = model->sigmas(); + const auto &robust = boost::dynamic_pointer_cast(model); + Vector sigmas; + if(robust) + sigmas[0] = 1; + else + sigmas = model->sigmas(); + size_t n = sigmas.size(); if (n == 1) { sigma = sigmas(0); // Rot2 @@ -46,8 +53,14 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { throw std::runtime_error("Can only convert Pose2/Pose3 noise models"); } } -exit: - return noiseModel::Isotropic::Sigma(d, sigma); + exit: + auto isoModel = noiseModel::Isotropic::Sigma(d, sigma); + const auto &robust = boost::dynamic_pointer_cast(model); + if(robust) + return noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), isoModel); + else + return isoModel; } //****************************************************************************** From 8be6d33714136a120c8d9b4faf33557b35e0eb0d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 24 Sep 2020 19:10:14 -0400 Subject: [PATCH 05/30] added nice unit test --- gtsam/sfm/ShonanAveraging.cpp | 21 --------------------- gtsam/sfm/tests/testShonanAveraging.cpp | 22 ++++++++++++---------- gtsam/slam/FrobeniusFactor.cpp | 12 ++++++------ gtsam/slam/FrobeniusFactor.h | 2 +- 4 files changed, 19 insertions(+), 38 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 0ac893b7c4..14f5665b64 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -139,35 +139,25 @@ ShonanAveraging::ShonanAveraging(const Measurements &measurements, /* ************************************************************************* */ template NonlinearFactorGraph ShonanAveraging::buildGraphAt(size_t p) const { - std::cout << "zz0" << std::endl; NonlinearFactorGraph graph; auto G = boost::make_shared(SO<-1>::VectorizedGenerators(p)); - std::cout << "zz1" << std::endl; for (const auto &measurement : measurements_) { const auto &keys = measurement.keys(); const auto &Rij = measurement.measured(); const auto &model = measurement.noiseModel(); - measurement.print("measurement"); - std::cout << "zzzz1" << std::endl; - model->print(); - std::cout << "zzzz2" << std::endl; graph.emplace_shared>(keys[0], keys[1], Rij, p, model, G); - std::cout << "zzzz3" << std::endl; } - // Possibly add Karcher prior if (parameters_.beta > 0) { const size_t dim = SOn::Dimension(p); graph.emplace_shared>(graph.keys(), dim); } - std::cout << "zz2" << std::endl; // Possibly add gauge factors - they are probably useless as gradient is zero if (parameters_.gamma > 0 && p > d + 1) { for (auto key : graph.keys()) graph.emplace_shared(key, p, d, parameters_.gamma); } - std::cout << "z3" << std::endl; return graph; } @@ -185,7 +175,6 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { // Build graph NonlinearFactorGraph graph = buildGraphAt(p); - std::cout << "yy1" << std::endl; // Anchor prior is added here as depends on initial value (and cost is zero) if (parameters_.alpha > 0) { size_t i; @@ -196,7 +185,6 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { graph.emplace_shared>(i, SOn::Lift(p, value.matrix()), model); } - std::cout << "yy2" << std::endl; // Optimize return boost::make_shared(graph, initial, parameters_.lm); @@ -206,9 +194,7 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { template Values ShonanAveraging::tryOptimizingAt(size_t p, const Values &initial) const { - std::cout << "xx1" << std::endl; auto lm = createOptimizerAt(p, initial); - std::cout << "xx2" << std::endl; return lm->optimize(); } @@ -814,29 +800,23 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, Values initialSOp = LiftTo(pMin, initialEstimate); // lift to pMin! for (size_t p = pMin; p <= pMax; p++) { // Optimize until convergence at this level - std::cout << "4a" << std::endl; Qstar = tryOptimizingAt(p, initialSOp); - std::cout << "4aa" << std::endl; if(parameters_.useHuber){ // in this case, there is no optimality verification - std::cout << "4aaa" << std::endl; if(pMin!=pMax) std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl; const Values SO3Values = roundSolution(Qstar); return std::make_pair(SO3Values, 0); } - std::cout << "4b" << std::endl; // Check certificate of global optimzality Vector minEigenVector; double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); - std::cout << "4bb" << std::endl; if (minEigenValue > parameters_.optimalityThreshold) { // If at global optimum, round and return solution const Values SO3Values = roundSolution(Qstar); return std::make_pair(SO3Values, minEigenValue); } - std::cout << "4c" << std::endl; // Not at global optimimum yet, so check whether we will go to next level if (p != pMax) { // Calculate initial estimate for next level by following minEigenVector @@ -844,7 +824,6 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue); } } - std::cout << "4d" << std::endl; throw std::runtime_error("Shonan::run did not converge for given pMax"); } diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index fd4d5e34fb..269b2c855a 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -330,23 +330,25 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { auto measurements = parseMeasurements(g2oFile); parameters.setUseHuber(true); parameters.print(); - std::cout << "1" << std::endl; ShonanAveraging2 shonan(measurements, parameters); EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns()); // Check graph building - std::cout << "2" << std::endl; -// NonlinearFactorGraph graph = shonan.buildGraphAt(2); -// graph.print(); -// EXPECT_LONGS_EQUAL(6, graph.size()); - std::cout << "3" << std::endl; + NonlinearFactorGraph graph = shonan.buildGraphAt(2); + EXPECT_LONGS_EQUAL(6, graph.size()); + + // test that each factor is actually robust + for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust + const auto &robust = boost::dynamic_pointer_cast( + boost::dynamic_pointer_cast(graph[i])->noiseModel()); + EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber) + } + // test result auto initial = shonan.initializeRandomly(kRandomNumberGenerator); - std::cout << "4" << std::endl; auto result = shonan.run(initial, 2,3); - std::cout << "5" << std::endl; -// EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); -// EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! + EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); + EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! } /* ************************************************************************* */ diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index 7af4958e82..2d74172837 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -23,18 +23,18 @@ using namespace std; namespace gtsam { //****************************************************************************** -boost::shared_ptr +SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { double sigma = 1.0; - std::cout << "111111" << std::endl; if (model != nullptr) { const auto &robust = boost::dynamic_pointer_cast(model); Vector sigmas; - if(robust) - sigmas[0] = 1; - else - sigmas = model->sigmas(); + if(robust){ + sigma = 1; // Rot2 + goto exit; + } //else: + sigmas = model->sigmas(); size_t n = sigmas.size(); if (n == 1) { sigma = sigmas(0); // Rot2 diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 1fc37c7852..9915a617d4 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -34,7 +34,7 @@ namespace gtsam { * isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an * error. If defaultToUnit == false throws an exception on unexepcted input. */ -GTSAM_EXPORT boost::shared_ptr +GTSAM_EXPORT SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t n, bool defaultToUnit = true); From 8cf3bc5059ad9485c35a30e2b7dc0c8d93a04dc4 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 24 Sep 2020 19:11:19 -0400 Subject: [PATCH 06/30] improved test --- gtsam/sfm/tests/testShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index 269b2c855a..9242b94a30 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -346,7 +346,7 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { // test result auto initial = shonan.initializeRandomly(kRandomNumberGenerator); - auto result = shonan.run(initial, 2,3); + auto result = shonan.run(initial, 2,2); EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6); EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate! } From 3734039bf5c51291ac7efc2f4f85d9da98f59b6b Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 26 Sep 2020 16:24:34 -0400 Subject: [PATCH 07/30] added check and unit test --- gtsam/sfm/BinaryMeasurement.h | 5 ++++- gtsam/sfm/tests/testBinaryMeasurement.cpp | 22 ++++++++++++++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index 9540564e0d..ef3fff70d0 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -71,8 +71,11 @@ template class BinaryMeasurement : public Factor { this->noiseModel_->print(" noise model: "); } + // TODO: make this more general? void makeNoiseModelRobust(){ - this->noiseModel_ = noiseModel::Robust::Create( + const auto &robust = boost::dynamic_pointer_cast(this->noiseModel_); + if(!robust) // make robust + this->noiseModel_ = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_); } diff --git a/gtsam/sfm/tests/testBinaryMeasurement.cpp b/gtsam/sfm/tests/testBinaryMeasurement.cpp index 3dd81c2c1f..a6a75b4ff1 100644 --- a/gtsam/sfm/tests/testBinaryMeasurement.cpp +++ b/gtsam/sfm/tests/testBinaryMeasurement.cpp @@ -36,6 +36,7 @@ static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05)); const Unit3 unit3Measured(Vector3(1, 1, 1)); const Rot3 rot3Measured; +/* ************************************************************************* */ TEST(BinaryMeasurement, Unit3) { BinaryMeasurement unit3Measurement(kKey1, kKey2, unit3Measured, unit3_model); @@ -48,6 +49,7 @@ TEST(BinaryMeasurement, Unit3) { EXPECT(unit3Measurement.equals(unit3MeasurementCopy)); } +/* ************************************************************************* */ TEST(BinaryMeasurement, Rot3) { // testing the accessors BinaryMeasurement rot3Measurement(kKey1, kKey2, rot3Measured, @@ -62,6 +64,26 @@ TEST(BinaryMeasurement, Rot3) { EXPECT(rot3Measurement.equals(rot3MeasurementCopy)); } +/* ************************************************************************* */ +TEST(BinaryMeasurement, Rot3MakeRobust) { + BinaryMeasurement rot3Measurement(kKey1, kKey2, rot3Measured, + rot3_model); + rot3Measurement.makeNoiseModelRobust(); + + EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1); + EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2); + EXPECT(rot3Measurement.measured().equals(rot3Measured)); + const auto &robust = boost::dynamic_pointer_cast( + rot3Measurement.noiseModel()); + EXPECT(robust); + + // test that if we call it again nothing changes: + rot3Measurement.makeNoiseModelRobust(); + const auto &robust2 = boost::dynamic_pointer_cast( + rot3Measurement.noiseModel()); + EXPECT(robust2); +} + /* ************************************************************************* */ int main() { TestResult tr; From 6567422ec54df3597ec0087eac27732fd3b191f9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 26 Sep 2020 19:06:55 -0400 Subject: [PATCH 08/30] added control over minimum rank in ShonanAveraging example, and resolved hard-coded sigma in FrobeniusFactor --- examples/ShonanAveragingCLI.cpp | 24 ++++++++++++++++++------ gtsam/slam/FrobeniusFactor.cpp | 8 ++++---- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/examples/ShonanAveragingCLI.cpp b/examples/ShonanAveragingCLI.cpp index 09221fda26..3ecb809aef 100644 --- a/examples/ShonanAveragingCLI.cpp +++ b/examples/ShonanAveragingCLI.cpp @@ -25,6 +25,8 @@ * Read 3D dataset sphere25000.txt and output to shonan.g2o (default) * ./ShonanAveragingCLI -i spere2500.txt * + * If you prefer using a robust Huber loss, you can add the option "-h true", for instance" + * ./ShonanAveragingCLI -i spere2500.txt -u true */ #include @@ -43,7 +45,8 @@ int main(int argc, char* argv[]) { string datasetName; string inputFile; string outputFile; - int d, seed; + int d, seed, pMin; + bool useHuberLoss; po::options_description desc( "Shonan Rotation Averaging CLI reads a *pose* graph, extracts the " "rotation constraints, and runs the Shonan algorithm."); @@ -58,6 +61,10 @@ int main(int argc, char* argv[]) { "Write solution to the specified file")( "dimension,d", po::value(&d)->default_value(3), "Optimize over 2D or 3D rotations")( + "useHuberLoss,h", po::value(&useHuberLoss)->default_value(false), + "set True to use Huber loss")( + "pMin,p", po::value(&pMin)->default_value(3), + "set to use desired rank pMin")( "seed,s", po::value(&seed)->default_value(42), "Random seed for initial estimate"); po::variables_map vm; @@ -85,11 +92,14 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph::shared_ptr inputGraph; Values::shared_ptr posesInFile; Values poses; + auto lmParams = LevenbergMarquardtParams::CeresDefaults(); if (d == 2) { cout << "Running Shonan averaging for SO(2) on " << inputFile << endl; - ShonanAveraging2 shonan(inputFile); + ShonanAveraging2::Parameters parameters(lmParams); + parameters.setUseHuber(useHuberLoss); + ShonanAveraging2 shonan(inputFile,parameters); auto initial = shonan.initializeRandomly(rng); - auto result = shonan.run(initial); + auto result = shonan.run(initial,pMin); // Parse file again to set up translation problem, adding a prior boost::tie(inputGraph, posesInFile) = load2D(inputFile); @@ -101,9 +111,11 @@ int main(int argc, char* argv[]) { poses = initialize::computePoses(result.first, &poseGraph); } else if (d == 3) { cout << "Running Shonan averaging for SO(3) on " << inputFile << endl; - ShonanAveraging3 shonan(inputFile); + ShonanAveraging3::Parameters parameters(lmParams); + parameters.setUseHuber(useHuberLoss); + ShonanAveraging3 shonan(inputFile,parameters); auto initial = shonan.initializeRandomly(rng); - auto result = shonan.run(initial); + auto result = shonan.run(initial,pMin); // Parse file again to set up translation problem, adding a prior boost::tie(inputGraph, posesInFile) = load3D(inputFile); @@ -118,7 +130,7 @@ int main(int argc, char* argv[]) { return 1; } cout << "Writing result to " << outputFile << endl; - writeG2o(NonlinearFactorGraph(), poses, outputFile); + writeG2o(*inputGraph, poses, outputFile); return 0; } diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index 2d74172837..5806fcfdb2 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -30,11 +30,11 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { const auto &robust = boost::dynamic_pointer_cast(model); Vector sigmas; if(robust){ - sigma = 1; // Rot2 - goto exit; - } //else: + sigmas = robust->noise()->sigmas(); + } else{ + sigmas = model->sigmas(); + } - sigmas = model->sigmas(); size_t n = sigmas.size(); if (n == 1) { sigma = sigmas(0); // Rot2 From 455f81dfc5cee89feb39cb0f2fee73913d790201 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 30 Sep 2020 16:07:45 -0400 Subject: [PATCH 09/30] reverted changes to cproject and language settings --- .cproject | 21 ++++++++++++++++++ .settings/language.settings.xml | 39 --------------------------------- 2 files changed, 21 insertions(+), 39 deletions(-) delete mode 100644 .settings/language.settings.xml diff --git a/.cproject b/.cproject index 7999522076..6c6a678250 100644 --- a/.cproject +++ b/.cproject @@ -344,4 +344,25 @@ + + + + + make + testShonanAveraging.run + testShonanAveraging + true + false + true + + + make + testBinaryMeasurement.run + testBinaryMeasurement + true + false + true + + + diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml deleted file mode 100644 index c6559f58f2..0000000000 --- a/.settings/language.settings.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 15060011669882f9a9065ce352369d669c4e3c9f Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 30 Sep 2020 16:10:27 -0400 Subject: [PATCH 10/30] fixed typo --- examples/ShonanAveragingCLI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ShonanAveragingCLI.cpp b/examples/ShonanAveragingCLI.cpp index 3ecb809aef..322622228b 100644 --- a/examples/ShonanAveragingCLI.cpp +++ b/examples/ShonanAveragingCLI.cpp @@ -25,7 +25,7 @@ * Read 3D dataset sphere25000.txt and output to shonan.g2o (default) * ./ShonanAveragingCLI -i spere2500.txt * - * If you prefer using a robust Huber loss, you can add the option "-h true", for instance" + * If you prefer using a robust Huber loss, you can add the option "-h true", for instance * ./ShonanAveragingCLI -i spere2500.txt -u true */ From fa26cf85abb481540c61ace4ad435a2ca2d0cc02 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 30 Sep 2020 16:13:51 -0400 Subject: [PATCH 11/30] reverted changes to cproject --- .cproject | 450 ++++++++++-------------------------------------------- 1 file changed, 82 insertions(+), 368 deletions(-) diff --git a/.cproject b/.cproject index 6c6a678250..9589ace566 100644 --- a/.cproject +++ b/.cproject @@ -1,368 +1,82 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - make - testShonanAveraging.run - testShonanAveraging - true - false - true - - - make - testBinaryMeasurement.run - testBinaryMeasurement - true - false - true - - - - + + + gtsam + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + -j4 + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${ProjDirPath}/build + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + + From c99cb14b49a2fcd7ec774c7ceeab30e87511cacb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Nov 2020 10:38:43 -0500 Subject: [PATCH 12/30] Roustify BinaryMeasurements in a functional way, plus formatting --- gtsam/sfm/BinaryMeasurement.h | 48 +++++++++---- gtsam/sfm/ShonanAveraging.cpp | 88 +++++++++++++---------- gtsam/sfm/ShonanAveraging.h | 28 ++++---- gtsam/sfm/tests/testBinaryMeasurement.cpp | 9 ++- 4 files changed, 103 insertions(+), 70 deletions(-) diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index ef3fff70d0..f27383175b 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -45,11 +45,43 @@ template class BinaryMeasurement : public Factor { T measured_; ///< The measurement SharedNoiseModel noiseModel_; ///< Noise model -public: + public: BinaryMeasurement(Key key1, Key key2, const T &measured, - const SharedNoiseModel &model = nullptr) - : Factor(std::vector({key1, key2})), measured_(measured), - noiseModel_(model) {} + const SharedNoiseModel &model = nullptr, + bool useHuber = false) + : Factor(std::vector({key1, key2})), + measured_(measured), + noiseModel_(model) { + if (useHuber) { + const auto &robust = + boost::dynamic_pointer_cast(this->noiseModel_); + if (!robust) { + // make robust + this->noiseModel_ = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_); + } + } + } + + /** + * Copy constructor to allow for making existing BinaryMeasurements as robust + * in a functional way. + * + * @param measurement BinaryMeasurement object. + * @param useHuber Boolean flag indicating whether to use Huber noise model. + */ + BinaryMeasurement(const BinaryMeasurement& measurement, bool useHuber = false) { + *this = measurement; + if (useHuber) { + const auto &robust = + boost::dynamic_pointer_cast(this->noiseModel_); + if (!robust) { + // make robust + this->noiseModel_ = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_); + } + } + } /// @name Standard Interface /// @{ @@ -71,14 +103,6 @@ template class BinaryMeasurement : public Factor { this->noiseModel_->print(" noise model: "); } - // TODO: make this more general? - void makeNoiseModelRobust(){ - const auto &robust = boost::dynamic_pointer_cast(this->noiseModel_); - if(!robust) // make robust - this->noiseModel_ = noiseModel::Robust::Create( - noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_); - } - bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const { const BinaryMeasurement *e = dynamic_cast *>(&expected); diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 14f5665b64..53a2222e4f 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -54,7 +54,7 @@ ShonanAveragingParameters::ShonanAveragingParameters( alpha(alpha), beta(beta), gamma(gamma), - useHuber(false){ + useHuber(false) { // By default, we will do conjugate gradient lm.linearSolverType = LevenbergMarquardtParams::Iterative; @@ -139,7 +139,7 @@ ShonanAveraging::ShonanAveraging(const Measurements &measurements, /* ************************************************************************* */ template NonlinearFactorGraph ShonanAveraging::buildGraphAt(size_t p) const { - NonlinearFactorGraph graph; + NonlinearFactorGraph graph; auto G = boost::make_shared(SO<-1>::VectorizedGenerators(p)); for (const auto &measurement : measurements_) { @@ -194,7 +194,7 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { template Values ShonanAveraging::tryOptimizingAt(size_t p, const Values &initial) const { - auto lm = createOptimizerAt(p, initial); + auto lm = createOptimizerAt(p, initial); return lm->optimize(); } @@ -334,24 +334,26 @@ double ShonanAveraging::cost(const Values &values) const { // Get kappa from noise model template static double Kappa(const BinaryMeasurement &measurement) { - const auto &isotropic = boost::dynamic_pointer_cast( - measurement.noiseModel()); - double sigma; - if (isotropic) { - sigma = isotropic->sigma(); - } else{ - const auto &robust = boost::dynamic_pointer_cast( - measurement.noiseModel()); - if (robust) { - std::cout << "Verification of optimality does not work with robust cost function" << std::endl; - sigma = 1; // setting arbitrary value - }else{ - throw std::invalid_argument( - "Shonan averaging noise models must be isotropic (but robust losses are allowed)."); - - } - } - return 1.0 / (sigma * sigma); + const auto &isotropic = boost::dynamic_pointer_cast( + measurement.noiseModel()); + double sigma; + if (isotropic) { + sigma = isotropic->sigma(); + } else { + const auto &robust = boost::dynamic_pointer_cast( + measurement.noiseModel()); + if (robust) { + std::cout << "Verification of optimality does not work with robust cost " + "function" + << std::endl; + sigma = 1; // setting arbitrary value + } else { + throw std::invalid_argument( + "Shonan averaging noise models must be isotropic (but robust losses " + "are allowed)."); + } + } + return 1.0 / (sigma * sigma); } /* ************************************************************************* */ @@ -802,10 +804,10 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, // Optimize until convergence at this level Qstar = tryOptimizingAt(p, initialSOp); if(parameters_.useHuber){ // in this case, there is no optimality verification - if(pMin!=pMax) - std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl; - const Values SO3Values = roundSolution(Qstar); - return std::make_pair(SO3Values, 0); + if(pMin!=pMax) + std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl; + const Values SO3Values = roundSolution(Qstar); + return std::make_pair(SO3Values, 0); } // Check certificate of global optimzality @@ -833,13 +835,17 @@ template class ShonanAveraging<2>; ShonanAveraging2::ShonanAveraging2(const Measurements &measurements, const Parameters ¶meters) - : ShonanAveraging<2>(parameters.useHuber? - makeNoiseModelRobust(measurements) : measurements, parameters) {} + : ShonanAveraging<2>(parameters.useHuber + ? makeNoiseModelRobust(measurements) + : measurements, + parameters) {} ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<2>(parameters.useHuber? - makeNoiseModelRobust( parseMeasurements(g2oFile) ) : - parseMeasurements(g2oFile), parameters) {} + : ShonanAveraging<2>( + parameters.useHuber + ? makeNoiseModelRobust(parseMeasurements(g2oFile)) + : parseMeasurements(g2oFile), + parameters) {} /* ************************************************************************* */ // Explicit instantiation for d=3 @@ -848,12 +854,14 @@ template class ShonanAveraging<3>; ShonanAveraging3::ShonanAveraging3(const Measurements &measurements, const Parameters ¶meters) : ShonanAveraging<3>(parameters.useHuber? - makeNoiseModelRobust(measurements) : measurements, parameters) {} + makeNoiseModelRobust(measurements) : measurements, parameters) {} ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<3>(parameters.useHuber? - makeNoiseModelRobust( parseMeasurements(g2oFile) ) : - parseMeasurements(g2oFile), parameters) {} + : ShonanAveraging<3>( + parameters.useHuber + ? makeNoiseModelRobust(parseMeasurements(g2oFile)) + : parseMeasurements(g2oFile), + parameters) {} // TODO(frank): Deprecate after we land pybind wrapper @@ -869,8 +877,8 @@ static BinaryMeasurement convert( "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); return BinaryMeasurement( - f->key1(), f->key2(), f->measured().rotation(), - noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); + f->key1(), f->key2(), f->measured().rotation(), + noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); } static ShonanAveraging3::Measurements extractRot3Measurements( @@ -883,9 +891,11 @@ static ShonanAveraging3::Measurements extractRot3Measurements( ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors, const Parameters ¶meters) - : ShonanAveraging<3>(parameters.useHuber? - makeNoiseModelRobust( extractRot3Measurements(factors) ): - extractRot3Measurements(factors), parameters) {} + : ShonanAveraging<3>( + parameters.useHuber + ? makeNoiseModelRobust(extractRot3Measurements(factors)) + : extractRot3Measurements(factors), + parameters) {} /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 6efcb045b3..7dd87391a1 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -53,7 +53,8 @@ struct GTSAM_EXPORT ShonanAveragingParameters { double alpha; // weight of anchor-based prior (default 0) double beta; // weight of Karcher-based prior (default 1) double gamma; // weight of gauge-fixing factors (default 0) - bool useHuber; // if enabled, the Huber loss is used in the optimization (default is false) + bool useHuber; // if enabled, the Huber loss is used in the optimization + // (default is false) ShonanAveragingParameters(const LevenbergMarquardtParams &lm = LevenbergMarquardtParams::CeresDefaults(), @@ -83,12 +84,12 @@ struct GTSAM_EXPORT ShonanAveragingParameters { bool getUseHuber() { return useHuber; } void print() const { - std::cout << " ShonanAveragingParameters: " << std::endl; - std::cout << " alpha: " << alpha << std::endl; - std::cout << " beta: " << beta << std::endl; - std::cout << " gamma: " << gamma << std::endl; - std::cout << " useHuber: " << useHuber << std::endl; - std::cout << " --------------------------" << std::endl; + std::cout << " ShonanAveragingParameters: " << std::endl; + std::cout << " alpha: " << alpha << std::endl; + std::cout << " beta: " << beta << std::endl; + std::cout << " gamma: " << gamma << std::endl; + std::cout << " useHuber: " << useHuber << std::endl; + std::cout << " --------------------------" << std::endl; } }; @@ -120,7 +121,6 @@ class GTSAM_EXPORT ShonanAveraging { using Rot = typename Parameters::Rot; // We store SO(d) BetweenFactors to get noise model - // TODO(frank): use BinaryMeasurement? using Measurements = std::vector>; private: @@ -165,12 +165,12 @@ class GTSAM_EXPORT ShonanAveraging { } /// wrap factors with robust Huber loss - static Measurements makeNoiseModelRobust(Measurements measurements){ - Measurements robustMeasurements = measurements; - for (auto &measurement : robustMeasurements) { - measurement.makeNoiseModelRobust(); - } - return robustMeasurements; + Measurements makeNoiseModelRobust(const Measurements& measurements) const { + Measurements robustMeasurements = measurements; + for (auto &measurement : robustMeasurements) { + measurement = BinaryMeasurement(measurement, true); + } + return robustMeasurements; } /// k^th measurement, as a Rot. diff --git a/gtsam/sfm/tests/testBinaryMeasurement.cpp b/gtsam/sfm/tests/testBinaryMeasurement.cpp index a6a75b4ff1..a079f7e04d 100644 --- a/gtsam/sfm/tests/testBinaryMeasurement.cpp +++ b/gtsam/sfm/tests/testBinaryMeasurement.cpp @@ -67,20 +67,19 @@ TEST(BinaryMeasurement, Rot3) { /* ************************************************************************* */ TEST(BinaryMeasurement, Rot3MakeRobust) { BinaryMeasurement rot3Measurement(kKey1, kKey2, rot3Measured, - rot3_model); - rot3Measurement.makeNoiseModelRobust(); + rot3_model, true); EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1); EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2); EXPECT(rot3Measurement.measured().equals(rot3Measured)); const auto &robust = boost::dynamic_pointer_cast( - rot3Measurement.noiseModel()); + rot3Measurement.noiseModel()); EXPECT(robust); // test that if we call it again nothing changes: - rot3Measurement.makeNoiseModelRobust(); + rot3Measurement = BinaryMeasurement(rot3Measurement, true); const auto &robust2 = boost::dynamic_pointer_cast( - rot3Measurement.noiseModel()); + rot3Measurement.noiseModel()); EXPECT(robust2); } From 5762ba5ac8a5c1ca1769a745db37f6ef97b9817b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Nov 2020 10:39:04 -0500 Subject: [PATCH 13/30] Remove goto, update docs, formatting --- gtsam/slam/FrobeniusFactor.cpp | 37 ++++++++++++++++++---------------- gtsam/slam/FrobeniusFactor.h | 17 ++++++++++------ 2 files changed, 31 insertions(+), 23 deletions(-) diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index 5806fcfdb2..8c70a1ebb6 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -26,41 +26,44 @@ namespace gtsam { SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { double sigma = 1.0; + bool exit = false; + if (model != nullptr) { - const auto &robust = boost::dynamic_pointer_cast(model); - Vector sigmas; - if(robust){ - sigmas = robust->noise()->sigmas(); - } else{ - sigmas = model->sigmas(); - } + const auto &robust = boost::dynamic_pointer_cast(model); + Vector sigmas; + if (robust) { + sigmas = robust->noise()->sigmas(); + } else { + sigmas = model->sigmas(); + } size_t n = sigmas.size(); if (n == 1) { sigma = sigmas(0); // Rot2 - goto exit; + exit = true; } - if (n == 3 || n == 6) { + else if (n == 3 || n == 6) { sigma = sigmas(2); // Pose2, Rot3, or Pose3 if (sigmas(0) != sigma || sigmas(1) != sigma) { if (!defaultToUnit) { throw std::runtime_error("Can only convert isotropic rotation noise"); } } - goto exit; + exit = true; } - if (!defaultToUnit) { + if (!defaultToUnit && !exit) { throw std::runtime_error("Can only convert Pose2/Pose3 noise models"); } } - exit: + auto isoModel = noiseModel::Isotropic::Sigma(d, sigma); const auto &robust = boost::dynamic_pointer_cast(model); - if(robust) - return noiseModel::Robust::Create( - noiseModel::mEstimator::Huber::Create(1.345), isoModel); - else - return isoModel; + if (robust) { + return noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), isoModel); + } else { + return isoModel; + } } //****************************************************************************** diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 9915a617d4..f17a9e4217 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -26,13 +26,18 @@ namespace gtsam { /** - * When creating (any) FrobeniusFactor we can convert a Rot/Pose - * BetweenFactor noise model into a n-dimensional isotropic noise - * model used to weight the Frobenius norm. If the noise model passed is - * null we return a n-dimensional isotropic noise model with sigma=1.0. If - * not, we we check if the d-dimensional noise model on rotations is + * When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor + * noise model into a n-dimensional isotropic noise + * model used to weight the Frobenius norm. + * If the noise model passed is null we return a n-dimensional isotropic noise + * model with sigma=1.0. + * If not, we we check if the d-dimensional noise model on rotations is * isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an - * error. If defaultToUnit == false throws an exception on unexepcted input. + * error. + * If the noise model is a robust error model, we use the sigmas of the + * underlying noise model. + * + * If defaultToUnit == false throws an exception on unexepcted input. */ GTSAM_EXPORT SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t n, From dd45797813f3de813309a22e1d34b080cce63034 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Nov 2020 10:39:25 -0500 Subject: [PATCH 14/30] delete old, unused file --- .cproject | 82 ------------------------------------------------------- 1 file changed, 82 deletions(-) delete mode 100644 .cproject diff --git a/.cproject b/.cproject deleted file mode 100644 index 9589ace566..0000000000 --- a/.cproject +++ /dev/null @@ -1,82 +0,0 @@ - - - gtsam - - - - - - org.eclipse.cdt.managedbuilder.core.genmakebuilder - clean,full,incremental, - - - ?name? - - - - org.eclipse.cdt.make.core.append_environment - true - - - org.eclipse.cdt.make.core.autoBuildTarget - all - - - org.eclipse.cdt.make.core.buildArguments - -j4 - - - org.eclipse.cdt.make.core.buildCommand - make - - - org.eclipse.cdt.make.core.buildLocation - ${ProjDirPath}/build - - - org.eclipse.cdt.make.core.cleanBuildTarget - clean - - - org.eclipse.cdt.make.core.contents - org.eclipse.cdt.make.core.activeConfigSettings - - - org.eclipse.cdt.make.core.enableAutoBuild - false - - - org.eclipse.cdt.make.core.enableCleanBuild - true - - - org.eclipse.cdt.make.core.enableFullBuild - true - - - org.eclipse.cdt.make.core.fullBuildTarget - all - - - org.eclipse.cdt.make.core.stopOnError - true - - - org.eclipse.cdt.make.core.useDefaultBuildCmd - true - - - - - org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder - - - - - - org.eclipse.cdt.core.cnature - org.eclipse.cdt.core.ccnature - org.eclipse.cdt.managedbuilder.core.managedBuildNature - org.eclipse.cdt.managedbuilder.core.ScannerConfigNature - - From fba918ce96ae19029767eb850b1f89ca47a20962 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 17:30:34 -0500 Subject: [PATCH 15/30] Removed unnecessary copy constructor and robust noise model is caller's responsibility --- gtsam/sfm/BinaryMeasurement.h | 35 ++--------------------------------- 1 file changed, 2 insertions(+), 33 deletions(-) diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index f27383175b..99e553f7a2 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -47,41 +47,10 @@ template class BinaryMeasurement : public Factor { public: BinaryMeasurement(Key key1, Key key2, const T &measured, - const SharedNoiseModel &model = nullptr, - bool useHuber = false) + const SharedNoiseModel &model = nullptr) : Factor(std::vector({key1, key2})), measured_(measured), - noiseModel_(model) { - if (useHuber) { - const auto &robust = - boost::dynamic_pointer_cast(this->noiseModel_); - if (!robust) { - // make robust - this->noiseModel_ = noiseModel::Robust::Create( - noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_); - } - } - } - - /** - * Copy constructor to allow for making existing BinaryMeasurements as robust - * in a functional way. - * - * @param measurement BinaryMeasurement object. - * @param useHuber Boolean flag indicating whether to use Huber noise model. - */ - BinaryMeasurement(const BinaryMeasurement& measurement, bool useHuber = false) { - *this = measurement; - if (useHuber) { - const auto &robust = - boost::dynamic_pointer_cast(this->noiseModel_); - if (!robust) { - // make robust - this->noiseModel_ = noiseModel::Robust::Create( - noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_); - } - } - } + noiseModel_(model) {} /// @name Standard Interface /// @{ From fd74ae933065a3d615b31bf3feb4f32552fd53c5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 17:31:08 -0500 Subject: [PATCH 16/30] throw runtime errors and explicitly form robust noise model --- gtsam/sfm/ShonanAveraging.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 53a2222e4f..1d3166a89a 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -343,10 +343,8 @@ static double Kappa(const BinaryMeasurement &measurement) { const auto &robust = boost::dynamic_pointer_cast( measurement.noiseModel()); if (robust) { - std::cout << "Verification of optimality does not work with robust cost " - "function" - << std::endl; - sigma = 1; // setting arbitrary value + throw std::runtime_error( + "Verification of optimality does not work with robust cost function"); } else { throw std::invalid_argument( "Shonan averaging noise models must be isotropic (but robust losses " @@ -804,8 +802,10 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, // Optimize until convergence at this level Qstar = tryOptimizingAt(p, initialSOp); if(parameters_.useHuber){ // in this case, there is no optimality verification - if(pMin!=pMax) - std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl; + if (pMin != pMax) { + throw std::runtime_error( + "When using robust norm, Shonan only tests a single rank"); + } const Values SO3Values = roundSolution(Qstar); return std::make_pair(SO3Values, 0); } @@ -876,9 +876,11 @@ static BinaryMeasurement convert( "parseMeasurements can only convert Pose3 measurements " "with Gaussian noise models."); const Matrix6 M = gaussian->covariance(); - return BinaryMeasurement( - f->key1(), f->key2(), f->measured().rotation(), - noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); + auto model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), + noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3))); + return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + model); } static ShonanAveraging3::Measurements extractRot3Measurements( From 9d15afaab1f1b56c9acb2bf577a14138c171d2de Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 17:31:49 -0500 Subject: [PATCH 17/30] makeNoiseModelRobust assumes responsibility for robustifying noise models --- gtsam/sfm/ShonanAveraging.h | 42 ++++++++++++++++++++++++++++--------- 1 file changed, 32 insertions(+), 10 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 7dd87391a1..5cb34c419e 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -83,13 +83,13 @@ struct GTSAM_EXPORT ShonanAveragingParameters { void setUseHuber(bool value) { useHuber = value; } bool getUseHuber() { return useHuber; } + /// Print the parameters and flags used for rotation averaging. void print() const { std::cout << " ShonanAveragingParameters: " << std::endl; - std::cout << " alpha: " << alpha << std::endl; - std::cout << " beta: " << beta << std::endl; - std::cout << " gamma: " << gamma << std::endl; - std::cout << " useHuber: " << useHuber << std::endl; - std::cout << " --------------------------" << std::endl; + std::cout << " alpha: " << alpha << std::endl; + std::cout << " beta: " << beta << std::endl; + std::cout << " gamma: " << gamma << std::endl; + std::cout << " useHuber: " << useHuber << std::endl; } }; @@ -164,11 +164,33 @@ class GTSAM_EXPORT ShonanAveraging { return measurements_[k]; } - /// wrap factors with robust Huber loss - Measurements makeNoiseModelRobust(const Measurements& measurements) const { - Measurements robustMeasurements = measurements; - for (auto &measurement : robustMeasurements) { - measurement = BinaryMeasurement(measurement, true); + /** + * Update factors to use robust Huber loss. + * + * @param measurements Vector of BinaryMeasurements. + * @param k Huber noise model threshold. + */ + Measurements makeNoiseModelRobust(const Measurements &measurements, + double k = 1.345) const { + Measurements robustMeasurements; + for (auto &measurement : measurements) { + + auto model = measurement.noiseModel(); + const auto &robust = + boost::dynamic_pointer_cast(model); + + SharedNoiseModel robust_model; + // Check if the noise model is already robust + if (robust) { + robust_model = model; + } else { + // make robust + robust_model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(k), model); + } + BinaryMeasurement meas(measurement.key1(), measurement.key2(), + measurement.measured(), robust_model); + robustMeasurements.push_back(meas); } return robustMeasurements; } From 3e6efe3a51355007f9ab2d1d6f6a61ba417de42b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 17:32:02 -0500 Subject: [PATCH 18/30] use goto flow --- gtsam/slam/FrobeniusFactor.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index 8c70a1ebb6..8c0baaf384 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -26,7 +26,6 @@ namespace gtsam { SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { double sigma = 1.0; - bool exit = false; if (model != nullptr) { const auto &robust = boost::dynamic_pointer_cast(model); @@ -40,7 +39,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { size_t n = sigmas.size(); if (n == 1) { sigma = sigmas(0); // Rot2 - exit = true; + goto exit; } else if (n == 3 || n == 6) { sigma = sigmas(2); // Pose2, Rot3, or Pose3 @@ -49,13 +48,13 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { throw std::runtime_error("Can only convert isotropic rotation noise"); } } - exit = true; + goto exit; } - if (!defaultToUnit && !exit) { + if (!defaultToUnit) { throw std::runtime_error("Can only convert Pose2/Pose3 noise models"); } } - + exit: auto isoModel = noiseModel::Isotropic::Sigma(d, sigma); const auto &robust = boost::dynamic_pointer_cast(model); if (robust) { From 799788672f14fc53b1cfebeaa055fb6c48ad8353 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 17:32:16 -0500 Subject: [PATCH 19/30] formatting --- examples/ShonanAveragingCLI.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/examples/ShonanAveragingCLI.cpp b/examples/ShonanAveragingCLI.cpp index 322622228b..9970f45da9 100644 --- a/examples/ShonanAveragingCLI.cpp +++ b/examples/ShonanAveragingCLI.cpp @@ -25,7 +25,8 @@ * Read 3D dataset sphere25000.txt and output to shonan.g2o (default) * ./ShonanAveragingCLI -i spere2500.txt * - * If you prefer using a robust Huber loss, you can add the option "-h true", for instance + * If you prefer using a robust Huber loss, you can add the option "-h true", + * for instance * ./ShonanAveragingCLI -i spere2500.txt -u true */ @@ -62,9 +63,9 @@ int main(int argc, char* argv[]) { "dimension,d", po::value(&d)->default_value(3), "Optimize over 2D or 3D rotations")( "useHuberLoss,h", po::value(&useHuberLoss)->default_value(false), - "set True to use Huber loss")( - "pMin,p", po::value(&pMin)->default_value(3), - "set to use desired rank pMin")( + "set True to use Huber loss")("pMin,p", + po::value(&pMin)->default_value(3), + "set to use desired rank pMin")( "seed,s", po::value(&seed)->default_value(42), "Random seed for initial estimate"); po::variables_map vm; @@ -97,9 +98,9 @@ int main(int argc, char* argv[]) { cout << "Running Shonan averaging for SO(2) on " << inputFile << endl; ShonanAveraging2::Parameters parameters(lmParams); parameters.setUseHuber(useHuberLoss); - ShonanAveraging2 shonan(inputFile,parameters); + ShonanAveraging2 shonan(inputFile, parameters); auto initial = shonan.initializeRandomly(rng); - auto result = shonan.run(initial,pMin); + auto result = shonan.run(initial, pMin); // Parse file again to set up translation problem, adding a prior boost::tie(inputGraph, posesInFile) = load2D(inputFile); @@ -113,9 +114,9 @@ int main(int argc, char* argv[]) { cout << "Running Shonan averaging for SO(3) on " << inputFile << endl; ShonanAveraging3::Parameters parameters(lmParams); parameters.setUseHuber(useHuberLoss); - ShonanAveraging3 shonan(inputFile,parameters); + ShonanAveraging3 shonan(inputFile, parameters); auto initial = shonan.initializeRandomly(rng); - auto result = shonan.run(initial,pMin); + auto result = shonan.run(initial, pMin); // Parse file again to set up translation problem, adding a prior boost::tie(inputGraph, posesInFile) = load3D(inputFile); From 7391c103ec0d6e74713c3316311d3096ad4953ea Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 17:51:50 -0500 Subject: [PATCH 20/30] fix tests --- gtsam/sfm/ShonanAveraging.cpp | 5 +++-- gtsam/sfm/tests/testBinaryMeasurement.cpp | 10 +++------- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 1d3166a89a..e08bc4dd65 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -343,8 +343,9 @@ static double Kappa(const BinaryMeasurement &measurement) { const auto &robust = boost::dynamic_pointer_cast( measurement.noiseModel()); if (robust) { - throw std::runtime_error( - "Verification of optimality does not work with robust cost function"); + std::cout << "Verification of optimality does not work with robust cost " + "function" + << std::endl; } else { throw std::invalid_argument( "Shonan averaging noise models must be isotropic (but robust losses " diff --git a/gtsam/sfm/tests/testBinaryMeasurement.cpp b/gtsam/sfm/tests/testBinaryMeasurement.cpp index a079f7e04d..ae13e54c47 100644 --- a/gtsam/sfm/tests/testBinaryMeasurement.cpp +++ b/gtsam/sfm/tests/testBinaryMeasurement.cpp @@ -66,8 +66,10 @@ TEST(BinaryMeasurement, Rot3) { /* ************************************************************************* */ TEST(BinaryMeasurement, Rot3MakeRobust) { + auto huber_model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), rot3_model); BinaryMeasurement rot3Measurement(kKey1, kKey2, rot3Measured, - rot3_model, true); + huber_model); EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1); EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2); @@ -75,12 +77,6 @@ TEST(BinaryMeasurement, Rot3MakeRobust) { const auto &robust = boost::dynamic_pointer_cast( rot3Measurement.noiseModel()); EXPECT(robust); - - // test that if we call it again nothing changes: - rot3Measurement = BinaryMeasurement(rot3Measurement, true); - const auto &robust2 = boost::dynamic_pointer_cast( - rot3Measurement.noiseModel()); - EXPECT(robust2); } /* ************************************************************************* */ From a00d37005bbcd2f6702be15d58ac8312a531b163 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 30 Nov 2020 18:21:36 -0500 Subject: [PATCH 21/30] Don't throw error for Kappa and test parameter print --- gtsam/sfm/ShonanAveraging.cpp | 1 + gtsam/sfm/tests/testShonanAveraging.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index e08bc4dd65..bc3783a279 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -346,6 +346,7 @@ static double Kappa(const BinaryMeasurement &measurement) { std::cout << "Verification of optimality does not work with robust cost " "function" << std::endl; + sigma = 1; // setting arbitrary value } else { throw std::invalid_argument( "Shonan averaging noise models must be isotropic (but robust losses " diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index 9242b94a30..0021094542 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -329,7 +330,11 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { ShonanAveraging2::Parameters parameters(lmParams); auto measurements = parseMeasurements(g2oFile); parameters.setUseHuber(true); - parameters.print(); + string parameters_print = + " ShonanAveragingParameters: \n alpha: 0\n beta: 1\n gamma: 0\n " + "useHuber: 1\n"; + assert_print_equal(parameters_print, parameters); + ShonanAveraging2 shonan(measurements, parameters); EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns()); From b788fb14c062be9d32613f07e5daf1fe3b13d938 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Dec 2020 14:33:03 -0500 Subject: [PATCH 22/30] mark getters as const --- gtsam/sfm/ShonanAveraging.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 5cb34c419e..ad8d2944ab 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -69,19 +69,19 @@ struct GTSAM_EXPORT ShonanAveragingParameters { double getOptimalityThreshold() const { return optimalityThreshold; } void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; } - std::pair getAnchor() { return anchor; } + std::pair getAnchor() const { return anchor; } void setAnchorWeight(double value) { alpha = value; } - double getAnchorWeight() { return alpha; } + double getAnchorWeight() const { return alpha; } void setKarcherWeight(double value) { beta = value; } - double getKarcherWeight() { return beta; } + double getKarcherWeight() const { return beta; } void setGaugesWeight(double value) { gamma = value; } - double getGaugesWeight() { return gamma; } + double getGaugesWeight() const { return gamma; } void setUseHuber(bool value) { useHuber = value; } - bool getUseHuber() { return useHuber; } + bool getUseHuber() const { return useHuber; } /// Print the parameters and flags used for rotation averaging. void print() const { From 636d5f4f1ca262cbe94f64e180620f930f720f49 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Dec 2020 14:34:18 -0500 Subject: [PATCH 23/30] Helper method to robustify measurements --- gtsam/sfm/ShonanAveraging.cpp | 34 +++++++++++++--------------------- gtsam/sfm/ShonanAveraging.h | 16 ++++++++++++++++ 2 files changed, 29 insertions(+), 21 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index bc3783a279..8fed0538df 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -812,7 +812,7 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, return std::make_pair(SO3Values, 0); } - // Check certificate of global optimzality + // Check certificate of global optimality Vector minEigenVector; double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); if (minEigenValue > parameters_.optimalityThreshold) { @@ -837,17 +837,13 @@ template class ShonanAveraging<2>; ShonanAveraging2::ShonanAveraging2(const Measurements &measurements, const Parameters ¶meters) - : ShonanAveraging<2>(parameters.useHuber - ? makeNoiseModelRobust(measurements) - : measurements, + : ShonanAveraging<2>(maybeRobust(measurements, parameters.getUseHuber()), parameters) {} ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<2>( - parameters.useHuber - ? makeNoiseModelRobust(parseMeasurements(g2oFile)) - : parseMeasurements(g2oFile), - parameters) {} + : ShonanAveraging<2>(maybeRobust(parseMeasurements(g2oFile), + parameters.getUseHuber()), + parameters) {} /* ************************************************************************* */ // Explicit instantiation for d=3 @@ -855,15 +851,13 @@ template class ShonanAveraging<3>; ShonanAveraging3::ShonanAveraging3(const Measurements &measurements, const Parameters ¶meters) - : ShonanAveraging<3>(parameters.useHuber? - makeNoiseModelRobust(measurements) : measurements, parameters) {} + : ShonanAveraging<3>(maybeRobust(measurements, parameters.getUseHuber()), + parameters) {} ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters ¶meters) - : ShonanAveraging<3>( - parameters.useHuber - ? makeNoiseModelRobust(parseMeasurements(g2oFile)) - : parseMeasurements(g2oFile), - parameters) {} + : ShonanAveraging<3>(maybeRobust(parseMeasurements(g2oFile), + parameters.getUseHuber()), + parameters) {} // TODO(frank): Deprecate after we land pybind wrapper @@ -895,11 +889,9 @@ static ShonanAveraging3::Measurements extractRot3Measurements( ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors, const Parameters ¶meters) - : ShonanAveraging<3>( - parameters.useHuber - ? makeNoiseModelRobust(extractRot3Measurements(factors)) - : extractRot3Measurements(factors), - parameters) {} + : ShonanAveraging<3>(maybeRobust(extractRot3Measurements(factors), + parameters.getUseHuber()), + parameters) {} /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index ad8d2944ab..48d873a1a4 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -389,6 +389,22 @@ class GTSAM_EXPORT ShonanAveraging { std::pair run(const Values &initialEstimate, size_t pMin = d, size_t pMax = 10) const; /// @} + + /** + * Helper function to convert measurements to robust noise model + * if flag is set. + * + * @tparam T the type of measurement, e.g. Rot3. + * @param measurements vector of BinaryMeasurements of type T. + * @param useRobustModel flag indicating whether use robust noise model + * instead. + */ + template + inline std::vector> maybeRobust( + const std::vector> &measurements, + bool useRobustModel = false) const { + return useRobustModel ? makeNoiseModelRobust(measurements) : measurements; + } }; // Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a From 06aa4ef780379ec4c20e6b3d7f1b41bb2181b19c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 3 Dec 2020 20:27:59 -0500 Subject: [PATCH 24/30] throw error if robust model used but not specified in parameters --- gtsam/sfm/ShonanAveraging.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 8fed0538df..58921988bb 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -332,8 +332,9 @@ double ShonanAveraging::cost(const Values &values) const { /* ************************************************************************* */ // Get kappa from noise model -template -static double Kappa(const BinaryMeasurement &measurement) { +template +static double Kappa(const BinaryMeasurement &measurement, + const ShonanAveragingParameters ¶meters) { const auto &isotropic = boost::dynamic_pointer_cast( measurement.noiseModel()); double sigma; @@ -343,10 +344,13 @@ static double Kappa(const BinaryMeasurement &measurement) { const auto &robust = boost::dynamic_pointer_cast( measurement.noiseModel()); if (robust) { - std::cout << "Verification of optimality does not work with robust cost " - "function" - << std::endl; - sigma = 1; // setting arbitrary value + if (parameters.getUseHuber()) { + // Cannot verify optimality, setting arbitrary value + sigma = 1; + } else { + throw std::invalid_argument( + "Robust cost function is invalid unless useHuber is set."); + } } else { throw std::invalid_argument( "Shonan averaging noise models must be isotropic (but robust losses " @@ -372,7 +376,7 @@ Sparse ShonanAveraging::buildD() const { const auto &keys = measurement.keys(); // Get kappa from noise model - double kappa = Kappa(measurement); + double kappa = Kappa(measurement, parameters_); const size_t di = d * keys[0], dj = d * keys[1]; for (size_t k = 0; k < d; k++) { @@ -410,7 +414,7 @@ Sparse ShonanAveraging::buildQ() const { const auto Rij = measurement.measured().matrix(); // Get kappa from noise model - double kappa = Kappa(measurement); + double kappa = Kappa(measurement, parameters_); const size_t di = d * keys[0], dj = d * keys[1]; for (size_t r = 0; r < d; r++) { @@ -803,7 +807,8 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, for (size_t p = pMin; p <= pMax; p++) { // Optimize until convergence at this level Qstar = tryOptimizingAt(p, initialSOp); - if(parameters_.useHuber){ // in this case, there is no optimality verification + if (parameters_ + .useHuber) { // in this case, there is no optimality verification if (pMin != pMax) { throw std::runtime_error( "When using robust norm, Shonan only tests a single rank"); From 5e82b72b60e594bd7d67ea3cbeb6485bfb2a14a2 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 5 Dec 2020 14:34:35 -0500 Subject: [PATCH 25/30] fixed typo --- examples/ShonanAveragingCLI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ShonanAveragingCLI.cpp b/examples/ShonanAveragingCLI.cpp index 9970f45da9..c72a320178 100644 --- a/examples/ShonanAveragingCLI.cpp +++ b/examples/ShonanAveragingCLI.cpp @@ -27,7 +27,7 @@ * * If you prefer using a robust Huber loss, you can add the option "-h true", * for instance - * ./ShonanAveragingCLI -i spere2500.txt -u true + * ./ShonanAveragingCLI -i spere2500.txt -h true */ #include From 553f5690387355a73b0b810c3c31d7474739ea6a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 5 Dec 2020 14:34:46 -0500 Subject: [PATCH 26/30] added more explanation on throw --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 58921988bb..06ff921600 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -811,7 +811,7 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, .useHuber) { // in this case, there is no optimality verification if (pMin != pMax) { throw std::runtime_error( - "When using robust norm, Shonan only tests a single rank"); + "When using robust norm, Shonan only tests a single rank. Set pMin = pMax"); } const Values SO3Values = roundSolution(Qstar); return std::make_pair(SO3Values, 0); From de5adb495f61f30aaf367ff915a438478cf129c8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 5 Dec 2020 16:06:27 -0500 Subject: [PATCH 27/30] added flag to enable optimality certification, some formatting --- gtsam/sfm/ShonanAveraging.cpp | 49 +++++++++++++++++++---------------- gtsam/sfm/ShonanAveraging.h | 26 +++++++++++-------- 2 files changed, 41 insertions(+), 34 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 06ff921600..e37bfee55f 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -54,7 +54,8 @@ ShonanAveragingParameters::ShonanAveragingParameters( alpha(alpha), beta(beta), gamma(gamma), - useHuber(false) { + useHuber(false), + certifyOptimality(true) { // By default, we will do conjugate gradient lm.linearSolverType = LevenbergMarquardtParams::Iterative; @@ -343,13 +344,15 @@ static double Kappa(const BinaryMeasurement &measurement, } else { const auto &robust = boost::dynamic_pointer_cast( measurement.noiseModel()); + // Check if noise model is robust if (robust) { - if (parameters.getUseHuber()) { - // Cannot verify optimality, setting arbitrary value - sigma = 1; - } else { + // If robust, check if optimality certificate is expected + if (parameters.getCertifyOptimality()) { throw std::invalid_argument( - "Robust cost function is invalid unless useHuber is set."); + "Verification of optimality does not work with robust cost."); + } else { + // Optimality certificate not required, so setting default sigma + sigma = 1; } } else { throw std::invalid_argument( @@ -807,30 +810,30 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, for (size_t p = pMin; p <= pMax; p++) { // Optimize until convergence at this level Qstar = tryOptimizingAt(p, initialSOp); - if (parameters_ - .useHuber) { // in this case, there is no optimality verification + if (parameters_.getUseHuber() || parameters_.getCertifyOptimality()) { + // in this case, there is no optimality verification if (pMin != pMax) { throw std::runtime_error( "When using robust norm, Shonan only tests a single rank. Set pMin = pMax"); } const Values SO3Values = roundSolution(Qstar); return std::make_pair(SO3Values, 0); - } - - // Check certificate of global optimality - Vector minEigenVector; - double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); - if (minEigenValue > parameters_.optimalityThreshold) { - // If at global optimum, round and return solution - const Values SO3Values = roundSolution(Qstar); - return std::make_pair(SO3Values, minEigenValue); - } + } else { + // Check certificate of global optimality + Vector minEigenVector; + double minEigenValue = computeMinEigenValue(Qstar, &minEigenVector); + if (minEigenValue > parameters_.optimalityThreshold) { + // If at global optimum, round and return solution + const Values SO3Values = roundSolution(Qstar); + return std::make_pair(SO3Values, minEigenValue); + } - // Not at global optimimum yet, so check whether we will go to next level - if (p != pMax) { - // Calculate initial estimate for next level by following minEigenVector - initialSOp = - initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue); + // Not at global optimimum yet, so check whether we will go to next level + if (p != pMax) { + // Calculate initial estimate for next level by following minEigenVector + initialSOp = + initializeWithDescent(p + 1, Qstar, minEigenVector, minEigenValue); + } } } throw std::runtime_error("Shonan::run did not converge for given pMax"); diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 48d873a1a4..8a620cdc5e 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -47,14 +47,16 @@ struct GTSAM_EXPORT ShonanAveragingParameters { using Anchor = std::pair; // Paremeters themselves: - LevenbergMarquardtParams lm; // LM parameters - double optimalityThreshold; // threshold used in checkOptimality - Anchor anchor; // pose to use as anchor if not Karcher - double alpha; // weight of anchor-based prior (default 0) - double beta; // weight of Karcher-based prior (default 1) - double gamma; // weight of gauge-fixing factors (default 0) - bool useHuber; // if enabled, the Huber loss is used in the optimization - // (default is false) + LevenbergMarquardtParams lm; ///< LM parameters + double optimalityThreshold; ///< threshold used in checkOptimality + Anchor anchor; ///< pose to use as anchor if not Karcher + double alpha; ///< weight of anchor-based prior (default 0) + double beta; ///< weight of Karcher-based prior (default 1) + double gamma; ///< weight of gauge-fixing factors (default 0) + ///< if enabled, the Huber loss is used (default false) + bool useHuber; + ///< if enabled solution optimality is certified (default true) + bool certifyOptimality; ShonanAveragingParameters(const LevenbergMarquardtParams &lm = LevenbergMarquardtParams::CeresDefaults(), @@ -83,6 +85,9 @@ struct GTSAM_EXPORT ShonanAveragingParameters { void setUseHuber(bool value) { useHuber = value; } bool getUseHuber() const { return useHuber; } + void setCertifyOptimality(bool value) { certifyOptimality = value; } + bool getCertifyOptimality() const { return certifyOptimality; } + /// Print the parameters and flags used for rotation averaging. void print() const { std::cout << " ShonanAveragingParameters: " << std::endl; @@ -166,7 +171,7 @@ class GTSAM_EXPORT ShonanAveraging { /** * Update factors to use robust Huber loss. - * + * * @param measurements Vector of BinaryMeasurements. * @param k Huber noise model threshold. */ @@ -174,7 +179,6 @@ class GTSAM_EXPORT ShonanAveraging { double k = 1.345) const { Measurements robustMeasurements; for (auto &measurement : measurements) { - auto model = measurement.noiseModel(); const auto &robust = boost::dynamic_pointer_cast(model); @@ -189,7 +193,7 @@ class GTSAM_EXPORT ShonanAveraging { noiseModel::mEstimator::Huber::Create(k), model); } BinaryMeasurement meas(measurement.key1(), measurement.key2(), - measurement.measured(), robust_model); + measurement.measured(), robust_model); robustMeasurements.push_back(meas); } return robustMeasurements; From 9a841a2c3431b843b9d62564190e9167c9deb214 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 5 Dec 2020 17:21:19 -0500 Subject: [PATCH 28/30] correct flag checking --- gtsam/sfm/ShonanAveraging.cpp | 2 +- gtsam/sfm/tests/testShonanAveraging.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index e37bfee55f..b407263129 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -810,7 +810,7 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, for (size_t p = pMin; p <= pMax; p++) { // Optimize until convergence at this level Qstar = tryOptimizingAt(p, initialSOp); - if (parameters_.getUseHuber() || parameters_.getCertifyOptimality()) { + if (parameters_.getUseHuber() || !parameters_.getCertifyOptimality()) { // in this case, there is no optimality verification if (pMin != pMax) { throw std::runtime_error( diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index 0021094542..c2ad71dad1 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -330,6 +330,8 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { ShonanAveraging2::Parameters parameters(lmParams); auto measurements = parseMeasurements(g2oFile); parameters.setUseHuber(true); + parameters.setCertifyOptimality(false); + string parameters_print = " ShonanAveragingParameters: \n alpha: 0\n beta: 1\n gamma: 0\n " "useHuber: 1\n"; From 0c079c66d0b3eaf3da8ab82ec04bf6b4215dbcf6 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 8 Dec 2020 19:26:10 -0500 Subject: [PATCH 29/30] fixed small typos --- gtsam/sfm/ShonanAveraging.cpp | 4 ++-- gtsam/sfm/ShonanAveraging.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index b407263129..5957047a3b 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -349,7 +349,7 @@ static double Kappa(const BinaryMeasurement &measurement, // If robust, check if optimality certificate is expected if (parameters.getCertifyOptimality()) { throw std::invalid_argument( - "Verification of optimality does not work with robust cost."); + "Certification of optimality does not work with robust cost."); } else { // Optimality certificate not required, so setting default sigma sigma = 1; @@ -811,7 +811,7 @@ std::pair ShonanAveraging::run(const Values &initialEstimate, // Optimize until convergence at this level Qstar = tryOptimizingAt(p, initialSOp); if (parameters_.getUseHuber() || !parameters_.getCertifyOptimality()) { - // in this case, there is no optimality verification + // in this case, there is no optimality certification if (pMin != pMax) { throw std::runtime_error( "When using robust norm, Shonan only tests a single rank. Set pMin = pMax"); diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 8a620cdc5e..2cb62ca555 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -46,7 +46,7 @@ struct GTSAM_EXPORT ShonanAveragingParameters { using Rot = typename std::conditional::type; using Anchor = std::pair; - // Paremeters themselves: + // Parameters themselves: LevenbergMarquardtParams lm; ///< LM parameters double optimalityThreshold; ///< threshold used in checkOptimality Anchor anchor; ///< pose to use as anchor if not Karcher From d89dd7d87492b6ba2954444782981e98f2da8120 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 11 Jan 2021 13:51:32 -0500 Subject: [PATCH 30/30] follow correct doxygen format --- gtsam/sfm/ShonanAveraging.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 2cb62ca555..a603dec98a 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -53,9 +53,9 @@ struct GTSAM_EXPORT ShonanAveragingParameters { double alpha; ///< weight of anchor-based prior (default 0) double beta; ///< weight of Karcher-based prior (default 1) double gamma; ///< weight of gauge-fixing factors (default 0) - ///< if enabled, the Huber loss is used (default false) + /// if enabled, the Huber loss is used (default false) bool useHuber; - ///< if enabled solution optimality is certified (default true) + /// if enabled solution optimality is certified (default true) bool certifyOptimality; ShonanAveragingParameters(const LevenbergMarquardtParams &lm =