Roustify BinaryMeasurements in a functional way, plus formatting

gtsam/sfm/BinaryMeasurement.h

@@ -47,9 +47,41 @@ private:
 
  public:
   BinaryMeasurement(Key key1, Key key2, const T &measured,
-                    const SharedNoiseModel &model = nullptr)
+                    const SharedNoiseModel &model = nullptr,
-      : Factor(std::vector<Key>({key1, key2})), measured_(measured),
+                    bool useHuber = false)
-        noiseModel_(model) {}
+      : Factor(std::vector<Key>({key1, key2})),
+        measured_(measured),
+        noiseModel_(model) {
+    if (useHuber) {
+      const auto &robust =
+          boost::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_);
+      if (!robust) {
+        // make robust
+        this->noiseModel_ = noiseModel::Robust::Create(
+            noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_);
+      }
+    }
+  }
+
+  /**
+   * Copy constructor to allow for making existing BinaryMeasurements as robust
+   * in a functional way.
+   * 
+   * @param measurement BinaryMeasurement object.
+   * @param useHuber Boolean flag indicating whether to use Huber noise model.
+   */
+  BinaryMeasurement(const BinaryMeasurement& measurement, bool useHuber = false) {
+    *this = measurement;
+    if (useHuber) {
+      const auto &robust =
+          boost::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_);
+      if (!robust) {
+        // make robust
+        this->noiseModel_ = noiseModel::Robust::Create(
+            noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_);
+      }
+    }
+  }
 
   /// @name Standard Interface
   /// @{
@@ -71,14 +103,6 @@ public:
     this->noiseModel_->print("  noise model: ");
   }
 
-  // TODO: make this more general?
-  void makeNoiseModelRobust(){
-	  const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_);
-	  if(!robust) // make robust
-		  this->noiseModel_ = noiseModel::Robust::Create(
-	            noiseModel::mEstimator::Huber::Create(1.345), this->noiseModel_);
-  }
-
   bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
     const BinaryMeasurement<T> *e =
         dynamic_cast<const BinaryMeasurement<T> *>(&expected);

gtsam/sfm/ShonanAveraging.cpp

@@ -343,12 +343,14 @@ static double Kappa(const BinaryMeasurement<T> &measurement) {
     const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
         measurement.noiseModel());
     if (robust) {
-			std::cout << "Verification of optimality does not work with robust cost function" << std::endl;
+      std::cout << "Verification of optimality does not work with robust cost "
+                   "function"
+                << std::endl;
       sigma = 1;  // setting arbitrary value
     } else {
       throw std::invalid_argument(
-					"Shonan averaging noise models must be isotropic (but robust losses are allowed).");
+          "Shonan averaging noise models must be isotropic (but robust losses "
-
+          "are allowed).");
     }
   }
   return 1.0 / (sigma * sigma);
@@ -833,13 +835,17 @@ template class ShonanAveraging<2>;
 
 ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
                                    const Parameters &parameters)
-    : ShonanAveraging<2>(parameters.useHuber?
+    : ShonanAveraging<2>(parameters.useHuber
-    		makeNoiseModelRobust(measurements) : measurements, parameters) {}
+                             ? makeNoiseModelRobust(measurements)
+                             : measurements,
+                         parameters) {}
 
 ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
-    : ShonanAveraging<2>(parameters.useHuber?
+    : ShonanAveraging<2>(
-    		makeNoiseModelRobust( parseMeasurements<Rot2>(g2oFile) ) :
+          parameters.useHuber
-    				parseMeasurements<Rot2>(g2oFile), parameters) {}
+              ? makeNoiseModelRobust(parseMeasurements<Rot2>(g2oFile))
+              : parseMeasurements<Rot2>(g2oFile),
+          parameters) {}
 
 /* ************************************************************************* */
 // Explicit instantiation for d=3
@@ -851,9 +857,11 @@ ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
         makeNoiseModelRobust(measurements) : measurements, parameters) {}
 
 ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
-    : ShonanAveraging<3>(parameters.useHuber?
+    : ShonanAveraging<3>(
-    		makeNoiseModelRobust( parseMeasurements<Rot3>(g2oFile) ) :
+          parameters.useHuber
-			parseMeasurements<Rot3>(g2oFile), parameters) {}
+              ? makeNoiseModelRobust(parseMeasurements<Rot3>(g2oFile))
+              : parseMeasurements<Rot3>(g2oFile),
+          parameters) {}
 
 // TODO(frank): Deprecate after we land pybind wrapper
 
@@ -883,9 +891,11 @@ static ShonanAveraging3::Measurements extractRot3Measurements(
 
 ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
                                    const Parameters &parameters)
-    : ShonanAveraging<3>(parameters.useHuber?
+    : ShonanAveraging<3>(
-    		makeNoiseModelRobust( extractRot3Measurements(factors) ):
+          parameters.useHuber
-			extractRot3Measurements(factors), parameters) {}
+              ? makeNoiseModelRobust(extractRot3Measurements(factors))
+              : extractRot3Measurements(factors),
+          parameters) {}
 
 /* ************************************************************************* */
 }  // namespace gtsam

gtsam/sfm/ShonanAveraging.h

@@ -53,7 +53,8 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
   double alpha;                 // weight of anchor-based prior (default 0)
   double beta;                  // weight of Karcher-based prior (default 1)
   double gamma;                 // weight of gauge-fixing factors (default 0)
-  bool useHuber; 				// if enabled, the Huber loss is used in the optimization (default is false)
+  bool useHuber;  // if enabled, the Huber loss is used in the optimization
+                  // (default is false)
 
   ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
                                 LevenbergMarquardtParams::CeresDefaults(),
@@ -120,7 +121,6 @@ class GTSAM_EXPORT ShonanAveraging {
   using Rot = typename Parameters::Rot;
 
   // We store SO(d) BetweenFactors to get noise model
-  // TODO(frank): use BinaryMeasurement?
   using Measurements = std::vector<BinaryMeasurement<Rot>>;
 
  private:
@@ -165,10 +165,10 @@ class GTSAM_EXPORT ShonanAveraging {
   }
 
   /// wrap factors with robust Huber loss
-  static Measurements makeNoiseModelRobust(Measurements measurements){
+  Measurements makeNoiseModelRobust(const Measurements& measurements) const {
     Measurements robustMeasurements = measurements;
     for (auto &measurement : robustMeasurements) {
-		  measurement.makeNoiseModelRobust();
+      measurement = BinaryMeasurement<Rot>(measurement, true);
     }
     return robustMeasurements;
   }

gtsam/sfm/tests/testBinaryMeasurement.cpp

@@ -67,8 +67,7 @@ TEST(BinaryMeasurement, Rot3) {
 /* ************************************************************************* */
 TEST(BinaryMeasurement, Rot3MakeRobust) {
   BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
-                                          rot3_model);
+                                          rot3_model, true);
-  rot3Measurement.makeNoiseModelRobust();
 
   EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
   EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
@@ -78,7 +77,7 @@ TEST(BinaryMeasurement, Rot3MakeRobust) {
   EXPECT(robust);
 
   // test that if we call it again nothing changes:
-  rot3Measurement.makeNoiseModelRobust();
+  rot3Measurement = BinaryMeasurement<Rot3>(rot3Measurement, true);
   const auto &robust2 = boost::dynamic_pointer_cast<noiseModel::Robust>(
       rot3Measurement.noiseModel());
   EXPECT(robust2);