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.
parent
e3aaeedbf2
commit
c323f41e8f
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue