Add ArcTanLoss, TolerantLoss and ComposedLossFunction. Based on work by James Roseborough. Change-Id: Idc4e0b099028f67702bfc7fe3e43dbd96b6f9256
diff --git a/include/ceres/loss_function.h b/include/ceres/loss_function.h index c95a0a0..f178991 100644 --- a/include/ceres/loss_function.h +++ b/include/ceres/loss_function.h
@@ -213,6 +213,77 @@ const double c_; }; +// Loss that is capped beyond a certain level using the arc-tangent function. +// The scaling parameter 'a' determines the level where falloff occurs. +// For costs much smaller than 'a', the loss function is linear and behaves like +// TrivialLoss, and for values much larger than 'a' the value asymptotically +// approaches the constant value of a * PI / 2. +// +// rho(s) = a atan(s / a). +// +// At s = 0: rho = [0, 1, 0]. +class ArctanLoss : public LossFunction { + public: + explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) { } + virtual void Evaluate(double, double*) const; + private: + const double a_; + // b = 1 / a^2. + const double b_; +}; + +// Loss function that maps to approximately zero cost in a range around the +// origin, and reverts to linear in error (quadratic in cost) beyond this range. +// The tolerance parameter 'a' sets the nominal point at which the +// transition occurs, and the transition size parameter 'b' sets the nominal +// distance over which most of the transition occurs. Both a and b must be +// greater than zero, and typically b will be set to a fraction of a. +// The slope rho'[s] varies smoothly from about 0 at s <= a - b to +// about 1 at s >= a + b. +// +// The term is computed as: +// +// rho(s) = b log(1 + exp((s - a) / b)) - c0. +// +// where c0 is chosen so that rho(0) == 0 +// +// c0 = b log(1 + exp(-a / b) +// +// This has the following useful properties: +// +// rho(s) == 0 for s = 0 +// rho'(s) ~= 0 for s << a - b +// rho'(s) ~= 1 for s >> a + b +// rho''(s) > 0 for all s +// +// In addition, all derivatives are continuous, and the curvature is +// concentrated in the range a - b to a + b. +// +// At s = 0: rho = [0, ~0, ~0]. +class TolerantLoss : public LossFunction { + public: + explicit TolerantLoss(double a, double b); + virtual void Evaluate(double, double*) const; + + private: + const double a_, b_, c_; +}; + +// 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. +class ComposedLoss : public LossFunction { + public: + explicit ComposedLoss(const LossFunction* f, Ownership ownership_f, + const LossFunction* g, Ownership ownership_g); + virtual ~ComposedLoss(); + virtual void Evaluate(double, double*) const; + + private: + internal::scoped_ptr<const LossFunction> f_, g_; + const Ownership ownership_f_, ownership_g_; +}; + // The discussion above has to do with length scaling: it affects the space // in which s is measured. Sometimes you want to simply scale the output // value of the robustifier. For example, you might want to weight
diff --git a/internal/ceres/loss_function.cc b/internal/ceres/loss_function.cc index 00b2b18..b948f28 100644 --- a/internal/ceres/loss_function.cc +++ b/internal/ceres/loss_function.cc
@@ -77,6 +77,70 @@ rho[2] = - c_ * (inv * inv); } +void ArctanLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1 + s * s * b_; + const double inv = 1 / sum; + // 'sum' and 'inv' are always positive. + rho[0] = a_ * atan2(s, a_); + rho[1] = inv; + rho[2] = -2 * s * b_ * (inv * inv); +} + +TolerantLoss::TolerantLoss(double a, double b) + : a_(a), + b_(b), + c_(b * log(1.0 + exp(-a / b))) { + CHECK_GE(a, 0.0); + CHECK_GT(b, 0.0); +} + +void TolerantLoss::Evaluate(double s, double rho[3]) const { + const double x = (s - a_) / b_; + // The basic equation is rho[0] = b ln(1 + e^x). However, if e^x is too + // large, it will overflow. Since numerically 1 + e^x == e^x when the + // x is greater than about ln(2^53) for doubles, beyond this threshold + // we substitute x for ln(1 + e^x) as a numerically equivalent approximation. + static const double kLog2Pow53 = 36.7; // ln(MathLimits<double>::kEpsilon). + if (x > kLog2Pow53) { + rho[0] = s - a_ - c_; + rho[1] = 1.0; + rho[2] = 0.0; + } else { + const double e_x = exp(x); + rho[0] = b_ * log(1.0 + e_x) - c_; + rho[1] = e_x / (1.0 + e_x); + rho[2] = 0.5 / (b_ * (1.0 + cosh(x))); + } +} + +ComposedLoss::ComposedLoss(const LossFunction* f, Ownership ownership_f, + const LossFunction* g, Ownership ownership_g) + : f_(CHECK_NOTNULL(f)), + g_(CHECK_NOTNULL(g)), + ownership_f_(ownership_f), + ownership_g_(ownership_g) { +} + +ComposedLoss::~ComposedLoss() { + if (ownership_f_ == DO_NOT_TAKE_OWNERSHIP) { + f_.release(); + } + if (ownership_g_ == DO_NOT_TAKE_OWNERSHIP) { + g_.release(); + } +} + +void ComposedLoss::Evaluate(double s, double rho[3]) const { + double rho_f[3], rho_g[3]; + g_->Evaluate(s, rho_g); + f_->Evaluate(rho_g[0], rho_f); + rho[0] = rho_f[0]; + // f'(g(s)) * g'(s). + rho[1] = rho_f[1] * rho_g[1]; + // f''(g(s)) * g'(s) * g'(s) + f'(g(s)) * g''(s). + rho[2] = rho_f[2] * rho_g[1] * rho_g[1] + rho_f[1] * rho_g[2]; +} + void ScaledLoss::Evaluate(double s, double rho[3]) const { if (rho_.get() == NULL) { rho[0] = a_ * s;
diff --git a/internal/ceres/loss_function_test.cc b/internal/ceres/loss_function_test.cc index 8afcbd8..0967406 100644 --- a/internal/ceres/loss_function_test.cc +++ b/internal/ceres/loss_function_test.cc
@@ -104,6 +104,49 @@ AssertLossFunctionIsValid(CauchyLoss(1.3), 1.792); } +TEST(LossFunction, ArctanLoss) { + AssertLossFunctionIsValid(ArctanLoss(0.7), 0.357); + AssertLossFunctionIsValid(ArctanLoss(0.7), 1.792); + AssertLossFunctionIsValid(ArctanLoss(1.3), 0.357); + AssertLossFunctionIsValid(ArctanLoss(1.3), 1.792); +} + +TEST(LossFunction, TolerantLoss) { + AssertLossFunctionIsValid(TolerantLoss(0.7, 0.4), 0.357); + AssertLossFunctionIsValid(TolerantLoss(0.7, 0.4), 1.792); + AssertLossFunctionIsValid(TolerantLoss(0.7, 0.4), 55.5); + AssertLossFunctionIsValid(TolerantLoss(1.3, 0.1), 0.357); + AssertLossFunctionIsValid(TolerantLoss(1.3, 0.1), 1.792); + AssertLossFunctionIsValid(TolerantLoss(1.3, 0.1), 55.5); + // Check the value at zero is actually zero. + double rho[3]; + TolerantLoss(0.7, 0.4).Evaluate(0.0, rho); + ASSERT_NEAR(rho[0], 0.0, 1e-6); + // Check that loss before and after the approximation threshold are good. + // A threshold of 36.7 is used by the implementation. + AssertLossFunctionIsValid(TolerantLoss(20.0, 1.0), 20.0 + 36.6); + AssertLossFunctionIsValid(TolerantLoss(20.0, 1.0), 20.0 + 36.7); + AssertLossFunctionIsValid(TolerantLoss(20.0, 1.0), 20.0 + 36.8); + AssertLossFunctionIsValid(TolerantLoss(20.0, 1.0), 20.0 + 1000.0); +} + +TEST(LossFunction, ComposedLoss) { + { + HuberLoss f(0.7); + CauchyLoss g(1.3); + ComposedLoss c(&f, DO_NOT_TAKE_OWNERSHIP, &g, DO_NOT_TAKE_OWNERSHIP); + AssertLossFunctionIsValid(c, 0.357); + AssertLossFunctionIsValid(c, 1.792); + } + { + CauchyLoss f(0.7); + HuberLoss g(1.3); + ComposedLoss c(&f, DO_NOT_TAKE_OWNERSHIP, &g, DO_NOT_TAKE_OWNERSHIP); + AssertLossFunctionIsValid(c, 0.357); + AssertLossFunctionIsValid(c, 1.792); + } +} + TEST(LossFunction, ScaledLoss) { // Wrap a few loss functions, and a few scale factors. This can't combine // construction with the call to AssertLossFunctionIsValid() because Apple's @@ -128,6 +171,22 @@ ScaledLoss scaled_loss(new CauchyLoss(1.3), 10, TAKE_OWNERSHIP); AssertLossFunctionIsValid(scaled_loss, 1.792); } + { + ScaledLoss scaled_loss(new ArctanLoss(1.3), 10, TAKE_OWNERSHIP); + AssertLossFunctionIsValid(scaled_loss, 1.792); + } + { + ScaledLoss scaled_loss( + new TolerantLoss(1.3, 0.1), 10, TAKE_OWNERSHIP); + AssertLossFunctionIsValid(scaled_loss, 1.792); + } + { + ScaledLoss scaled_loss( + new ComposedLoss( + new HuberLoss(0.8), TAKE_OWNERSHIP, + new TolerantLoss(1.3, 0.5), TAKE_OWNERSHIP), 10, TAKE_OWNERSHIP); + AssertLossFunctionIsValid(scaled_loss, 1.792); + } } TEST(LossFunction, LossFunctionWrapper) {