From 6d3bc21a183d7feb67d7279a1948911831aa7adf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Sep 2024 16:05:23 -0400 Subject: [PATCH 1/3] non-loop versions for checking sigma values --- gtsam/linear/NoiseModel.cpp | 35 ++++++++++++++++++----------------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index de69fdce9..7cb93b17a 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -270,10 +270,7 @@ double Gaussian::logNormalizationConstant() const { /* ************************************************************************* */ // Diagonal /* ************************************************************************* */ -Diagonal::Diagonal() : - Gaussian(1) // TODO: Frank asks: really sure about this? -{ -} +Diagonal::Diagonal() : Gaussian() {} /* ************************************************************************* */ Diagonal::Diagonal(const Vector& sigmas) @@ -287,29 +284,32 @@ Diagonal::Diagonal(const Vector& sigmas) Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { if (smart) { // check whether all the same entry - size_t n = variances.size(); - for (size_t j = 1; j < n; j++) - if (variances(j) != variances(0)) goto full; - return Isotropic::Variance(n, variances(0), true); + if ((variances.array() == variances(0)).all()) { + return Isotropic::Variance(variances.size(), variances(0), true); + } else { + return shared_ptr(new Diagonal(variances.cwiseSqrt())); + } } - full: return shared_ptr(new Diagonal(variances.cwiseSqrt())); + return shared_ptr(new Diagonal(variances.cwiseSqrt())); } /* ************************************************************************* */ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { if (smart) { size_t n = sigmas.size(); - if (n==0) goto full; + if (n == 0) return Diagonal::shared_ptr(new Diagonal(sigmas)); + // look for zeros to make a constraint - for (size_t j=0; j< n; ++j) - if (sigmas(j)<1e-8) - return Constrained::MixedSigmas(sigmas); + if ((sigmas.array() < 1e-8).any()) { + return Constrained::MixedSigmas(sigmas); + } + // check whether all the same entry - for (size_t j = 1; j < n; j++) - if (sigmas(j) != sigmas(0)) goto full; - return Isotropic::Sigma(n, sigmas(0), true); + if ((sigmas.array() == sigmas(0)).all()) { + return Isotropic::Sigma(n, sigmas(0), true); + } } - full: return Diagonal::shared_ptr(new Diagonal(sigmas)); + return Diagonal::shared_ptr(new Diagonal(sigmas)); } /* ************************************************************************* */ @@ -317,6 +317,7 @@ Diagonal::shared_ptr Diagonal::Precisions(const Vector& precisions, bool smart) { return Variances(precisions.array().inverse(), smart); } + /* ************************************************************************* */ void Diagonal::print(const string& name) const { gtsam::print(sigmas_, name + "diagonal sigmas "); From 2682cde25995630452f06ceb3ce32361ba056cb7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Sep 2024 22:57:05 -0400 Subject: [PATCH 2/3] re-add the gotos --- gtsam/linear/NoiseModel.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 7cb93b17a..18a29f5b3 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -286,10 +286,10 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { // check whether all the same entry if ((variances.array() == variances(0)).all()) { return Isotropic::Variance(variances.size(), variances(0), true); - } else { - return shared_ptr(new Diagonal(variances.cwiseSqrt())); - } + } else + goto full; } +full: return shared_ptr(new Diagonal(variances.cwiseSqrt())); } @@ -297,7 +297,7 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { if (smart) { size_t n = sigmas.size(); - if (n == 0) return Diagonal::shared_ptr(new Diagonal(sigmas)); + if (n == 0) goto full; // look for zeros to make a constraint if ((sigmas.array() < 1e-8).any()) { @@ -309,6 +309,7 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { return Isotropic::Sigma(n, sigmas(0), true); } } +full: return Diagonal::shared_ptr(new Diagonal(sigmas)); } From 9243655e3b612a94892c44c7a8193d53de74e689 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 25 Sep 2024 14:13:14 -0400 Subject: [PATCH 3/3] simplify --- gtsam/linear/NoiseModel.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 18a29f5b3..7ee2158be 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -282,15 +282,10 @@ Diagonal::Diagonal(const Vector& sigmas) /* ************************************************************************* */ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { - if (smart) { - // check whether all the same entry - if ((variances.array() == variances(0)).all()) { - return Isotropic::Variance(variances.size(), variances(0), true); - } else - goto full; - } -full: - return shared_ptr(new Diagonal(variances.cwiseSqrt())); + // check whether all the same entry + return (smart && (variances.array() == variances(0)).all()) + ? Isotropic::Variance(variances.size(), variances(0), true) + : shared_ptr(new Diagonal(variances.cwiseSqrt())); } /* ************************************************************************* */