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

View File

@ -463,6 +463,16 @@ Vector Base::sqrtWeight(const Vector &error) const {
/** The following three functions reweight block matrices and a vector
* 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 */
void Base::reweight(vector<Matrix> &A, Vector &error) const {
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);
}
void Robust::WhitenSystem(Vector& b) const {
noise_->whitenInPlace(b);
robust_->reweight(b);
}
void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
noise_->WhitenSystem(A,b);
robust_->reweight(A,b);

View File

@ -582,7 +582,7 @@ namespace gtsam {
typedef boost::shared_ptr<Base> shared_ptr;
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 */
ReweightScheme reweight_;
@ -608,6 +608,7 @@ namespace gtsam {
Vector sqrtWeight(const Vector &error) const;
/** reweight block matrices and a vector according to their weight implementation */
void reweight(Vector &error) const;
void reweight(std::vector<Matrix> &A, Vector &error) const;
void reweight(Matrix &A, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
@ -692,17 +693,16 @@ namespace gtsam {
/// Return the contained noise model
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
{ return noise_->whiten(v); }
{ Vector r = v; this->WhitenSystem(r); return r; }
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
{ 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
virtual void WhitenSystem(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& A1, Matrix& A2, Vector& b) const;