diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 5734c6c2e..131dd04ff 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -9,12 +9,11 @@ * -------------------------------------------------------------------------- */ -/* - * NoiseModel - * - * Created on: Jan 13, 2010 - * Author: Richard Roberts - * Author: Frank Dellaert +/** + * @file NoiseModel.cpp + * @date Jan 13, 2010 + * @author Richard Roberts + * @author Frank Dellaert */ #include @@ -437,6 +436,7 @@ void Unit::print(const std::string& name) const { /* ************************************************************************* */ namespace MEstimator { + Vector Base::weight(const Vector &error) const { const size_t n = error.rows(); Vector w(n); @@ -490,6 +490,10 @@ void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { } } +/* ************************************************************************* */ +// Null model +/* ************************************************************************* */ + void Null::print(const std::string &s) const { cout << s << ": null ()" << endl; } @@ -504,6 +508,10 @@ Fair::Fair(const double c, const ReweightScheme reweight) } } +/* ************************************************************************* */ +// Fair +/* ************************************************************************* */ + double Fair::weight(const double &error) const { return 1.0 / (1.0 + fabs(error)/c_); } @@ -527,23 +535,30 @@ Huber::Huber(const double k, const ReweightScheme reweight) } } -double Huber::weight(const double &error) const -{ return (error < k_) ? (1.0) : (k_ / fabs(error)); } +/* ************************************************************************* */ +// Huber +/* ************************************************************************* */ -void Huber::print(const std::string &s) const -{ cout << s << ": huber (" << k_ << ")" << endl; } +double Huber::weight(const double &error) const { + return (error < k_) ? (1.0) : (k_ / fabs(error)); +} + +void Huber::print(const std::string &s) const { + cout << s << ": huber (" << k_ << ")" << endl; +} bool Huber::equals(const Base &expected, const double tol) const { - const Huber* p = dynamic_cast (&expected); - if (p == NULL) return false; - return fabs(k_ - p->k_ ) < tol; + const Huber* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(k_ - p->k_) < tol; } -Huber::shared_ptr Huber::Create(const double c) -{ return shared_ptr(new Huber(c)); } - +Huber::shared_ptr Huber::Create(const double c) { + return shared_ptr(new Huber(c)); } +} // namespace MEstimator + /* ************************************************************************* */ // Robust /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 9577b2b09..2bce50d3c 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -9,12 +9,11 @@ * -------------------------------------------------------------------------- */ -/* - * NoiseModel.h - * - * Created on: Jan 13, 2010 - * Author: Richard Roberts - * Author: Frank Dellaert +/** + * @file NoiseModel.h + * @date Jan 13, 2010 + * @author Richard Roberts + * @author Frank Dellaert */ #pragma once @@ -27,6 +26,7 @@ namespace gtsam { struct SharedDiagonal; // forward declare + /// All noise models live in the noiseModel namespace namespace noiseModel { class Gaussian; @@ -35,6 +35,8 @@ namespace gtsam { class Isotropic; class Unit; + //--------------------------------------------------------------------------------------- + /** * noiseModel::Base is the abstract base class for all noise models. * @@ -56,9 +58,7 @@ namespace gtsam { Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} - /** - * Dimensionality - */ + /// Dimensionality inline size_t dim() const { return dim_;} virtual void print(const std::string& name) const = 0; @@ -100,6 +100,8 @@ namespace gtsam { } }; + //--------------------------------------------------------------------------------------- + /** * Gaussian implements the mathematical model * |R*x|^2 = |y|^2 with R'*R=inv(Sigma) @@ -114,7 +116,7 @@ namespace gtsam { protected: - /* Matrix square root of information matrix (R) */ + /** Matrix square root of information matrix (R) */ boost::optional sqrt_information_; private: @@ -223,6 +225,8 @@ namespace gtsam { }; // Gaussian + //--------------------------------------------------------------------------------------- + /** * A diagonal noise model implements a diagonal covariance matrix, with the * elements of the diagonal specified in a Vector. This class has no public @@ -233,8 +237,10 @@ namespace gtsam { /** sigmas and reciprocal */ Vector sigmas_; + private: - boost::optional invsigmas_; /// optional to allow for constraints + + boost::optional invsigmas_; ///< optional to allow for constraints protected: /** protected constructor takes sigmas */ @@ -311,6 +317,8 @@ namespace gtsam { } }; // Diagonal + //--------------------------------------------------------------------------------------- + /** * A Constrained constrained model is a specialization of Diagonal which allows * some or all of the sigmas to be zero, forcing the error to be zero there. @@ -397,6 +405,8 @@ namespace gtsam { }; // Constrained + //--------------------------------------------------------------------------------------- + /** * An isotropic noise model corresponds to a scaled diagonal covariance * To construct, use one of the static methods @@ -467,6 +477,8 @@ namespace gtsam { }; + //--------------------------------------------------------------------------------------- + /** * Unit: i.i.d. unit-variance noise on all m dimensions. */ @@ -504,8 +516,12 @@ namespace gtsam { } }; + // TODO: should not really exist + /// The MEstimator namespace contains all robust error functions (not models) namespace MEstimator { + //--------------------------------------------------------------------------------------- + class Base { public: enum ReweightScheme { Scalar, Block }; @@ -518,26 +534,32 @@ namespace gtsam { Base(): reweight_(Block) {} Base(const ReweightScheme reweight):reweight_(reweight) {} virtual ~Base() {} + + /// robust error function to implement virtual double weight(const double &error) const = 0; + virtual void print(const std::string &s) const = 0; virtual bool equals(const Base& expected, const double tol=1e-8) const = 0; + Vector weight(const Vector &error) const; void reweight(Matrix &A, Vector &error) const; void reweight(Matrix &A1, Matrix &A2, Vector &error) const; void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; }; + /// Null class is not robust so is a Gaussian ? class Null : public Base { public: typedef boost::shared_ptr shared_ptr; Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} virtual double weight(const double &error) const { return 1.0; } - virtual void print(const std::string &s) const ; + virtual void print(const std::string &s) const; virtual bool equals(const Base& expected, const double tol=1e-8) const { return true; } static shared_ptr Create() ; }; + /// Fair implements the "Fair" robust error model (ZhangXXvvvv) class Fair : public Base { public: typedef boost::shared_ptr shared_ptr; @@ -554,6 +576,7 @@ namespace gtsam { Fair(){} }; + /// Huber implements the "Huber" robust error model (HuberXXvvvv) class Huber : public Base { public: typedef boost::shared_ptr shared_ptr; @@ -569,8 +592,10 @@ namespace gtsam { private: Huber(){} }; - } + } ///\namespace MEstimator + + /// Base class for robust error models class Robust : public Base { public: typedef boost::shared_ptr shared_ptr; @@ -579,21 +604,32 @@ namespace gtsam { typedef MEstimator::Base RobustModel; typedef noiseModel::Base NoiseModel; - const RobustModel::shared_ptr robust_; - const NoiseModel::shared_ptr noise_; + const RobustModel::shared_ptr robust_; ///< robust error function used + const NoiseModel::shared_ptr noise_; ///< noise model used public: + + /// Constructor Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise) : Base(noise->dim()), robust_(robust), noise_(noise) {} + + /// Destructor virtual ~Robust() {} + virtual void print(const std::string& name) const; virtual bool equals(const Base& expected, double tol=1e-9) const; + + // TODO: all function below are called whitening but really are dummy + inline virtual Vector whiten(const Vector& v) const { return noise_->whiten(v); } inline virtual Vector unwhiten(const Vector& v) const { return noise_->unwhiten(v); } inline virtual double distance(const Vector& v) const { return noise_->distance(v); } + + // TODO: these are really robust iterated re-weighting support functions + virtual void WhitenSystem(Matrix& A, Vector& b) const ; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;