Roustify BinaryMeasurements in a functional way, plus formatting

gtsam/sfm/BinaryMeasurement.h

@@ -45,11 +45,43 @@ private:
   T measured_;                  ///< The measurement
   SharedNoiseModel noiseModel_; ///< Noise model
 
-public:
+ 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

@@ -54,7 +54,7 @@ ShonanAveragingParameters<d>::ShonanAveragingParameters(
       alpha(alpha),
       beta(beta),
       gamma(gamma),
-	  useHuber(false){
+      useHuber(false) {
   // By default, we will do conjugate gradient
   lm.linearSolverType = LevenbergMarquardtParams::Iterative;
 
@@ -139,7 +139,7 @@ ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
 /* ************************************************************************* */
 template <size_t d>
 NonlinearFactorGraph ShonanAveraging<d>::buildGraphAt(size_t p) const {
-	NonlinearFactorGraph graph;
+  NonlinearFactorGraph graph;
   auto G = boost::make_shared<Matrix>(SO<-1>::VectorizedGenerators(p));
 
   for (const auto &measurement : measurements_) {
@@ -194,7 +194,7 @@ ShonanAveraging<d>::createOptimizerAt(size_t p, const Values &initial) const {
 template <size_t d>
 Values ShonanAveraging<d>::tryOptimizingAt(size_t p,
                                            const Values &initial) const {
-	auto lm = createOptimizerAt(p, initial);
+  auto lm = createOptimizerAt(p, initial);
   return lm->optimize();
 }
 
@@ -334,24 +334,26 @@ double ShonanAveraging<d>::cost(const Values &values) const {
 // Get kappa from noise model
 template <typename T>
 static double Kappa(const BinaryMeasurement<T> &measurement) {
-	const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
+  const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
-			measurement.noiseModel());
+      measurement.noiseModel());
-	double sigma;
+  double sigma;
-	if (isotropic) {
+  if (isotropic) {
-		sigma = isotropic->sigma();
+    sigma = isotropic->sigma();
-	} else{
+  } else {
-		const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
+    const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
-				measurement.noiseModel());
+        measurement.noiseModel());
-		if (robust) {
+    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 "
-			sigma = 1; // setting arbitrary value
+                   "function"
-		}else{
+                << std::endl;
-			throw std::invalid_argument(
+      sigma = 1;  // setting arbitrary value
-					"Shonan averaging noise models must be isotropic (but robust losses are allowed).");
+    } else {
-
+      throw std::invalid_argument(
-		}
+          "Shonan averaging noise models must be isotropic (but robust losses "
-	}
+          "are allowed).");
-	return 1.0 / (sigma * sigma);
+    }
+  }
+  return 1.0 / (sigma * sigma);
 }
 
 /* ************************************************************************* */
@@ -802,10 +804,10 @@ std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
     // Optimize until convergence at this level
     Qstar = tryOptimizingAt(p, initialSOp);
     if(parameters_.useHuber){ // in this case, there is no optimality verification
-    	if(pMin!=pMax)
+      if(pMin!=pMax)
-    		 std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl;
+         std::cout << "When using robust norm, Shonan only tests a single rank" << std::endl;
-    	const Values SO3Values = roundSolution(Qstar);
+      const Values SO3Values = roundSolution(Qstar);
-    	return std::make_pair(SO3Values, 0);
+      return std::make_pair(SO3Values, 0);
     }
 
     // Check certificate of global optimzality
@@ -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
@@ -848,12 +854,14 @@ template class ShonanAveraging<3>;
 ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
                                    const Parameters &parameters)
     : ShonanAveraging<3>(parameters.useHuber?
-    		makeNoiseModelRobust(measurements) : measurements, parameters) {}
+        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
 
@@ -869,8 +877,8 @@ static BinaryMeasurement<Rot3> convert(
         "with Gaussian noise models.");
   const Matrix6 M = gaussian->covariance();
   return BinaryMeasurement<Rot3>(
-		  f->key1(), f->key2(), f->measured().rotation(),
+      f->key1(), f->key2(), f->measured().rotation(),
-		  noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
+      noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
 }
 
 static ShonanAveraging3::Measurements extractRot3Measurements(
@@ -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(),
@@ -83,12 +84,12 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
   bool getUseHuber() { return useHuber; }
 
   void print() const {
-	  std::cout << " ShonanAveragingParameters: " << std::endl;
+    std::cout << " ShonanAveragingParameters: " << std::endl;
-	  std::cout << " alpha: " << alpha  << std::endl;
+    std::cout << " alpha: " << alpha  << std::endl;
-	  std::cout << " beta: " << beta  << std::endl;
+    std::cout << " beta: " << beta  << std::endl;
-	  std::cout << " gamma: " << gamma  << std::endl;
+    std::cout << " gamma: " << gamma  << std::endl;
-	  std::cout << " useHuber: " << useHuber  << std::endl;
+    std::cout << " useHuber: " << useHuber  << std::endl;
-	  std::cout << " --------------------------" << std::endl;
+    std::cout << " --------------------------" << std::endl;
   }
 };
 
@@ -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,12 +165,12 @@ class GTSAM_EXPORT ShonanAveraging {
   }
 
   /// wrap factors with robust Huber loss
-  static Measurements makeNoiseModelRobust(Measurements measurements){
+  Measurements makeNoiseModelRobust(const Measurements& measurements) const {
-	  Measurements robustMeasurements = measurements;
+    Measurements robustMeasurements = measurements;
-	  for (auto &measurement : robustMeasurements) {
+    for (auto &measurement : robustMeasurements) {
-		  measurement.makeNoiseModelRobust();
+      measurement = BinaryMeasurement<Rot>(measurement, true);
-	  }
+    }
-	  return robustMeasurements;
+    return robustMeasurements;
   }
 
   /// k^th measurement, as a Rot.

gtsam/sfm/tests/testBinaryMeasurement.cpp

@@ -67,20 +67,19 @@ 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);
   EXPECT(rot3Measurement.measured().equals(rot3Measured));
   const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
-		  rot3Measurement.noiseModel());
+      rot3Measurement.noiseModel());
   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());
+      rot3Measurement.noiseModel());
   EXPECT(robust2);
 }