// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2015 Google Inc. All rights reserved.

// http://ceres-solver.org/

//

// Redistribution and use in source and binary forms, with or without

// modification, are permitted provided that the following conditions are met:

//

// * Redistributions of source code must retain the above copyright notice,

//   this list of conditions and the following disclaimer.

// * Redistributions in binary form must reproduce the above copyright notice,

//   this list of conditions and the following disclaimer in the documentation

//   and/or other materials provided with the distribution.

// * Neither the name of Google Inc. nor the names of its contributors may be

//   used to endorse or promote products derived from this software without

//   specific prior written permission.

//

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// POSSIBILITY OF SUCH DAMAGE.

//

// Author: sameeragarwal@google.com (Sameer Agarwal)

//

// The LossFunction interface is the way users describe how residuals

// are converted to cost terms for the overall problem cost function.

// For the exact manner in which loss functions are converted to the

// overall cost for a problem, see problem.h.

//

// For least squares problem where there are no outliers and standard

// squared loss is expected, it is not necessary to create a loss

// function; instead passing a NULL to the problem when adding

// residuals implies a standard squared loss.

//

// For least squares problems where the minimization may encounter

// input terms that contain outliers, that is, completely bogus

// measurements, it is important to use a loss function that reduces

// their associated penalty.

//

// Consider a structure from motion problem. The unknowns are 3D

// points and camera parameters, and the measurements are image

// coordinates describing the expected reprojected position for a

// point in a camera. For example, we want to model the geometry of a

// street scene with fire hydrants and cars, observed by a moving

// camera with unknown parameters, and the only 3D points we care

// about are the pointy tippy-tops of the fire hydrants. Our magic

// image processing algorithm, which is responsible for producing the

// measurements that are input to Ceres, has found and matched all

// such tippy-tops in all image frames, except that in one of the

// frame it mistook a car's headlight for a hydrant. If we didn't do

// anything special (i.e. if we used a basic quadratic loss), the

// residual for the erroneous measurement will result in extreme error

// due to the quadratic nature of squared loss. This results in the

// entire solution getting pulled away from the optimimum to reduce

// the large error that would otherwise be attributed to the wrong

// measurement.

//

// Using a robust loss function, the cost for large residuals is

// reduced. In the example above, this leads to outlier terms getting

// downweighted so they do not overly influence the final solution.

//

// What cost function is best?

//

// In general, there isn't a principled way to select a robust loss

// function. The authors suggest starting with a non-robust cost, then

// only experimenting with robust loss functions if standard squared

// loss doesn't work.

#ifndef CERES_PUBLIC_LOSS_FUNCTION_H_

#define CERES_PUBLIC_LOSS_FUNCTION_H_

#include "glog/logging.h"

#include "ceres/internal/macros.h"

#include "ceres/internal/scoped_ptr.h"

#include "ceres/types.h"

#include "ceres/internal/disable_warnings.h"

namespace ceres {

class CERES_EXPORT LossFunction {

 public:

  virtual ~LossFunction() {}

  // For a residual vector with squared 2-norm 'sq_norm', this method

  // is required to fill in the value and derivatives of the loss

  // function (rho in this example):

  //

  //   out[0] = rho(sq_norm),

  //   out[1] = rho'(sq_norm),

  //   out[2] = rho''(sq_norm),

  //

  // Here the convention is that the contribution of a term to the

  // cost function is given by 1/2 rho(s),  where

  //

  //   s = ||residuals||^2.

  //

  // Calling the method with a negative value of 's' is an error and

  // the implementations are not required to handle that case.

  //

  // Most sane choices of rho() satisfy:

  //

  //   rho(0) = 0,

  //   rho'(0) = 1,

  //   rho'(s) < 1 in outlier region,

  //   rho''(s) < 0 in outlier region,

  //

  // so that they mimic the least squares cost for small residuals.

  virtual void Evaluate(double sq_norm, double out[3]) const = 0;

};

// Some common implementations follow below.

//

// Note: in the region of interest (i.e. s < 3) we have:

//   TrivialLoss >= HuberLoss >= SoftLOneLoss >= CauchyLoss

// This corresponds to no robustification.

//

//   rho(s) = s

//

// At s = 0: rho = [0, 1, 0].

//

// It is not normally necessary to use this, as passing NULL for the

// loss function when building the problem accomplishes the same

// thing.

class CERES_EXPORT TrivialLoss : public LossFunction {

 public:

  virtual void Evaluate(double, double*) const;

};

// Scaling

// -------

// Given one robustifier

//   s -> rho(s)

// one can change the length scale at which robustification takes

// place, by adding a scale factor 'a' as follows:

//

//   s -> a^2 rho(s / a^2).

//

// The first and second derivatives are:

//

//   s -> rho'(s / a^2),

//   s -> (1 / a^2) rho''(s / a^2),

//

// but the behaviour near s = 0 is the same as the original function,

// i.e.

//

//   rho(s) = s + higher order terms,

//   a^2 rho(s / a^2) = s + higher order terms.

//

// The scalar 'a' should be positive.

//

// The reason for the appearance of squaring is that 'a' is in the

// units of the residual vector norm whereas 's' is a squared

// norm. For applications it is more convenient to specify 'a' than

// its square. The commonly used robustifiers below are described in

// un-scaled format (a = 1) but their implementations work for any

// non-zero value of 'a'.

// Huber.

//

//   rho(s) = s               for s <= 1,

//   rho(s) = 2 sqrt(s) - 1   for s >= 1.

//

// At s = 0: rho = [0, 1, 0].

//

// The scaling parameter 'a' corresponds to 'delta' on this page:

//   http://en.wikipedia.org/wiki/Huber_Loss_Function

class CERES_EXPORT HuberLoss : public LossFunction {

 public:

  explicit HuberLoss(double a) : a_(a), b_(a * a) { }

  virtual void Evaluate(double, double*) const;

 private:

  const double a_;

  // b = a^2.

  const double b_;

};

// Soft L1, similar to Huber but smooth.

//

//   rho(s) = 2 (sqrt(1 + s) - 1).

//

// At s = 0: rho = [0, 1, -1/2].

class CERES_EXPORT SoftLOneLoss : public LossFunction {

 public:

  explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) { }

  virtual void Evaluate(double, double*) const;

 private:

  // b = a^2.

  const double b_;

  // c = 1 / a^2.

  const double c_;

};

// Inspired by the Cauchy distribution

//

//   rho(s) = log(1 + s).

//

// At s = 0: rho = [0, 1, -1].

class CERES_EXPORT CauchyLoss : public LossFunction {

 public:

  explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) { }

  virtual void Evaluate(double, double*) const;

 private:

  // b = a^2.

  const double b_;

  // c = 1 / a^2.

  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 CERES_EXPORT 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 CERES_EXPORT TolerantLoss : public LossFunction {

 public:

  explicit TolerantLoss(double a, double b);

  virtual void Evaluate(double, double*) const;

 private:

  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_squared_(a * a) { }

  virtual void Evaluate(double, double*) const;

 private:

  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.

class CERES_EXPORT 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

// different error terms differently (e.g., weight pixel reprojection

// errors differently from terrain errors).

//

// If rho is the wrapped robustifier, then this simply outputs

// s -> a * rho(s)

//

// The first and second derivatives are, not surprisingly

// s -> a * rho'(s)

// s -> a * rho''(s)

//

// Since we treat the a NULL Loss function as the Identity loss

// function, rho = NULL is a valid input and will result in the input

// being scaled by a. This provides a simple way of implementing a

// scaled ResidualBlock.

class CERES_EXPORT ScaledLoss : public LossFunction {

 public:

  // Constructs a ScaledLoss wrapping another loss function. Takes

  // ownership of the wrapped loss function or not depending on the

  // ownership parameter.

  ScaledLoss(const LossFunction* rho, double a, Ownership ownership) :

      rho_(rho), a_(a), ownership_(ownership) { }

  virtual ~ScaledLoss() {

    if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {

      rho_.release();

    }

  }

  virtual void Evaluate(double, double*) const;

 private:

  internal::scoped_ptr<const LossFunction> rho_;

  const double a_;

  const Ownership ownership_;

  CERES_DISALLOW_COPY_AND_ASSIGN(ScaledLoss);

};

// Sometimes after the optimization problem has been constructed, we

// wish to mutate the scale of the loss function. For example, when

// performing estimation from data which has substantial outliers,

// convergence can be improved by starting out with a large scale,

// optimizing the problem and then reducing the scale. This can have

// better convergence behaviour than just using a loss function with a

// small scale.

//

// This templated class allows the user to implement a loss function

// whose scale can be mutated after an optimization problem has been

// constructed.

//

// Since we treat the a NULL Loss function as the Identity loss

// function, rho = NULL is a valid input.

//

// Example usage

//

//  Problem problem;

//

//  // Add parameter blocks

//

//  CostFunction* cost_function =

//    new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>(

//      new UW_Camera_Mapper(feature_x, feature_y));

//

//  LossFunctionWrapper* loss_function(new HuberLoss(1.0), TAKE_OWNERSHIP);

//

//  problem.AddResidualBlock(cost_function, loss_function, parameters);

//

//  Solver::Options options;

//  Solger::Summary summary;

//

//  Solve(options, &problem, &summary)

//

//  loss_function->Reset(new HuberLoss(1.0), TAKE_OWNERSHIP);

//

//  Solve(options, &problem, &summary)

//

class CERES_EXPORT LossFunctionWrapper : public LossFunction {

 public:

  LossFunctionWrapper(LossFunction* rho, Ownership ownership)

      : rho_(rho), ownership_(ownership) {

  }

  virtual ~LossFunctionWrapper() {

    if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {

      rho_.release();

    }

  }

  virtual void Evaluate(double sq_norm, double out[3]) const {

    if (rho_.get() == NULL) {

      out[0] = sq_norm;

      out[1] = 1.0;

      out[2] = 0.0;

    }

    else {

      rho_->Evaluate(sq_norm, out);

    }

  }

  void Reset(LossFunction* rho, Ownership ownership) {

    if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {

      rho_.release();

    }

    rho_.reset(rho);

    ownership_ = ownership;

  }

 private:

  internal::scoped_ptr<const LossFunction> rho_;

  Ownership ownership_;

  CERES_DISALLOW_COPY_AND_ASSIGN(LossFunctionWrapper);

};

}  // namespace ceres

#include "ceres/internal/reenable_warnings.h"

#endif  // CERES_PUBLIC_LOSS_FUNCTION_H_