deprecate error in noisemodel, use loss instead; revise virtual with override
yetongumich
parent
eb1a2b8fb3
commit
7db7455c12
|
|
@ -74,6 +74,13 @@ Vector Base::sigmas() const {
|
|||
throw("Base::sigmas: sigmas() not implemented for this noise model");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Base::squaredMahalanobisDistance(const Vector& v) const {
|
||||
// Note: for Diagonal, which does ediv_, will be correct for constraints
|
||||
Vector w = whiten(v);
|
||||
return w.dot(w);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
|
||||
size_t m = R.rows(), n = R.cols();
|
||||
|
|
@ -164,13 +171,6 @@ Vector Gaussian::unwhiten(const Vector& v) const {
|
|||
return backSubstituteUpper(thisR(), v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Gaussian::squaredMahalanobisDistance(const Vector& v) const {
|
||||
// Note: for Diagonal, which does ediv_, will be correct for constraints
|
||||
Vector w = whiten(v);
|
||||
return w.dot(w);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Gaussian::Whiten(const Matrix& H) const {
|
||||
return thisR() * H;
|
||||
|
|
@ -376,6 +376,7 @@ Vector Constrained::whiten(const Vector& v) const {
|
|||
return c;
|
||||
}
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/* ************************************************************************* */
|
||||
double Constrained::error(const Vector& v) const {
|
||||
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
|
||||
|
|
@ -384,6 +385,16 @@ double Constrained::error(const Vector& v) const {
|
|||
w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
|
||||
return 0.5 * w.dot(w);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Constrained::squaredMahalanobisDistance(const Vector& v) const {
|
||||
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
|
||||
for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables
|
||||
if (constrained(i)) // whiten makes constrained variables zero
|
||||
w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
|
||||
return w.dot(w);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Constrained::Whiten(const Matrix& H) const {
|
||||
|
|
@ -662,14 +673,9 @@ void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
|
|||
robust_->reweight(A1,A2,A3,b);
|
||||
}
|
||||
|
||||
Robust::shared_ptr Robust::Create(const RobustModel::shared_ptr& robust,
|
||||
const noiseModel::Base::shared_ptr noise) {
|
||||
SharedGaussian gaussian;
|
||||
if (!(gaussian = boost::dynamic_pointer_cast<noiseModel::Gaussian>(noise)))
|
||||
{
|
||||
throw std::invalid_argument("The noise model inside robust must be Gaussian");
|
||||
};
|
||||
return shared_ptr(new Robust(robust, gaussian));
|
||||
Robust::shared_ptr Robust::Create(
|
||||
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
|
||||
return shared_ptr(new Robust(robust,noise));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -90,13 +90,26 @@ namespace gtsam {
|
|||
/// Unwhiten an error vector.
|
||||
virtual Vector unwhiten(const Vector& v) const = 0;
|
||||
|
||||
/// Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
|
||||
virtual double squaredMahalanobisDistance(const Vector& v) const;
|
||||
|
||||
/// Mahalanobis distance
|
||||
virtual double mahalanobisDistance(const Vector& v) const {
|
||||
return std::sqrt(squaredMahalanobisDistance(v));
|
||||
}
|
||||
|
||||
/// loss function, input is Mahalanobis distance
|
||||
virtual double loss(const double squared_distance) const {
|
||||
return 0.5 * squared_distance;
|
||||
}
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/// calculate the error value given measurement error vector
|
||||
virtual double error(const Vector& v) const = 0;
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
virtual double distance(const Vector& v) {
|
||||
return error(v) * 2;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
|
||||
|
|
@ -207,42 +220,30 @@ namespace gtsam {
|
|||
*/
|
||||
static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual bool equals(const Base& expected, double tol=1e-9) const;
|
||||
virtual Vector sigmas() const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
|
||||
/**
|
||||
* Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
|
||||
*/
|
||||
virtual double squaredMahalanobisDistance(const Vector& v) const;
|
||||
|
||||
/**
|
||||
* Mahalanobis distance
|
||||
*/
|
||||
virtual double mahalanobisDistance(const Vector& v) const {
|
||||
return std::sqrt(squaredMahalanobisDistance(v));
|
||||
}
|
||||
void print(const std::string& name) const override;
|
||||
bool equals(const Base& expected, double tol=1e-9) const override;
|
||||
Vector sigmas() const override;
|
||||
Vector whiten(const Vector& v) const override;
|
||||
Vector unwhiten(const Vector& v) const override;
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
virtual double Mahalanobis(const Vector& v) const {
|
||||
return squaredMahalanobisDistance(v);
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* error value 0.5 * v'*R'*R*v
|
||||
*/
|
||||
inline virtual double error(const Vector& v) const {
|
||||
inline double error(const Vector& v) const override {
|
||||
return 0.5 * squaredMahalanobisDistance(v);
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Multiply a derivative with R (derivative of whiten)
|
||||
* Equivalent to whitening each column of the input matrix.
|
||||
*/
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
Matrix Whiten(const Matrix& H) const override;
|
||||
|
||||
/**
|
||||
* In-place version
|
||||
|
|
@ -257,10 +258,10 @@ namespace gtsam {
|
|||
/**
|
||||
* Whiten a system, in place as well
|
||||
*/
|
||||
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;
|
||||
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
|
||||
void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
|
||||
void WhitenSystem(Matrix& A, Vector& b) const override;
|
||||
void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
|
||||
void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
|
||||
|
||||
/**
|
||||
* Apply appropriately weighted QR factorization to the system [A b]
|
||||
|
|
@ -345,13 +346,13 @@ namespace gtsam {
|
|||
return Variances(precisions.array().inverse(), smart);
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual Vector sigmas() const { return sigmas_; }
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
|
||||
void print(const std::string& name) const override;
|
||||
Vector sigmas() const override { return sigmas_; }
|
||||
Vector whiten(const Vector& v) const override;
|
||||
Vector unwhiten(const Vector& v) const override;
|
||||
Matrix Whiten(const Matrix& H) const override;
|
||||
void WhitenInPlace(Matrix& H) const override;
|
||||
void WhitenInPlace(Eigen::Block<Matrix> H) const override;
|
||||
|
||||
/**
|
||||
* Return standard deviations (sqrt of diagonal)
|
||||
|
|
@ -373,7 +374,7 @@ namespace gtsam {
|
|||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
*/
|
||||
virtual Matrix R() const {
|
||||
Matrix R() const override {
|
||||
return invsigmas().asDiagonal();
|
||||
}
|
||||
|
||||
|
|
@ -427,10 +428,10 @@ namespace gtsam {
|
|||
|
||||
typedef boost::shared_ptr<Constrained> shared_ptr;
|
||||
|
||||
virtual ~Constrained() {}
|
||||
~Constrained() {}
|
||||
|
||||
/// true if a constrained noise mode, saves slow/clumsy dynamic casting
|
||||
virtual bool isConstrained() const { return true; }
|
||||
bool isConstrained() const override { return true; }
|
||||
|
||||
/// Return true if a particular dimension is free or constrained
|
||||
bool constrained(size_t i) const;
|
||||
|
|
@ -482,12 +483,16 @@ namespace gtsam {
|
|||
return MixedVariances(precisions.array().inverse());
|
||||
}
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/**
|
||||
* The error function for a constrained noisemodel,
|
||||
* for non-constrained versions, uses sigmas, otherwise
|
||||
* uses the penalty function with mu
|
||||
*/
|
||||
virtual double error(const Vector& v) const;
|
||||
double error(const Vector& v) const override;
|
||||
#endif
|
||||
|
||||
double squaredMahalanobisDistance(const Vector& v) const override;
|
||||
|
||||
/** Fully constrained variations */
|
||||
static shared_ptr All(size_t dim) {
|
||||
|
|
@ -504,16 +509,16 @@ namespace gtsam {
|
|||
return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
void print(const std::string& name) const override;
|
||||
|
||||
/// Calculates error vector with weights applied
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
Vector whiten(const Vector& v) const override;
|
||||
|
||||
/// Whitening functions will perform partial whitening on rows
|
||||
/// with a non-zero sigma. Other rows remain untouched.
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
|
||||
Matrix Whiten(const Matrix& H) const override;
|
||||
void WhitenInPlace(Matrix& H) const override;
|
||||
void WhitenInPlace(Eigen::Block<Matrix> H) const override;
|
||||
|
||||
/**
|
||||
* Apply QR factorization to the system [A b], taking into account constraints
|
||||
|
|
@ -524,7 +529,7 @@ namespace gtsam {
|
|||
* @param Ab is the m*(n+1) augmented system matrix [A b]
|
||||
* @return diagonal noise model can be all zeros, mixed, or not-constrained
|
||||
*/
|
||||
virtual Diagonal::shared_ptr QR(Matrix& Ab) const;
|
||||
Diagonal::shared_ptr QR(Matrix& Ab) const override;
|
||||
|
||||
/**
|
||||
* Returns a Unit version of a constrained noisemodel in which
|
||||
|
|
@ -586,14 +591,14 @@ namespace gtsam {
|
|||
return Variance(dim, 1.0/precision, smart);
|
||||
}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual double squaredMahalanobisDistance(const Vector& v) const;
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
virtual void whitenInPlace(Vector& v) const;
|
||||
virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
|
||||
void print(const std::string& name) const override;
|
||||
double squaredMahalanobisDistance(const Vector& v) const override;
|
||||
Vector whiten(const Vector& v) const override;
|
||||
Vector unwhiten(const Vector& v) const override;
|
||||
Matrix Whiten(const Matrix& H) const override;
|
||||
void WhitenInPlace(Matrix& H) const override;
|
||||
void whitenInPlace(Vector& v) const override;
|
||||
void WhitenInPlace(Eigen::Block<Matrix> H) const override;
|
||||
|
||||
/**
|
||||
* Return standard deviation
|
||||
|
|
@ -626,7 +631,7 @@ namespace gtsam {
|
|||
|
||||
typedef boost::shared_ptr<Unit> shared_ptr;
|
||||
|
||||
virtual ~Unit() {}
|
||||
~Unit() {}
|
||||
|
||||
/**
|
||||
* Create a unit covariance noise model
|
||||
|
|
@ -636,19 +641,19 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/// true if a unit noise model, saves slow/clumsy dynamic casting
|
||||
virtual bool isUnit() const { return true; }
|
||||
bool isUnit() const override { return true; }
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual double squaredMahalanobisDistance(const Vector& v) const {return v.dot(v); }
|
||||
virtual Vector whiten(const Vector& v) const { return v; }
|
||||
virtual Vector unwhiten(const Vector& v) const { return v; }
|
||||
virtual Matrix Whiten(const Matrix& H) const { return H; }
|
||||
virtual void WhitenInPlace(Matrix& /*H*/) const {}
|
||||
virtual void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const {}
|
||||
virtual void whitenInPlace(Vector& /*v*/) const {}
|
||||
virtual void unwhitenInPlace(Vector& /*v*/) const {}
|
||||
virtual void whitenInPlace(Eigen::Block<Vector>& /*v*/) const {}
|
||||
virtual void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const {}
|
||||
void print(const std::string& name) const override;
|
||||
double squaredMahalanobisDistance(const Vector& v) const override {return v.dot(v); }
|
||||
Vector whiten(const Vector& v) const override { return v; }
|
||||
Vector unwhiten(const Vector& v) const override { return v; }
|
||||
Matrix Whiten(const Matrix& H) const override { return H; }
|
||||
void WhitenInPlace(Matrix& /*H*/) const override {}
|
||||
void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
|
||||
void whitenInPlace(Vector& /*v*/) const override {}
|
||||
void unwhitenInPlace(Vector& /*v*/) const override {}
|
||||
void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
|
||||
void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -682,7 +687,7 @@ namespace gtsam {
|
|||
|
||||
protected:
|
||||
typedef mEstimator::Base RobustModel;
|
||||
typedef noiseModel::Gaussian NoiseModel;
|
||||
typedef noiseModel::Base NoiseModel;
|
||||
|
||||
const RobustModel::shared_ptr robust_; ///< robust error function used
|
||||
const NoiseModel::shared_ptr noise_; ///< noise model used
|
||||
|
|
@ -697,10 +702,10 @@ namespace gtsam {
|
|||
: Base(noise->dim()), robust_(robust), noise_(noise) {}
|
||||
|
||||
/// Destructor
|
||||
virtual ~Robust() {}
|
||||
~Robust() {}
|
||||
|
||||
virtual void print(const std::string& name) const;
|
||||
virtual bool equals(const Base& expected, double tol=1e-9) const;
|
||||
void print(const std::string& name) const override;
|
||||
bool equals(const Base& expected, double tol=1e-9) const override;
|
||||
|
||||
/// Return the contained robust error function
|
||||
const RobustModel::shared_ptr& robust() const { return robust_; }
|
||||
|
|
@ -709,37 +714,42 @@ namespace gtsam {
|
|||
const NoiseModel::shared_ptr& noise() const { return noise_; }
|
||||
|
||||
// TODO: functions below are dummy but necessary for the noiseModel::Base
|
||||
inline virtual Vector whiten(const Vector& v) const
|
||||
inline Vector whiten(const Vector& v) const override
|
||||
{ Vector r = v; this->WhitenSystem(r); return r; }
|
||||
inline virtual Matrix Whiten(const Matrix& A) const
|
||||
inline Matrix Whiten(const Matrix& A) const override
|
||||
{ Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
|
||||
inline virtual Vector unwhiten(const Vector& /*v*/) const
|
||||
inline Vector unwhiten(const Vector& /*v*/) const override
|
||||
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
inline virtual double distance(const Vector& v) {
|
||||
inline double distance(const Vector& v) override {
|
||||
return robust_->loss(this->unweightedWhiten(v).norm());
|
||||
}
|
||||
#endif
|
||||
// Fold the use of the m-estimator loss(...) function into error(...)
|
||||
inline virtual double error(const Vector& v) const
|
||||
inline double error(const Vector& v) const override
|
||||
{ return robust_->loss(noise_->mahalanobisDistance(v)); }
|
||||
#endif
|
||||
|
||||
double loss(const double squared_distance) const override {
|
||||
return robust_->loss(std::sqrt(squared_distance));
|
||||
}
|
||||
|
||||
// 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;
|
||||
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
|
||||
void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
|
||||
void WhitenSystem(Matrix& A, Vector& b) const override;
|
||||
void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
|
||||
void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
|
||||
|
||||
virtual Vector unweightedWhiten(const Vector& v) const {
|
||||
Vector unweightedWhiten(const Vector& v) const override {
|
||||
return noise_->unweightedWhiten(v);
|
||||
}
|
||||
virtual double weight(const Vector& v) const {
|
||||
double weight(const Vector& v) const override {
|
||||
// Todo(mikebosse): make the robust weight function input a vector.
|
||||
return robust_->weight(v.norm());
|
||||
}
|
||||
|
||||
static shared_ptr Create(
|
||||
const RobustModel::shared_ptr &robust, const noiseModel::Base::shared_ptr noise);
|
||||
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -121,7 +121,7 @@ double NoiseModelFactor::error(const Values& c) const {
|
|||
const Vector b = unwhitenedError(c);
|
||||
check(noiseModel_, b.size());
|
||||
if (noiseModel_)
|
||||
return noiseModel_->error(b);
|
||||
return noiseModel_->loss(noiseModel_->squaredMahalanobisDistance(b));
|
||||
else
|
||||
return 0.5 * b.squaredNorm();
|
||||
} else {
|
||||
|
|
|
|||
Loading…
Reference in New Issue