429 lines
14 KiB
C
429 lines
14 KiB
C
|
// 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_
|