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

Fixed silently ignoring robust noise model when calling whiten on one - now throws exception. Also added function to whiten only the rhs vector in robust noise models.

release/4.3a0
Richard Roberts 2012-09-25 18:45:28 +00:00
parent e3aaeedbf2
commit c323f41e8f
2 changed files with 98 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/linear/NoiseModel.cpp
View File

@ -463,6 +463,16 @@ Vector Base::sqrtWeight(const Vector &error) const {
/** The following three functions reweight block matrices and a vector /** The following three functions reweight block matrices and a vector
* according to their weight implementation */ * according to their weight implementation */
void Base::reweight(Vector& error) const {
if(reweight_ == Block) {
const double w = sqrtWeight(error.norm());
error *= w;
} else {
const Vector w = sqrtWeight(error);
error.array() *= w.array();
}
}
/** Reweight n block matrices with one error vector */ /** Reweight n block matrices with one error vector */
void Base::reweight(vector<Matrix> &A, Vector &error) const { void Base::reweight(vector<Matrix> &A, Vector &error) const {
if ( reweight_ == Block ) { if ( reweight_ == Block ) {
@ -613,6 +623,11 @@ bool Robust::equals(const Base& expected, double tol) const {
return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol); return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
} }
void Robust::WhitenSystem(Vector& b) const {
noise_->whitenInPlace(b);
robust_->reweight(b);
}
void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const { void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
noise_->WhitenSystem(A,b); noise_->WhitenSystem(A,b);
robust_->reweight(A,b); robust_->reweight(A,b);

166
gtsam/linear/NoiseModel.h
View File

@ -570,97 +570,98 @@ namespace gtsam {
} }
}; };
// TODO: should not really exist // TODO: should not really exist
/// The MEstimator namespace contains all robust error functions (not models) /// 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 };
typedef boost::shared_ptr<Base> shared_ptr; typedef boost::shared_ptr<Base> shared_ptr;
protected: protected:
/** the rows can be weighted independently accordint to the error /** the rows can be weighted independently according to the error
* or uniformly with the norm of the right hand side */ * or uniformly with the norm of the right hand side */
ReweightScheme reweight_; ReweightScheme reweight_;
public: public:
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 /// 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;
inline double sqrtWeight(const double &error) const inline double sqrtWeight(const double &error) const
{ return std::sqrt(weight(error)); } { return std::sqrt(weight(error)); }
/** produce a weight vector according to an error vector and the implemented /** produce a weight vector according to an error vector and the implemented
* robust function */ * robust function */
Vector weight(const Vector &error) const; Vector weight(const Vector &error) const;
/** square root version of the weight function */ /** square root version of the weight function */
Vector sqrtWeight(const Vector &error) const; Vector sqrtWeight(const Vector &error) const;
/** reweight block matrices and a vector according to their weight implementation */ /** reweight block matrices and a vector according to their weight implementation */
void reweight(std::vector<Matrix> &A, Vector &error) const; void reweight(Vector &error) const;
void reweight(Matrix &A, Vector &error) const; void reweight(std::vector<Matrix> &A, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Vector &error) const; void reweight(Matrix &A, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Matrix &A3, 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 ? /// 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;
Null(const ReweightScheme reweight = Block) : Base(reweight) {} Null(const ReweightScheme reweight = Block) : Base(reweight) {}
virtual ~Null() {} virtual ~Null() {}
virtual double weight(const double &error) const { return 1.0; } 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; } virtual bool equals(const Base& expected, const double tol=1e-8) const { return true; }
static shared_ptr Create() ; static shared_ptr Create() ;
}; };
/// Fair implements the "Fair" robust error model (Zhang97ivc) /// Fair implements the "Fair" robust error model (Zhang97ivc)
class Fair : public Base { class Fair : public Base {
public: public:
typedef boost::shared_ptr<Fair> shared_ptr; typedef boost::shared_ptr<Fair> shared_ptr;
protected: protected:
double c_; double c_;
public: public:
Fair(const double c, const ReweightScheme reweight = Block); Fair(const double c, const ReweightScheme reweight = Block);
virtual ~Fair() {} virtual ~Fair() {}
virtual double weight(const double &error) const ; virtual double weight(const double &error) const ;
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 ; virtual bool equals(const Base& expected, const double tol=1e-8) const ;
static shared_ptr Create(const double c, const ReweightScheme reweight = Block) ; static shared_ptr Create(const double c, const ReweightScheme reweight = Block) ;
private: private:
Fair(){} Fair(){}
}; };
/// Huber implements the "Huber" robust error model (Zhang97ivc) /// Huber implements the "Huber" robust error model (Zhang97ivc)
class Huber : public Base { class Huber : public Base {
public: public:
typedef boost::shared_ptr<Huber> shared_ptr; typedef boost::shared_ptr<Huber> shared_ptr;
protected: protected:
double k_; double k_;
public: public:
Huber(const double k, const ReweightScheme reweight = Block); Huber(const double k, const ReweightScheme reweight = Block);
virtual ~Huber() {} virtual ~Huber() {}
virtual double weight(const double &error) const ; virtual double weight(const double &error) const ;
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 ; virtual bool equals(const Base& expected, const double tol=1e-8) const ;
static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ;
private: private:
Huber(){} Huber(){}
}; };
} ///\namespace MEstimator } ///\namespace MEstimator
/// Base class for robust error models /// Base class for robust error models
class Robust : public Base { class Robust : public Base {
@ -692,17 +693,16 @@ namespace gtsam {
/// Return the contained noise model /// Return the contained noise model
const NoiseModel::shared_ptr& noise() const { return noise_; } const NoiseModel::shared_ptr& noise() const { return noise_; }
// TODO: all function below are dummy but necessary for the noiseModel::Base // TODO: functions below are dummy but necessary for the noiseModel::Base
inline virtual Vector whiten(const Vector& v) const inline virtual Vector whiten(const Vector& v) const
{ return noise_->whiten(v); } { Vector r = v; this->WhitenSystem(r); return r; }
inline virtual Vector unwhiten(const Vector& v) const inline virtual Vector unwhiten(const Vector& v) const
{ return noise_->unwhiten(v); } { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
inline virtual double distance(const Vector& v) const inline virtual double distance(const Vector& v) const
{ return noise_->distance(v); } { throw std::invalid_argument("distance is not currently supported for robust noise models."); }
// TODO: these are really robust iterated re-weighting support functions // TODO: these are really robust iterated re-weighting support functions
virtual void WhitenSystem(Vector& b) const;
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const; virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
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;