From b0f7d3498b8509413177a2db287ec3cacc9e50e9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Mar 2020 15:37:08 -0400 Subject: [PATCH] simpler checks for infinity in NoiseModel --- gtsam/linear/NoiseModel.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index a6ebea394..58a569d2b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -136,7 +136,8 @@ bool Gaussian::equals(const Base& expected, double tol) const { const Gaussian* p = dynamic_cast (&expected); if (p == NULL) return false; if (typeid(*this) != typeid(*p)) return false; - //if (!sqrt_information_) return true; // ALEX todo; + //TODO(Alex); + //if (!sqrt_information_) return true; return equal_with_abs_tol(R(), p->R(), sqrt(tol)); } @@ -311,10 +312,9 @@ void Diagonal::WhitenInPlace(Eigen::Block H) const { namespace internal { // switch precisions and invsigmas to finite value -// TODO: why?? And, why not just ask s==0.0 below ? static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { for (Vector::Index i = 0; i < sigmas.size(); ++i) - if (!std::isfinite(1. / sigmas[i])) { + if (sigmas[i] <= 1e-12) { precisions[i] = 0.0; invsigmas[i] = 0.0; } @@ -341,8 +341,8 @@ Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu, /* ************************************************************************* */ bool Constrained::constrained(size_t i) const { - // TODO why not just check sigmas_[i]==0.0 ? - return !std::isfinite(1./sigmas_[i]); + // numerically stable way, rather than comparing to 0.0 directly. + return sigmas_[i] <= 1e-12; } /* ************************************************************************* */