12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #267 from borglab/feature/robust_theory 

Feature/robust theory
release/4.3a0
yetongumich 2020-04-03 13:47:12 -04:00 committed by GitHub
parent e2ed25328b ae85b640a8
commit 84a33e959f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 24 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/GaussianBayesNet.h
View File

@ -143,7 +143,7 @@ namespace gtsam {
* allocateVectorValues */ * allocateVectorValues */
VectorValues gradientAtZero() const; VectorValues gradientAtZero() const;
/** Mahalanobis norm error. */ /** 0.5 * sum of squared Mahalanobis distances. */
double error(const VectorValues& x) const; double error(const VectorValues& x) const;
/** /**

2
gtsam/linear/GaussianBayesTree.h
View File

@ -106,7 +106,7 @@ namespace gtsam {
* @return A VectorValues storing the gradient. */ * @return A VectorValues storing the gradient. */
VectorValues gradientAtZero() const; VectorValues gradientAtZero() const;
/** Mahalanobis norm error. */ /** 0.5 * sum of squared Mahalanobis distances. */
double error(const VectorValues& x) const; double error(const VectorValues& x) const;
/** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a

4
gtsam/linear/NoiseModel.cpp
View File

@ -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 // Note: for Diagonal, which does ediv_, will be correct for constraints
Vector w = whiten(v); Vector w = whiten(v);
return w.dot(w); 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_; return v.dot(v) * invsigma_ * invsigma_;
} }

23
gtsam/linear/NoiseModel.h
View File

@ -207,12 +207,25 @@ namespace gtsam {
virtual Vector unwhiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const;
/** /**
* Mahalanobis distance v'*R'*R*v = <R*v,R*v> * Squared Mahalanobis distance v'*R'*R*v = <R*v,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 { 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 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 whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const; virtual Matrix Whiten(const Matrix& H) const;
@ -616,7 +629,7 @@ namespace gtsam {
virtual bool isUnit() const { return true; } virtual bool isUnit() const { return true; }
virtual void print(const std::string& name) const; 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 whiten(const Vector& v) const { return v; }
virtual Vector unwhiten(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 Matrix Whiten(const Matrix& H) const { return H; }

4
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -68,10 +68,10 @@ TEST(NoiseModel, constructors)
for(Gaussian::shared_ptr mi: m) for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
// test Mahalanobis distance // test squared Mahalanobis distance
double distance = 5*5+10*10+15*15; double distance = 5*5+10*10+15*15;
for(Gaussian::shared_ptr mi: m) 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 // test R matrix
for(Gaussian::shared_ptr mi: m) for(Gaussian::shared_ptr mi: m)