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

Revert "renamed residual to loss, follow PR #188" 

This reverts commit 4e3542a743.
release/4.3a0
yetongumich 2020-04-02 11:24:26 -04:00
parent 17e8f21e75
commit 0acca9f8da
6 changed files with 68 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam.h
View File

@ -1458,7 +1458,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
void serializable() const; void serializable() const;
double weight(double error) const; double weight(double error) const;
double loss(double error) const; double residual(double error) const;
}; };
virtual class Fair: gtsam::noiseModel::mEstimator::Base { virtual class Fair: gtsam::noiseModel::mEstimator::Base {
@ -1469,7 +1469,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
void serializable() const; void serializable() const;
double weight(double error) const; double weight(double error) const;
double loss(double error) const; double residual(double error) const;
}; };
virtual class Huber: gtsam::noiseModel::mEstimator::Base { virtual class Huber: gtsam::noiseModel::mEstimator::Base {
@ -1480,7 +1480,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
void serializable() const; void serializable() const;
double weight(double error) const; double weight(double error) const;
double loss(double error) const; double residual(double error) const;
}; };
virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
@ -1491,7 +1491,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
void serializable() const; void serializable() const;
double weight(double error) const; double weight(double error) const;
double loss(double error) const; double residual(double error) const;
}; };
virtual class Tukey: gtsam::noiseModel::mEstimator::Base { virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
@ -1502,7 +1502,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
void serializable() const; void serializable() const;
double weight(double error) const; double weight(double error) const;
double loss(double error) const; double residual(double error) const;
}; };
virtual class Welsch: gtsam::noiseModel::mEstimator::Base { virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
@ -1513,7 +1513,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
void serializable() const; void serializable() const;
double weight(double error) const; double weight(double error) const;
double loss(double error) const; double residual(double error) const;
}; };
virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
@ -1524,7 +1524,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
void serializable() const; void serializable() const;
double weight(double error) const; double weight(double error) const;
double loss(double error) const; double residual(double error) const;
}; };
virtual class DCS: gtsam::noiseModel::mEstimator::Base { virtual class DCS: gtsam::noiseModel::mEstimator::Base {
@ -1535,7 +1535,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base {
void serializable() const; void serializable() const;
double weight(double error) const; double weight(double error) const;
double loss(double error) const; double residual(double error) const;
}; };
virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
@ -1546,7 +1546,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
void serializable() const; void serializable() const;
double weight(double error) const; double weight(double error) const;
double loss(double error) const; double residual(double error) const;
}; };
}///\namespace mEstimator }///\namespace mEstimator

16
gtsam/linear/LossFunctions.cpp
View File

@ -141,7 +141,7 @@ double Fair::weight(double error) const {
return 1.0 / (1.0 + std::abs(error) / c_); return 1.0 / (1.0 + std::abs(error) / c_);
} }
double Fair::loss(double error) const { 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_;
@ -175,7 +175,7 @@ double Huber::weight(double error) const {
return (absError <= k_) ? (1.0) : (k_ / absError); return (absError <= k_) ? (1.0) : (k_ / absError);
} }
double Huber::loss(double error) const { double Huber::residual(double error) const {
const double absError = std::abs(error); const double absError = std::abs(error);
if (absError <= k_) { // |x| <= k if (absError <= k_) { // |x| <= k
return error*error / 2; return error*error / 2;
@ -212,7 +212,7 @@ double Cauchy::weight(double error) const {
return ksquared_ / (ksquared_ + error*error); return ksquared_ / (ksquared_ + error*error);
} }
double Cauchy::loss(double error) const { double Cauchy::residual(double error) const {
const double val = std::log1p(error * error / ksquared_); const double val = std::log1p(error * error / ksquared_);
return ksquared_ * val * 0.5; return ksquared_ * val * 0.5;
} }
@ -249,7 +249,7 @@ double Tukey::weight(double error) const {
return 0.0; return 0.0;
} }
double Tukey::loss(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 one_minus_xc2 = 1.0 - error*error/csquared_; const double one_minus_xc2 = 1.0 - error*error/csquared_;
@ -285,7 +285,7 @@ double Welsch::weight(double error) const {
return std::exp(-xc2); return std::exp(-xc2);
} }
double Welsch::loss(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 * -std::expm1(-xc2); return csquared_ * 0.5 * -std::expm1(-xc2);
} }
@ -318,7 +318,7 @@ double GemanMcClure::weight(double error) const {
return c4/(c2error*c2error); return c4/(c2error*c2error);
} }
double GemanMcClure::loss(double error) const { double GemanMcClure::residual(double error) const {
const double c2 = c_*c_; const double c2 = c_*c_;
const double error2 = error*error; const double error2 = error*error;
return 0.5 * (c2 * error2) / (c2 + error2); return 0.5 * (c2 * error2) / (c2 + error2);
@ -356,7 +356,7 @@ double DCS::weight(double error) const {
return 1.0; return 1.0;
} }
double DCS::loss(double error) const { double DCS::residual(double error) const {
// This is the simplified version of Eq 9 from (Agarwal13icra) // This is the simplified version of Eq 9 from (Agarwal13icra)
// after you simplify and cancel terms. // after you simplify and cancel terms.
const double e2 = error*error; const double e2 = error*error;
@ -400,7 +400,7 @@ double L2WithDeadZone::weight(double error) const {
else return (k_+error)/error; else return (k_+error)/error;
} }
double L2WithDeadZone::loss(double error) const { double L2WithDeadZone::residual(double error) const {
const double abs_error = std::abs(error); const double abs_error = std::abs(error);
return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
} }

33
gtsam/linear/LossFunctions.h
View File

@ -36,12 +36,12 @@ namespace noiseModel {
* The mEstimator name space contains all robust error functions. * The mEstimator name space contains all robust error functions.
* It mirrors the exposition at * It mirrors the exposition at
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
* which talks about minimizing \sum \rho(r_i), where \rho is a loss function of choice. * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice.
* *
* To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples:
* *
* Name Symbol Least-Squares L1-norm Huber * Name Symbol Least-Squares L1-norm Huber
* Loss \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x|<k, 0.5*k^2 + k|x-k| otherwise * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x|<k, 0.5*k^2 + k|x-k| otherwise
* Derivative \phi(x) x sgn(x) x if |x|<k, k sgn(x) otherwise * Derivative \phi(x) x sgn(x) x if |x|<k, k sgn(x) otherwise
* Weight w(x)=\phi(x)/x 1 1/|x| 1 if |x|<k, k/|x| otherwise * Weight w(x)=\phi(x)/x 1 1/|x| 1 if |x|<k, k/|x| otherwise
* *
@ -75,20 +75,17 @@ class GTSAM_EXPORT Base {
* the quadratic function for an L2 penalty, the absolute value function for * the quadratic function for an L2 penalty, the absolute value function for
* an L1 penalty, etc. * an L1 penalty, etc.
* *
* TODO(mikebosse): When the loss function has as input the norm of the * TODO(mikebosse): When the residual function has as input the norm of the
* residual vector, then it prevents implementations of asymmeric loss * residual vector, then it prevents implementations of asymmeric loss
* functions. It would be better for this function to accept the vector and * functions. It would be better for this function to accept the vector and
* internally call the norm if necessary. * internally call the norm if necessary.
*/ */
virtual double loss(double error) const { return 0; }; virtual double residual(double error) const { return 0; };
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
virtual double residual(double error) const { return loss(error); };
#endif
/* /*
* This method is responsible for returning the weight function for a given * This method is responsible for returning the weight function for a given
* amount of error. The weight function is related to the analytic derivative * amount of error. The weight function is related to the analytic derivative
* of the loss function. See * of the residual function. See
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
* for details. This method is required when optimizing cost functions with * for details. This method is required when optimizing cost functions with
* robust penalties using iteratively re-weighted least squares. * robust penalties using iteratively re-weighted least squares.
@ -126,7 +123,7 @@ class GTSAM_EXPORT Base {
} }
}; };
/// Null class is the L2 norm and not robust; equivalent to a Gaussian noise model with no loss function. /// Null class is not robust so is a Gaussian ?
class GTSAM_EXPORT Null : public Base { class GTSAM_EXPORT Null : public Base {
public: public:
typedef boost::shared_ptr<Null> shared_ptr; typedef boost::shared_ptr<Null> shared_ptr;
@ -134,7 +131,7 @@ class GTSAM_EXPORT Null : public Base {
Null(const ReweightScheme reweight = Block) : Base(reweight) {} Null(const ReweightScheme reweight = Block) : Base(reweight) {}
~Null() {} ~Null() {}
double weight(double /*error*/) const { return 1.0; } double weight(double /*error*/) const { return 1.0; }
double loss(double error) const { return 0.5 * error * error; } double residual(double error) const { return error; }
void print(const std::string &s) const; void print(const std::string &s) const;
bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } bool equals(const Base & /*expected*/, double /*tol*/) const { return true; }
static shared_ptr Create(); static shared_ptr Create();
@ -158,7 +155,7 @@ class GTSAM_EXPORT Fair : public Base {
Fair(double c = 1.3998, const ReweightScheme reweight = Block); Fair(double c = 1.3998, const ReweightScheme reweight = Block);
double weight(double error) const override; double weight(double error) const override;
double loss(double error) const override; double residual(double error) const override;
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double c, const ReweightScheme reweight = Block); static shared_ptr Create(double c, const ReweightScheme reweight = Block);
@ -183,7 +180,7 @@ class GTSAM_EXPORT Huber : public Base {
Huber(double k = 1.345, const ReweightScheme reweight = Block); Huber(double k = 1.345, const ReweightScheme reweight = Block);
double weight(double error) const override; double weight(double error) const override;
double loss(double error) const override; double residual(double error) const override;
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -213,7 +210,7 @@ class GTSAM_EXPORT Cauchy : public Base {
Cauchy(double k = 0.1, const ReweightScheme reweight = Block); Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
double weight(double error) const override; double weight(double error) const override;
double loss(double error) const override; double residual(double error) const override;
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -238,7 +235,7 @@ class GTSAM_EXPORT Tukey : public Base {
Tukey(double c = 4.6851, const ReweightScheme reweight = Block); Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
double weight(double error) const override; double weight(double error) const override;
double loss(double error) const override; double residual(double error) const override;
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -263,7 +260,7 @@ class GTSAM_EXPORT Welsch : public Base {
Welsch(double c = 2.9846, const ReweightScheme reweight = Block); Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
double weight(double error) const override; double weight(double error) const override;
double loss(double error) const override; double residual(double error) const override;
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -299,7 +296,7 @@ class GTSAM_EXPORT GemanMcClure : public Base {
GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
~GemanMcClure() {} ~GemanMcClure() {}
double weight(double error) const override; double weight(double error) const override;
double loss(double error) const override; double residual(double error) const override;
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -329,7 +326,7 @@ class GTSAM_EXPORT DCS : public Base {
DCS(double c = 1.0, const ReweightScheme reweight = Block); DCS(double c = 1.0, const ReweightScheme reweight = Block);
~DCS() {} ~DCS() {}
double weight(double error) const override; double weight(double error) const override;
double loss(double error) const override; double residual(double error) const override;
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -362,7 +359,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
double weight(double error) const override; double weight(double error) const override;
double loss(double error) const override; double residual(double error) const override;
void print(const std::string &s) const override; void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override; bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);

6
gtsam/linear/NoiseModel.h
View File

@ -703,11 +703,11 @@ namespace gtsam {
{ Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
inline virtual Vector unwhiten(const Vector& /*v*/) const inline virtual Vector unwhiten(const Vector& /*v*/) const
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
// Fold the use of the m-estimator loss(...) function into distance(...) // Fold the use of the m-estimator residual(...) function into distance(...)
inline virtual double distance(const Vector& v) const inline virtual double distance(const Vector& v) const
{ return robust_->loss(this->unweightedWhiten(v).norm()); } { return robust_->residual(this->unweightedWhiten(v).norm()); }
inline virtual double distance_non_whitened(const Vector& v) const inline virtual double distance_non_whitened(const Vector& v) const
{ return robust_->loss(v.norm()); } { return robust_->residual(v.norm()); }
// TODO: these are really robust iterated re-weighting support functions // TODO: these are really robust iterated re-weighting support functions
virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(Vector& b) const;
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const; virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;

64
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -467,10 +467,10 @@ TEST(NoiseModel, robustFunctionFair)
DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8); DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8);
DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8); DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8);
DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8); DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8);
DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8); DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8);
DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8); DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8);
DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8); DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8);
} }
TEST(NoiseModel, robustFunctionHuber) TEST(NoiseModel, robustFunctionHuber)
@ -483,10 +483,10 @@ TEST(NoiseModel, robustFunctionHuber)
DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);
DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8); DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8);
DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8); DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8);
DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8); DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8);
DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8); DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8);
} }
TEST(NoiseModel, robustFunctionCauchy) TEST(NoiseModel, robustFunctionCauchy)
@ -499,10 +499,10 @@ TEST(NoiseModel, robustFunctionCauchy)
DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8); DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8);
DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8); DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8);
DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8);
DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8); DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8);
DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8); DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8);
DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8); DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8);
} }
TEST(NoiseModel, robustFunctionGemanMcClure) TEST(NoiseModel, robustFunctionGemanMcClure)
@ -514,10 +514,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure)
DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8); DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8);
DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8); DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8);
DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8); DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8);
DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8); DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8);
DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8); DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8);
DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8); DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8);
} }
TEST(NoiseModel, robustFunctionWelsch) TEST(NoiseModel, robustFunctionWelsch)
@ -530,10 +530,10 @@ TEST(NoiseModel, robustFunctionWelsch)
DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8); DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8);
DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8); DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8);
DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8); DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8);
DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8); DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8);
DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8); DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8);
DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8); DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8);
} }
TEST(NoiseModel, robustFunctionTukey) TEST(NoiseModel, robustFunctionTukey)
@ -546,10 +546,10 @@ TEST(NoiseModel, robustFunctionTukey)
DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8); DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8);
DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8); DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8);
DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8);
DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8); DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8);
DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8); DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8);
DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8);
} }
TEST(NoiseModel, robustFunctionDCS) TEST(NoiseModel, robustFunctionDCS)
@ -560,8 +560,8 @@ TEST(NoiseModel, robustFunctionDCS)
DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8); DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8);
DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8); DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8);
DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8); DOUBLES_EQUAL(0.5 , dcs->residual(error1), 1e-8);
DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8); DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8);
} }
TEST(NoiseModel, robustFunctionL2WithDeadZone) TEST(NoiseModel, robustFunctionL2WithDeadZone)
@ -576,12 +576,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone)
DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8); DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8);
DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8); DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8);
DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8); DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8);
DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8); DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8);
DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8); DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8);
DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8); DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8);
DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8); DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8);
DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8); DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8);
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
matlab/gtsam_examples/VisualizeMEstimators.m
View File

@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename)
rho = zeros(size(x)); rho = zeros(size(x));
for i = 1:size(x, 2) for i = 1:size(x, 2)
w(i) = model.weight(x(i)); w(i) = model.weight(x(i));
rho(i) = model.loss(x(i)); rho(i) = model.residual(x(i));
end end
psi = w .* x; psi = w .* x;