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

Documentation and TODO

release/4.3a0
Frank Dellaert 2011-09-09 21:16:57 +00:00
parent 3014daa140
commit 24ea02fe69
2 changed files with 82 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam/linear/NoiseModel.cpp
View File

@ -9,12 +9,11 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/* /**
* NoiseModel * @file NoiseModel.cpp
* * @date Jan 13, 2010
* Created on: Jan 13, 2010 * @author Richard Roberts
* Author: Richard Roberts * @author Frank Dellaert
* Author: Frank Dellaert
*/ */
#include <limits> #include <limits>
@ -437,6 +436,7 @@ void Unit::print(const std::string& name) const {
/* ************************************************************************* */ /* ************************************************************************* */
namespace MEstimator { namespace MEstimator {
Vector Base::weight(const Vector &error) const { Vector Base::weight(const Vector &error) const {
const size_t n = error.rows(); const size_t n = error.rows();
Vector w(n); 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 void Null::print(const std::string &s) const
{ cout << s << ": null ()" << endl; } { cout << s << ": null ()" << endl; }
@ -504,6 +508,10 @@ Fair::Fair(const double c, const ReweightScheme reweight)
} }
} }
/* ************************************************************************* */
// Fair
/* ************************************************************************* */
double Fair::weight(const double &error) const double Fair::weight(const double &error) const
{ return 1.0 / (1.0 + fabs(error)/c_); } { return 1.0 / (1.0 + fabs(error)/c_); }
@ -527,11 +535,17 @@ 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 double Huber::weight(const double &error) const {
{ cout << s << ": huber (" << k_ << ")" << endl; } 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 { bool Huber::equals(const Base &expected, const double tol) const {
const Huber* p = dynamic_cast<const Huber*>(&expected); const Huber* p = dynamic_cast<const Huber*>(&expected);
@ -539,11 +553,12 @@ bool Huber::equals(const Base &expected, const double tol) const {
return fabs(k_ - p->k_) < tol; return fabs(k_ - p->k_) < tol;
} }
Huber::shared_ptr Huber::Create(const double c) Huber::shared_ptr Huber::Create(const double c) {
{ return shared_ptr(new Huber(c)); } return shared_ptr(new Huber(c));
} }
} // namespace MEstimator
/* ************************************************************************* */ /* ************************************************************************* */
// Robust // Robust
/* ************************************************************************* */ /* ************************************************************************* */

64
gtsam/linear/NoiseModel.h
View File

@ -9,12 +9,11 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/* /**
* NoiseModel.h * @file NoiseModel.h
* * @date Jan 13, 2010
* Created on: Jan 13, 2010 * @author Richard Roberts
* Author: Richard Roberts * @author Frank Dellaert
* Author: Frank Dellaert
*/ */
#pragma once #pragma once
@ -27,6 +26,7 @@ namespace gtsam {
struct SharedDiagonal; // forward declare struct SharedDiagonal; // forward declare
/// All noise models live in the noiseModel namespace
namespace noiseModel { namespace noiseModel {
class Gaussian; class Gaussian;
@ -35,6 +35,8 @@ namespace gtsam {
class Isotropic; class Isotropic;
class Unit; class Unit;
//---------------------------------------------------------------------------------------
/** /**
* noiseModel::Base is the abstract base class for all noise models. * noiseModel::Base is the abstract base class for all noise models.
* *
@ -56,9 +58,7 @@ namespace gtsam {
Base(size_t dim = 1):dim_(dim) {} Base(size_t dim = 1):dim_(dim) {}
virtual ~Base() {} virtual ~Base() {}
/** /// Dimensionality
* Dimensionality
*/
inline size_t dim() const { return dim_;} inline size_t dim() const { return dim_;}
virtual void print(const std::string& name) const = 0; virtual void print(const std::string& name) const = 0;
@ -100,6 +100,8 @@ namespace gtsam {
} }
}; };
//---------------------------------------------------------------------------------------
/** /**
* Gaussian implements the mathematical model * Gaussian implements the mathematical model
* |R*x|^2 = |y|^2 with R'*R=inv(Sigma) * |R*x|^2 = |y|^2 with R'*R=inv(Sigma)
@ -114,7 +116,7 @@ namespace gtsam {
protected: protected:
/* Matrix square root of information matrix (R) */ /** Matrix square root of information matrix (R) */
boost::optional<Matrix> sqrt_information_; boost::optional<Matrix> sqrt_information_;
private: private:
@ -223,6 +225,8 @@ namespace gtsam {
}; // Gaussian }; // Gaussian
//---------------------------------------------------------------------------------------
/** /**
* A diagonal noise model implements a diagonal covariance matrix, with the * A diagonal noise model implements a diagonal covariance matrix, with the
* elements of the diagonal specified in a Vector. This class has no public * elements of the diagonal specified in a Vector. This class has no public
@ -233,8 +237,10 @@ namespace gtsam {
/** sigmas and reciprocal */ /** sigmas and reciprocal */
Vector sigmas_; Vector sigmas_;
private: private:
boost::optional<Vector> invsigmas_; /// optional to allow for constraints
boost::optional<Vector> invsigmas_; ///< optional to allow for constraints
protected: protected:
/** protected constructor takes sigmas */ /** protected constructor takes sigmas */
@ -311,6 +317,8 @@ namespace gtsam {
} }
}; // Diagonal }; // Diagonal
//---------------------------------------------------------------------------------------
/** /**
* A Constrained constrained model is a specialization of Diagonal which allows * 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. * some or all of the sigmas to be zero, forcing the error to be zero there.
@ -397,6 +405,8 @@ namespace gtsam {
}; // Constrained }; // Constrained
//---------------------------------------------------------------------------------------
/** /**
* An isotropic noise model corresponds to a scaled diagonal covariance * An isotropic noise model corresponds to a scaled diagonal covariance
* To construct, use one of the static methods * 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. * 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 { namespace MEstimator {
//---------------------------------------------------------------------------------------
class Base { class Base {
public: public:
enum ReweightScheme { Scalar, Block }; enum ReweightScheme { Scalar, Block };
@ -518,15 +534,20 @@ namespace gtsam {
Base(): reweight_(Block) {} Base(): reweight_(Block) {}
Base(const ReweightScheme reweight):reweight_(reweight) {} Base(const ReweightScheme reweight):reweight_(reweight) {}
virtual ~Base() {} virtual ~Base() {}
/// robust error function to implement
virtual double weight(const double &error) const = 0; virtual double weight(const 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, const double tol=1e-8) const = 0; virtual bool equals(const Base& expected, const double tol=1e-8) const = 0;
Vector weight(const Vector &error) const; Vector weight(const Vector &error) const;
void reweight(Matrix &A, 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, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Matrix &A3, 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 { class Null : public Base {
public: public:
typedef boost::shared_ptr<Null> shared_ptr; typedef boost::shared_ptr<Null> shared_ptr;
@ -538,6 +559,7 @@ namespace gtsam {
static shared_ptr Create() ; static shared_ptr Create() ;
}; };
/// Fair implements the "Fair" robust error model (ZhangXXvvvv)
class Fair : public Base { class Fair : public Base {
public: public:
typedef boost::shared_ptr<Fair> shared_ptr; typedef boost::shared_ptr<Fair> shared_ptr;
@ -554,6 +576,7 @@ namespace gtsam {
Fair(){} Fair(){}
}; };
/// Huber implements the "Huber" robust error model (HuberXXvvvv)
class Huber : public Base { class Huber : public Base {
public: public:
typedef boost::shared_ptr<Huber> shared_ptr; typedef boost::shared_ptr<Huber> shared_ptr;
@ -569,8 +592,10 @@ namespace gtsam {
private: private:
Huber(){} Huber(){}
}; };
}
} ///\namespace MEstimator
/// Base class for robust error models
class Robust : public Base { class Robust : public Base {
public: public:
typedef boost::shared_ptr<Robust> shared_ptr; typedef boost::shared_ptr<Robust> shared_ptr;
@ -579,21 +604,32 @@ namespace gtsam {
typedef MEstimator::Base RobustModel; typedef MEstimator::Base RobustModel;
typedef noiseModel::Base NoiseModel; typedef noiseModel::Base NoiseModel;
const RobustModel::shared_ptr robust_; const RobustModel::shared_ptr robust_; ///< robust error function used
const NoiseModel::shared_ptr noise_; const NoiseModel::shared_ptr noise_; ///< noise model used
public: public:
/// Constructor
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise) Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
: Base(noise->dim()), robust_(robust), noise_(noise) {} : Base(noise->dim()), robust_(robust), noise_(noise) {}
/// Destructor
virtual ~Robust() {} virtual ~Robust() {}
virtual void print(const std::string& name) const; virtual void print(const std::string& name) const;
virtual bool equals(const Base& expected, double tol=1e-9) 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 inline virtual Vector whiten(const Vector& v) const
{ return noise_->whiten(v); } { return noise_->whiten(v); }
inline virtual Vector unwhiten(const Vector& v) const inline virtual Vector unwhiten(const Vector& v) const
{ return noise_->unwhiten(v); } { return noise_->unwhiten(v); }
inline virtual double distance(const Vector& v) const inline virtual double distance(const Vector& v) const
{ return noise_->distance(v); } { 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& A, Vector& b) const ;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, 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; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;