Merge pull request #1842 from borglab/better-noisemodel-checking

Non-loop versions for checking sigma values

gtsam/linear/NoiseModel.cpp

@@ -269,10 +269,7 @@ double Gaussian::negLogConstant() const {
 /* ************************************************************************* */
 // Diagonal
 /* ************************************************************************* */
-Diagonal::Diagonal() :
-    Gaussian(1) // TODO: Frank asks: really sure about this?
-{
-}
+Diagonal::Diagonal() : Gaussian() {}
 
 /* ************************************************************************* */
 Diagonal::Diagonal(const Vector& sigmas)
@@ -284,38 +281,38 @@ 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);
-  }
-  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()));
 }
 
 /* ************************************************************************* */
 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) goto full;
+
     // 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));
+full:
+  return Diagonal::shared_ptr(new Diagonal(sigmas));
 }
 
 /* ************************************************************************* */
 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 ");