diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 9da7a1609..3f6d69e91 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -143,7 +143,7 @@ namespace gtsam { * allocateVectorValues */ VectorValues gradientAtZero() const; - /** Mahalanobis norm error. */ + /** 0.5 * sum of squared Mahalanobis distances. */ double error(const VectorValues& x) const; /** diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index fcc73f80e..b6f5acd52 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -106,7 +106,7 @@ namespace gtsam { * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; - /** Mahalanobis norm error. */ + /** 0.5 * sum of squared Mahalanobis distances. */ double error(const VectorValues& x) const; /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index a6ebea394..b324ea784 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -157,7 +157,7 @@ Vector Gaussian::unwhiten(const Vector& v) const { } /* ************************************************************************* */ -double Gaussian::Mahalanobis(const Vector& v) const { +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); @@ -573,7 +573,7 @@ void Isotropic::print(const string& name) const { } /* ************************************************************************* */ -double Isotropic::Mahalanobis(const Vector& v) const { +double Isotropic::squaredMahalanobisDistance(const Vector& v) const { return v.dot(v) * invsigma_ * invsigma_; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 6c42fc7ba..4d86e77da 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -207,12 +207,25 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; /** - * Mahalanobis distance v'*R'*R*v = + * Squared Mahalanobis distance v'*R'*R*v = */ - virtual double Mahalanobis(const Vector& v) const; + virtual double squaredMahalanobisDistance(const Vector& v) const; + + /** + * Mahalanobis distance + */ + virtual double mahalanobisDistance(const Vector& v) const { + return std::sqrt(squaredMahalanobisDistance(v)); + } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + virtual double Mahalanobis(const Vector& v) const { + return squaredMahalanobisDistance(v); + } +#endif inline virtual double distance(const Vector& v) const { - return Mahalanobis(v); + return squaredMahalanobisDistance(v); } /** @@ -564,7 +577,7 @@ namespace gtsam { } virtual void print(const std::string& name) const; - virtual double Mahalanobis(const Vector& v) 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; @@ -616,7 +629,7 @@ namespace gtsam { virtual bool isUnit() const { return true; } virtual void print(const std::string& name) const; - virtual double Mahalanobis(const Vector& v) const {return v.dot(v); } + 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; } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 10578627f..3f6550b9f 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -68,10 +68,10 @@ TEST(NoiseModel, constructors) for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); - // test Mahalanobis distance + // test squared Mahalanobis distance double distance = 5*5+10*10+15*15; for(Gaussian::shared_ptr mi: m) - DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); + DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1e-9); // test R matrix for(Gaussian::shared_ptr mi: m)