12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Improve numerical precision of residual functions

release/4.3a0
Michael Bosse 2019-11-27 13:17:59 -08:00
parent 8cdb009af9
commit 516ccf532c
1 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/linear/LossFunctions.cpp
View File

@ -145,7 +145,7 @@ double Fair::residual(double error) const {
const double absError = std::abs(error); const double absError = std::abs(error);
const double normalizedError = absError / c_; const double normalizedError = absError / c_;
const double c_2 = c_ * c_; const double c_2 = c_ * c_;
return c_2 * (normalizedError - std::log(1 + normalizedError)); return c_2 * (normalizedError - std::log1p(normalizedError));
} }
void Fair::print(const std::string &s="") const void Fair::print(const std::string &s="") const
@ -213,8 +213,7 @@ double Cauchy::weight(double error) const {
} }
double Cauchy::residual(double error) const { double Cauchy::residual(double error) const {
const double xc2 = error / k_; const double val = std::log1p(error * error / ksquared_);
const double val = std::log(1 + (xc2*xc2));
return ksquared_ * val * 0.5; return ksquared_ * val * 0.5;
} }
@ -244,8 +243,8 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c
double Tukey::weight(double error) const { double Tukey::weight(double error) const {
if (std::abs(error) <= c_) { if (std::abs(error) <= c_) {
const double xc2 = error*error/csquared_; const double one_minus_xc2 = 1.0 - error*error/csquared_;
return (1.0-xc2)*(1.0-xc2); return one_minus_xc2 * one_minus_xc2;
} }
return 0.0; return 0.0;
} }
@ -253,8 +252,8 @@ double Tukey::weight(double error) const {
double Tukey::residual(double error) const { double Tukey::residual(double error) const {
double absError = std::abs(error); double absError = std::abs(error);
if (absError <= c_) { if (absError <= c_) {
const double xc2 = error*error/csquared_; const double one_minus_xc2 = 1.0 - error*error/csquared_;
const double t = (1 - xc2)*(1 - xc2)*(1 - xc2); const double t = one_minus_xc2*one_minus_xc2*one_minus_xc2;
return csquared_ * (1 - t) / 6.0; return csquared_ * (1 - t) / 6.0;
} else { } else {
return csquared_ / 6.0; return csquared_ / 6.0;
@ -288,7 +287,7 @@ double Welsch::weight(double error) const {
double Welsch::residual(double error) const { double Welsch::residual(double error) const {
const double xc2 = (error*error)/csquared_; const double xc2 = (error*error)/csquared_;
return csquared_ * 0.5 * (1 - std::exp(-xc2) ); return csquared_ * 0.5 * -std::expm1(-xc2);
} }
void Welsch::print(const std::string &s="") const { void Welsch::print(const std::string &s="") const {