diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 609c03acf..cb77902d0 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -696,7 +696,7 @@ Huber::Huber(double k, const ReweightScheme reweight) } double Huber::weight(double error) const { - return (error < k_) ? (1.0) : (k_ / fabs(error)); + return (fabs(error) > k_) ? k_ / fabs(error) : 1.0; } void Huber::print(const std::string &s="") const { @@ -799,6 +799,66 @@ Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsh(c, reweight)); } +/* ************************************************************************* */ +// GemanMcClure +/* ************************************************************************* */ +GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double GemanMcClure::weight(double error) const { + const double c2 = c_*c_; + const double c4 = c2*c2; + const double c2error = c2 + error*error; + return c4/(c2error*c2error); +} + +void GemanMcClure::print(const std::string &s="") const { + std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl; +} + +bool GemanMcClure::equals(const Base &expected, double tol) const { + const GemanMcClure* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new GemanMcClure(c, reweight)); +} + +/* ************************************************************************* */ +// DCS +/* ************************************************************************* */ +DCS::DCS(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double DCS::weight(double error) const { + const double e2 = error*error; + if (e2 > c_) + { + const double w = 2.0*c_/(c_ + e2); + return w*w; + } + + return 1.0; +} + +void DCS::print(const std::string &s="") const { + std::cout << s << ": DCS (" << c_ << ")" << std::endl; +} + +bool DCS::equals(const Base &expected, double tol) const { + const DCS* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new DCS(c, reweight)); +} + } // namespace mEstimator /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2a4d7c746..db01152f6 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -823,6 +823,65 @@ namespace gtsam { } }; + /// GemanMcClure implements the "Geman-McClure" robust error model + /// (Zhang97ivc). + /// + /// Note that Geman-McClure weight function uses the parameter c == 1.0, + /// but here it's allowed to use different values, so we actually have + /// the generalized Geman-McClure from (Agarwal15phd). + class GTSAM_EXPORT GemanMcClure : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); + virtual ~GemanMcClure() {} + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; + + protected: + double c_; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(c_); + } + }; + + /// DCS implements the Dynamic Covariance Scaling robust error model + /// from the paper Robust Map Optimization (Agarwal13icra). + /// + /// Under the special condition of the parameter c == 1.0 and not + /// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. + class GTSAM_EXPORT DCS : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + DCS(double c = 1.0, const ReweightScheme reweight = Block); + virtual ~DCS() {} + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; + + protected: + double c_; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(c_); + } + }; + } ///\namespace mEstimator /// Base class for robust error models diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 37b55fc03..d6857c6ff 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -322,7 +322,7 @@ TEST(NoiseModel, WhitenInPlace) } /* ************************************************************************* */ -TEST(NoiseModel, robustFunction) +TEST(NoiseModel, robustFunctionHuber) { const double k = 5.0, error1 = 1.0, error2 = 10.0; const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k); @@ -332,8 +332,28 @@ TEST(NoiseModel, robustFunction) DOUBLES_EQUAL(0.5, weight2, 1e-8); } +TEST(NoiseModel, robustFunctionGemanMcClure) +{ + const double k = 1.0, error1 = 1.0, error2 = 10.0; + const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k); + const double weight1 = gmc->weight(error1), + weight2 = gmc->weight(error2); + DOUBLES_EQUAL(0.25 , weight1, 1e-8); + DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8); +} + +TEST(NoiseModel, robustFunctionDCS) +{ + const double k = 1.0, error1 = 1.0, error2 = 10.0; + const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k); + const double weight1 = dcs->weight(error1), + weight2 = dcs->weight(error2); + DOUBLES_EQUAL(1.0 , weight1, 1e-8); + DOUBLES_EQUAL(0.00039211, weight2, 1e-8); +} + /* ************************************************************************* */ -TEST(NoiseModel, robustNoise) +TEST(NoiseModel, robustNoiseHuber) { const double k = 10.0, error1 = 1.0, error2 = 100.0; Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished(); @@ -342,7 +362,7 @@ TEST(NoiseModel, robustNoise) mEstimator::Huber::Create(k, mEstimator::Huber::Scalar), Unit::Create(2)); - robust->WhitenSystem(A,b); + robust->WhitenSystem(A, b); DOUBLES_EQUAL(error1, b(0), 1e-8); DOUBLES_EQUAL(sqrt(k*error2), b(1), 1e-8); @@ -353,6 +373,57 @@ TEST(NoiseModel, robustNoise) DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8); } +TEST(NoiseModel, robustNoiseGemanMcClure) +{ + const double k = 1.0, error1 = 1.0, error2 = 100.0; + const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0; + Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished(); + Vector b = Vector2(error1, error2); + const Robust::shared_ptr robust = Robust::Create( + mEstimator::GemanMcClure::Create(k, mEstimator::GemanMcClure::Scalar), + Unit::Create(2)); + + robust->WhitenSystem(A, b); + + const double k2 = k*k; + const double k4 = k2*k2; + const double k2error = k2 + error2*error2; + + const double sqrt_weight_error1 = sqrt(0.25); + const double sqrt_weight_error2 = sqrt(k4/(k2error*k2error)); + + DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8); + + DOUBLES_EQUAL(sqrt_weight_error1*a00, A(0,0), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error1*a01, A(0,1), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error2*a10, A(1,0), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error2*a11, A(1,1), 1e-8); +} + +TEST(NoiseModel, robustNoiseDCS) +{ + const double k = 1.0, error1 = 1.0, error2 = 100.0; + const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0; + Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished(); + Vector b = Vector2(error1, error2); + const Robust::shared_ptr robust = Robust::Create( + mEstimator::DCS::Create(k, mEstimator::DCS::Scalar), + Unit::Create(2)); + + robust->WhitenSystem(A, b); + + const double sqrt_weight = 2.0*k/(k + error2*error2); + + DOUBLES_EQUAL(error1, b(0), 1e-8); + DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8); + + DOUBLES_EQUAL(a00, A(0,0), 1e-8); + DOUBLES_EQUAL(a01, A(0,1), 1e-8); + DOUBLES_EQUAL(sqrt_weight*a10, A(1,0), 1e-8); + DOUBLES_EQUAL(sqrt_weight*a11, A(1,1), 1e-8); +} + /* ************************************************************************* */ #define TEST_GAUSSIAN(gaussian)\ EQUALITY(info, gaussian->information());\ diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 04234bacc..68c27e76b 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -247,6 +247,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, // Parse the pose constraints Key id1, id2; bool haveLandmark = false; + const bool useModelInFile = !model; while (!is.eof()) { if (!(is >> tag)) break; @@ -267,7 +268,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (maxID && (id1 >= maxID || id2 >= maxID)) continue; - if (!model) + if (useModelInFile) model = modelInFile; if (addNoise)