From 6d3bc21a183d7feb67d7279a1948911831aa7adf Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 23 Sep 2024 16:05:23 -0400
Subject: [PATCH] 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 ");