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

remove distance in noisemodel, replace with error

release/4.3a0
yetongumich 2020-04-01 20:07:49 -04:00
parent 4e3542a743
commit 201539680f
5 changed files with 21 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/JacobianFactor.cpp
View File

@ -515,7 +515,7 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const {
double JacobianFactor::error(const VectorValues& c) const {
Vector e = unweighted_error(c);
// Use the noise model distance function to get the correct error if available.
if (model_) return 0.5 * model_->distance(e);
if (model_) return model_->error(e);
return 0.5 * e.dot(e);
}

4
gtsam/linear/NoiseModel.cpp
View File

@ -369,12 +369,12 @@ Vector Constrained::whiten(const Vector& v) const {
}
/* ************************************************************************* */
double Constrained::distance(const Vector& v) const {
double Constrained::error(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);
return 0.5 * w.dot(w);
}
/* ************************************************************************* */

19
gtsam/linear/NoiseModel.h
View File

@ -90,7 +90,12 @@ namespace gtsam {
/// Unwhiten an error vector.
virtual Vector unwhiten(const Vector& v) const = 0;
/// calculate the error value given error vector
virtual double error(const Vector& v) const = 0;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
virtual double distance(const Vector& v) const = 0;
#endif
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
@ -224,8 +229,8 @@ namespace gtsam {
}
#endif
inline virtual double distance(const Vector& v) const {
return SquaredMahalanobisDistance(v);
inline virtual double error(const Vector& v) const {
return 0.5 * SquaredMahalanobisDistance(v);
}
/**
@ -477,7 +482,7 @@ namespace gtsam {
* for non-constrained versions, uses sigmas, otherwise
* uses the penalty function with mu
*/
virtual double distance(const Vector& v) const;
virtual double error(const Vector& v) const;
/** Fully constrained variations */
static shared_ptr All(size_t dim) {
@ -705,11 +710,11 @@ namespace gtsam {
{ Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
inline virtual Vector unwhiten(const Vector& /*v*/) const
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
// Fold the use of the m-estimator loss(...) function into distance(...)
inline virtual double distance(const Vector& v) const
// Fold the use of the m-estimator loss(...) function into error(...)
inline virtual double error(const Vector& v) const
{ return robust_->loss(this->unweightedWhiten(v).norm()); }
inline virtual double distance_non_whitened(const Vector& v) const
{ return robust_->loss(v.norm()); }
// inline virtual double distance_non_whitened(const Vector& v) const
// { return robust_->loss(v.norm()); }
// 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;

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

@ -182,8 +182,8 @@ TEST(NoiseModel, ConstrainedMixed )
EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible)));
EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible)));
DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
DOUBLES_EQUAL(0.5 * (1000.0 + 0.25 + 0.25),d->error(infeasible),1e-9);
DOUBLES_EQUAL(0.5 * 0.5,d->error(feasible),1e-9);
}
/* ************************************************************************* */
@ -197,8 +197,8 @@ TEST(NoiseModel, ConstrainedAll )
EXPECT(assert_equal(Vector3(1.0, 1.0, 1.0),i->whiten(infeasible)));
EXPECT(assert_equal(Vector3(0.0, 0.0, 0.0),i->whiten(feasible)));
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
DOUBLES_EQUAL(0.5 * 1000.0 * 3.0,i->error(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->error(feasible),1e-9);
}
/* ************************************************************************* */
@ -687,7 +687,7 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
EQUALITY(cov, gaussian->covariance());\
EXPECT(assert_equal(white, gaussian->whiten(e)));\
EXPECT(assert_equal(e, gaussian->unwhiten(white)));\
EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\
EXPECT_DOUBLES_EQUAL(0.5 * 251, gaussian->error(e), 1e-9);\
Matrix A = R.inverse(); Vector b = e;\
gaussian->WhitenSystem(A, b);\
EXPECT(assert_equal(I, A));\

2
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -121,7 +121,7 @@ double NoiseModelFactor::error(const Values& c) const {
const Vector b = unwhitenedError(c);
check(noiseModel_, b.size());
if (noiseModel_)
return 0.5 * noiseModel_->distance(b);
return noiseModel_->error(b);
else
return 0.5 * b.squaredNorm();
} else {