| .. default-domain:: cpp |
| |
| .. 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 more familiar 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 = \frac{\partial}{\partial x_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) = 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) |
| |
| 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 ``NULL``. |
| |
| ``residuals`` is an array of size ``num_residuals_``. |
| |
| ``residuals`` is never ``NULL``. |
| |
| ``jacobians`` is an array of arrays of size |
| ``CostFunction::parameter_block_sizes_.size()``. |
| |
| If ``jacobians`` is ``NULL``, 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** ``NULL``, 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 ``NULL``, 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 N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, |
| int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> |
| 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 N0, // Number of parameters in block 0. |
| int N1 = 0, // Number of parameters in block 1. |
| int N2 = 0, // Number of parameters in block 2. |
| int N3 = 0, // Number of parameters in block 3. |
| int N4 = 0, // Number of parameters in block 4. |
| int N5 = 0, // Number of parameters in block 5. |
| int N6 = 0, // Number of parameters in block 6. |
| int N7 = 0, // Number of parameters in block 7. |
| int N8 = 0, // Number of parameters in block 8. |
| int N9 = 0> // Number of parameters in block 9. |
| class AutoDiffCostFunction : public |
| SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> { |
| public: |
| explicit AutoDiffCostFunction(CostFunctor* functor); |
| // Ignore the template parameter kNumResiduals and use |
| // num_residuals instead. |
| AutoDiffCostFunction(CostFunctor* functor, int num_residuals); |
| }; |
| |
| 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. |
| |
| :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 ---------------------------------------+ |
| |
| The framework can currently accommodate cost functions of up to 10 |
| independent variables, and there is no limit on the dimensionality |
| of each of them. |
| |
| **WARNING 1** 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. It also has an |
| upper limit of 10 parameter blocks. 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 N0, // Number of parameters in block 0. |
| int N1 = 0, // Number of parameters in block 1. |
| int N2 = 0, // Number of parameters in block 2. |
| int N3 = 0, // Number of parameters in block 3. |
| int N4 = 0, // Number of parameters in block 4. |
| int N5 = 0, // Number of parameters in block 5. |
| int N6 = 0, // Number of parameters in block 6. |
| int N7 = 0, // Number of parameters in block 7. |
| int N8 = 0, // Number of parameters in block 8. |
| int N9 = 0> // Number of parameters in block 9. |
| class NumericDiffCostFunction : public |
| SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> { |
| }; |
| |
| 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 ---------------------------------------------------+ |
| |
| |
| The framework can currently accommodate cost functions of up to 10 |
| independent variables, and there is no limit on the dimensionality |
| of each of them. |
| |
| 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 & LocalParameterization |
| ----------------------------------------------- |
| |
| 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. In doing so, we assume that the |
| parameter blocks live in an Euclidean space and ignore the |
| structure of manifold that they live As a result some of the |
| perturbations may not lie on the manifold corresponding to the |
| parameter block. |
| |
| 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:`LocalParameterization` 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. It also has an upper limit of 10 parameter |
| blocks. 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 local parameterizations |
| 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) { |
| 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, residual); |
| } |
| |
| 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 (by the user) multiplied by the local parameterization Jacobian, |
| :math:`J'_{ij}` is the jacobian as computed by finite differences, |
| multiplied by the local parameterization Jacobian as well, and :math:`r` |
| is the relative precision. |
| |
| Usage: |
| |
| .. code-block:: c++ |
| |
| // my_cost_function takes two parameter blocks. The first has a local |
| // parameterization associated with it. |
| CostFunction* my_cost_function = ... |
| LocalParameterization* my_parameterization = ... |
| NumericDiffOptions numeric_diff_options; |
| |
| std::vector<LocalParameterization*> local_parameterizations; |
| local_parameterizations.push_back(my_parameterization); |
| local_parameterizations.push_back(NULL); |
| |
| 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, |
| local_parameterizations, 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:: 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 ``NULL`` Loss function as the Identity loss |
| function, :math:`rho` = ``NULL``: 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 problem and 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 corresponding linear least squares problem for |
| the robustified Gauss-Newton step. |
| |
| |
| 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 use any Jacobian based non-linear |
| least squares algorithm to robustified non-linear least squares |
| problems. |
| |
| |
| :class:`LocalParameterization` |
| ============================== |
| |
| .. class:: LocalParameterization |
| |
| .. code-block:: c++ |
| |
| class LocalParameterization { |
| public: |
| virtual ~LocalParameterization() {} |
| virtual bool Plus(const double* x, |
| const double* delta, |
| double* x_plus_delta) const = 0; |
| virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; |
| virtual bool MultiplyByJacobian(const double* x, |
| const int num_rows, |
| const double* global_matrix, |
| double* local_matrix) const; |
| virtual int GlobalSize() const = 0; |
| virtual int LocalSize() const = 0; |
| }; |
| |
| Sometimes the parameters :math:`x` can overparameterize a |
| problem. In that case it is desirable to choose a parameterization |
| to remove the null directions of the cost. More generally, if |
| :math:`x` lies on a manifold of a smaller dimension than the |
| ambient space that it is embedded in, then it is numerically and |
| computationally more effective to optimize it using a |
| parameterization that lives in the tangent space of that manifold |
| at each point. |
| |
| For example, a sphere in three dimensions is a two dimensional |
| manifold, embedded in a three dimensional space. At each point on |
| the sphere, the plane tangent to it defines a two dimensional |
| tangent space. For a cost function defined on this sphere, given a |
| point :math:`x`, moving in the direction normal to the sphere at |
| that point is not useful. Thus a better way to parameterize a point |
| on a sphere is to optimize over two dimensional vector |
| :math:`\Delta x` in the tangent space at the point on the sphere |
| point and then "move" to the point :math:`x + \Delta x`, where the |
| move operation involves projecting back onto the sphere. Doing so |
| removes a redundant dimension from the optimization, making it |
| numerically more robust and efficient. |
| |
| More generally we can define a function |
| |
| .. math:: x' = \boxplus(x, \Delta x), |
| |
| where :math:`x'` has the same size as :math:`x`, and :math:`\Delta |
| x` is of size less than or equal to :math:`x`. The function |
| :math:`\boxplus`, generalizes the definition of vector |
| addition. Thus it satisfies the identity |
| |
| .. math:: \boxplus(x, 0) = x,\quad \forall x. |
| |
| Instances of :class:`LocalParameterization` implement the |
| :math:`\boxplus` operation and its derivative with respect to |
| :math:`\Delta x` at :math:`\Delta x = 0`. |
| |
| |
| .. function:: int LocalParameterization::GlobalSize() |
| |
| The dimension of the ambient space in which the parameter block |
| :math:`x` lives. |
| |
| .. function:: int LocalParameterization::LocalSize() |
| |
| The size of the tangent space |
| that :math:`\Delta x` lives in. |
| |
| .. function:: bool LocalParameterization::Plus(const double* x, const double* delta, double* x_plus_delta) const |
| |
| :func:`LocalParameterization::Plus` implements :math:`\boxplus(x,\Delta x)`. |
| |
| .. function:: bool LocalParameterization::ComputeJacobian(const double* x, double* jacobian) const |
| |
| Computes the Jacobian matrix |
| |
| .. math:: J = \left . \frac{\partial }{\partial \Delta x} \boxplus(x,\Delta x)\right|_{\Delta x = 0} |
| |
| in row major form. |
| |
| .. function:: bool MultiplyByJacobian(const double* x, const int num_rows, const double* global_matrix, double* local_matrix) const |
| |
| local_matrix = global_matrix * jacobian |
| |
| global_matrix is a num_rows x GlobalSize row major matrix. |
| local_matrix is a num_rows x LocalSize row major matrix. |
| jacobian is the matrix returned by :func:`LocalParameterization::ComputeJacobian` at :math:`x`. |
| |
| This is only used by GradientProblem. For most normal uses, it is |
| okay to use the default implementation. |
| |
| Instances |
| --------- |
| |
| .. class:: IdentityParameterization |
| |
| A trivial version of :math:`\boxplus` is when :math:`\Delta x` is |
| of the same size as :math:`x` and |
| |
| .. math:: \boxplus(x, \Delta x) = x + \Delta x |
| |
| .. class:: SubsetParameterization |
| |
| A more interesting case if :math:`x` is a two dimensional vector, |
| and the user wishes to hold the first coordinate constant. Then, |
| :math:`\Delta x` is a scalar and :math:`\boxplus` is defined as |
| |
| .. math:: |
| |
| \boxplus(x, \Delta x) = x + \left[ \begin{array}{c} 0 \\ 1 |
| \end{array} \right] \Delta x |
| |
| :class:`SubsetParameterization` generalizes this construction to |
| hold any part of a parameter block constant. |
| |
| .. class:: QuaternionParameterization |
| |
| Another example that occurs commonly in Structure from Motion |
| problems is when camera rotations are parameterized using a |
| quaternion. There, it is useful only to make updates orthogonal to |
| that 4-vector defining the quaternion. One way to do this is to let |
| :math:`\Delta x` be a 3 dimensional vector and define |
| :math:`\boxplus` to be |
| |
| .. math:: \boxplus(x, \Delta x) = \left[ \cos(|\Delta x|), \frac{\sin\left(|\Delta x|\right)}{|\Delta x|} \Delta x \right] * x |
| :label: quaternion |
| |
| The multiplication between the two 4-vectors on the right hand side |
| is the standard quaternion |
| product. :class:`QuaternionParameterization` is an implementation |
| of :eq:`quaternion`. |
| |
| .. class:: EigenQuaternionParameterization |
| |
| Eigen uses a different internal memory layout for the elements of the |
| quaternion than what is commonly used. Specifically, Eigen stores the |
| elements in memory as [x, y, z, w] where the real part is last |
| whereas it is typically stored first. Note, when creating an Eigen |
| quaternion through the constructor the elements are accepted in w, x, |
| y, z order. Since Ceres operates on parameter blocks which are raw |
| double pointers this difference is important and requires a different |
| parameterization. :class:`EigenQuaternionParameterization` uses the |
| same update as :class:`QuaternionParameterization` but takes into |
| account Eigen's internal memory element ordering. |
| |
| .. class:: HomogeneousVectorParameterization |
| |
| In computer vision, homogeneous vectors are commonly used to |
| represent entities in projective geometry such as points in |
| projective space. One example where it is useful to use this |
| over-parameterization is in representing points whose triangulation |
| is ill-conditioned. Here it is advantageous to use homogeneous |
| vectors, instead of an Euclidean vector, because it can represent |
| points at infinity. |
| |
| When using homogeneous vectors it is useful to only make updates |
| orthogonal to that :math:`n`-vector defining the homogeneous |
| vector [HartleyZisserman]_. One way to do this is to let :math:`\Delta x` |
| be a :math:`n-1` dimensional vector and define :math:`\boxplus` to be |
| |
| .. math:: \boxplus(x, \Delta x) = \left[ \frac{\sin\left(0.5 |\Delta x|\right)}{|\Delta x|} \Delta x, \cos(0.5 |\Delta x|) \right] * x |
| |
| The multiplication between the two vectors on the right hand side |
| is defined as an operator which applies the update orthogonal to |
| :math:`x` to remain on the sphere. Note, it is assumed that |
| last element of :math:`x` is the scalar component of the homogeneous |
| vector. |
| |
| |
| .. class:: ProductParameterization |
| |
| Consider an optimization problem over the space of rigid |
| transformations :math:`SE(3)`, which is the Cartesian product of |
| :math:`SO(3)` and :math:`\mathbb{R}^3`. Suppose you are using |
| Quaternions to represent the rotation, Ceres ships with a local |
| parameterization for that and :math:`\mathbb{R}^3` requires no, or |
| :class:`IdentityParameterization` parameterization. So how do we |
| construct a local parameterization for a parameter block a rigid |
| transformation? |
| |
| In cases, where a parameter block is the Cartesian product of a |
| number of manifolds and you have the local parameterization of the |
| individual manifolds available, :class:`ProductParameterization` |
| can be used to construct a local parameterization 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, a local parameterization |
| can be constructed as |
| |
| .. code-block:: c++ |
| |
| ProductParameterization se3_param(new QuaternionParameterization(), |
| new IdentityTransformation(3)); |
| |
| |
| :class:`AutoDiffLocalParameterization` |
| ====================================== |
| |
| .. class:: AutoDiffLocalParameterization |
| |
| :class:`AutoDiffLocalParameterization` does for |
| :class:`LocalParameterization` what :class:`AutoDiffCostFunction` |
| does for :class:`CostFunction`. It allows the user to define a |
| templated functor that implements the |
| :func:`LocalParameterization::Plus` operation and it uses automatic |
| differentiation to implement the computation of the Jacobian. |
| |
| To get an auto differentiated local parameterization, you must |
| define a class with a templated operator() (a functor) that computes |
| |
| .. math:: x' = \boxplus(x, \Delta x), |
| |
| For example, Quaternions have a three dimensional local |
| parameterization. Its plus operation can be implemented as (taken |
| from `internal/ceres/autodiff_local_parameterization_test.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/internal/ceres/autodiff_local_parameterization_test.cc>`_ |
| ) |
| |
| .. code-block:: c++ |
| |
| struct QuaternionPlus { |
| template<typename T> |
| bool operator()(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 > 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; |
| } |
| }; |
| |
| Given this struct, the auto differentiated local |
| parameterization can now be constructed as |
| |
| .. code-block:: c++ |
| |
| LocalParameterization* local_parameterization = |
| new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>; |
| | | |
| Global Size ---------------+ | |
| Local Size -------------------+ |
| |
| |
| :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::AddResidualBlock` 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 ``NULL``, 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:`LocalParameterization` 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 ``local_parameterization`` |
| 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 ``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. |
| |
| .. function:: ResidualBlockId Problem::AddResidualBlock(CostFunction* cost_function, LossFunction* loss_function, const vector<double*> parameter_blocks) |
| .. function:: ResidualBlockId Problem::AddResidualBlock(CostFunction* cost_function, LossFunction* loss_function, double *x0, double *x1, ...) |
| |
| 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 |
| NULL, 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 as up to ten separate ``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(...), NULL, x1); |
| problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x1); |
| problem.AddResidualBlock(new MyUnaryCostFunction(...), NULL, v1); |
| problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, v2); |
| |
| .. function:: void Problem::AddParameterBlock(double* values, int size, LocalParameterization* local_parameterization) |
| |
| Add a parameter block with appropriate size 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::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. Any parameters that the residual |
| block depends on are not removed. The cost and loss functions for the |
| residual block will not get deleted immediately; won't happen until the |
| problem itself is deleted. If Problem::Options::enable_fast_removal is |
| true, then the removal is fast (almost constant time). Otherwise, removing a |
| residual block will incur a scan of the entire Problem object to verify that |
| the residual_block represents a valid residual in the 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. |
| Hold the indicated parameter block constant during optimization. |
| |
| .. function:: void Problem::RemoveParameterBlock(double* values) |
| |
| Remove a parameter block from the problem. The parameterization of |
| the parameter block, if it exists, will persist until the deletion |
| of the problem (similar to cost/loss functions in residual block |
| removal). Any residual blocks that depend on the parameter are also |
| removed, as described above in RemoveResidualBlock(). If |
| Problem::Options::enable_fast_removal is true, then |
| the removal is fast (almost constant time). Otherwise, removing a |
| parameter block will incur a scan of the entire Problem object. |
| |
| **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(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:: void Problem::SetParameterization(double* values, LocalParameterization* local_parameterization) |
| |
| Set the local parameterization for one of the parameter blocks. |
| The local_parameterization is owned by the Problem by default. It |
| is acceptable to set the same parameterization for multiple |
| parameters; the destructor is careful to delete local |
| parameterizations only once. The local parameterization can only be |
| set once per parameter, and cannot be changed once set. |
| |
| .. function:: LocalParameterization* Problem::GetParameterization(double* values) const |
| |
| Get the local parameterization object associated with this |
| parameter block. If there is no parameterization object associated |
| then `NULL` is returned |
| |
| .. 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 :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 |
| :math:`\infty`. |
| |
| .. 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::ParameterBlockLocalSize(const double* values) const |
| |
| The size of local parameterization for the parameter block. If |
| there is no local parameterization associated with this parameter |
| block, then ``ParameterBlockLocalSize`` = ``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 `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 incur a scan of the entire |
| :class:`Problem` object. |
| |
| .. function:: const CostFunction* GetCostFunctionForResidualBlock(const ResidualBlockId residual_block) const |
| |
| Get the :class:`CostFunction` for the given residual block. |
| |
| .. function:: const LossFunction* GetLossFunctionForResidualBlock(const ResidualBlockId residual_block) const |
| |
| Get the :class:`LossFunction` for the given residual block. |
| |
| .. 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 |
| `NULL`. 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, NULL, &x); |
| |
| double cost = 0.0; |
| problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL); |
| |
| 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, NULL, NULL, NULL); |
| |
| is the way to do so. |
| |
| .. NOTE:: |
| |
| If no local parameterizations 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 local parameterization, then |
| it contributes "LocalSize" 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. |
| |
| .. 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 parameter 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. (Requires OpenMP). |
| |
| ``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 3x3 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 3x3 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 3x3 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(q1 * q2) |
| = R(q1) * R(q2)` 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:`Q*Q' = 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 * w |
| |
| where :math:`*` 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 data[] = {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. |