diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index c525c1b9e..9540564e0 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -71,6 +71,11 @@ public: 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 @@ public: } /// @} }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index df2d72c28..e99328302 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 edd9f33a2..9718805b8 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 1200c8ebb..e23f9e20b 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) {