ceres-solver / ceres-solver / b54f05b8eede19145f3cb9b5c48ef7b0080b2be3 / . / docs / source / nnls_modeling.rst

.. default-domain:: cpp | |

.. highlight:: c++ | |

.. cpp:namespace:: ceres | |

.. _`chapter-nnls_modeling`: | |

================================= | |

Modeling Non-linear Least Squares | |

================================= | |

Introduction | |

============ | |

Ceres solver consists of two distinct parts. A modeling API which | |

provides a rich set of tools to construct an optimization problem one | |

term at a time and a solver API that controls the minimization | |

algorithm. This chapter is devoted to the task of modeling | |

optimization problems using Ceres. :ref:`chapter-nnls_solving` discusses | |

the various ways in which an optimization problem can be solved using | |

Ceres. | |

Ceres solves robustified bounds constrained non-linear least squares | |

problems of the form: | |

.. math:: :label: ceresproblem_modeling | |

\min_{\mathbf{x}} &\quad \frac{1}{2}\sum_{i} | |

\rho_i\left(\left\|f_i\left(x_{i_1}, | |

... ,x_{i_k}\right)\right\|^2\right) \\ | |

\text{s.t.} &\quad l_j \le x_j \le u_j | |

In Ceres parlance, the expression | |

:math:`\rho_i\left(\left\|f_i\left(x_{i_1},...,x_{i_k}\right)\right\|^2\right)` | |

is known as a **residual block**, where :math:`f_i(\cdot)` is a | |

:class:`CostFunction` that depends on the **parameter blocks** | |

:math:`\left\{x_{i_1},... , x_{i_k}\right\}`. | |

In most optimization problems small groups of scalars occur | |

together. For example the three components of a translation vector and | |

the four components of the quaternion that define the pose of a | |

camera. We refer to such a group of scalars as a **parameter block**. Of | |

course a parameter block can be just a single scalar too. | |

:math:`\rho_i` is a :class:`LossFunction`. A :class:`LossFunction` is | |

a scalar valued function that is used to reduce the influence of | |

outliers on the solution of non-linear least squares problems. | |

:math:`l_j` and :math:`u_j` are lower and upper bounds on the | |

parameter block :math:`x_j`. | |

As a special case, when :math:`\rho_i(x) = x`, i.e., the identity | |

function, and :math:`l_j = -\infty` and :math:`u_j = \infty` we get | |

the usual unconstrained `non-linear least squares problem | |

<http://en.wikipedia.org/wiki/Non-linear_least_squares>`_. | |

.. math:: :label: ceresproblemunconstrained | |

\frac{1}{2}\sum_{i} \left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2. | |

:class:`CostFunction` | |

===================== | |

For each term in the objective function, a :class:`CostFunction` is | |

responsible for computing a vector of residuals and Jacobian | |

matrices. Concretely, consider a function | |

:math:`f\left(x_{1},...,x_{k}\right)` that depends on parameter blocks | |

:math:`\left[x_{1}, ... , x_{k}\right]`. | |

Then, given :math:`\left[x_{1}, ... , x_{k}\right]`, | |

:class:`CostFunction` is responsible for computing the vector | |

:math:`f\left(x_{1},...,x_{k}\right)` and the Jacobian matrices | |

.. math:: J_i = D_i f(x_1, ..., x_k) \quad \forall i \in \{1, \ldots, k\} | |

.. class:: CostFunction | |

.. code-block:: c++ | |

class CostFunction { | |

public: | |

virtual bool Evaluate(double const* const* parameters, | |

double* residuals, | |

double** jacobians) const = 0; | |

const vector<int32>& parameter_block_sizes(); | |

int num_residuals() const; | |

protected: | |

vector<int32>* mutable_parameter_block_sizes(); | |

void set_num_residuals(int num_residuals); | |

}; | |

The signature of the :class:`CostFunction` (number and sizes of input | |

parameter blocks and number of outputs) is stored in | |

:member:`CostFunction::parameter_block_sizes_` and | |

:member:`CostFunction::num_residuals_` respectively. User code | |

inheriting from this class is expected to set these two members with | |

the corresponding accessors. This information will be verified by the | |

:class:`Problem` when added with :func:`Problem::AddResidualBlock`. | |

.. function:: bool CostFunction::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const | |

Compute the residual vector and the Jacobian matrices. | |

``parameters`` is an array of arrays of size | |

``CostFunction::parameter_block_sizes_.size()`` and | |

``parameters[i]`` is an array of size ``parameter_block_sizes_[i]`` | |

that contains the :math:`i^{\text{th}}` parameter block that the | |

``CostFunction`` depends on. | |

``parameters`` is never ``nullptr``. | |

``residuals`` is an array of size ``num_residuals_``. | |

``residuals`` is never ``nullptr``. | |

``jacobians`` is an array of arrays of size | |

``CostFunction::parameter_block_sizes_.size()``. | |

If ``jacobians`` is ``nullptr``, the user is only expected to compute | |

the residuals. | |

``jacobians[i]`` is a row-major array of size ``num_residuals x | |

parameter_block_sizes_[i]``. | |

If ``jacobians[i]`` is **not** ``nullptr``, the user is required to | |

compute the Jacobian of the residual vector with respect to | |

``parameters[i]`` and store it in this array, i.e. | |

``jacobians[i][r * parameter_block_sizes_[i] + c]`` = | |

:math:`\frac{\displaystyle \partial \text{residual}[r]}{\displaystyle \partial \text{parameters}[i][c]}` | |

If ``jacobians[i]`` is ``nullptr``, then this computation can be | |

skipped. This is the case when the corresponding parameter block is | |

marked constant. | |

The return value indicates whether the computation of the residuals | |

and/or jacobians was successful or not. This can be used to | |

communicate numerical failures in Jacobian computations for | |

instance. | |

:class:`SizedCostFunction` | |

========================== | |

.. class:: SizedCostFunction | |

If the size of the parameter blocks and the size of the residual | |

vector is known at compile time (this is the common case), | |

:class:`SizeCostFunction` can be used where these values can be | |

specified as template parameters and the user only needs to | |

implement :func:`CostFunction::Evaluate`. | |

.. code-block:: c++ | |

template<int kNumResiduals, int... Ns> | |

class SizedCostFunction : public CostFunction { | |

public: | |

virtual bool Evaluate(double const* const* parameters, | |

double* residuals, | |

double** jacobians) const = 0; | |

}; | |

:class:`AutoDiffCostFunction` | |

============================= | |

.. class:: AutoDiffCostFunction | |

Defining a :class:`CostFunction` or a :class:`SizedCostFunction` | |

can be a tedious and error prone especially when computing | |

derivatives. To this end Ceres provides `automatic differentiation | |

<http://en.wikipedia.org/wiki/Automatic_differentiation>`_. | |

.. code-block:: c++ | |

template <typename CostFunctor, | |

int kNumResiduals, // Number of residuals, or ceres::DYNAMIC. | |

int... Ns> // Size of each parameter block | |

class AutoDiffCostFunction : public | |

SizedCostFunction<kNumResiduals, Ns> { | |

public: | |

AutoDiffCostFunction(CostFunctor* functor, ownership = TAKE_OWNERSHIP); | |

// Ignore the template parameter kNumResiduals and use | |

// num_residuals instead. | |

AutoDiffCostFunction(CostFunctor* functor, | |

int num_residuals, | |

ownership = TAKE_OWNERSHIP); | |

}; | |

To get an auto differentiated cost function, you must define a | |

class with a templated ``operator()`` (a functor) that computes the | |

cost function in terms of the template parameter ``T``. The | |

autodiff framework substitutes appropriate ``Jet`` objects for | |

``T`` in order to compute the derivative when necessary, but this | |

is hidden, and you should write the function as if ``T`` were a | |

scalar type (e.g. a double-precision floating point number). | |

The function must write the computed value in the last argument | |

(the only non-``const`` one) and return true to indicate success. | |

For example, consider a scalar error :math:`e = k - x^\top y`, | |

where both :math:`x` and :math:`y` are two-dimensional vector | |

parameters and :math:`k` is a constant. The form of this error, | |

which is the difference between a constant and an expression, is a | |

common pattern in least squares problems. For example, the value | |

:math:`x^\top y` might be the model expectation for a series of | |

measurements, where there is an instance of the cost function for | |

each measurement :math:`k`. | |

The actual cost added to the total problem is :math:`e^2`, or | |

:math:`(k - x^\top y)^2`; however, the squaring is implicitly done | |

by the optimization framework. | |

To write an auto-differentiable cost function for the above model, | |

first define the object | |

.. code-block:: c++ | |

class MyScalarCostFunctor { | |

MyScalarCostFunctor(double k): k_(k) {} | |

template <typename T> | |

bool operator()(const T* const x , const T* const y, T* e) const { | |

e[0] = k_ - x[0] * y[0] - x[1] * y[1]; | |

return true; | |

} | |

private: | |

double k_; | |

}; | |

Note that in the declaration of ``operator()`` the input parameters | |

``x`` and ``y`` come first, and are passed as const pointers to arrays | |

of ``T``. If there were three input parameters, then the third input | |

parameter would come after ``y``. The output is always the last | |

parameter, and is also a pointer to an array. In the example above, | |

``e`` is a scalar, so only ``e[0]`` is set. | |

Then given this class definition, the auto differentiated cost | |

function for it can be constructed as follows. | |

.. code-block:: c++ | |

CostFunction* cost_function | |

= new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>( | |

new MyScalarCostFunctor(1.0)); ^ ^ ^ | |

| | | | |

Dimension of residual ------+ | | | |

Dimension of x ----------------+ | | |

Dimension of y -------------------+ | |

In this example, there is usually an instance for each measurement | |

of ``k``. | |

In the instantiation above, the template parameters following | |

``MyScalarCostFunction``, ``<1, 2, 2>`` describe the functor as | |

computing a 1-dimensional output from two arguments, both | |

2-dimensional. | |

By default :class:`AutoDiffCostFunction` will take ownership of the cost | |

functor pointer passed to it, ie. will call `delete` on the cost functor | |

when the :class:`AutoDiffCostFunction` itself is deleted. However, this may | |

be undesirable in certain cases, therefore it is also possible to specify | |

:class:`DO_NOT_TAKE_OWNERSHIP` as a second argument in the constructor, | |

while passing a pointer to a cost functor which does not need to be deleted | |

by the AutoDiffCostFunction. For example: | |

.. code-block:: c++ | |

MyScalarCostFunctor functor(1.0) | |

CostFunction* cost_function | |

= new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>( | |

&functor, DO_NOT_TAKE_OWNERSHIP); | |

:class:`AutoDiffCostFunction` also supports cost functions with a | |

runtime-determined number of residuals. For example: | |

.. code-block:: c++ | |

CostFunction* cost_function | |

= new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>( | |

new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ | |

runtime_number_of_residuals); <----+ | | | | |

| | | | | |

| | | | | |

Actual number of residuals ------+ | | | | |

Indicate dynamic number of residuals --------+ | | | |

Dimension of x ------------------------------------+ | | |

Dimension of y ---------------------------------------+ | |

.. warning:: | |

A common beginner's error when first using :class:`AutoDiffCostFunction` | |

is to get the sizing wrong. In particular, there is a tendency to set the | |

template parameters to (dimension of residual, number of parameters) | |

instead of passing a dimension parameter for *every parameter block*. In | |

the example above, that would be ``<MyScalarCostFunction, 1, 2>``, which | |

is missing the 2 as the last template argument. | |

:class:`DynamicAutoDiffCostFunction` | |

==================================== | |

.. class:: DynamicAutoDiffCostFunction | |

:class:`AutoDiffCostFunction` requires that the number of parameter | |

blocks and their sizes be known at compile time. In a number of | |

applications, this is not enough e.g., Bezier curve fitting, Neural | |

Network training etc. | |

.. code-block:: c++ | |

template <typename CostFunctor, int Stride = 4> | |

class DynamicAutoDiffCostFunction : public CostFunction { | |

}; | |

In such cases :class:`DynamicAutoDiffCostFunction` can be | |

used. Like :class:`AutoDiffCostFunction` the user must define a | |

templated functor, but the signature of the functor differs | |

slightly. The expected interface for the cost functors is: | |

.. code-block:: c++ | |

struct MyCostFunctor { | |

template<typename T> | |

bool operator()(T const* const* parameters, T* residuals) const { | |

} | |

} | |

Since the sizing of the parameters is done at runtime, you must | |

also specify the sizes after creating the dynamic autodiff cost | |

function. For example: | |

.. code-block:: c++ | |

DynamicAutoDiffCostFunction<MyCostFunctor, 4>* cost_function = | |

new DynamicAutoDiffCostFunction<MyCostFunctor, 4>( | |

new MyCostFunctor()); | |

cost_function->AddParameterBlock(5); | |

cost_function->AddParameterBlock(10); | |

cost_function->SetNumResiduals(21); | |

Under the hood, the implementation evaluates the cost function | |

multiple times, computing a small set of the derivatives (four by | |

default, controlled by the ``Stride`` template parameter) with each | |

pass. There is a performance tradeoff with the size of the passes; | |

Smaller sizes are more cache efficient but result in larger number | |

of passes, and larger stride lengths can destroy cache-locality | |

while reducing the number of passes over the cost function. The | |

optimal value depends on the number and sizes of the various | |

parameter blocks. | |

As a rule of thumb, try using :class:`AutoDiffCostFunction` before | |

you use :class:`DynamicAutoDiffCostFunction`. | |

:class:`NumericDiffCostFunction` | |

================================ | |

.. class:: NumericDiffCostFunction | |

In some cases, its not possible to define a templated cost functor, | |

for example when the evaluation of the residual involves a call to a | |

library function that you do not have control over. In such a | |

situation, `numerical differentiation | |

<http://en.wikipedia.org/wiki/Numerical_differentiation>`_ can be | |

used. | |

.. NOTE :: | |

TODO(sameeragarwal): Add documentation for the constructor and for | |

NumericDiffOptions. Update DynamicNumericDiffOptions in a similar | |

manner. | |

.. code-block:: c++ | |

template <typename CostFunctor, | |

NumericDiffMethodType method = CENTRAL, | |

int kNumResiduals, // Number of residuals, or ceres::DYNAMIC. | |

int... Ns> // Size of each parameter block. | |

class NumericDiffCostFunction : public | |

SizedCostFunction<kNumResiduals, Ns> { | |

}; | |

To get a numerically differentiated :class:`CostFunction`, you must | |

define a class with a ``operator()`` (a functor) that computes the | |

residuals. The functor must write the computed value in the last | |

argument (the only non-``const`` one) and return ``true`` to | |

indicate success. Please see :class:`CostFunction` for details on | |

how the return value may be used to impose simple constraints on the | |

parameter block. e.g., an object of the form | |

.. code-block:: c++ | |

struct ScalarFunctor { | |

public: | |

bool operator()(const double* const x1, | |

const double* const x2, | |

double* residuals) const; | |

} | |

For example, consider a scalar error :math:`e = k - x'y`, where both | |

:math:`x` and :math:`y` are two-dimensional column vector | |

parameters, the prime sign indicates transposition, and :math:`k` is | |

a constant. The form of this error, which is the difference between | |

a constant and an expression, is a common pattern in least squares | |

problems. For example, the value :math:`x'y` might be the model | |

expectation for a series of measurements, where there is an instance | |

of the cost function for each measurement :math:`k`. | |

To write an numerically-differentiable class:`CostFunction` for the | |

above model, first define the object | |

.. code-block:: c++ | |

class MyScalarCostFunctor { | |

MyScalarCostFunctor(double k): k_(k) {} | |

bool operator()(const double* const x, | |

const double* const y, | |

double* residuals) const { | |

residuals[0] = k_ - x[0] * y[0] + x[1] * y[1]; | |

return true; | |

} | |

private: | |

double k_; | |

}; | |

Note that in the declaration of ``operator()`` the input parameters | |

``x`` and ``y`` come first, and are passed as const pointers to | |

arrays of ``double`` s. If there were three input parameters, then | |

the third input parameter would come after ``y``. The output is | |

always the last parameter, and is also a pointer to an array. In the | |

example above, the residual is a scalar, so only ``residuals[0]`` is | |

set. | |

Then given this class definition, the numerically differentiated | |

:class:`CostFunction` with central differences used for computing | |

the derivative can be constructed as follows. | |

.. code-block:: c++ | |

CostFunction* cost_function | |

= new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>( | |

new MyScalarCostFunctor(1.0)); ^ ^ ^ ^ | |

| | | | | |

Finite Differencing Scheme -+ | | | | |

Dimension of residual ------------+ | | | |

Dimension of x ----------------------+ | | |

Dimension of y -------------------------+ | |

In this example, there is usually an instance for each measurement | |

of `k`. | |

In the instantiation above, the template parameters following | |

``MyScalarCostFunctor``, ``1, 2, 2``, describe the functor as | |

computing a 1-dimensional output from two arguments, both | |

2-dimensional. | |

NumericDiffCostFunction also supports cost functions with a | |

runtime-determined number of residuals. For example: | |

.. code-block:: c++ | |

CostFunction* cost_function | |

= new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>( | |

new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ | |

TAKE_OWNERSHIP, | | | | |

runtime_number_of_residuals); <----+ | | | | |

| | | | | |

| | | | | |

Actual number of residuals ------+ | | | | |

Indicate dynamic number of residuals --------------------+ | | | |

Dimension of x ------------------------------------------------+ | | |

Dimension of y ---------------------------------------------------+ | |

There are three available numeric differentiation schemes in ceres-solver: | |

The ``FORWARD`` difference method, which approximates :math:`f'(x)` | |

by computing :math:`\frac{f(x+h)-f(x)}{h}`, computes the cost | |

function one additional time at :math:`x+h`. It is the fastest but | |

least accurate method. | |

The ``CENTRAL`` difference method is more accurate at the cost of | |

twice as many function evaluations than forward difference, | |

estimating :math:`f'(x)` by computing | |

:math:`\frac{f(x+h)-f(x-h)}{2h}`. | |

The ``RIDDERS`` difference method[Ridders]_ is an adaptive scheme | |

that estimates derivatives by performing multiple central | |

differences at varying scales. Specifically, the algorithm starts at | |

a certain :math:`h` and as the derivative is estimated, this step | |

size decreases. To conserve function evaluations and estimate the | |

derivative error, the method performs Richardson extrapolations | |

between the tested step sizes. The algorithm exhibits considerably | |

higher accuracy, but does so by additional evaluations of the cost | |

function. | |

Consider using ``CENTRAL`` differences to begin with. Based on the | |

results, either try forward difference to improve performance or | |

Ridders' method to improve accuracy. | |

.. warning:: | |

A common beginner's error when first using | |

:class:`NumericDiffCostFunction` is to get the sizing wrong. In | |

particular, there is a tendency to set the template parameters to | |

(dimension of residual, number of parameters) instead of passing a | |

dimension parameter for *every parameter*. In the example above, that | |

would be ``<MyScalarCostFunctor, 1, 2>``, which is missing the last ``2`` | |

argument. Please be careful when setting the size parameters. | |

Numeric Differentiation & Manifolds | |

----------------------------------- | |

If your cost function depends on a parameter block that must lie on | |

a manifold and the functor cannot be evaluated for values of that | |

parameter block not on the manifold then you may have problems | |

numerically differentiating such functors. | |

This is because numeric differentiation in Ceres is performed by | |

perturbing the individual coordinates of the parameter blocks that | |

a cost functor depends on. This perturbation assumes that the | |

parameter block lives on a Euclidean Manifold rather than the | |

actual manifold associated with the parameter block. As a result | |

some of the perturbed points may not lie on the manifold anymore. | |

For example consider a four dimensional parameter block that is | |

interpreted as a unit Quaternion. Perturbing the coordinates of | |

this parameter block will violate the unit norm property of the | |

parameter block. | |

Fixing this problem requires that :class:`NumericDiffCostFunction` | |

be aware of the :class:`Manifold` associated with each | |

parameter block and only generate perturbations in the local | |

tangent space of each parameter block. | |

For now this is not considered to be a serious enough problem to | |

warrant changing the :class:`NumericDiffCostFunction` API. Further, | |

in most cases it is relatively straightforward to project a point | |

off the manifold back onto the manifold before using it in the | |

functor. For example in case of the Quaternion, normalizing the | |

4-vector before using it does the trick. | |

**Alternate Interface** | |

For a variety of reasons, including compatibility with legacy code, | |

:class:`NumericDiffCostFunction` can also take | |

:class:`CostFunction` objects as input. The following describes | |

how. | |

To get a numerically differentiated cost function, define a | |

subclass of :class:`CostFunction` such that the | |

:func:`CostFunction::Evaluate` function ignores the ``jacobians`` | |

parameter. The numeric differentiation wrapper will fill in the | |

jacobian parameter if necessary by repeatedly calling the | |

:func:`CostFunction::Evaluate` with small changes to the | |

appropriate parameters, and computing the slope. For performance, | |

the numeric differentiation wrapper class is templated on the | |

concrete cost function, even though it could be implemented only in | |

terms of the :class:`CostFunction` interface. | |

The numerically differentiated version of a cost function for a | |

cost function can be constructed as follows: | |

.. code-block:: c++ | |

CostFunction* cost_function | |

= new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>( | |

new MyCostFunction(...), TAKE_OWNERSHIP); | |

where ``MyCostFunction`` has 1 residual and 2 parameter blocks with | |

sizes 4 and 8 respectively. Look at the tests for a more detailed | |

example. | |

:class:`DynamicNumericDiffCostFunction` | |

======================================= | |

.. class:: DynamicNumericDiffCostFunction | |

Like :class:`AutoDiffCostFunction` :class:`NumericDiffCostFunction` | |

requires that the number of parameter blocks and their sizes be | |

known at compile time. In a number of applications, this is not enough. | |

.. code-block:: c++ | |

template <typename CostFunctor, NumericDiffMethodType method = CENTRAL> | |

class DynamicNumericDiffCostFunction : public CostFunction { | |

}; | |

In such cases when numeric differentiation is desired, | |

:class:`DynamicNumericDiffCostFunction` can be used. | |

Like :class:`NumericDiffCostFunction` the user must define a | |

functor, but the signature of the functor differs slightly. The | |

expected interface for the cost functors is: | |

.. code-block:: c++ | |

struct MyCostFunctor { | |

bool operator()(double const* const* parameters, double* residuals) const { | |

} | |

} | |

Since the sizing of the parameters is done at runtime, you must | |

also specify the sizes after creating the dynamic numeric diff cost | |

function. For example: | |

.. code-block:: c++ | |

DynamicNumericDiffCostFunction<MyCostFunctor>* cost_function = | |

new DynamicNumericDiffCostFunction<MyCostFunctor>(new MyCostFunctor); | |

cost_function->AddParameterBlock(5); | |

cost_function->AddParameterBlock(10); | |

cost_function->SetNumResiduals(21); | |

As a rule of thumb, try using :class:`NumericDiffCostFunction` before | |

you use :class:`DynamicNumericDiffCostFunction`. | |

.. warning:: | |

The same caution about mixing manifolds with numeric differentiation | |

applies as is the case with :class:`NumericDiffCostFunction`. | |

:class:`CostFunctionToFunctor` | |

============================== | |

.. class:: CostFunctionToFunctor | |

:class:`CostFunctionToFunctor` is an adapter class that allows | |

users to use :class:`CostFunction` objects in templated functors | |

which are to be used for automatic differentiation. This allows | |

the user to seamlessly mix analytic, numeric and automatic | |

differentiation. | |

For example, let us assume that | |

.. code-block:: c++ | |

class IntrinsicProjection : public SizedCostFunction<2, 5, 3> { | |

public: | |

IntrinsicProjection(const double* observation); | |

virtual bool Evaluate(double const* const* parameters, | |

double* residuals, | |

double** jacobians) const; | |

}; | |

is a :class:`CostFunction` that implements the projection of a | |

point in its local coordinate system onto its image plane and | |

subtracts it from the observed point projection. It can compute its | |

residual and either via analytic or numerical differentiation can | |

compute its jacobians. | |

Now we would like to compose the action of this | |

:class:`CostFunction` with the action of camera extrinsics, i.e., | |

rotation and translation. Say we have a templated function | |

.. code-block:: c++ | |

template<typename T> | |

void RotateAndTranslatePoint(const T* rotation, | |

const T* translation, | |

const T* point, | |

T* result); | |

Then we can now do the following, | |

.. code-block:: c++ | |

struct CameraProjection { | |

CameraProjection(double* observation) | |

: intrinsic_projection_(new IntrinsicProjection(observation)) { | |

} | |

template <typename T> | |

bool operator()(const T* rotation, | |

const T* translation, | |

const T* intrinsics, | |

const T* point, | |

T* residual) const { | |

T transformed_point[3]; | |

RotateAndTranslatePoint(rotation, translation, point, transformed_point); | |

// Note that we call intrinsic_projection_, just like it was | |

// any other templated functor. | |

return intrinsic_projection_(intrinsics, transformed_point, residual); | |

} | |

private: | |

CostFunctionToFunctor<2,5,3> intrinsic_projection_; | |

}; | |

Note that :class:`CostFunctionToFunctor` takes ownership of the | |

:class:`CostFunction` that was passed in to the constructor. | |

In the above example, we assumed that ``IntrinsicProjection`` is a | |

``CostFunction`` capable of evaluating its value and its | |

derivatives. Suppose, if that were not the case and | |

``IntrinsicProjection`` was defined as follows: | |

.. code-block:: c++ | |

struct IntrinsicProjection { | |

IntrinsicProjection(const double* observation) { | |

observation_[0] = observation[0]; | |

observation_[1] = observation[1]; | |

} | |

bool operator()(const double* calibration, | |

const double* point, | |

double* residuals) const { | |

double projection[2]; | |

ThirdPartyProjectionFunction(calibration, point, projection); | |

residuals[0] = observation_[0] - projection[0]; | |

residuals[1] = observation_[1] - projection[1]; | |

return true; | |

} | |

double observation_[2]; | |

}; | |

Here ``ThirdPartyProjectionFunction`` is some third party library | |

function that we have no control over. So this function can compute | |

its value and we would like to use numeric differentiation to | |

compute its derivatives. In this case we can use a combination of | |

``NumericDiffCostFunction`` and ``CostFunctionToFunctor`` to get the | |

job done. | |

.. code-block:: c++ | |

struct CameraProjection { | |

CameraProjection(double* observation) | |

: intrinsic_projection_( | |

new NumericDiffCostFunction<IntrinsicProjection, CENTRAL, 2, 5, 3>( | |

new IntrinsicProjection(observation))) {} | |

template <typename T> | |

bool operator()(const T* rotation, | |

const T* translation, | |

const T* intrinsics, | |

const T* point, | |

T* residuals) const { | |

T transformed_point[3]; | |

RotateAndTranslatePoint(rotation, translation, point, transformed_point); | |

return intrinsic_projection_(intrinsics, transformed_point, residuals); | |

} | |

private: | |

CostFunctionToFunctor<2, 5, 3> intrinsic_projection_; | |

}; | |

:class:`DynamicCostFunctionToFunctor` | |

===================================== | |

.. class:: DynamicCostFunctionToFunctor | |

:class:`DynamicCostFunctionToFunctor` provides the same functionality as | |

:class:`CostFunctionToFunctor` for cases where the number and size of the | |

parameter vectors and residuals are not known at compile-time. The API | |

provided by :class:`DynamicCostFunctionToFunctor` matches what would be | |

expected by :class:`DynamicAutoDiffCostFunction`, i.e. it provides a | |

templated functor of this form: | |

.. code-block:: c++ | |

template<typename T> | |

bool operator()(T const* const* parameters, T* residuals) const; | |

Similar to the example given for :class:`CostFunctionToFunctor`, let us | |

assume that | |

.. code-block:: c++ | |

class IntrinsicProjection : public CostFunction { | |

public: | |

IntrinsicProjection(const double* observation); | |

virtual bool Evaluate(double const* const* parameters, | |

double* residuals, | |

double** jacobians) const; | |

}; | |

is a :class:`CostFunction` that projects a point in its local coordinate | |

system onto its image plane and subtracts it from the observed point | |

projection. | |

Using this :class:`CostFunction` in a templated functor would then look like | |

this: | |

.. code-block:: c++ | |

struct CameraProjection { | |

CameraProjection(double* observation) | |

: intrinsic_projection_(new IntrinsicProjection(observation)) { | |

} | |

template <typename T> | |

bool operator()(T const* const* parameters, | |

T* residual) const { | |

const T* rotation = parameters[0]; | |

const T* translation = parameters[1]; | |

const T* intrinsics = parameters[2]; | |

const T* point = parameters[3]; | |

T transformed_point[3]; | |

RotateAndTranslatePoint(rotation, translation, point, transformed_point); | |

const T* projection_parameters[2]; | |

projection_parameters[0] = intrinsics; | |

projection_parameters[1] = transformed_point; | |

return intrinsic_projection_(projection_parameters, residual); | |

} | |

private: | |

DynamicCostFunctionToFunctor intrinsic_projection_; | |

}; | |

Like :class:`CostFunctionToFunctor`, :class:`DynamicCostFunctionToFunctor` | |

takes ownership of the :class:`CostFunction` that was passed in to the | |

constructor. | |

:class:`ConditionedCostFunction` | |

================================ | |

.. class:: ConditionedCostFunction | |

This class allows you to apply different conditioning to the residual | |

values of a wrapped cost function. An example where this is useful is | |

where you have an existing cost function that produces N values, but you | |

want the total cost to be something other than just the sum of these | |

squared values - maybe you want to apply a different scaling to some | |

values, to change their contribution to the cost. | |

Usage: | |

.. code-block:: c++ | |

// my_cost_function produces N residuals | |

CostFunction* my_cost_function = ... | |

CHECK_EQ(N, my_cost_function->num_residuals()); | |

vector<CostFunction*> conditioners; | |

// Make N 1x1 cost functions (1 parameter, 1 residual) | |

CostFunction* f_1 = ... | |

conditioners.push_back(f_1); | |

CostFunction* f_N = ... | |

conditioners.push_back(f_N); | |

ConditionedCostFunction* ccf = | |

new ConditionedCostFunction(my_cost_function, conditioners); | |

Now ``ccf`` 's ``residual[i]`` (i=0..N-1) will be passed though the | |

:math:`i^{\text{th}}` conditioner. | |

.. code-block:: c++ | |

ccf_residual[i] = f_i(my_cost_function_residual[i]) | |

and the Jacobian will be affected appropriately. | |

:class:`GradientChecker` | |

======================== | |

.. class:: GradientChecker | |

This class compares the Jacobians returned by a cost function | |

against derivatives estimated using finite differencing. It is | |

meant as a tool for unit testing, giving you more fine-grained | |

control than the check_gradients option in the solver options. | |

The condition enforced is that | |

.. math:: \forall{i,j}: \frac{J_{ij} - J'_{ij}}{max_{ij}(J_{ij} - J'_{ij})} < r | |

where :math:`J_{ij}` is the jacobian as computed by the supplied | |

cost function multiplied by the `Manifold::PlusJacobian`, | |

:math:`J'_{ij}` is the jacobian as computed by finite differences, | |

multiplied by the `Manifold::PlusJacobian` as well, and :math:`r` | |

is the relative precision. | |

Usage: | |

.. code-block:: c++ | |

// my_cost_function takes two parameter blocks. The first has a | |

// manifold associated with it. | |

CostFunction* my_cost_function = ... | |

Manifold* my_manifold = ... | |

NumericDiffOptions numeric_diff_options; | |

std::vector<Manifold*> manifolds; | |

manifolds.push_back(my_manifold); | |

manifolds.push_back(nullptr); | |

std::vector parameter1; | |

std::vector parameter2; | |

// Fill parameter 1 & 2 with test data... | |

std::vector<double*> parameter_blocks; | |

parameter_blocks.push_back(parameter1.data()); | |

parameter_blocks.push_back(parameter2.data()); | |

GradientChecker gradient_checker(my_cost_function, | |

manifolds, | |

numeric_diff_options); | |

GradientCheckResults results; | |

if (!gradient_checker.Probe(parameter_blocks.data(), 1e-9, &results) { | |

LOG(ERROR) << "An error has occurred:\n" << results.error_log; | |

} | |

:class:`NormalPrior` | |

==================== | |

.. class:: NormalPrior | |

.. code-block:: c++ | |

class NormalPrior: public CostFunction { | |

public: | |

// Check that the number of rows in the vector b are the same as the | |

// number of columns in the matrix A, crash otherwise. | |

NormalPrior(const Matrix& A, const Vector& b); | |

virtual bool Evaluate(double const* const* parameters, | |

double* residuals, | |

double** jacobians) const; | |

}; | |

Implements a cost function of the form | |

.. math:: cost(x) = ||A(x - b)||^2 | |

where, the matrix :math:`A` and the vector :math:`b` are fixed and :math:`x` | |

is the variable. In case the user is interested in implementing a cost | |

function of the form | |

.. math:: cost(x) = (x - \mu)^T S^{-1} (x - \mu) | |

where, :math:`\mu` is a vector and :math:`S` is a covariance matrix, | |

then, :math:`A = S^{-1/2}`, i.e the matrix :math:`A` is the square | |

root of the inverse of the covariance, also known as the stiffness | |

matrix. There are however no restrictions on the shape of | |

:math:`A`. It is free to be rectangular, which would be the case if | |

the covariance matrix :math:`S` is rank deficient. | |

.. _`section-loss_function`: | |

:class:`LossFunction` | |

===================== | |

.. class:: LossFunction | |

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 influence. | |

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 the residual for the erroneous measurement will | |

result in the entire solution getting pulled away from the optimum | |

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

down-weighted so they do not overly influence the final solution. | |

.. code-block:: c++ | |

class LossFunction { | |

public: | |

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

}; | |

The key method is :func:`LossFunction::Evaluate`, which given a | |

non-negative scalar ``s``, computes | |

.. math:: out = \begin{bmatrix}\rho(s), & \rho'(s), & \rho''(s)\end{bmatrix} | |

Here the convention is that the contribution of a term to the cost | |

function is given by :math:`\frac{1}{2}\rho(s)`, where :math:`s | |

=\|f_i\|^2`. Calling the method with a negative value of :math:`s` | |

is an error and the implementations are not required to handle that | |

case. | |

Most sane choices of :math:`\rho` satisfy: | |

.. math:: | |

\rho(0) &= 0\\ | |

\rho'(0) &= 1\\ | |

\rho'(s) &< 1 \text{ in the outlier region}\\ | |

\rho''(s) &< 0 \text{ in the outlier region} | |

so that they mimic the squared cost for small residuals. | |

**Scaling** | |

Given one robustifier :math:`\rho(s)` one can change the length | |

scale at which robustification takes place, by adding a scale | |

factor :math:`a > 0` which gives us :math:`\rho(s,a) = a^2 \rho(s / | |

a^2)` and the first and second derivatives as :math:`\rho'(s / | |

a^2)` and :math:`(1 / a^2) \rho''(s / a^2)` respectively. | |

The reason for the appearance of squaring is that :math:`a` is in | |

the units of the residual vector norm whereas :math:`s` is a squared | |

norm. For applications it is more convenient to specify :math:`a` than | |

its square. | |

Instances | |

--------- | |

Ceres includes a number of predefined loss functions. For simplicity | |

we described their unscaled versions. The figure below illustrates | |

their shape graphically. More details can be found in | |

``include/ceres/loss_function.h``. | |

.. figure:: loss.png | |

:figwidth: 500px | |

:height: 400px | |

:align: center | |

Shape of the various common loss functions. | |

.. class:: TrivialLoss | |

.. math:: \rho(s) = s | |

.. class:: HuberLoss | |

.. math:: \rho(s) = \begin{cases} s & s \le 1\\ 2 \sqrt{s} - 1 & s > 1 \end{cases} | |

.. class:: SoftLOneLoss | |

.. math:: \rho(s) = 2 (\sqrt{1+s} - 1) | |

.. class:: CauchyLoss | |

.. math:: \rho(s) = \log(1 + s) | |

.. class:: ArctanLoss | |

.. math:: \rho(s) = \arctan(s) | |

.. class:: TolerantLoss | |

.. math:: \rho(s,a,b) = b \log(1 + e^{(s - a) / b}) - b \log(1 + e^{-a / b}) | |

.. class:: TukeyLoss | |

.. math:: \rho(s) = \begin{cases} \frac{1}{3} (1 - (1 - s)^3) & s \le 1\\ \frac{1}{3} & s > 1 \end{cases} | |

.. class:: ComposedLoss | |

Given two loss functions ``f`` and ``g``, implements the loss | |

function ``h(s) = f(g(s))``. | |

.. code-block:: c++ | |

class ComposedLoss : public LossFunction { | |

public: | |

explicit ComposedLoss(const LossFunction* f, | |

Ownership ownership_f, | |

const LossFunction* g, | |

Ownership ownership_g); | |

}; | |

.. class:: ScaledLoss | |

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). | |

Given a loss function :math:`\rho(s)` and a scalar :math:`a`, :class:`ScaledLoss` | |

implements the function :math:`a \rho(s)`. | |

Since we treat a ``nullptr`` Loss function as the Identity loss | |

function, :math:`rho` = ``nullptr``: is a valid input and will result | |

in the input being scaled by :math:`a`. This provides a simple way | |

of implementing a scaled ResidualBlock. | |

.. class:: LossFunctionWrapper | |

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 behavior 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, e.g, | |

.. code-block:: c++ | |

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; | |

Solver::Summary summary; | |

Solve(options, &problem, &summary); | |

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

Solve(options, &problem, &summary); | |

Theory | |

------ | |

Let us consider a problem with a single parameter block. | |

.. math:: | |

\min_x \frac{1}{2}\rho(f^2(x)) | |

Then, the robustified gradient and the Gauss-Newton Hessian are | |

.. math:: | |

g(x) &= \rho'J^\top(x)f(x)\\ | |

H(x) &= J^\top(x)\left(\rho' + 2 \rho''f(x)f^\top(x)\right)J(x) | |

where the terms involving the second derivatives of :math:`f(x)` have | |

been ignored. Note that :math:`H(x)` is indefinite if | |

:math:`\rho''f(x)^\top f(x) + \frac{1}{2}\rho' < 0`. If this is not | |

the case, then its possible to re-weight the residual and the Jacobian | |

matrix such that the robustified Gauss-Newton step corresponds to an | |

ordinary linear least squares problem. | |

Let :math:`\alpha` be a root of | |

.. math:: \frac{1}{2}\alpha^2 - \alpha - \frac{\rho''}{\rho'}\|f(x)\|^2 = 0. | |

Then, define the rescaled residual and Jacobian as | |

.. math:: | |

\tilde{f}(x) &= \frac{\sqrt{\rho'}}{1 - \alpha} f(x)\\ | |

\tilde{J}(x) &= \sqrt{\rho'}\left(1 - \alpha | |

\frac{f(x)f^\top(x)}{\left\|f(x)\right\|^2} \right)J(x) | |

In the case :math:`2 \rho''\left\|f(x)\right\|^2 + \rho' \lesssim 0`, | |

we limit :math:`\alpha \le 1- \epsilon` for some small | |

:math:`\epsilon`. For more details see [Triggs]_. | |

With this simple rescaling, one can apply any Jacobian based non-linear | |

least squares algorithm to robustified non-linear least squares | |

problems. | |

While the theory described above is elegant, in practice we observe | |

that using the Triggs correction when :math:`\rho'' > 0` leads to poor | |

performance, so we upper bound it by zero. For more details see | |

`corrector.cc <https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/corrector.cc#L51>`_ | |

:class:`Manifolds` | |

================== | |

.. class:: Manifold | |

In sensor fusion problems, we often have to model quantities that live | |

in spaces known as `Manifolds | |

<https://en.wikipedia.org/wiki/Manifold>`_, for example the | |

rotation/orientation of a sensor that is represented by a `Quaternion | |

<https://en.wikipedia.org/wiki/Quaternion>`_. | |

Manifolds are spaces which locally look like Euclidean spaces. More | |

precisely, at each point on the manifold there is a linear space that | |

is tangent to the manifold. It has dimension equal to the intrinsic | |

dimension of the manifold itself, which is less than or equal to the | |

ambient space in which the manifold is embedded. | |

For example, the tangent space to a point on a sphere in three | |

dimensions is the two dimensional plane that is tangent to the sphere | |

at that point. There are two reasons tangent spaces are interesting: | |

1. They are Eucliean spaces so the usual vector space operations apply | |

there, which makes numerical operations easy. | |

2. Movements in the tangent space translate into movements along the | |

manifold. Movements perpendicular to the tangent space do not | |

translate into movements on the manifold. | |

However, moving along the 2 dimensional plane tangent to the sphere | |

and projecting back onto the sphere will move you away from the point | |

you started from but moving along the normal at the same point and the | |

projecting back onto the sphere brings you back to the point. | |

Besides the mathematical niceness, modeling manifold valued | |

quantities correctly and paying attention to their geometry has | |

practical benefits too: | |

1. It naturally constrains the quantity to the manifold throughout the | |

optimization, freeing the user from hacks like *quaternion | |

normalization*. | |

2. It reduces the dimension of the optimization problem to its | |

*natural* size. For example, a quantity restricted to a line is a | |

one dimensional object regardless of the dimension of the ambient | |

space in which this line lives. | |

Working in the tangent space reduces not just the computational | |

complexity of the optimization algorithm, but also improves the | |

numerical behaviour of the algorithm. | |

A basic operation one can perform on a manifold is the | |

:math:`\boxplus` operation that computes the result of moving along | |

:math:`\delta` in the tangent space at :math:`x`, and then projecting | |

back onto the manifold that :math:`x` belongs to. Also known as a | |

*Retraction*, :math:`\boxplus` is a generalization of vector addition | |

in Euclidean spaces. | |

The inverse of :math:`\boxplus` is :math:`\boxminus`, which given two | |

points :math:`y` and :math:`x` on the manifold computes the tangent | |

vector :math:`\Delta` at :math:`x` s.t. :math:`\boxplus(x, \Delta) = | |

y`. | |

Let us now consider two examples. | |

The `Euclidean space <https://en.wikipedia.org/wiki/Euclidean_space>`_ | |

:math:`\mathbb{R}^n` is the simplest example of a manifold. It has | |

dimension :math:`n` (and so does its tangent space) and | |

:math:`\boxplus` and :math:`\boxminus` are the familiar vector sum and | |

difference operations. | |

.. math:: | |

\begin{align*} | |

\boxplus(x, \Delta) &= x + \Delta = y\\ | |

\boxminus(y, x) &= y - x = \Delta. | |

\end{align*} | |

A more interesting case is the case :math:`SO(3)`, the `special | |

orthogonal group <https://en.wikipedia.org/wiki/3D_rotation_group>`_ | |

in three dimensions - the space of :math:`3\times3` rotation | |

matrices. :math:`SO(3)` is a three dimensional manifold embedded in | |

:math:`\mathbb{R}^9` or :math:`\mathbb{R}^{3\times 3}`. So points on :math:`SO(3)` are | |

represented using 9 dimensional vectors or :math:`3\times 3` matrices, | |

and points in its tangent spaces are represented by 3 dimensional | |

vectors. | |

For :math:`SO(3)`, :math:`\boxplus` and :math:`\boxminus` are defined | |

in terms of the matrix :math:`\exp` and :math:`\log` operations as | |

follows: | |

Given a 3-vector :math:`\Delta = [\begin{matrix}p,&q,&r\end{matrix}]`, we have | |

.. math:: | |

\exp(\Delta) & = \left [ \begin{matrix} | |

\cos \theta + cp^2 & -sr + cpq & sq + cpr \\ | |

sr + cpq & \cos \theta + cq^2& -sp + cqr \\ | |

-sq + cpr & sp + cqr & \cos \theta + cr^2 | |

\end{matrix} \right ] | |

where, | |

.. math:: | |

\begin{align} | |

\theta &= \sqrt{p^2 + q^2 + r^2},\\ | |

s &= \frac{\sin \theta}{\theta},\\ | |

c &= \frac{1 - \cos \theta}{\theta^2}. | |

\end{align} | |

Given :math:`x \in SO(3)`, we have | |

.. math:: | |

\log(x) = 1/(2 \sin(\theta)/\theta)\left[\begin{matrix} x_{32} - x_{23},& x_{13} - x_{31},& x_{21} - x_{12}\end{matrix} \right] | |

where, | |

.. math:: \theta = \cos^{-1}((\operatorname{Trace}(x) - 1)/2) | |

Then, | |

.. math:: | |

\begin{align*} | |

\boxplus(x, \Delta) &= x \exp(\Delta) | |

\\ | |

\boxminus(y, x) &= \log(x^T y) | |

\end{align*} | |

For :math:`\boxplus` and :math:`\boxminus` to be mathematically | |

consistent, the following identities must be satisfied at all points | |

:math:`x` on the manifold: | |

1. :math:`\boxplus(x, 0) = x`. This ensures that the tangent space is | |

*centered* at :math:`x`, and the zero vector is the identity | |

element. | |

2. For all :math:`y` on the manifold, :math:`\boxplus(x, | |

\boxminus(y,x)) = y`. This ensures that any :math:`y` can be | |

reached from math:`x`. | |

3. For all :math:`\Delta`, :math:`\boxminus(\boxplus(x, \Delta), x) = | |

\Delta`. This ensures that :math:`\boxplus` is an injective | |

(one-to-one) map. | |

4. For all :math:`\Delta_1, \Delta_2\ |\boxminus(\boxplus(x, \Delta_1), | |

\boxplus(x, \Delta_2)) \leq |\Delta_1 - \Delta_2|`. Allows us to define | |

a metric on the manifold. | |

Additionally we require that :math:`\boxplus` and :math:`\boxminus` be | |

sufficiently smooth. In particular they need to be differentiable | |

everywhere on the manifold. | |

For more details, please see `Integrating Generic Sensor Fusion | |

Algorithms with Sound State Representations through Encapsulation of | |

Manifolds <https://arxiv.org/pdf/1107.1119.pdf>`_ | |

By C. Hertzberg, R. Wagner, U. Frese and L. Schroder | |

The :class:`Manifold` interface allows the user to define a manifold | |

for the purposes optimization by implementing ``Plus`` and ``Minus`` | |

operations and their derivatives (corresponding naturally to | |

:math:`\boxplus` and :math:`\boxminus`). | |

.. code-block:: c++ | |

class Manifold { | |

public: | |

virtual ~Manifold(); | |

virtual int AmbientSize() const = 0; | |

virtual int TangentSize() const = 0; | |

virtual bool Plus(const double* x, | |

const double* delta, | |

double* x_plus_delta) const = 0; | |

virtual bool PlusJacobian(const double* x, double* jacobian) const = 0; | |

virtual bool RightMultiplyByPlusJacobian(const double* x, | |

const int num_rows, | |

const double* ambient_matrix, | |

double* tangent_matrix) const; | |

virtual bool Minus(const double* y, | |

const double* x, | |

double* y_minus_x) const = 0; | |

virtual bool MinusJacobian(const double* x, double* jacobian) const = 0; | |

}; | |

.. function:: int Manifold::AmbientSize() const; | |

Dimension of the ambient space in which the manifold is embedded. | |

.. function:: int Manifold::TangentSize() const; | |

Dimension of the manifold/tangent space. | |

.. function:: bool Plus(const double* x, const double* delta, double* x_plus_delta) const; | |

Implements the :math:`\boxplus(x,\Delta)` operation for the manifold. | |

A generalization of vector addition in Euclidean space, ``Plus`` | |

computes the result of moving along ``delta`` in the tangent space | |

at ``x``, and then projecting back onto the manifold that ``x`` | |

belongs to. | |

``x`` and ``x_plus_delta`` are :func:`Manifold::AmbientSize` vectors. | |

``delta`` is a :func:`Manifold::TangentSize` vector. | |

Return value indicates if the operation was successful or not. | |

.. function:: bool PlusJacobian(const double* x, double* jacobian) const; | |

Compute the derivative of :math:`\boxplus(x, \Delta)` w.r.t | |

:math:`\Delta` at :math:`\Delta = 0`, i.e. :math:`(D_2 | |

\boxplus)(x, 0)`. | |

``jacobian`` is a row-major :func:`Manifold::AmbientSize` | |

:math:`\times` :func:`Manifold::TangentSize` matrix. | |

Return value indicates whether the operation was successful or not. | |

.. function:: bool RightMultiplyByPlusJacobian(const double* x, const int num_rows, const double* ambient_matrix, double* tangent_matrix) const; | |

``tangent_matrix`` = ``ambient_matrix`` :math:`\times` plus_jacobian. | |

``ambient_matrix`` is a row-major ``num_rows`` :math:`\times` | |

:func:`Manifold::AmbientSize` matrix. | |

``tangent_matrix`` is a row-major ``num_rows`` :math:`\times` | |

:func:`Manifold::TangentSize` matrix. | |

Return value indicates whether the operation was successful or not. | |

This function is only used by the :class:`GradientProblemSolver`, | |

where the dimension of the parameter block can be large and it may | |

be more efficient to compute this product directly rather than | |

first evaluating the Jacobian into a matrix and then doing a matrix | |

vector product. | |

Because this is not an often used function, we provide a default | |

implementation for convenience. If performance becomes an issue | |

then the user should consider implementing a specialization. | |

.. function:: bool Minus(const double* y, const double* x, double* y_minus_x) const; | |

Implements :math:`\boxminus(y,x)` operation for the manifold. | |

A generalization of vector subtraction in Euclidean spaces, given | |

two points ``x`` and ``y`` on the manifold, ``Minus`` computes the | |

change to ``x`` in the tangent space at ``x``, that will take it to | |

``y``. | |

``x`` and ``y`` are :func:`Manifold::AmbientSize` vectors. | |

``y_minus_x`` is a ::func:`Manifold::TangentSize` vector. | |

Return value indicates if the operation was successful or not. | |

.. function:: bool MinusJacobian(const double* x, double* jacobian) const = 0; | |

Compute the derivative of :math:`\boxminus(y, x)` w.r.t :math:`y` | |

at :math:`y = x`, i.e :math:`(D_1 \boxminus) (x, x)`. | |

``jacobian`` is a row-major :func:`Manifold::TangentSize` | |

:math:`\times` :func:`Manifold::AmbientSize` matrix. | |

Return value indicates whether the operation was successful or not. | |

Ceres Solver ships with a number of commonly used instances of | |

:class:`Manifold`. | |

For `Lie Groups <https://en.wikipedia.org/wiki/Lie_group>`_, a great | |

place to find high quality implementations is the `Sophus | |

<https://github.com/strasdat/Sophus>`_ library developed by Hauke | |

Strasdat and his collaborators. | |

:class:`EuclideanManifold` | |

-------------------------- | |

.. class:: EuclideanManifold | |

:class:`EuclideanManifold` as the name implies represents a Euclidean | |

space, where the :math:`\boxplus` and :math:`\boxminus` operations are | |

the usual vector addition and subtraction. | |

.. math:: | |

\begin{align*} | |

\boxplus(x, \Delta) &= x + \Delta\\ | |

\boxminus(y,x) &= y - x | |

\end{align*} | |

By default parameter blocks are assumed to be Euclidean, so there is | |

no need to use this manifold on its own. It is provided for the | |

purpose of testing and for use in combination with other manifolds | |

using :class:`ProductManifold`. | |

The class works with dynamic and static ambient space dimensions. If | |

the ambient space dimensions is known at compile time use | |

.. code-block:: c++ | |

EuclideanManifold<3> manifold; | |

If the ambient space dimensions is not known at compile time the | |

template parameter needs to be set to `ceres::DYNAMIC` and the actual | |

dimension needs to be provided as a constructor argument: | |

.. code-block:: c++ | |

EuclideanManifold<ceres::DYNAMIC> manifold(ambient_dim); | |

:class:`SubsetManifold` | |

----------------------- | |

.. class:: SubsetManifold | |

Suppose :math:`x` is a two dimensional vector, and the user wishes to | |

hold the first coordinate constant. Then, :math:`\Delta` is a scalar | |

and :math:`\boxplus` is defined as | |

.. math:: | |

\boxplus(x, \Delta) = x + \left[ \begin{array}{c} 0 \\ 1 \end{array} \right] \Delta | |

and given two, two-dimensional vectors :math:`x` and :math:`y` with | |

the same first coordinate, :math:`\boxminus` is defined as: | |

.. math:: | |

\boxminus(y, x) = y[1] - x[1] | |

:class:`SubsetManifold` generalizes this construction to hold | |

any part of a parameter block constant by specifying the set of | |

coordinates that are held constant. | |

.. NOTE:: | |

It is legal to hold *all* coordinates of a parameter block to | |

constant using a :class:`SubsetManifold`. It is the same as calling | |

:func:`Problem::SetParameterBlockConstant` on that parameter block. | |

:class:`ProductManifold` | |

------------------------ | |

.. class:: ProductManifold | |

In cases, where a parameter block is the Cartesian product of a number | |

of manifolds and you have the manifold of the individual | |

parameter blocks available, :class:`ProductManifold` can be used to | |

construct a :class:`Manifold` of the Cartesian product. | |

For the case of the rigid transformation, where say you have a | |

parameter block of size 7, where the first four entries represent the | |

rotation as a quaternion, and the next three the translation, a | |

manifold can be constructed as: | |

.. code-block:: c++ | |

ProductManifold<QuaternionManifold, EuclideanManifold<3>> se3; | |

Manifolds can be copied and moved to :class:`ProductManifold`: | |

.. code-block:: c++ | |

SubsetManifold manifold1(5, {2}); | |

SubsetManifold manifold2(3, {0, 1}); | |

ProductManifold<SubsetManifold, SubsetManifold> manifold(manifold1, | |

manifold2); | |

In advanced use cases, manifolds can be dynamically allocated and passed as (smart) pointers: | |

.. code-block:: c++ | |

ProductManifold<std::unique_ptr<QuaternionManifold>, EuclideanManifold<3>> se3 | |

{std::make_unique<QuaternionManifold>(), EuclideanManifold<3>{}}; | |

In C++17, the template parameters can be left out as they are automatically | |

deduced making the initialization much simpler: | |

.. code-block:: c++ | |

ProductManifold se3{QuaternionManifold{}, EuclideanManifold<3>{}}; | |

:class:`QuaternionManifold` | |

--------------------------- | |

.. class:: QuaternionManifold | |

.. NOTE:: | |

If you are using ``Eigen`` quaternions, then you should use | |

:class:`EigenQuaternionManifold` instead because ``Eigen`` uses a | |

different memory layout for its Quaternions. | |

Manifold for a Hamilton `Quaternion | |

<https://en.wikipedia.org/wiki/Quaternion>`_. Quaternions are a three | |

dimensional manifold represented as unit norm 4-vectors, i.e. | |

.. math:: q = \left [\begin{matrix}q_0,& q_1,& q_2,& q_3\end{matrix}\right], \quad \|q\| = 1 | |

is the ambient space representation. Here :math:`q_0` is the scalar | |

part. :math:`q_1` is the coefficient of :math:`i`, :math:`q_2` is the | |

coefficient of :math:`j`, and :math:`q_3` is the coefficient of | |

:math:`k`. Where: | |

.. math:: | |

\begin{align*} | |

i\times j &= k,\\ | |

j\times k &= i,\\ | |

k\times i &= j,\\ | |

i\times i &= -1,\\ | |

j\times j &= -1,\\ | |

k\times k &= -1. | |

\end{align*} | |

The tangent space is three dimensional and the :math:`\boxplus` and | |

:math:`\boxminus` operators are defined in term of :math:`\exp` and | |

:math:`\log` operations. | |

.. math:: | |

\begin{align*} | |

\boxplus(x, \Delta) &= \exp\left(\Delta\right) \otimes x \\ | |

\boxminus(y,x) &= \log\left(y \otimes x^{-1}\right) | |

\end{align*} | |

Where :math:`\otimes` is the `Quaternion product | |

<https://en.wikipedia.org/wiki/Quaternion#Hamilton_product>`_ and | |

since :math:`x` is a unit quaternion, :math:`x^{-1} = [\begin{matrix} | |

q_0,& -q_1,& -q_2,& -q_3\end{matrix}]`. Given a vector :math:`\Delta | |

\in \mathbb{R}^3`, | |

.. math:: | |

\exp(\Delta) = \left[ \begin{matrix} | |

\cos\left(\|\Delta\|\right)\\ | |

\frac{\displaystyle \sin\left(|\Delta\|\right)}{\displaystyle \|\Delta\|} \Delta | |

\end{matrix} \right] | |

and given a unit quaternion :math:`q = \left [\begin{matrix}q_0,& q_1,& q_2,& q_3\end{matrix}\right]` | |

.. math:: | |

\log(q) = \frac{\operatorname{atan2}\left(\sqrt{1-q_0^2},q_0\right)}{\sqrt{1-q_0^2}} \left [\begin{matrix}q_1,& q_2,& q_3\end{matrix}\right] | |

:class:`EigenQuaternionManifold` | |

-------------------------------- | |

.. class:: EigenQuaternionManifold | |

Implements the quaternion manifold for `Eigen's | |

<http://eigen.tuxfamily.org/index.php?title=Main_Page>`_ | |

representation of the Hamilton quaternion. Geometrically it is exactly | |

the same as the :class:`QuaternionManifold` defined above. However, | |

Eigen uses a different internal memory layout for the elements of the | |

quaternion than what is commonly used. It stores the quaternion in | |

memory as :math:`[q_1, q_2, q_3, q_0]` or :math:`[x, y, z, w]` where | |

the real (scalar) part is last. | |

Since Ceres operates on parameter blocks which are raw double pointers | |

this difference is important and requires a different manifold. | |

:class:`SphereManifold` | |

----------------------- | |

.. class:: SphereManifold | |

This provides a manifold on a sphere meaning that the norm of the | |

vector stays the same. Such cases often arises in Structure for Motion | |

problems. One example where they are used is in representing points | |

whose triangulation is ill-conditioned. Here it is advantageous to use | |

an over-parameterization since homogeneous vectors can represent | |

points at infinity. | |

The ambient space dimension is required to be greater than 1. | |

The class works with dynamic and static ambient space dimensions. If | |

the ambient space dimensions is known at compile time use | |

.. code-block:: c++ | |

SphereManifold<3> manifold; | |

If the ambient space dimensions is not known at compile time the | |

template parameter needs to be set to `ceres::DYNAMIC` and the actual | |

dimension needs to be provided as a constructor argument: | |

.. code-block:: c++ | |

SphereManifold<ceres::DYNAMIC> manifold(ambient_dim); | |

For more details, please see Section B.2 (p.25) in `Integrating | |

Generic Sensor Fusion Algorithms with Sound State Representations | |

through Encapsulation of Manifolds | |

<https://arxiv.org/pdf/1107.1119.pdf>`_ | |

By C. Hertzberg, R. Wagner, U. Frese and L. Schroder | |

:class:`LineManifold` | |

--------------------- | |

.. class:: LineManifold | |

This class provides a manifold for lines, where the line is defined | |

using an origin point and a direction vector. So the ambient size | |

needs to be two times the dimension of the space in which the line | |

lives. The first half of the parameter block is interpreted as the | |

origin point and the second half as the direction. This manifold is a | |

special case of the `Affine Grassmannian manifold | |

<https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold))>`_ for | |

the case :math:`\operatorname{Graff}_1(R^n)`. | |

Note that this is a manifold for a line, rather than a point | |

constrained to lie on a line. It is useful when one wants to optimize | |

over the space of lines. For example, given :math:`n` distinct points | |

in 3D (measurements) we want to find the line that minimizes the sum | |

of squared distances to all the points. | |

:class:`AutoDiffManifold` | |

========================= | |

.. class:: AutoDiffManifold | |

Create a :class:`Manifold` with Jacobians computed via automatic | |

differentiation. | |

To get an auto differentiated manifold, you must define a Functor with | |

templated ``Plus`` and ``Minus`` functions that compute: | |

.. code-block:: c++ | |

x_plus_delta = Plus(x, delta); | |

y_minus_x = Minus(y, x); | |

Where, ``x``, ``y`` and ``x_plus_y`` are vectors on the manifold in | |

the ambient space (so they are ``kAmbientSize`` vectors) and | |

``delta``, ``y_minus_x`` are vectors in the tangent space (so they are | |

``kTangentSize`` vectors). | |

The Functor should have the signature: | |

.. code-block:: c++ | |

struct Functor { | |

template <typename T> | |

bool Plus(const T* x, const T* delta, T* x_plus_delta) const; | |

template <typename T> | |

bool Minus(const T* y, const T* x, T* y_minus_x) const; | |

}; | |

Observe that the ``Plus`` and ``Minus`` operations are templated on | |

the parameter ``T``. The autodiff framework substitutes appropriate | |

``Jet`` objects for ``T`` in order to compute the derivative when | |

necessary. This is the same mechanism that is used to compute | |

derivatives when using :class:`AutoDiffCostFunction`. | |

``Plus`` and ``Minus`` should return true if the computation is | |

successful and false otherwise, in which case the result will not be | |

used. | |

Given this Functor, the corresponding :class:`Manifold` can be constructed as: | |

.. code-block:: c++ | |

AutoDiffManifold<Functor, kAmbientSize, kTangentSize> manifold; | |

.. NOTE:: | |

The following is only used for illustration purposes. Ceres Solver | |

ships with an optimized, production grade :class:`QuaternionManifold` | |

implementation. | |

As a concrete example consider the case of `Quaternions | |

<https://en.wikipedia.org/wiki/Quaternion>`_. Quaternions form a three | |

dimensional manifold embedded in :math:`\mathbb{R}^4`, i.e. they have | |

an ambient dimension of 4 and their tangent space has dimension 3. The | |

following Functor defines the ``Plus`` and ``Minus`` operations on the | |

Quaternion manifold. It assumes that the quaternions are laid out as | |

``[w,x,y,z]`` in memory, i.e. the real or scalar part is the first | |

coordinate. | |

.. code-block:: c++ | |

struct QuaternionFunctor { | |

template <typename T> | |

bool Plus(const T* x, const T* delta, T* x_plus_delta) const { | |

const T squared_norm_delta = | |

delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; | |

T q_delta[4]; | |

if (squared_norm_delta > T(0.0)) { | |

T norm_delta = sqrt(squared_norm_delta); | |

const T sin_delta_by_delta = sin(norm_delta) / norm_delta; | |

q_delta[0] = cos(norm_delta); | |

q_delta[1] = sin_delta_by_delta * delta[0]; | |

q_delta[2] = sin_delta_by_delta * delta[1]; | |

q_delta[3] = sin_delta_by_delta * delta[2]; | |

} else { | |

// We do not just use q_delta = [1,0,0,0] here because that is a | |

// constant and when used for automatic differentiation will | |

// lead to a zero derivative. Instead we take a first order | |

// approximation and evaluate it at zero. | |

q_delta[0] = T(1.0); | |

q_delta[1] = delta[0]; | |

q_delta[2] = delta[1]; | |

q_delta[3] = delta[2]; | |

} | |

QuaternionProduct(q_delta, x, x_plus_delta); | |

return true; | |

} | |

template <typename T> | |

bool Minus(const T* y, const T* x, T* y_minus_x) const { | |

T minus_x[4] = {x[0], -x[1], -x[2], -x[3]}; | |

T ambient_y_minus_x[4]; | |

QuaternionProduct(y, minus_x, ambient_y_minus_x); | |

T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] + | |

ambient_y_minus_x[2] * ambient_y_minus_x[2] + | |

ambient_y_minus_x[3] * ambient_y_minus_x[3]); | |

if (u_norm > 0.0) { | |

T theta = atan2(u_norm, ambient_y_minus_x[0]); | |

y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm; | |

y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm; | |

y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm; | |

} else { | |

We do not use [0,0,0] here because even though the value part is | |

a constant, the derivative part is not. | |

y_minus_x[0] = ambient_y_minus_x[1]; | |

y_minus_x[1] = ambient_y_minus_x[2]; | |

y_minus_x[2] = ambient_y_minus_x[3]; | |

} | |

return true; | |

} | |

}; | |

Then given this struct, the auto differentiated Quaternion Manifold can now | |

be constructed as | |

.. code-block:: c++ | |

Manifold* manifold = new AutoDiffManifold<QuaternionFunctor, 4, 3>; | |

:class:`Problem` | |

================ | |

.. class:: Problem | |

:class:`Problem` holds the robustified bounds constrained | |

non-linear least squares problem :eq:`ceresproblem_modeling`. To | |

create a least squares problem, use the | |

:func:`Problem::AddResidalBlock` and | |

:func:`Problem::AddParameterBlock` methods. | |

For example a problem containing 3 parameter blocks of sizes 3, 4 | |

and 5 respectively and two residual blocks of size 2 and 6: | |

.. code-block:: c++ | |

double x1[] = { 1.0, 2.0, 3.0 }; | |

double x2[] = { 1.0, 2.0, 3.0, 5.0 }; | |

double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 }; | |

Problem problem; | |

problem.AddResidualBlock(new MyUnaryCostFunction(...), x1); | |

problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3); | |

:func:`Problem::AddResidualBlock` as the name implies, adds a | |

residual block to the problem. It adds a :class:`CostFunction`, an | |

optional :class:`LossFunction` and connects the | |

:class:`CostFunction` to a set of parameter block. | |

The cost function carries with it information about the sizes of | |

the parameter blocks it expects. The function checks that these | |

match the sizes of the parameter blocks listed in | |

``parameter_blocks``. The program aborts if a mismatch is | |

detected. ``loss_function`` can be ``nullptr``, in which case the cost | |

of the term is just the squared norm of the residuals. | |

The user has the option of explicitly adding the parameter blocks | |

using :func:`Problem::AddParameterBlock`. This causes additional | |

correctness checking; however, :func:`Problem::AddResidualBlock` | |

implicitly adds the parameter blocks if they are not present, so | |

calling :func:`Problem::AddParameterBlock` explicitly is not | |

required. | |

:func:`Problem::AddParameterBlock` explicitly adds a parameter | |

block to the :class:`Problem`. Optionally it allows the user to | |

associate a :class:`Manifold` object with the parameter block | |

too. Repeated calls with the same arguments are ignored. Repeated | |

calls with the same double pointer but a different size results in | |

undefined behavior. | |

You can set any parameter block to be constant using | |

:func:`Problem::SetParameterBlockConstant` and undo this using | |

:func:`SetParameterBlockVariable`. | |

In fact you can set any number of parameter blocks to be constant, | |

and Ceres is smart enough to figure out what part of the problem | |

you have constructed depends on the parameter blocks that are free | |

to change and only spends time solving it. So for example if you | |

constructed a problem with a million parameter blocks and 2 million | |

residual blocks, but then set all but one parameter blocks to be | |

constant and say only 10 residual blocks depend on this one | |

non-constant parameter block. Then the computational effort Ceres | |

spends in solving this problem will be the same if you had defined | |

a problem with one parameter block and 10 residual blocks. | |

**Ownership** | |

:class:`Problem` by default takes ownership of the | |

``cost_function``, ``loss_function`` and ``manifold`` pointers. These | |

objects remain live for the life of the :class:`Problem`. If the user wishes | |

to keep control over the destruction of these objects, then they can do this | |

by setting the corresponding enums in the :class:`Problem::Options` struct. | |

Note that even though the Problem takes ownership of objects, | |

``cost_function`` and ``loss_function``, it does not preclude the | |

user from re-using them in another residual block. Similarly the | |

same ``manifold`` object can be used with multiple parameter blocks. The | |

destructor takes care to call delete on each owned object exactly once. | |

.. class:: Problem::Options | |

Options struct that is used to control :class:`Problem`. | |

.. member:: Ownership Problem::Options::cost_function_ownership | |

Default: ``TAKE_OWNERSHIP`` | |

This option controls whether the Problem object owns the cost | |

functions. | |

If set to ``TAKE_OWNERSHIP``, then the problem object will delete the | |

cost functions on destruction. The destructor is careful to delete | |

the pointers only once, since sharing cost functions is allowed. | |

.. member:: Ownership Problem::Options::loss_function_ownership | |

Default: ``TAKE_OWNERSHIP`` | |

This option controls whether the Problem object owns the loss | |

functions. | |

If set to ``TAKE_OWNERSHIP``, then the problem object will delete the | |

loss functions on destruction. The destructor is careful to delete | |

the pointers only once, since sharing loss functions is allowed. | |

.. member:: Ownership Problem::Options::manifold_ownership | |

Default: ``TAKE_OWNERSHIP`` | |

This option controls whether the Problem object owns the manifolds. | |

If set to ``TAKE_OWNERSHIP``, then the problem object will delete the | |

manifolds on destruction. The destructor is careful to delete the | |

pointers only once, since sharing manifolds is allowed. | |

.. member:: bool Problem::Options::enable_fast_removal | |

Default: ``false`` | |

If true, trades memory for faster | |

:func:`Problem::RemoveResidualBlock` and | |

:func:`Problem::RemoveParameterBlock` operations. | |

By default, :func:`Problem::RemoveParameterBlock` and | |

:func:`Problem::RemoveResidualBlock` take time proportional to | |

the size of the entire problem. If you only ever remove | |

parameters or residuals from the problem occasionally, this might | |

be acceptable. However, if you have memory to spare, enable this | |

option to make :func:`Problem::RemoveParameterBlock` take time | |

proportional to the number of residual blocks that depend on it, | |

and :func:`Problem::RemoveResidualBlock` take (on average) | |

constant time. | |

The increase in memory usage is twofold: an additional hash set | |

per parameter block containing all the residuals that depend on | |

the parameter block; and a hash set in the problem containing all | |

residuals. | |

.. member:: bool Problem::Options::disable_all_safety_checks | |

Default: `false` | |

By default, Ceres performs a variety of safety checks when | |

constructing the problem. There is a small but measurable | |

performance penalty to these checks, typically around 5% of | |

construction time. If you are sure your problem construction is | |

correct, and 5% of the problem construction time is truly an | |

overhead you want to avoid, then you can set | |

disable_all_safety_checks to true. | |

.. warning:: | |

Do not set this to true, unless you are absolutely sure of what you are | |

doing. | |

.. member:: Context* Problem::Options::context | |

Default: ``nullptr`` | |

A Ceres global context to use for solving this problem. This may | |

help to reduce computation time as Ceres can reuse expensive | |

objects to create. The context object can be `nullptr`, in which | |

case Ceres may create one. | |

Ceres does NOT take ownership of the pointer. | |

.. member:: EvaluationCallback* Problem::Options::evaluation_callback | |

Default: ``nullptr`` | |

Using this callback interface, Ceres will notify you when it is | |

about to evaluate the residuals or Jacobians. | |

If an ``evaluation_callback`` is present, Ceres will update the | |

user's parameter blocks to the values that will be used when | |

calling :func:`CostFunction::Evaluate` before calling | |

:func:`EvaluationCallback::PrepareForEvaluation`. One can then use | |

this callback to share (or cache) computation between cost | |

functions by doing the shared computation in | |

:func:`EvaluationCallback::PrepareForEvaluation` before Ceres | |

calls :func:`CostFunction::Evaluate`. | |

Problem does NOT take ownership of the callback. | |

.. NOTE:: | |

Evaluation callbacks are incompatible with inner iterations. So | |

calling Solve with | |

:member:`Solver::Options::use_inner_iterations` set to ``true`` | |

on a :class:`Problem` with a non-null evaluation callback is an | |

error. | |

.. function:: ResidualBlockId Problem::AddResidualBlock(CostFunction* cost_function, LossFunction* loss_function, const vector<double*> parameter_blocks) | |

.. function:: template <typename Ts...> ResidualBlockId Problem::AddResidualBlock(CostFunction* cost_function, LossFunction* loss_function, double* x0, Ts... xs) | |

Add a residual block to the overall cost function. The cost | |

function carries with it information about the sizes of the | |

parameter blocks it expects. The function checks that these match | |

the sizes of the parameter blocks listed in parameter_blocks. The | |

program aborts if a mismatch is detected. loss_function can be | |

``nullptr``, in which case the cost of the term is just the squared | |

norm of the residuals. | |

The parameter blocks may be passed together as a | |

``vector<double*>``, or ``double*`` pointers. | |

The user has the option of explicitly adding the parameter blocks | |

using AddParameterBlock. This causes additional correctness | |

checking; however, AddResidualBlock implicitly adds the parameter | |

blocks if they are not present, so calling AddParameterBlock | |

explicitly is not required. | |

The Problem object by default takes ownership of the | |

cost_function and loss_function pointers. These objects remain | |

live for the life of the Problem object. If the user wishes to | |

keep control over the destruction of these objects, then they can | |

do this by setting the corresponding enums in the Options struct. | |

.. note:: | |

Even though the Problem takes ownership of ``cost_function`` | |

and ``loss_function``, it does not preclude the user from re-using | |

them in another residual block. The destructor takes care to call | |

delete on each cost_function or loss_function pointer only once, | |

regardless of how many residual blocks refer to them. | |

Example usage: | |

.. code-block:: c++ | |

double x1[] = {1.0, 2.0, 3.0}; | |

double x2[] = {1.0, 2.0, 5.0, 6.0}; | |

double x3[] = {3.0, 6.0, 2.0, 5.0, 1.0}; | |

vector<double*> v1; | |

v1.push_back(x1); | |

vector<double*> v2; | |

v2.push_back(x2); | |

v2.push_back(x1); | |

Problem problem; | |

problem.AddResidualBlock(new MyUnaryCostFunction(...), nullptr, x1); | |

problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x1); | |

problem.AddResidualBlock(new MyUnaryCostFunction(...), nullptr, v1); | |

problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, v2); | |

.. function:: void Problem::AddParameterBlock(double* values, int size, Manifold* manifold) | |

Add a parameter block with appropriate size and Manifold to the | |

problem. It is okay for ``manifold`` to be ``nullptr``. | |

Repeated calls with the same arguments are ignored. Repeated calls | |

with the same double pointer but a different size results in a crash | |

(unless :member:`Solver::Options::disable_all_safety_checks` is set to true). | |

Repeated calls with the same double pointer and size but different | |

:class:`Manifold` is equivalent to calling `SetManifold(manifold)`, | |

i.e., any previously associated :class:`Manifold` object will be replaced | |

with the `manifold`. | |

.. function:: void Problem::AddParameterBlock(double* values, int size) | |

Add a parameter block with appropriate size and parameterization to | |

the problem. Repeated calls with the same arguments are | |

ignored. Repeated calls with the same double pointer but a | |

different size results in undefined behavior. | |

.. function:: void Problem::RemoveResidualBlock(ResidualBlockId residual_block) | |

Remove a residual block from the problem. | |

Since residual blocks are allowed to share cost function and loss | |

function objects, Ceres Solver uses a reference counting | |

mechanism. So when a residual block is deleted, the reference count | |

for the corresponding cost function and loss function objects are | |

decreased and when this count reaches zero, they are deleted. | |

If :member:`Problem::Options::enable_fast_removal` is ``true``, then the removal | |

is fast (almost constant time). Otherwise it is linear, requiring a | |

scan of the entire problem. | |

Removing a residual block has no effect on the parameter blocks | |

that the problem depends on. | |

.. warning:: | |

Removing a residual or parameter block will destroy the implicit | |

ordering, rendering the jacobian or residuals returned from the solver | |

uninterpretable. If you depend on the evaluated jacobian, do not use | |

remove! This may change in a future release. Hold the indicated parameter | |

block constant during optimization. | |

.. function:: void Problem::RemoveParameterBlock(const double* values) | |

Remove a parameter block from the problem. Any residual blocks that | |

depend on the parameter are also removed, as described above in | |

:func:`RemoveResidualBlock()`. | |

The manifold of the parameter block, if it exists, will persist until the | |

deletion of the problem. | |

If :member:`Problem::Options::enable_fast_removal` is ``true``, then the removal | |

is fast (almost constant time). Otherwise, removing a parameter | |

block will scan the entire Problem. | |

.. warning:: | |

Removing a residual or parameter block will destroy the implicit | |

ordering, rendering the jacobian or residuals returned from the solver | |

uninterpretable. If you depend on the evaluated jacobian, do not use | |

remove! This may change in a future release. | |

.. function:: void Problem::SetParameterBlockConstant(const double* values) | |

Hold the indicated parameter block constant during optimization. | |

.. function:: void Problem::SetParameterBlockVariable(double* values) | |

Allow the indicated parameter to vary during optimization. | |

.. function:: bool Problem::IsParameterBlockConstant(const double* values) const | |

Returns ``true`` if a parameter block is set constant, and false | |

otherwise. A parameter block may be set constant in two ways: | |

either by calling ``SetParameterBlockConstant`` or by associating a | |

:class:`Manifold` with a zero dimensional tangent space with it. | |

.. function:: void SetManifold(double* values, Manifold* manifold); | |

Set the :class:`Manifold` for the parameter block. Calling | |

:func:`Problem::SetManifold` with ``nullptr`` will clear any | |

previously set :class:`Manifold` for the parameter block. | |

Repeated calls will result in any previously associated | |

:class:`Manifold` object to be replaced with ``manifold``. | |

``manifold`` is owned by :class:`Problem` by default (See | |

:class:`Problem::Options` to override this behaviour). | |

It is acceptable to set the same :class:`Manifold` for multiple | |

parameter blocks. | |

.. function:: const Manifold* GetManifold(const double* values) const; | |

Get the :class:`Manifold` object associated with this parameter block. | |

If there is no :class:`Manifold` object associated with the parameter block, | |

then ``nullptr`` is returned. | |

.. function:: bool HasManifold(const double* values) const; | |

Returns ``true`` if a :class:`Manifold` is associated with this parameter | |

block, ``false`` otherwise. | |

.. function:: void Problem::SetParameterLowerBound(double* values, int index, double lower_bound) | |

Set the lower bound for the parameter at position `index` in the | |

parameter block corresponding to `values`. By default the lower | |

bound is ``-std::numeric_limits<double>::max()``, which is treated | |

by the solver as the same as :math:`-\infty`. | |

.. function:: void Problem::SetParameterUpperBound(double* values, int index, double upper_bound) | |

Set the upper bound for the parameter at position `index` in the | |

parameter block corresponding to `values`. By default the value is | |

``std::numeric_limits<double>::max()``, which is treated by the | |

solver as the same as :math:`\infty`. | |

.. function:: double Problem::GetParameterLowerBound(const double* values, int index) | |

Get the lower bound for the parameter with position `index`. If the | |

parameter is not bounded by the user, then its lower bound is | |

``-std::numeric_limits<double>::max()``. | |

.. function:: double Problem::GetParameterUpperBound(const double* values, int index) | |

Get the upper bound for the parameter with position `index`. If the | |

parameter is not bounded by the user, then its upper bound is | |

``std::numeric_limits<double>::max()``. | |

.. function:: int Problem::NumParameterBlocks() const | |

Number of parameter blocks in the problem. Always equals | |

parameter_blocks().size() and parameter_block_sizes().size(). | |

.. function:: int Problem::NumParameters() const | |

The size of the parameter vector obtained by summing over the sizes | |

of all the parameter blocks. | |

.. function:: int Problem::NumResidualBlocks() const | |

Number of residual blocks in the problem. Always equals | |

residual_blocks().size(). | |

.. function:: int Problem::NumResiduals() const | |

The size of the residual vector obtained by summing over the sizes | |

of all of the residual blocks. | |

.. function:: int Problem::ParameterBlockSize(const double* values) const | |

The size of the parameter block. | |

.. function:: int Problem::ParameterBlockTangentSize(const double* values) const | |

The dimension of the tangent space of the :class:`Manifold` for the | |

parameter block. If there is no :class:`Manifold` associated with this | |

parameter block, then ``ParameterBlockTangentSize = ParameterBlockSize``. | |

.. function:: bool Problem::HasParameterBlock(const double* values) const | |

Is the given parameter block present in the problem or not? | |

.. function:: void Problem::GetParameterBlocks(vector<double*>* parameter_blocks) const | |

Fills the passed ``parameter_blocks`` vector with pointers to the | |

parameter blocks currently in the problem. After this call, | |

``parameter_block.size() == NumParameterBlocks``. | |

.. function:: void Problem::GetResidualBlocks(vector<ResidualBlockId>* residual_blocks) const | |

Fills the passed `residual_blocks` vector with pointers to the | |

residual blocks currently in the problem. After this call, | |

`residual_blocks.size() == NumResidualBlocks`. | |

.. function:: void Problem::GetParameterBlocksForResidualBlock(const ResidualBlockId residual_block, vector<double*>* parameter_blocks) const | |

Get all the parameter blocks that depend on the given residual | |

block. | |

.. function:: void Problem::GetResidualBlocksForParameterBlock(const double* values, vector<ResidualBlockId>* residual_blocks) const | |

Get all the residual blocks that depend on the given parameter | |

block. | |

If :member:`Problem::Options::enable_fast_removal` is | |

``true``, then getting the residual blocks is fast and depends only | |

on the number of residual blocks. Otherwise, getting the residual | |

blocks for a parameter block will scan the entire problem. | |

.. function:: const CostFunction* Problem::GetCostFunctionForResidualBlock(const ResidualBlockId residual_block) const | |

Get the :class:`CostFunction` for the given residual block. | |

.. function:: const LossFunction* Problem::GetLossFunctionForResidualBlock(const ResidualBlockId residual_block) const | |

Get the :class:`LossFunction` for the given residual block. | |

.. function:: bool EvaluateResidualBlock(ResidualBlockId residual_block_id, bool apply_loss_function, double* cost,double* residuals, double** jacobians) const | |

Evaluates the residual block, storing the scalar cost in ``cost``, the | |

residual components in ``residuals``, and the jacobians between the | |

parameters and residuals in ``jacobians[i]``, in row-major order. | |

If ``residuals`` is ``nullptr``, the residuals are not computed. | |

If ``jacobians`` is ``nullptr``, no Jacobians are computed. If | |

``jacobians[i]`` is ``nullptr``, then the Jacobian for that | |

parameter block is not computed. | |

It is not okay to request the Jacobian w.r.t a parameter block | |

that is constant. | |

The return value indicates the success or failure. Even if the | |

function returns false, the caller should expect the output | |

memory locations to have been modified. | |

The returned cost and jacobians have had robustification and | |

:class:`Manifold` applied already; for example, the jacobian for a | |

4-dimensional quaternion parameter using the :class:`QuaternionManifold` is | |

``num_residuals x 3`` instead of ``num_residuals x 4``. | |

``apply_loss_function`` as the name implies allows the user to | |

switch the application of the loss function on and off. | |

.. NOTE:: If an :class:`EvaluationCallback` is associated with the | |

problem, then its | |

:func:`EvaluationCallback::PrepareForEvaluation` method will be | |

called every time this method is called with `new_point = | |

true`. This conservatively assumes that the user may have | |

changed the parameter values since the previous call to evaluate | |

/ solve. For improved efficiency, and only if you know that the | |

parameter values have not changed between calls, see | |

:func:`Problem::EvaluateResidualBlockAssumingParametersUnchanged`. | |

.. function:: bool EvaluateResidualBlockAssumingParametersUnchanged(ResidualBlockId residual_block_id, bool apply_loss_function, double* cost,double* residuals, double** jacobians) const | |

Same as :func:`Problem::EvaluateResidualBlock` except that if an | |

:class:`EvaluationCallback` is associated with the problem, then | |

its :func:`EvaluationCallback::PrepareForEvaluation` method will | |

be called every time this method is called with new_point = false. | |

This means, if an :class:`EvaluationCallback` is associated with | |

the problem then it is the user's responsibility to call | |

:func:`EvaluationCallback::PrepareForEvaluation` before calling | |

this method if necessary, i.e. iff the parameter values have been | |

changed since the last call to evaluate / solve.' | |

This is because, as the name implies, we assume that the parameter | |

blocks did not change since the last time | |

:func:`EvaluationCallback::PrepareForEvaluation` was called (via | |

:func:`Solve`, :func:`Problem::Evaluate` or | |

:func:`Problem::EvaluateResidualBlock`). | |

.. function:: bool Problem::Evaluate(const Problem::EvaluateOptions& options, double* cost, vector<double>* residuals, vector<double>* gradient, CRSMatrix* jacobian) | |

Evaluate a :class:`Problem`. Any of the output pointers can be | |

``nullptr``. Which residual blocks and parameter blocks are used is | |

controlled by the :class:`Problem::EvaluateOptions` struct below. | |

.. NOTE:: | |

The evaluation will use the values stored in the memory | |

locations pointed to by the parameter block pointers used at the | |

time of the construction of the problem, for example in the | |

following code: | |

.. code-block:: c++ | |

Problem problem; | |

double x = 1; | |

problem.Add(new MyCostFunction, nullptr, &x); | |

double cost = 0.0; | |

problem.Evaluate(Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr); | |

The cost is evaluated at `x = 1`. If you wish to evaluate the | |

problem at `x = 2`, then | |

.. code-block:: c++ | |

x = 2; | |

problem.Evaluate(Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr); | |

is the way to do so. | |

.. NOTE:: | |

If no :class:`Manifold` are used, then the size of the gradient vector is | |

the sum of the sizes of all the parameter blocks. If a parameter block has | |

a manifold then it contributes "TangentSize" entries to the gradient | |

vector. | |

.. NOTE:: | |

This function cannot be called while the problem is being | |

solved, for example it cannot be called from an | |

:class:`IterationCallback` at the end of an iteration during a | |

solve. | |

.. NOTE:: | |

If an EvaluationCallback is associated with the problem, then | |

its PrepareForEvaluation method will be called everytime this | |

method is called with ``new_point = true``. | |

.. class:: Problem::EvaluateOptions | |

Options struct that is used to control :func:`Problem::Evaluate`. | |

.. member:: vector<double*> Problem::EvaluateOptions::parameter_blocks | |

The set of parameter blocks for which evaluation should be | |

performed. This vector determines the order in which parameter | |

blocks occur in the gradient vector and in the columns of the | |

jacobian matrix. If parameter_blocks is empty, then it is assumed | |

to be equal to a vector containing ALL the parameter | |

blocks. Generally speaking the ordering of the parameter blocks in | |

this case depends on the order in which they were added to the | |

problem and whether or not the user removed any parameter blocks. | |

**NOTE** This vector should contain the same pointers as the ones | |

used to add parameter blocks to the Problem. These parameter block | |

should NOT point to new memory locations. Bad things will happen if | |

you do. | |

.. member:: vector<ResidualBlockId> Problem::EvaluateOptions::residual_blocks | |

The set of residual blocks for which evaluation should be | |

performed. This vector determines the order in which the residuals | |

occur, and how the rows of the jacobian are ordered. If | |

residual_blocks is empty, then it is assumed to be equal to the | |

vector containing all the residual blocks. | |

.. member:: bool Problem::EvaluateOptions::apply_loss_function | |

Even though the residual blocks in the problem may contain loss | |

functions, setting apply_loss_function to false will turn off the | |

application of the loss function to the output of the cost | |

function. This is of use for example if the user wishes to analyse | |

the solution quality by studying the distribution of residuals | |

before and after the solve. | |

.. member:: int Problem::EvaluateOptions::num_threads | |

Number of threads to use. | |

:class:`EvaluationCallback` | |

=========================== | |

.. class:: EvaluationCallback | |

Interface for receiving callbacks before Ceres evaluates residuals or | |

Jacobians: | |

.. code-block:: c++ | |

class EvaluationCallback { | |

public: | |

virtual ~EvaluationCallback() = default; | |

virtual void PrepareForEvaluation()(bool evaluate_jacobians | |

bool new_evaluation_point) = 0; | |

}; | |

.. function:: void EvaluationCallback::PrepareForEvaluation(bool evaluate_jacobians, bool new_evaluation_point) | |

Ceres will call :func:`EvaluationCallback::PrepareForEvaluation` | |

every time, and once before it computes the residuals and/or the | |

Jacobians. | |

User parameters (the double* values provided by the user) are fixed | |

until the next call to | |

:func:`EvaluationCallback::PrepareForEvaluation`. If | |

``new_evaluation_point == true``, then this is a new point that is | |

different from the last evaluated point. Otherwise, it is the same | |

point that was evaluated previously (either Jacobian or residual) | |

and the user can use cached results from previous evaluations. If | |

``evaluate_jacobians`` is ``true``, then Ceres will request Jacobians | |

in the upcoming cost evaluation. | |

Using this callback interface, Ceres can notify you when it is | |

about to evaluate the residuals or Jacobians. With the callback, | |

you can share computation between residual blocks by doing the | |

shared computation in | |

:func:`EvaluationCallback::PrepareForEvaluation` before Ceres calls | |

:func:`CostFunction::Evaluate` on all the residuals. It also | |

enables caching results between a pure residual evaluation and a | |

residual & Jacobian evaluation, via the ``new_evaluation_point`` | |

argument. | |

One use case for this callback is if the cost function compute is | |

moved to the GPU. In that case, the prepare call does the actual | |

cost function evaluation, and subsequent calls from Ceres to the | |

actual cost functions merely copy the results from the GPU onto the | |

corresponding blocks for Ceres to plug into the solver. | |

**Note**: Ceres provides no mechanism to share data other than the | |

notification from the callback. Users must provide access to | |

pre-computed shared data to their cost functions behind the scenes; | |

this all happens without Ceres knowing. One approach is to put a | |

pointer to the shared data in each cost function (recommended) or | |

to use a global shared variable (discouraged; bug-prone). As far | |

as Ceres is concerned, it is evaluating cost functions like any | |

other; it just so happens that behind the scenes the cost functions | |

reuse pre-computed data to execute faster. | |

See ``evaluation_callback_test.cc`` for code that explicitly | |

verifies the preconditions between | |

:func:`EvaluationCallback::PrepareForEvaluation` and | |

:func:`CostFunction::Evaluate`. | |

``rotation.h`` | |

============== | |

Many applications of Ceres Solver involve optimization problems where | |

some of the variables correspond to rotations. To ease the pain of | |

work with the various representations of rotations (angle-axis, | |

quaternion and matrix) we provide a handy set of templated | |

functions. These functions are templated so that the user can use them | |

within Ceres Solver's automatic differentiation framework. | |

.. function:: template <typename T> void AngleAxisToQuaternion(T const* angle_axis, T* quaternion) | |

Convert a value in combined axis-angle representation to a | |

quaternion. | |

The value ``angle_axis`` is a triple whose norm is an angle in radians, | |

and whose direction is aligned with the axis of rotation, and | |

``quaternion`` is a 4-tuple that will contain the resulting quaternion. | |

.. function:: template <typename T> void QuaternionToAngleAxis(T const* quaternion, T* angle_axis) | |

Convert a quaternion to the equivalent combined axis-angle | |

representation. | |

The value ``quaternion`` must be a unit quaternion - it is not | |

normalized first, and ``angle_axis`` will be filled with a value | |

whose norm is the angle of rotation in radians, and whose direction | |

is the axis of rotation. | |

.. function:: template <typename T, int row_stride, int col_stride> void RotationMatrixToAngleAxis(const MatrixAdapter<const T, row_stride, col_stride>& R, T * angle_axis) | |

.. function:: template <typename T, int row_stride, int col_stride> void AngleAxisToRotationMatrix(T const * angle_axis, const MatrixAdapter<T, row_stride, col_stride>& R) | |

.. function:: template <typename T> void RotationMatrixToAngleAxis(T const * R, T * angle_axis) | |

.. function:: template <typename T> void AngleAxisToRotationMatrix(T const * angle_axis, T * R) | |

Conversions between :math:`3\times3` rotation matrix with given column and row strides and | |

axis-angle rotation representations. The functions that take a pointer to T instead | |

of a MatrixAdapter assume a column major representation with unit row stride and a column stride of 3. | |

.. function:: template <typename T, int row_stride, int col_stride> void EulerAnglesToRotationMatrix(const T* euler, const MatrixAdapter<T, row_stride, col_stride>& R) | |

.. function:: template <typename T> void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R) | |

Conversions between :math:`3\times3` rotation matrix with given column and row strides and | |

Euler angle (in degrees) rotation representations. | |

The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} | |

axes, respectively. They are applied in that same order, so the | |

total rotation R is Rz * Ry * Rx. | |

The function that takes a pointer to T as the rotation matrix assumes a row | |

major representation with unit column stride and a row stride of 3. | |

The additional parameter row_stride is required to be 3. | |

.. function:: template <typename T, int row_stride, int col_stride> void QuaternionToScaledRotation(const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R) | |

.. function:: template <typename T> void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) | |

Convert a 4-vector to a :math:`3\times3` scaled rotation matrix. | |

The choice of rotation is such that the quaternion | |

:math:`\begin{bmatrix} 1 &0 &0 &0\end{bmatrix}` goes to an identity | |

matrix and for small :math:`a, b, c` the quaternion | |

:math:`\begin{bmatrix}1 &a &b &c\end{bmatrix}` goes to the matrix | |

.. math:: | |

I + 2 \begin{bmatrix} 0 & -c & b \\ c & 0 & -a\\ -b & a & 0 | |

\end{bmatrix} + O(q^2) | |

which corresponds to a Rodrigues approximation, the last matrix | |

being the cross-product matrix of :math:`\begin{bmatrix} a& b& | |

c\end{bmatrix}`. Together with the property that :math:`R(q_1 \otimes q_2) | |

= R(q_1) R(q_2)` this uniquely defines the mapping from :math:`q` to | |

:math:`R`. | |

In the function that accepts a pointer to T instead of a MatrixAdapter, | |

the rotation matrix ``R`` is a row-major matrix with unit column stride | |

and a row stride of 3. | |

No normalization of the quaternion is performed, i.e. | |

:math:`R = \|q\|^2 Q`, where :math:`Q` is an orthonormal matrix | |

such that :math:`\det(Q) = 1` and :math:`QQ' = I`. | |

.. function:: template <typename T> void QuaternionToRotation(const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R) | |

.. function:: template <typename T> void QuaternionToRotation(const T q[4], T R[3 * 3]) | |

Same as above except that the rotation matrix is normalized by the | |

Frobenius norm, so that :math:`R R' = I` (and :math:`\det(R) = 1`). | |

.. function:: template <typename T> void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) | |

Rotates a point pt by a quaternion q: | |

.. math:: \text{result} = R(q) \text{pt} | |

Assumes the quaternion is unit norm. If you pass in a quaternion | |

with :math:`|q|^2 = 2` then you WILL NOT get back 2 times the | |

result you get for a unit quaternion. | |

.. function:: template <typename T> void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) | |

With this function you do not need to assume that :math:`q` has unit norm. | |

It does assume that the norm is non-zero. | |

.. function:: template <typename T> void QuaternionProduct(const T z[4], const T w[4], T zw[4]) | |

.. math:: zw = z \otimes w | |

where :math:`\otimes` is the Quaternion product between 4-vectors. | |

.. function:: template <typename T> void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) | |

.. math:: \text{x_cross_y} = x \times y | |

.. function:: template <typename T> void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) | |

.. math:: y = R(\text{angle_axis}) x | |

Cubic Interpolation | |

=================== | |

Optimization problems often involve functions that are given in the | |

form of a table of values, for example an image. Evaluating these | |

functions and their derivatives requires interpolating these | |

values. Interpolating tabulated functions is a vast area of research | |

and there are a lot of libraries which implement a variety of | |

interpolation schemes. However, using them within the automatic | |

differentiation framework in Ceres is quite painful. To this end, | |

Ceres provides the ability to interpolate one dimensional and two | |

dimensional tabular functions. | |

The one dimensional interpolation is based on the Cubic Hermite | |

Spline, also known as the Catmull-Rom Spline. This produces a first | |

order differentiable interpolating function. The two dimensional | |

interpolation scheme is a generalization of the one dimensional scheme | |

where the interpolating function is assumed to be separable in the two | |

dimensions, | |

More details of the construction can be found `Linear Methods for | |

Image Interpolation <http://www.ipol.im/pub/art/2011/g_lmii/>`_ by | |

Pascal Getreuer. | |

.. class:: CubicInterpolator | |

Given as input an infinite one dimensional grid, which provides the | |

following interface. | |

.. code:: | |

struct Grid1D { | |

enum { DATA_DIMENSION = 2; }; | |

void GetValue(int n, double* f) const; | |

}; | |

Where, ``GetValue`` gives us the value of a function :math:`f` | |

(possibly vector valued) for any integer :math:`n` and the enum | |

``DATA_DIMENSION`` indicates the dimensionality of the function being | |

interpolated. For example if you are interpolating rotations in | |

axis-angle format over time, then ``DATA_DIMENSION = 3``. | |

:class:`CubicInterpolator` uses Cubic Hermite splines to produce a | |

smooth approximation to it that can be used to evaluate the | |

:math:`f(x)` and :math:`f'(x)` at any point on the real number | |

line. For example, the following code interpolates an array of four | |

numbers. | |

.. code:: | |

const double x[] = {1.0, 2.0, 5.0, 6.0}; | |

Grid1D<double, 1> array(x, 0, 4); | |

CubicInterpolator interpolator(array); | |

double f, dfdx; | |

interpolator.Evaluate(1.5, &f, &dfdx); | |

In the above code we use ``Grid1D`` a templated helper class that | |

allows easy interfacing between ``C++`` arrays and | |

:class:`CubicInterpolator`. | |

``Grid1D`` supports vector valued functions where the various | |

coordinates of the function can be interleaved or stacked. It also | |

allows the use of any numeric type as input, as long as it can be | |

safely cast to a double. | |

.. class:: BiCubicInterpolator | |

Given as input an infinite two dimensional grid, which provides the | |

following interface: | |

.. code:: | |

struct Grid2D { | |

enum { DATA_DIMENSION = 2 }; | |

void GetValue(int row, int col, double* f) const; | |

}; | |

Where, ``GetValue`` gives us the value of a function :math:`f` | |

(possibly vector valued) for any pair of integers :code:`row` and | |

:code:`col` and the enum ``DATA_DIMENSION`` indicates the | |

dimensionality of the function being interpolated. For example if you | |

are interpolating a color image with three channels (Red, Green & | |

Blue), then ``DATA_DIMENSION = 3``. | |

:class:`BiCubicInterpolator` uses the cubic convolution interpolation | |

algorithm of R. Keys [Keys]_, to produce a smooth approximation to it | |

that can be used to evaluate the :math:`f(r,c)`, :math:`\frac{\partial | |

f(r,c)}{\partial r}` and :math:`\frac{\partial f(r,c)}{\partial c}` at | |

any any point in the real plane. | |

For example the following code interpolates a two dimensional array. | |

.. code:: | |

const double data[] = {1.0, 3.0, -1.0, 4.0, | |

3.6, 2.1, 4.2, 2.0, | |

2.0, 1.0, 3.1, 5.2}; | |

Grid2D<double, 1> array(data, 0, 3, 0, 4); | |

BiCubicInterpolator interpolator(array); | |

double f, dfdr, dfdc; | |

interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc); | |

In the above code, the templated helper class ``Grid2D`` is used to | |

make a ``C++`` array look like a two dimensional table to | |

:class:`BiCubicInterpolator`. | |

``Grid2D`` supports row or column major layouts. It also supports | |

vector valued functions where the individual coordinates of the | |

function may be interleaved or stacked. It also allows the use of any | |

numeric type as input, as long as it can be safely cast to double. |