Add Tukey loss function.

Change-Id: I7c76f13e01863440fc207e99b3fc7ad3fb6f7d1a

include/ceres/loss_function.h

diff --git a/include/ceres/loss_function.h b/include/ceres/loss_function.h
index 2c58500..70c981d 100644
--- a/include/ceres/loss_function.h
+++ b/include/ceres/loss_function.h
@@ -1,5 +1,5 @@
 // Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2014 Google Inc. All rights reserved.
 // http://code.google.com/p/ceres-solver/
 //
 // Redistribution and use in source and binary forms, with or without
@@ -274,6 +274,25 @@
   const double a_, b_, c_;
 };
 
+// This is the Tukey biweight loss function which aggressively
+// attempts to suppress large errors.
+//
+// The term is computed as:
+//
+//   rho(s) = a^2 / 6 * (1 - (1 - s / a^2)^3 )   for s <= a^2,
+//   rho(s) = a^2 / 6                            for s >  a^2.
+//
+// At s = 0: rho = [0, 0.5, -1 / a^2]
+class CERES_EXPORT TukeyLoss : public ceres::LossFunction {
+ public:
+  explicit TukeyLoss(double a) : a_(a), a_squared_(a * a) { }
+  virtual void Evaluate(double, double*) const;
+
+ private:
+  const double a_;
+  const double a_squared_;
+};
+
 // Composition of two loss functions.  The error is the result of first
 // evaluating g followed by f to yield the composition f(g(s)).
 // The loss functions must not be NULL.

internal/ceres/loss_function.cc

diff --git a/internal/ceres/loss_function.cc b/internal/ceres/loss_function.cc
index 62b545b..6500247 100644
--- a/internal/ceres/loss_function.cc
+++ b/internal/ceres/loss_function.cc
@@ -1,5 +1,5 @@
 // Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// Copyright 2014 Google Inc. All rights reserved.
 // http://code.google.com/p/ceres-solver/
 //
 // Redistribution and use in source and binary forms, with or without
@@ -114,6 +114,22 @@
   }
 }
 
+void TukeyLoss::Evaluate(double s, double* rho) const {
+  if (s <= a_squared_) {
+    // Inlier region.
+    const double value = 1.0 - s / a_squared_;
+    const double value_sq = value * value;
+    rho[0] = a_squared_ / 6.0 * (1.0 - value_sq * value);
+    rho[1] = 0.5 * value_sq;
+    rho[2] = -1.0 / a_squared_ * value;
+  } else {
+    // Outlier region.
+    rho[0] = a_squared_ / 6.0;
+    rho[1] = 0.0;
+    rho[2] = 0.0;
+  }
+}
+
 ComposedLoss::ComposedLoss(const LossFunction* f, Ownership ownership_f,
                            const LossFunction* g, Ownership ownership_g)
     : f_(CHECK_NOTNULL(f)),

internal/ceres/loss_function_test.cc

diff --git a/internal/ceres/loss_function_test.cc b/internal/ceres/loss_function_test.cc
index 0967406..04b6ce2 100644
--- a/internal/ceres/loss_function_test.cc
+++ b/internal/ceres/loss_function_test.cc
@@ -130,6 +130,13 @@
   AssertLossFunctionIsValid(TolerantLoss(20.0, 1.0), 20.0 + 1000.0);
 }
 
+TEST(LossFunction, TukeyLoss) {
+  AssertLossFunctionIsValid(TukeyLoss(0.7), 0.357);
+  AssertLossFunctionIsValid(TukeyLoss(0.7), 1.792);
+  AssertLossFunctionIsValid(TukeyLoss(1.3), 0.357);
+  AssertLossFunctionIsValid(TukeyLoss(1.3), 1.792);
+}
+
 TEST(LossFunction, ComposedLoss) {
   {
     HuberLoss f(0.7);