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

noise in robust should be gaussian, change variable names

release/4.3a0
yetongumich 2020-04-03 14:04:02 -04:00
parent 9e4be90111
commit 12930acdf6
4 changed files with 71 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
gtsam/linear/LossFunctions.cpp
View File

@ -137,12 +137,12 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
} }
} }
double Fair::weight(double error) const { double Fair::weight(double distance) const {
return 1.0 / (1.0 + std::abs(error) / c_); return 1.0 / (1.0 + std::abs(distance) / c_);
} }
double Fair::loss(double error) const { double Fair::loss(double distance) const {
const double absError = std::abs(error); const double absError = std::abs(distance);
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::log1p(normalizedError)); return c_2 * (normalizedError - std::log1p(normalizedError));
@ -170,15 +170,15 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
} }
} }
double Huber::weight(double error) const { double Huber::weight(double distance) const {
const double absError = std::abs(error); const double absError = std::abs(distance);
return (absError <= k_) ? (1.0) : (k_ / absError); return (absError <= k_) ? (1.0) : (k_ / absError);
} }
double Huber::loss(double error) const { double Huber::loss(double distance) const {
const double absError = std::abs(error); const double absError = std::abs(distance);
if (absError <= k_) { // |x| <= k if (absError <= k_) { // |x| <= k
return error*error / 2; return distance*distance / 2;
} else { // |x| > k } else { // |x| > k
return k_ * (absError - (k_/2)); return k_ * (absError - (k_/2));
} }
@ -208,12 +208,12 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k),
} }
} }
double Cauchy::weight(double error) const { double Cauchy::weight(double distance) const {
return ksquared_ / (ksquared_ + error*error); return ksquared_ / (ksquared_ + distance*distance);
} }
double Cauchy::loss(double error) const { double Cauchy::loss(double distance) const {
const double val = std::log1p(error * error / ksquared_); const double val = std::log1p(distance * distance / ksquared_);
return ksquared_ * val * 0.5; return ksquared_ * val * 0.5;
} }
@ -241,18 +241,18 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c
} }
} }
double Tukey::weight(double error) const { double Tukey::weight(double distance) const {
if (std::abs(error) <= c_) { if (std::abs(distance) <= c_) {
const double one_minus_xc2 = 1.0 - error*error/csquared_; const double one_minus_xc2 = 1.0 - distance*distance/csquared_;
return one_minus_xc2 * one_minus_xc2; return one_minus_xc2 * one_minus_xc2;
} }
return 0.0; return 0.0;
} }
double Tukey::loss(double error) const { double Tukey::loss(double distance) const {
double absError = std::abs(error); double absError = std::abs(distance);
if (absError <= c_) { if (absError <= c_) {
const double one_minus_xc2 = 1.0 - error*error/csquared_; const double one_minus_xc2 = 1.0 - distance*distance/csquared_;
const double t = one_minus_xc2*one_minus_xc2*one_minus_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 {
@ -280,13 +280,13 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
double Welsch::weight(double error) const { double Welsch::weight(double distance) const {
const double xc2 = (error*error)/csquared_; const double xc2 = (distance*distance)/csquared_;
return std::exp(-xc2); return std::exp(-xc2);
} }
double Welsch::loss(double error) const { double Welsch::loss(double distance) const {
const double xc2 = (error*error)/csquared_; const double xc2 = (distance*distance)/csquared_;
return csquared_ * 0.5 * -std::expm1(-xc2); return csquared_ * 0.5 * -std::expm1(-xc2);
} }
@ -311,16 +311,16 @@ GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) { : Base(reweight), c_(c) {
} }
double GemanMcClure::weight(double error) const { double GemanMcClure::weight(double distance) const {
const double c2 = c_*c_; const double c2 = c_*c_;
const double c4 = c2*c2; const double c4 = c2*c2;
const double c2error = c2 + error*error; const double c2error = c2 + distance*distance;
return c4/(c2error*c2error); return c4/(c2error*c2error);
} }
double GemanMcClure::loss(double error) const { double GemanMcClure::loss(double distance) const {
const double c2 = c_*c_; const double c2 = c_*c_;
const double error2 = error*error; const double error2 = distance*distance;
return 0.5 * (c2 * error2) / (c2 + error2); return 0.5 * (c2 * error2) / (c2 + error2);
} }
@ -345,8 +345,8 @@ DCS::DCS(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) { : Base(reweight), c_(c) {
} }
double DCS::weight(double error) const { double DCS::weight(double distance) const {
const double e2 = error*error; const double e2 = distance*distance;
if (e2 > c_) if (e2 > c_)
{ {
const double w = 2.0*c_/(c_ + e2); const double w = 2.0*c_/(c_ + e2);
@ -356,10 +356,10 @@ double DCS::weight(double error) const {
return 1.0; return 1.0;
} }
double DCS::loss(double error) const { double DCS::loss(double distance) 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 = distance*distance;
const double e4 = e2*e2; const double e4 = e2*e2;
const double c2 = c_*c_; const double c2 = c_*c_;
@ -391,17 +391,17 @@ L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight)
} }
} }
double L2WithDeadZone::weight(double error) const { double L2WithDeadZone::weight(double distance) const {
// note that this code is slightly uglier than residual, because there are three distinct // 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 // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
// cases (deadzone, non-deadzone) in residual. // cases (deadzone, non-deadzone) in residual.
if (std::abs(error) <= k_) return 0.0; if (std::abs(distance) <= k_) return 0.0;
else if (error > k_) return (-k_+error)/error; else if (distance > k_) return (-k_+distance)/distance;
else return (k_+error)/error; else return (k_+distance)/distance;
} }
double L2WithDeadZone::loss(double error) const { double L2WithDeadZone::loss(double distance) const {
const double abs_error = std::abs(error); const double abs_error = std::abs(distance);
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);
} }

40
gtsam/linear/LossFunctions.h
View File

@ -80,10 +80,10 @@ class GTSAM_EXPORT Base {
* 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 loss(double distance) const { return 0; };
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
virtual double residual(double error) const { return loss(error); }; virtual double residual(double distance) const { return loss(distance); };
#endif #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
@ -93,12 +93,12 @@ class GTSAM_EXPORT Base {
* 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.
*/ */
virtual double weight(double error) const = 0; virtual double weight(double distance) const = 0;
virtual void print(const std::string &s) const = 0; virtual void print(const std::string &s) const = 0;
virtual bool equals(const Base &expected, double tol = 1e-8) const = 0; virtual bool equals(const Base &expected, double tol = 1e-8) const = 0;
double sqrtWeight(double error) const { return std::sqrt(weight(error)); } double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); }
/** produce a weight vector according to an error vector and the implemented /** produce a weight vector according to an error vector and the implemented
* robust function */ * robust function */
@ -157,8 +157,8 @@ class GTSAM_EXPORT Fair : public Base {
typedef boost::shared_ptr<Fair> shared_ptr; typedef boost::shared_ptr<Fair> shared_ptr;
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 distance) const override;
double loss(double error) const override; double loss(double distance) 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);
@ -182,8 +182,8 @@ class GTSAM_EXPORT Huber : public Base {
typedef boost::shared_ptr<Huber> shared_ptr; typedef boost::shared_ptr<Huber> shared_ptr;
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 distance) const override;
double loss(double error) const override; double loss(double distance) 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);
@ -212,8 +212,8 @@ class GTSAM_EXPORT Cauchy : public Base {
typedef boost::shared_ptr<Cauchy> shared_ptr; typedef boost::shared_ptr<Cauchy> shared_ptr;
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 distance) const override;
double loss(double error) const override; double loss(double distance) 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);
@ -237,8 +237,8 @@ class GTSAM_EXPORT Tukey : public Base {
typedef boost::shared_ptr<Tukey> shared_ptr; typedef boost::shared_ptr<Tukey> shared_ptr;
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 distance) const override;
double loss(double error) const override; double loss(double distance) 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);
@ -262,8 +262,8 @@ class GTSAM_EXPORT Welsch : public Base {
typedef boost::shared_ptr<Welsch> shared_ptr; typedef boost::shared_ptr<Welsch> shared_ptr;
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 distance) const override;
double loss(double error) const override; double loss(double distance) 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);
@ -298,8 +298,8 @@ 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 distance) const override;
double loss(double error) const override; double loss(double distance) 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);
@ -328,8 +328,8 @@ 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 distance) const override;
double loss(double error) const override; double loss(double distance) 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);
@ -361,8 +361,8 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
typedef boost::shared_ptr<L2WithDeadZone> shared_ptr; typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
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 distance) const override;
double loss(double error) const override; double loss(double distance) 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);

7
gtsam/linear/NoiseModel.cpp
View File

@ -654,6 +654,13 @@ void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
robust_->reweight(A1,A2,A3,b); robust_->reweight(A1,A2,A3,b);
} }
Robust::shared_ptr Robust::Create(const RobustModel::shared_ptr& robust,
const noiseModel::Base::shared_ptr noise) {
SharedGaussian gaussian;
gaussian = boost::dynamic_pointer_cast<noiseModel::Gaussian>(noise);
return shared_ptr(new Robust(robust, gaussian));
}
Robust::shared_ptr Robust::Create( Robust::shared_ptr Robust::Create(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
return shared_ptr(new Robust(robust,noise)); return shared_ptr(new Robust(robust,noise));

13
gtsam/linear/NoiseModel.h
View File

@ -90,7 +90,7 @@ namespace gtsam {
/// Unwhiten an error vector. /// Unwhiten an error vector.
virtual Vector unwhiten(const Vector& v) const = 0; virtual Vector unwhiten(const Vector& v) const = 0;
/// calculate the error value given error vector /// calculate the error value given measurement error vector
virtual double error(const Vector& v) const = 0; virtual double error(const Vector& v) const = 0;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
@ -677,7 +677,7 @@ namespace gtsam {
protected: protected:
typedef mEstimator::Base RobustModel; typedef mEstimator::Base RobustModel;
typedef noiseModel::Base NoiseModel; typedef noiseModel::Gaussian NoiseModel;
const RobustModel::shared_ptr robust_; ///< robust error function used const RobustModel::shared_ptr robust_; ///< robust error function used
const NoiseModel::shared_ptr noise_; ///< noise model used const NoiseModel::shared_ptr noise_; ///< noise model used
@ -712,9 +712,7 @@ namespace gtsam {
{ 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 error(...) // Fold the use of the m-estimator loss(...) function into error(...)
inline virtual double error(const Vector& v) const inline virtual double error(const Vector& v) const
{ return robust_->loss(this->unweightedWhiten(v).norm()); } { return robust_->loss(noise_->mahalanobisDistance(v)); }
// inline virtual double distance_non_whitened(const Vector& v) const
// { return robust_->loss(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;
@ -723,13 +721,16 @@ namespace gtsam {
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
virtual Vector unweightedWhiten(const Vector& v) const { virtual Vector unweightedWhiten(const Vector& v) const {
return noise_->unweightedWhiten(v); return noise_->whiten(v);
} }
virtual double weight(const Vector& v) const { virtual double weight(const Vector& v) const {
// Todo(mikebosse): make the robust weight function input a vector. // Todo(mikebosse): make the robust weight function input a vector.
return robust_->weight(v.norm()); return robust_->weight(v.norm());
} }
static shared_ptr Create(
const RobustModel::shared_ptr &robust, const noiseModel::Base::shared_ptr noise);
static shared_ptr Create( static shared_ptr Create(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);