diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index be2e8bb5b..4e48fb5f2 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -614,6 +614,36 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { return shared_ptr(new Huber(c, reweight)); } +/* ************************************************************************* */ +// Cauchy +/* ************************************************************************* */ + +Cauchy::Cauchy(const double k, const ReweightScheme reweight) + : Base(reweight), k_(k) { + if ( k_ <= 0 ) { + cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl; + k_ = 1.0; + } +} + +double Cauchy::weight(const double &error) const { + return k_*k_ / (k_*k_ + error*error); +} + +void Cauchy::print(const std::string &s="") const { + cout << s << "cauchy (" << k_ << ")" << endl; +} + +bool Cauchy::equals(const Base &expected, const double tol) const { + const Cauchy* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(k_ - p->k_) < tol; +} + +Cauchy::shared_ptr Cauchy::Create(const double c, const ReweightScheme reweight) { + return shared_ptr(new Cauchy(c, reweight)); +} + /* ************************************************************************* */ // Tukey /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 6dc5640e0..a07f80c60 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -689,6 +689,35 @@ namespace gtsam { } }; + /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by: + /// Dipl.-Inform. Jan Oberländer (M.Sc.), FZI Research Center for + /// Information Technology, Karlsruhe, Germany. + /// oberlaender@fzi.de + /// Thanks Jan! + class GTSAM_EXPORT Cauchy : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + virtual ~Cauchy() {} + Cauchy(const double k = 0.1, const ReweightScheme reweight = Block); + 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) ; + + protected: + double k_; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(k_); + } + }; + /// Tukey implements the "Tukey" robust error model (Zhang97ivc) class GTSAM_EXPORT Tukey : public Base { public: