diff --git a/gtsam.h b/gtsam.h index 04575f84d..09b5b59e6 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1422,8 +1422,8 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { - GemanMcClure(double k); - static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double k); + GemanMcClure(double c); + static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); // enabling serialization functionality void serializable() const; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 68e579718..e0b7c8b17 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -731,6 +731,7 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { double Fair::weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } + double Fair::residual(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; @@ -839,6 +840,7 @@ double Tukey::weight(double error) const { } return 0.0; } + double Tukey::residual(double error) const { double absError = std::abs(error); if (absError <= c_) { @@ -894,24 +896,6 @@ Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsch(c, reweight)); } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 -Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} - -void Welsh::print(const std::string &s="") const { - std::cout << s << ": Welsh (" << c_ << ")" << std::endl; -} - -bool Welsh::equals(const Base &expected, double tol) const { - const Welsh* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Welsh(c, reweight)); -} -#endif - /* ************************************************************************* */ // GemanMcClure /* ************************************************************************* */ @@ -992,16 +976,13 @@ DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { // L2WithDeadZone /* ************************************************************************* */ -L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { +L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) + : Base(reweight), k_(k) { if (k_ <= 0) { throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor."); } } -double L2WithDeadZone::residual(double error) const { - const double abs_error = std::abs(error); - return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); -} double L2WithDeadZone::weight(double error) const { // note that this code is slightly uglier than residual, because there are three distinct // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two @@ -1011,6 +992,11 @@ double L2WithDeadZone::weight(double error) const { else return (k_+error)/error; } +double L2WithDeadZone::residual(double error) const { + const double abs_error = std::abs(error); + return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); +} + void L2WithDeadZone::print(const std::string &s="") const { std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index d93a7f825..d6b0a7331 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -725,11 +725,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Null(const ReweightScheme reweight = Block) : Base(reweight) {} - virtual ~Null() {} - virtual double weight(double /*error*/) const { return 1.0; } - virtual double residual(double error) const { return error; } - virtual void print(const std::string &s) const; - virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } + ~Null() {} + double weight(double /*error*/) const { return 1.0; } + double residual(double error) const { return error; } + void print(const std::string &s) const; + bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; private: @@ -951,9 +951,9 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - L2WithDeadZone(double k, const ReweightScheme reweight = Block); - double residual(double error) const override; + L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double error) const override; + double residual(double error) const override; void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block);