diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 4d9c7883b..1569f61da 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -833,11 +833,10 @@ GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double GemanMcClure::weight(double error) const { +double GemanMcClure::sqrtWeight(double error) const { const double c2 = c_*c_; - const double c4 = c2*c2; const double c2error = c2 + error*error; - return c4/(c2error*c2error); + return c2/c2error; } void GemanMcClure::print(const std::string &s="") const { @@ -861,12 +860,12 @@ DCS::DCS(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double DCS::weight(double error) const { +double DCS::sqrtWeight(double error) const { const double e2 = error*error; if (e2 > c_) { const double w = 2.0*c_/(c_ + e2); - return w*w; + return w; } return 1.0; diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index e495921c2..b1e7d562a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -661,6 +661,7 @@ namespace gtsam { public: Base(const ReweightScheme reweight = Block):reweight_(reweight) {} virtual ~Base() {} + ReweightScheme reweightScheme() const { return reweight_; } /* * This method is responsible for returning the total penalty for a given amount of error. @@ -685,14 +686,14 @@ namespace gtsam { * for details. This method is required when optimizing cost functions with robust penalties * using iteratively re-weighted least squares. */ - virtual double weight(double error) const = 0; + double weight(double error) const { + return sqrtWeight(error)*sqrtWeight(error); + } virtual void print(const std::string &s) const = 0; virtual bool equals(const Base& expected, double tol=1e-8) const = 0; - double sqrtWeight(double error) const { - return std::sqrt(weight(error)); - } + virtual double sqrtWeight(double error) const = 0; /** produce a weight vector according to an error vector and the implemented * robust function */ @@ -726,7 +727,7 @@ namespace gtsam { Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} - virtual double weight(double /*error*/) const { return 1.0; } + virtual double sqrtWeight(double /*error*/) const { return 1.0; } virtual void print(const std::string &s) const; virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; @@ -749,8 +750,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Fair(double c = 1.3998, const ReweightScheme reweight = Block); - double weight(double error) const { - return 1.0 / (1.0 + fabs(error) / c_); + double sqrtWeight(double error) const { + return 1.0 / sqrt(1.0 + fabs(error) / c_); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; @@ -775,8 +776,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Huber(double k = 1.345, const ReweightScheme reweight = Block); - double weight(double error) const { - return (error < k_) ? (1.0) : (k_ / fabs(error)); + double sqrtWeight(double error) const { + return (error < k_) ? (1.0) : sqrt(k_ / fabs(error)); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; @@ -805,8 +806,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Cauchy(double k = 0.1, const ReweightScheme reweight = Block); - double weight(double error) const { - return ksquared_ / (ksquared_ + error*error); + double sqrtWeight(double error) const { + return k_ / sqrt(ksquared_ + error*error); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; @@ -831,10 +832,10 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - double weight(double error) const { + double sqrtWeight(double error) const { if (std::fabs(error) <= c_) { double xc2 = error*error/csquared_; - return (1.0-xc2)*(1.0-xc2); + return (1.0-xc2); } return 0.0; } @@ -861,9 +862,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Welsh(double c = 2.9846, const ReweightScheme reweight = Block); - double weight(double error) const { + double sqrtWeight(double error) const { double xc2 = (error*error)/csquared_; - return std::exp(-xc2); + return std::exp(-xc2/2.0); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const; @@ -891,7 +892,7 @@ namespace gtsam { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); virtual ~GemanMcClure() {} - virtual double weight(double error) const; + virtual double sqrtWeight(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) ; @@ -920,7 +921,7 @@ namespace gtsam { DCS(double c = 1.0, const ReweightScheme reweight = Block); virtual ~DCS() {} - virtual double weight(double error) const; + virtual double sqrtWeight(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) ; @@ -955,13 +956,13 @@ namespace gtsam { const double abs_error = fabs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } - double weight(double error) const { + double sqrtWeight(double error) const { // note that this code is slightly uglier than above, because there are three distinct // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two // cases (deadzone, non-deadzone) above. if (fabs(error) <= k_) return 0.0; - else if (error > k_) return (-k_+error)/error; - else return (k_+error)/error; + else if (error > k_) return sqrt((-k_+error)/error); + else return sqrt((k_+error)/error); } void print(const std::string &s) const; bool equals(const Base& expected, double tol=1e-8) const;