diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index 9540564e0..ef3fff70d 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -71,8 +71,11 @@ public: 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 3dd81c2c1..a6a75b4ff 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;