From c99cb14b49a2fcd7ec774c7ceeab30e87511cacb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Nov 2020 10:38:43 -0500 Subject: [PATCH] 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 ef3fff70d..f27383175 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -45,11 +45,43 @@ private: 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 @@ public: 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 14f5665b6..53a2222e4 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 6efcb045b..7dd87391a 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 a6a75b4ff..a079f7e04 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); }