diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 609c03acf..29dc5aa0b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -799,6 +799,32 @@ Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsh(c, reweight)); } +/* ************************************************************************* */ +// DCS +/* ************************************************************************* */ +DCS::DCS(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double DCS::weight(double error) const { + double scale = 2.0*c_/(c_ + error*error); + return std::min(scale, 1.0); +} + +void DCS::print(const std::string &s="") const { + std::cout << s << ": DCS (" << c_ << ")" << std::endl; +} + +bool DCS::equals(const Base &expected, double tol) const { + const DCS* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new DCS(c, reweight)); +} + } // namespace mEstimator /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2a4d7c746..4c2f68719 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -823,6 +823,32 @@ namespace gtsam { } }; + /// DCS implements the Dynamic Covariance Scaling robust error model + /// from the paper Robust Map Optimization (Agarwal13icra). + class GTSAM_EXPORT DCS : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + DCS(double c = 1.0, const ReweightScheme reweight = Block); + virtual ~DCS() {} + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; + + protected: + double c_; + + 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(c_); + } + }; + } ///\namespace mEstimator /// Base class for robust error models