Merge pull request #188 from michaelbosse/fix_bug_robust_residuals

gtsam/linear/NoiseModel.h

@@ -460,6 +460,11 @@ namespace gtsam {
         return MixedVariances(precisions.array().inverse());
       }
 
+      /**
+       * The squaredMahalanobisDistance function for a constrained noisemodel,
+       * for non-constrained versions, uses sigmas, otherwise
+       * uses the penalty function with mu
+       */
       double squaredMahalanobisDistance(const Vector& v) const override;
 
       /** Fully constrained variations */
@@ -680,19 +685,19 @@ namespace gtsam {
       /// Return the contained noise model
       const NoiseModel::shared_ptr& noise() const { return noise_; }
 
-      // TODO: functions below are dummy but necessary for the noiseModel::Base
+      // Functions below are dummy but necessary for the noiseModel::Base
       inline Vector whiten(const Vector& v) const override
       { Vector r = v; this->WhitenSystem(r); return r; }
       inline Matrix Whiten(const Matrix& A) const override
       { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
       inline Vector unwhiten(const Vector& /*v*/) const override
       { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
-
+      /// Compute loss from the m-estimator using the Mahalanobis distance.
       double loss(const double squared_distance) const override {
         return robust_->loss(std::sqrt(squared_distance));
       }
 
-      // TODO: these are really robust iterated re-weighting support functions
+      // These are really robust iterated re-weighting support functions
       virtual void WhitenSystem(Vector& b) const;
       void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
       void WhitenSystem(Matrix& A, Vector& b) const override;
@@ -703,7 +708,6 @@ namespace gtsam {
         return noise_->unweightedWhiten(v);
       }
       double weight(const Vector& v) const override {
-        // Todo(mikebosse): make the robust weight function input a vector.
         return robust_->weight(v.norm());
       }
 

gtsam/linear/tests/testNoiseModel.cpp

@@ -662,25 +662,14 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
 {
   double dead_zone_size = 1.0;
   SharedNoiseModel robust = noiseModel::Robust::Create(
-    noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
-    Unit::Create(3));
-
-/*
- * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes
- * implement a loss function, and GTSAM calls the weight function to evaluate the
- * total penalty, rather than calling the loss function. The weight function should be
- * used during iteratively reweighted least squares optimization, but should not be used to
- * evaluate the total penalty. The long-term solution is for all mEstimators to implement
- * both a weight and a loss function, and for GTSAM to call the loss function when
- * evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it
- * commented out until the underlying bug in GTSAM is fixed.
- *
- * for (int i = 0; i < 5; i++) {
- *   Vector3 error = Vector3(i, 0, 0);
- *   DOUBLES_EQUAL(0.5*max(0,i-1)*max(0,i-1), robust->distance(error), 1e-8);
- * }
- */
+      noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
+      Unit::Create(3));
 
+  for (int i = 0; i < 5; i++) {
+    Vector3 error = Vector3(i, 0, 0);
+    DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
+                  robust->squaredMahalanobisDistance(error), 1e-8);
+  }
 }
 
 TEST(NoiseModel, lossFunctionAtZero)
@@ -707,9 +696,9 @@ TEST(NoiseModel, lossFunctionAtZero)
   auto dcs = mEstimator::DCS::Create(k);
   DOUBLES_EQUAL(dcs->loss(0), 0, 1e-8);
   DOUBLES_EQUAL(dcs->weight(0), 1, 1e-8);
-  // auto lsdz = mEstimator::L2WithDeadZone::Create(k);
-  // DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
-  // DOUBLES_EQUAL(lsdz->weight(0), 1, 1e-8);
+  auto lsdz = mEstimator::L2WithDeadZone::Create(k);
+  DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
+  DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8);
 }
 
 

gtsam/nonlinear/NonlinearFactor.cpp

@@ -114,7 +114,7 @@ double NoiseModelFactor::weight(const Values& c) const {
     if (noiseModel_) {
       const Vector b = unwhitenedError(c);
       check(noiseModel_, b.size());
-      return 0.5 * noiseModel_->weight(b);
+      return noiseModel_->weight(b);
     }
     else
       return 1.0;

tests/testNonlinearFactor.cpp

@@ -101,6 +101,82 @@ TEST( NonlinearFactor, NonlinearFactor )
   DOUBLES_EQUAL(expected,actual,0.00000001);
 }
 
+/* ************************************************************************* */
+TEST(NonlinearFactor, Weight) {
+  // create a values structure for the non linear factor graph
+  Values values;
+
+  // Instantiate a concrete class version of a NoiseModelFactor
+  PriorFactor<Point2> factor1(X(1), Point2(0, 0));
+  values.insert(X(1), Point2(0.1, 0.1));
+
+  CHECK(assert_equal(1.0, factor1.weight(values)));
+
+  // Factor with noise model
+  auto noise = noiseModel::Isotropic::Sigma(2, 0.2);
+  PriorFactor<Point2> factor2(X(2), Point2(1, 1), noise);
+  values.insert(X(2), Point2(1.1, 1.1));
+
+  CHECK(assert_equal(1.0, factor2.weight(values)));
+
+  Point2 estimate(3, 3), prior(1, 1);
+  double distance = (estimate - prior).norm();
+
+  auto gaussian = noiseModel::Isotropic::Sigma(2, 0.2);
+
+  PriorFactor<Point2> factor;
+
+  // vector to store all the robust models in so we can test iteratively.
+  vector<noiseModel::Robust::shared_ptr> robust_models;
+
+  // Fair noise model
+  auto fair = noiseModel::Robust::Create(
+      noiseModel::mEstimator::Fair::Create(1.3998), gaussian);
+  robust_models.push_back(fair);
+
+  // Huber noise model
+  auto huber = noiseModel::Robust::Create(
+      noiseModel::mEstimator::Huber::Create(1.345), gaussian);
+  robust_models.push_back(huber);
+
+  // Cauchy noise model
+  auto cauchy = noiseModel::Robust::Create(
+      noiseModel::mEstimator::Cauchy::Create(0.1), gaussian);
+  robust_models.push_back(cauchy);
+
+  // Tukey noise model
+  auto tukey = noiseModel::Robust::Create(
+      noiseModel::mEstimator::Tukey::Create(4.6851), gaussian);
+  robust_models.push_back(tukey);
+
+  // Welsch noise model
+  auto welsch = noiseModel::Robust::Create(
+      noiseModel::mEstimator::Welsch::Create(2.9846), gaussian);
+  robust_models.push_back(welsch);
+
+  // Geman-McClure noise model
+  auto gm = noiseModel::Robust::Create(
+      noiseModel::mEstimator::GemanMcClure::Create(1.0), gaussian);
+  robust_models.push_back(gm);
+
+  // DCS noise model
+  auto dcs = noiseModel::Robust::Create(
+      noiseModel::mEstimator::DCS::Create(1.0), gaussian);
+  robust_models.push_back(dcs);
+
+  // L2WithDeadZone noise model
+  auto l2 = noiseModel::Robust::Create(
+      noiseModel::mEstimator::L2WithDeadZone::Create(1.0), gaussian);
+  robust_models.push_back(l2);
+
+  for(auto&& model: robust_models) {
+    factor = PriorFactor<Point2>(X(3), prior, model);
+    values.clear();
+    values.insert(X(3), estimate);
+    CHECK(assert_equal(model->robust()->weight(distance), factor.weight(values)));
+  }
+}
+
 /* ************************************************************************* */
 TEST( NonlinearFactor, linearize_f1 )
 {