From 7db7455c12fb9b1c06e9ffc3bc47e27ed489eff1 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Sun, 12 Jul 2020 23:05:24 -0400 Subject: [PATCH] deprecate error in noisemodel, use loss instead; revise virtual with override --- gtsam/linear/NoiseModel.cpp | 36 +++--- gtsam/linear/NoiseModel.h | 172 +++++++++++++++------------- gtsam/nonlinear/NonlinearFactor.cpp | 2 +- 3 files changed, 113 insertions(+), 97 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index ec4fd08fd..f5ec95696 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -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; ireweight(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(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)); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 449a70cf3..7494e0501 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -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 = + 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& 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 = - */ - 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& 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& 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 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 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 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 H) const; + Matrix Whiten(const Matrix& H) const override; + void WhitenInPlace(Matrix& H) const override; + void WhitenInPlace(Eigen::Block 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 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 H) const override; /** * Return standard deviation @@ -626,7 +631,7 @@ namespace gtsam { typedef boost::shared_ptr 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 /*H*/) const {} - virtual void whitenInPlace(Vector& /*v*/) const {} - virtual void unwhitenInPlace(Vector& /*v*/) const {} - virtual void whitenInPlace(Eigen::Block& /*v*/) const {} - virtual void unwhitenInPlace(Eigen::Block& /*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 /*H*/) const override {} + void whitenInPlace(Vector& /*v*/) const override {} + void unwhitenInPlace(Vector& /*v*/) const override {} + void whitenInPlace(Eigen::Block& /*v*/) const override {} + void unwhitenInPlace(Eigen::Block& /*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& 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& 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 */ diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 40fc1c427..1cfcba274 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -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 {