diff --git a/gtsam.h b/gtsam.h index a2dd9ea43..33c075665 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1014,6 +1014,43 @@ virtual class Unit : gtsam::noiseModel::Isotropic { static gtsam::noiseModel::Unit* Create(size_t dim); void print(string s) const; }; + +namespace mEstimator { +virtual class Base { +}; + +virtual class Null: gtsam::noiseModel::mEstimator::Base { + Null(); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Null* Create(); +}; + +virtual class Fair: gtsam::noiseModel::mEstimator::Base { + Fair(double c); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Fair* Create(double c); +}; + +virtual class Huber: gtsam::noiseModel::mEstimator::Base { + Huber(double k); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Huber* Create(double k); +}; + +virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Tukey* Create(double k); +}; + +}///\namespace mEstimator + +virtual class Robust : gtsam::noiseModel::Base { + Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + void print(string s) const; +}; + }///\namespace noiseModel #include diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index d41cec293..9cdb85c70 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -438,7 +438,7 @@ void Unit::print(const std::string& name) const { // M-Estimator /* ************************************************************************* */ -namespace MEstimator { +namespace mEstimator { /** produce a weight vector according to an error vector and the implemented * robust function */ @@ -606,6 +606,36 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { return shared_ptr(new Huber(c, reweight)); } +/* ************************************************************************* */ +// Tukey +/* ************************************************************************* */ +Tukey::Tukey(const double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double Tukey::weight(const double &error) const { + if (fabs(error) <= c_) { + double xc2 = (error/c_)*(error/c_); + double one_xc22 = (1.0-xc2)*(1.0-xc2); + return one_xc22; + } + return 0.0; +} + +void Tukey::print(const std::string &s="") const { + std::cout << s << ": Tukey (" << c_ << ")" << std::endl; +} + +bool Tukey::equals(const Base &expected, const double tol) const { + const Tukey* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) { + return shared_ptr(new Tukey(c, reweight)); +} + } // namespace MEstimator /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index c70de1c7c..73d9756a1 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -572,7 +572,7 @@ namespace gtsam { // TODO: should not really exist /// The MEstimator namespace contains all robust error functions (not models) - namespace MEstimator { + namespace mEstimator { //--------------------------------------------------------------------------------------- @@ -661,6 +661,23 @@ namespace gtsam { Huber(){} }; + /// Tukey implements the "Tukey" robust error model (Zhang97ivc) + class Tukey : public Base { + public: + typedef boost::shared_ptr shared_ptr; + protected: + double c_; + public: + Tukey(const double c, const ReweightScheme reweight = Block); + virtual ~Tukey() {} + virtual double weight(const double &error) const ; + virtual void print(const std::string &s) const ; + virtual bool equals(const Base& expected, const double tol=1e-8) const ; + static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + private: + Tukey(){} + }; + } ///\namespace MEstimator /// Base class for robust error models @@ -669,7 +686,7 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; protected: - typedef MEstimator::Base RobustModel; + typedef mEstimator::Base RobustModel; typedef noiseModel::Base NoiseModel; const RobustModel::shared_ptr robust_; ///< robust error function used