From 201539680f6e2e1db00b6d2b49ceb1a8c9b8db4f Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 20:07:49 -0400 Subject: [PATCH] remove distance in noisemodel, replace with error --- gtsam/linear/JacobianFactor.cpp | 2 +- gtsam/linear/NoiseModel.cpp | 4 ++-- gtsam/linear/NoiseModel.h | 19 ++++++++++++------- gtsam/linear/tests/testNoiseModel.cpp | 10 +++++----- gtsam/nonlinear/NonlinearFactor.cpp | 2 +- 5 files changed, 21 insertions(+), 16 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2310d88f0..2e634190c 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -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); } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 33f51e1f0..d1a03eb5b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -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& 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& A, Vector& b) const; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 0290fc5d8..52e1eefee 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -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));\ diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index ee14e8073..40fc1c427 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 0.5 * noiseModel_->distance(b); + return noiseModel_->error(b); else return 0.5 * b.squaredNorm(); } else {