From 001a55ad3a53fcd97c07dae4fee66225b29012d1 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 24 Sep 2020 16:35:41 -0400 Subject: [PATCH] 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 e99328302..1e6fcf6da 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 9718805b8..6efcb045b 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 e23f9e20b..ae24094de 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());