From 5762ba5ac8a5c1ca1769a745db37f6ef97b9817b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 9 Nov 2020 10:39:04 -0500
Subject: [PATCH] Remove goto, update docs, formatting

---
 gtsam/slam/FrobeniusFactor.cpp | 37 ++++++++++++++++++----------------
 gtsam/slam/FrobeniusFactor.h   | 17 ++++++++++------
 2 files changed, 31 insertions(+), 23 deletions(-)

diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp
index 5806fcfdb..8c70a1ebb 100644
--- a/gtsam/slam/FrobeniusFactor.cpp
+++ b/gtsam/slam/FrobeniusFactor.cpp
@@ -26,41 +26,44 @@ namespace gtsam {
 SharedNoiseModel
 ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
   double sigma = 1.0;
+  bool exit = false;
+
   if (model != nullptr) {
-	const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
-	Vector sigmas;
-	if(robust){
-		sigmas = robust->noise()->sigmas();
-	} else{
-		sigmas = model->sigmas();
-	}
+    const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
+    Vector sigmas;
+    if (robust) {
+      sigmas = robust->noise()->sigmas();
+    } else {
+      sigmas = model->sigmas();
+    }
 
     size_t n = sigmas.size();
     if (n == 1) {
       sigma = sigmas(0); // Rot2
-      goto exit;
+      exit = true;
     }
-    if (n == 3 || n == 6) {
+    else if (n == 3 || n == 6) {
       sigma = sigmas(2); // Pose2, Rot3, or Pose3
       if (sigmas(0) != sigma || sigmas(1) != sigma) {
         if (!defaultToUnit) {
           throw std::runtime_error("Can only convert isotropic rotation noise");
         }
       }
-      goto exit;
+      exit = true;
     }
-    if (!defaultToUnit) {
+    if (!defaultToUnit && !exit) {
       throw std::runtime_error("Can only convert Pose2/Pose3 noise models");
     }
   }
-  exit:
+
   auto isoModel = noiseModel::Isotropic::Sigma(d, sigma);
   const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
-  if(robust)
-	  return noiseModel::Robust::Create(
-	            noiseModel::mEstimator::Huber::Create(1.345), isoModel);
-  else
-	  return isoModel;
+  if (robust) {
+    return noiseModel::Robust::Create(
+        noiseModel::mEstimator::Huber::Create(1.345), isoModel);
+  } else {
+    return isoModel;
+  }
 }
 
 //******************************************************************************
diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h
index 9915a617d..f17a9e421 100644
--- a/gtsam/slam/FrobeniusFactor.h
+++ b/gtsam/slam/FrobeniusFactor.h
@@ -26,13 +26,18 @@
 namespace gtsam {
 
 /**
- * When creating (any) FrobeniusFactor we can convert a Rot/Pose
- * BetweenFactor noise model into a n-dimensional isotropic noise
- * model used to weight the Frobenius norm.  If the noise model passed is
- * null we return a n-dimensional isotropic noise model with sigma=1.0. If
- * not, we we check if the d-dimensional noise model on rotations is
+ * When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor
+ * noise model into a n-dimensional isotropic noise
+ * model used to weight the Frobenius norm.
+ * If the noise model passed is null we return a n-dimensional isotropic noise
+ * model with sigma=1.0.
+ * If not, we we check if the d-dimensional noise model on rotations is
  * isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an
- * error. If defaultToUnit == false throws an exception on unexepcted input.
+ * error.
+ * If the noise model is a robust error model, we use the sigmas of the
+ * underlying noise model.
+ *
+ * If defaultToUnit == false throws an exception on unexepcted input.
  */
 GTSAM_EXPORT SharedNoiseModel
 ConvertNoiseModel(const SharedNoiseModel &model, size_t n,