adding robust cost function - version 1

gtsam/sfm/BinaryMeasurement.h

@@ -71,6 +71,11 @@ public:
     this->noiseModel_->print("  noise model: ");
   }
 
+  void makeNoiseModelRobust(){
+	  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);
@@ -80,4 +85,4 @@ public:
   }
   /// @}
 };
-} // namespace gtsam
+} // namespace gtsam

gtsam/sfm/ShonanAveraging.cpp

@@ -53,7 +53,8 @@ ShonanAveragingParameters<d>::ShonanAveragingParameters(
       optimalityThreshold(optimalityThreshold),
       alpha(alpha),
       beta(beta),
-      gamma(gamma) {
+      gamma(gamma),
+	  useHuber(false){
   // By default, we will do conjugate gradient
   lm.linearSolverType = LevenbergMarquardtParams::Iterative;
 
@@ -819,9 +820,15 @@ template class ShonanAveraging<2>;
 
 ShonanAveraging2::ShonanAveraging2(const Measurements &measurements,
                                    const Parameters &parameters)
-    : ShonanAveraging<2>(measurements, parameters) {}
+    : ShonanAveraging<2>(measurements, parameters) {
+	if (parameters.useHuber == true)
+		std::cout << "Parameter useHuber is disregarded when you pass measurements to ShonanAveraging2."
+				"Pass g2o file as input to enable this functionality" << std::endl;
+}
+
 ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
-    : ShonanAveraging<2>(parseMeasurements<Rot2>(g2oFile), parameters) {}
+    : ShonanAveraging<2>(parseMeasurements<Rot2>(g2oFile,
+    		parameters.useHuber? nullptr : nullptr), parameters) {}
 
 /* ************************************************************************* */
 // Explicit instantiation for d=3
@@ -829,17 +836,22 @@ template class ShonanAveraging<3>;
 
 ShonanAveraging3::ShonanAveraging3(const Measurements &measurements,
                                    const Parameters &parameters)
-    : ShonanAveraging<3>(measurements, parameters) {}
+    : ShonanAveraging<3>(measurements, parameters) {
+	if (parameters.useHuber == true)
+			std::cout << "Parameter useHuber is disregarded when you pass measurements to ShonanAveraging2."
+					"Pass g2o file as input to enable this functionality" << std::endl;
+}
 
 ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
-    : ShonanAveraging<3>(parseMeasurements<Rot3>(g2oFile), parameters) {}
+    : ShonanAveraging<3>(parseMeasurements<Rot3>(g2oFile,
+    		parameters.useHuber? nullptr : nullptr), parameters) {}
 
 // TODO(frank): Deprecate after we land pybind wrapper
 
 // Extract Rot3 measurement from Pose3 betweenfactors
 // Modeled after similar function in dataset.cpp
 static BinaryMeasurement<Rot3> convert(
-    const BetweenFactor<Pose3>::shared_ptr &f) {
+    const BetweenFactor<Pose3>::shared_ptr &f, bool useHuber = false) {
   auto gaussian =
       boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
   if (!gaussian)
@@ -847,22 +859,33 @@ static BinaryMeasurement<Rot3> convert(
         "parseMeasurements<Rot3> can only convert Pose3 measurements "
         "with Gaussian noise models.");
   const Matrix6 M = gaussian->covariance();
-  return BinaryMeasurement<Rot3>(
-      f->key1(), f->key2(), f->measured().rotation(),
-      noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
+  if(!useHuber){
+	  return BinaryMeasurement<Rot3>(
+			  f->key1(), f->key2(), f->measured().rotation(),
+			  noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
+  }else{ // wrap noise mode in Huber loss
+	  std::cout << "setting robust huber loss " << std::endl;
+	  return BinaryMeasurement<Rot3>(
+			  f->key1(), f->key2(), f->measured().rotation(),
+			  noiseModel::Robust::Create(
+			            noiseModel::mEstimator::Huber::Create(1.345),
+						noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)));
+  }
 }
 
 static ShonanAveraging3::Measurements extractRot3Measurements(
-    const BetweenFactorPose3s &factors) {
+    const BetweenFactorPose3s &factors, bool useHuber = false) {
   ShonanAveraging3::Measurements result;
   result.reserve(factors.size());
-  for (auto f : factors) result.push_back(convert(f));
+  for (auto f : factors) result.push_back(convert(f,useHuber));
   return result;
 }
 
 ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
                                    const Parameters &parameters)
-    : ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {}
+    : ShonanAveraging<3>(parameters.useHuber?
+    		extractRot3Measurements(factors) :
+			extractRot3Measurements(factors), parameters) {}
 
 /* ************************************************************************* */
 }  // namespace gtsam

gtsam/sfm/ShonanAveraging.h

@@ -53,6 +53,7 @@ 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)
 
   ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
                                 LevenbergMarquardtParams::CeresDefaults(),
@@ -77,6 +78,18 @@ struct GTSAM_EXPORT ShonanAveragingParameters {
 
   void setGaugesWeight(double value) { gamma = value; }
   double getGaugesWeight() { return gamma; }
+
+  void setUseHuber(bool value) { useHuber = value; }
+  bool getUseHuber() { return useHuber; }
+
+  void print() const {
+	  std::cout << " ShonanAveragingParameters: " << std::endl;
+	  std::cout << " alpha: " << alpha  << std::endl;
+	  std::cout << " beta: " << beta  << std::endl;
+	  std::cout << " gamma: " << gamma  << std::endl;
+	  std::cout << " useHuber: " << useHuber  << std::endl;
+	  std::cout << " --------------------------" << std::endl;
+  }
 };
 
 using ShonanAveragingParameters2 = ShonanAveragingParameters<2>;

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -321,6 +321,30 @@ TEST(ShonanAveraging2, noisyToyGraph) {
   EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
 }
 
+/* ************************************************************************* */
+TEST(ShonanAveraging2, noisyToyGraphWithHuber) {
+  // Load 2D toy example
+  auto lmParams = LevenbergMarquardtParams::CeresDefaults();
+  string g2oFile = findExampleDataFile("noisyToyGraph.txt");
+  ShonanAveraging2::Parameters parameters(lmParams);
+  auto measurements = parseMeasurements<Rot2>(g2oFile);
+  std::cout << "----- changing huber before " << std::endl;
+  parameters.setUseHuber(true);
+  parameters.print();
+  std::cout << "----- changing huber after " << std::endl;
+  ShonanAveraging2 shonan(measurements, parameters);
+  EXPECT_LONGS_EQUAL(4, shonan.nrUnknowns());
+
+  // Check graph building
+  NonlinearFactorGraph graph = shonan.buildGraphAt(2);
+  graph.print();
+  EXPECT_LONGS_EQUAL(6, graph.size());
+  auto initial = shonan.initializeRandomly(kRandomNumberGenerator);
+  auto result = shonan.run(initial, 2);
+  EXPECT_DOUBLES_EQUAL(0.0008211, shonan.cost(result.first), 1e-6);
+  EXPECT_DOUBLES_EQUAL(0, result.second, 1e-10); // certificate!
+}
+
 /* ************************************************************************* */
 // Test alpha/beta/gamma prior weighting.
 TEST(ShonanAveraging3, PriorWeights) {