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 {
return 1.0 / (1.0 + std::abs(error) / c_);
double Fair::weight(double distance) const {
return 1.0 / (1.0 + std::abs(distance) / c_);
}
double Fair::loss(double error) const {
const double absError = std::abs(error);
double Fair::loss(double distance) const {
const double absError = std::abs(distance);
const double normalizedError = absError / c_;
const double c_2 = c_ * c_;
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 {
const double absError = std::abs(error);
double Huber::weight(double distance) const {
const double absError = std::abs(distance);
return (absError <= k_) ? (1.0) : (k_ / absError);
}
double Huber::loss(double error) const {
const double absError = std::abs(error);
double Huber::loss(double distance) const {
const double absError = std::abs(distance);
if (absError <= k_) { // |x| <= k
return error*error / 2;
return distance*distance / 2;
} else { // |x| > k
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 {
return ksquared_ / (ksquared_ + error*error);
double Cauchy::weight(double distance) const {
return ksquared_ / (ksquared_ + distance*distance);
}
double Cauchy::loss(double error) const {
const double val = std::log1p(error * error / ksquared_);
double Cauchy::loss(double distance) const {
const double val = std::log1p(distance * distance / ksquared_);
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 {
if (std::abs(error) <= c_) {
const double one_minus_xc2 = 1.0 - error*error/csquared_;
double Tukey::weight(double distance) const {
if (std::abs(distance) <= c_) {
const double one_minus_xc2 = 1.0 - distance*distance/csquared_;
return one_minus_xc2 * one_minus_xc2;
}
return 0.0;
}
double Tukey::loss(double error) const {
double absError = std::abs(error);
double Tukey::loss(double distance) const {
double absError = std::abs(distance);
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;
return csquared_ * (1 - t) / 6.0;
} 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) {}
double Welsch::weight(double error) const {
const double xc2 = (error*error)/csquared_;
double Welsch::weight(double distance) const {
const double xc2 = (distance*distance)/csquared_;
return std::exp(-xc2);
}
double Welsch::loss(double error) const {
const double xc2 = (error*error)/csquared_;
double Welsch::loss(double distance) const {
const double xc2 = (distance*distance)/csquared_;
return csquared_ * 0.5 * -std::expm1(-xc2);
}
@ -311,16 +311,16 @@ GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) {
}
double GemanMcClure::weight(double error) const {
double GemanMcClure::weight(double distance) const {
const double c2 = c_*c_;
const double c4 = c2*c2;
const double c2error = c2 + error*error;
const double c2error = c2 + distance*distance;
return c4/(c2error*c2error);
}
double GemanMcClure::loss(double error) const {
double GemanMcClure::loss(double distance) const {
const double c2 = c_*c_;
const double error2 = error*error;
const double error2 = distance*distance;
return 0.5 * (c2 * error2) / (c2 + error2);
}
@ -345,8 +345,8 @@ DCS::DCS(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) {
}
double DCS::weight(double error) const {
const double e2 = error*error;
double DCS::weight(double distance) const {
const double e2 = distance*distance;
if (e2 > c_)
{
const double w = 2.0*c_/(c_ + e2);
@ -356,10 +356,10 @@ double DCS::weight(double error) const {
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)
// after you simplify and cancel terms.
const double e2 = error*error;
const double e2 = distance*distance;
const double e4 = e2*e2;
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
// cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
// cases (deadzone, non-deadzone) in residual.
if (std::abs(error) <= k_) return 0.0;
else if (error > k_) return (-k_+error)/error;
else return (k_+error)/error;
if (std::abs(distance) <= k_) return 0.0;
else if (distance > k_) return (-k_+distance)/distance;
else return (k_+distance)/distance;
}
double L2WithDeadZone::loss(double error) const {
const double abs_error = std::abs(error);
double L2WithDeadZone::loss(double distance) const {
const double abs_error = std::abs(distance);
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
* 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
virtual double residual(double error) const { return loss(error); };
virtual double residual(double distance) const { return loss(distance); };
#endif
/*
* 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
* 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 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
* robust function */
@ -157,8 +157,8 @@ class GTSAM_EXPORT Fair : public Base {
typedef boost::shared_ptr<Fair> shared_ptr;
Fair(double c = 1.3998, const ReweightScheme reweight = Block);
double weight(double error) const override;
double loss(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
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;
Huber(double k = 1.345, const ReweightScheme reweight = Block);
double weight(double error) const override;
double loss(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
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;
Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
double weight(double error) const override;
double loss(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
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;
Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
double weight(double error) const override;
double loss(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
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;
Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
double weight(double error) const override;
double loss(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
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 weight(double error) const override;
double loss(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
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 weight(double error) const override;
double loss(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
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;
L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
double weight(double error) const override;
double loss(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
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::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(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr 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.
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;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
@ -677,7 +677,7 @@ namespace gtsam {
protected:
typedef mEstimator::Base RobustModel;
typedef noiseModel::Base NoiseModel;
typedef noiseModel::Gaussian NoiseModel;
const RobustModel::shared_ptr robust_; ///< robust error function 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."); }
// Fold the use of the m-estimator loss(...) function into error(...)
inline virtual double error(const Vector& v) const
{ return robust_->loss(this->unweightedWhiten(v).norm()); }
// inline virtual double distance_non_whitened(const Vector& v) const
// { return robust_->loss(v.norm()); }
{ return robust_->loss(noise_->mahalanobisDistance(v)); }
// TODO: these are really robust iterated re-weighting support functions
virtual void WhitenSystem(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 Vector unweightedWhiten(const Vector& v) const {
return noise_->unweightedWhiten(v);
return noise_->whiten(v);
}
virtual double weight(const Vector& v) const {
// Todo(mikebosse): make the robust weight function input a vector.
return robust_->weight(v.norm());
}
static shared_ptr Create(
const RobustModel::shared_ptr &robust, const noiseModel::Base::shared_ptr noise);
static shared_ptr Create(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);