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

removed const declarator from and functions, implemented DCS residual

release/4.3a0
Varun Agrawal 2019-10-08 21:25:41 -04:00
parent 4be8d8a758
commit 431c37090a
2 changed files with 59 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
gtsam/linear/NoiseModel.cpp
View File

@ -728,10 +728,10 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
} }
} }
double Fair::weight(const double error) const { 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::residual(const 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_;
@ -760,12 +760,12 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
} }
} }
double Huber::weight(const double error) const { double Huber::weight(double error) const {
const double absError = std::abs(error); const double absError = std::abs(error);
return (absError <= k_) ? (1.0) : (k_ / absError); return (absError <= k_) ? (1.0) : (k_ / absError);
} }
double Huber::residual(const 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;
@ -798,11 +798,11 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k),
} }
} }
double Cauchy::weight(const double error) const { double Cauchy::weight(double error) const {
return ksquared_ / (ksquared_ + error*error); return ksquared_ / (ksquared_ + error*error);
} }
double Cauchy::residual(const double error) const { double Cauchy::residual(double error) const {
const double xc2 = error / k_; const double xc2 = error / k_;
const double val = std::log(1 + (xc2*xc2)); const double val = std::log(1 + (xc2*xc2));
return ksquared_ * val * 0.5; return ksquared_ * val * 0.5;
@ -832,14 +832,14 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c
} }
} }
double Tukey::weight(const 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 xc2 = error*error/csquared_;
return (1.0-xc2)*(1.0-xc2); return (1.0-xc2)*(1.0-xc2);
} }
return 0.0; return 0.0;
} }
double Tukey::residual(const 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 xc2 = error*error/csquared_;
@ -870,12 +870,12 @@ 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(const double error) const { double Welsch::weight(double error) const {
const double xc2 = (error*error)/csquared_; const double xc2 = (error*error)/csquared_;
return std::exp(-xc2); return std::exp(-xc2);
} }
double Welsch::residual(const 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 * (1 - std::exp(-xc2) );
} }
@ -964,6 +964,16 @@ double DCS::weight(double error) const {
return 1.0; return 1.0;
} }
double DCS::residual(double error) const {
// This is the simplified version of Eq 9 from (Agarwal13icra)
// after you simplify and cancel terms.
const double e2 = error*error;
const double e4 = e2*e2;
const double c2 = c_*c_;
return (c2*e2 + c_*e4) / ((e2 + c_)*(e2 + c_));
}
void DCS::print(const std::string &s="") const { void DCS::print(const std::string &s="") const {
std::cout << s << ": DCS (" << c_ << ")" << std::endl; std::cout << s << ": DCS (" << c_ << ")" << std::endl;
} }
@ -988,6 +998,19 @@ L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) : Base(r
} }
} }
double L2WithDeadZone::residual(double error) const {
const double abs_error = std::abs(error);
return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
}
double L2WithDeadZone::weight(double error) 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;
}
void L2WithDeadZone::print(const std::string &s="") const { void L2WithDeadZone::print(const std::string &s="") const {
std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl;
} }

61
gtsam/linear/NoiseModel.h
View File

@ -676,7 +676,7 @@ namespace gtsam {
* evaluating the total penalty. But for now, I'm leaving this residual method as pure * evaluating the total penalty. But for now, I'm leaving this residual method as pure
* virtual, so the existing mEstimators can inherit this default fallback behavior. * virtual, so the existing mEstimators can inherit this default fallback behavior.
*/ */
virtual double residual(const double error) const { return 0; }; virtual double residual(double error) const { return 0; };
/* /*
* This method is responsible for returning the weight function for a given amount of error. * This method is responsible for returning the weight function for a given amount of error.
@ -685,7 +685,7 @@ namespace gtsam {
* for details. This method is required when optimizing cost functions with robust penalties * for details. This method is required when optimizing cost functions with robust penalties
* using iteratively re-weighted least squares. * using iteratively re-weighted least squares.
*/ */
virtual double weight(const double error) const = 0; virtual double weight(double error) 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;
@ -726,8 +726,8 @@ namespace gtsam {
Null(const ReweightScheme reweight = Block) : Base(reweight) {} Null(const ReweightScheme reweight = Block) : Base(reweight) {}
virtual ~Null() {} virtual ~Null() {}
virtual double weight(const double /*error*/) const { return 1.0; } virtual double weight(double /*error*/) const { return 1.0; }
virtual double residual(const double error) const { return error; } virtual double residual(double error) const { return error; }
virtual void print(const std::string &s) const; virtual void print(const std::string &s) const;
virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
static shared_ptr Create() ; static shared_ptr Create() ;
@ -750,8 +750,8 @@ namespace gtsam {
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(const double error) const; double weight(double error) const;
double residual(const double error) const; double residual(double error) const;
void print(const std::string &s) const; void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const; bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;
@ -775,8 +775,8 @@ namespace gtsam {
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(const double error) const; double weight(double error) const;
double residual(const double error) const; double residual(double error) const;
void print(const std::string &s) const; void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const; bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
@ -804,8 +804,8 @@ namespace gtsam {
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(const double error) const; double weight(double error) const;
double residual(const double error) const; double residual(double error) const;
void print(const std::string &s) const; void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const; bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
@ -829,8 +829,8 @@ namespace gtsam {
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(const double error) const; double weight(double error) const;
double residual(const double error) const; double residual(double error) const;
void print(const std::string &s) const; void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const; bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
@ -854,8 +854,8 @@ namespace gtsam {
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(const double error) const; double weight(double error) const;
double residual(const double error) const; double residual(double error) const;
void print(const std::string &s) const; void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const; bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
@ -889,11 +889,11 @@ namespace gtsam {
typedef boost::shared_ptr<GemanMcClure> shared_ptr; typedef boost::shared_ptr<GemanMcClure> shared_ptr;
GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
virtual ~GemanMcClure() {} ~GemanMcClure() {}
virtual double weight(const double error) const; double weight(double error) const;
virtual double residual(const double error) const; double residual(double error) const;
virtual void print(const std::string &s) const; void print(const std::string &s) const;
virtual bool equals(const Base& expected, double tol=1e-8) const; bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
protected: protected:
@ -919,10 +919,11 @@ namespace gtsam {
typedef boost::shared_ptr<DCS> shared_ptr; typedef boost::shared_ptr<DCS> shared_ptr;
DCS(double c = 1.0, const ReweightScheme reweight = Block); DCS(double c = 1.0, const ReweightScheme reweight = Block);
virtual ~DCS() {} ~DCS() {}
virtual double weight(const double error) const; double weight(double error) const;
virtual void print(const std::string &s) const; double residual(double error) const;
virtual bool equals(const Base& expected, double tol=1e-8) const; void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
protected: protected:
@ -951,18 +952,8 @@ namespace gtsam {
typedef boost::shared_ptr<L2WithDeadZone> shared_ptr; typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
L2WithDeadZone(double k, const ReweightScheme reweight = Block); L2WithDeadZone(double k, const ReweightScheme reweight = Block);
double residual(const double error) const { double residual(double error) const;
const double abs_error = std::abs(error); double weight(double error) const;
return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
}
double weight(const 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 (std::abs(error) <= k_) return 0.0;
else if (error > k_) return (-k_+error)/error;
else return (k_+error)/error;
}
void print(const std::string &s) const; void print(const std::string &s) const;
bool equals(const Base& expected, double tol=1e-8) const; bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block); static shared_ptr Create(double k, const ReweightScheme reweight = Block);