| .. highlight:: c++ |
| |
| .. default-domain:: cpp |
| |
| .. _chapter-nnls_tutorial: |
| |
| ======================== |
| Non-linear Least Squares |
| ======================== |
| |
| Introduction |
| ============ |
| |
| Ceres can solve bounds constrained robustified non-linear least |
| squares problems of the form |
| |
| .. math:: :label: ceresproblem |
| |
| \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 |
| |
| Problems of this form comes up in a broad range of areas across |
| science and engineering - from `fitting curves`_ in statistics, to |
| constructing `3D models from photographs`_ in computer vision. |
| |
| .. _fitting curves: http://en.wikipedia.org/wiki/Nonlinear_regression |
| .. _3D models from photographs: http://en.wikipedia.org/wiki/Bundle_adjustment |
| |
| In this chapter we will learn how to solve :eq:`ceresproblem` using |
| Ceres Solver. Full working code for all the examples described in this |
| chapter and more can be found in the `examples |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/>`_ |
| directory. |
| |
| The expression |
| :math:`\rho_i\left(\left\|f_i\left(x_{i_1},...,x_{i_k}\right)\right\|^2\right)` |
| is known as a ``ResidualBlock``, 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 small scalars as a ``ParameterBlock``. Of course a |
| ``ParameterBlock`` can just be a single parameter. :math:`l_j` and |
| :math:`u_j` are bounds on the parameter block :math:`x_j`. |
| |
| :math:`\rho_i` is a :class:`LossFunction`. A :class:`LossFunction` is |
| a scalar function that is used to reduce the influence of outliers on |
| the solution of non-linear least squares problems. |
| |
| 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 `non-linear least squares problem |
| <http://en.wikipedia.org/wiki/Non-linear_least_squares>`_. |
| |
| .. math:: \frac{1}{2}\sum_{i} \left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2. |
| :label: ceresproblemnonrobust |
| |
| .. _section-hello-world: |
| |
| Hello World! |
| ============ |
| |
| To get started, consider the problem of finding the minimum of the |
| function |
| |
| .. math:: \frac{1}{2}(10 -x)^2. |
| |
| This is a trivial problem, whose minimum is located at :math:`x = 10`, |
| but it is a good place to start to illustrate the basics of solving a |
| problem with Ceres [#f1]_. |
| |
| The first step is to write a functor that will evaluate this the |
| function :math:`f(x) = 10 - x`: |
| |
| .. code-block:: c++ |
| |
| struct CostFunctor { |
| template <typename T> |
| bool operator()(const T* const x, T* residual) const { |
| residual[0] = T(10.0) - x[0]; |
| return true; |
| } |
| }; |
| |
| The important thing to note here is that ``operator()`` is a templated |
| method, which assumes that all its inputs and outputs are of some type |
| ``T``. The use of templating here allows Ceres to call |
| ``CostFunctor::operator<T>()``, with ``T=double`` when just the value |
| of the residual is needed, and with a special type ``T=Jet`` when the |
| Jacobians are needed. In :ref:`section-derivatives` we will discuss the |
| various ways of supplying derivatives to Ceres in more detail. |
| |
| Once we have a way of computing the residual function, it is now time |
| to construct a non-linear least squares problem using it and have |
| Ceres solve it. |
| |
| .. code-block:: c++ |
| |
| int main(int argc, char** argv) { |
| google::InitGoogleLogging(argv[0]); |
| |
| // The variable to solve for with its initial value. |
| double initial_x = 5.0; |
| double x = initial_x; |
| |
| // Build the problem. |
| Problem problem; |
| |
| // Set up the only cost function (also known as residual). This uses |
| // auto-differentiation to obtain the derivative (jacobian). |
| CostFunction* cost_function = |
| new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); |
| problem.AddResidualBlock(cost_function, NULL, &x); |
| |
| // Run the solver! |
| Solver::Options options; |
| options.linear_solver_type = ceres::DENSE_QR; |
| options.minimizer_progress_to_stdout = true; |
| Solver::Summary summary; |
| Solve(options, &problem, &summary); |
| |
| std::cout << summary.BriefReport() << "\n"; |
| std::cout << "x : " << initial_x |
| << " -> " << x << "\n"; |
| return 0; |
| } |
| |
| :class:`AutoDiffCostFunction` takes a ``CostFunctor`` as input, |
| automatically differentiates it and gives it a :class:`CostFunction` |
| interface. |
| |
| Compiling and running `examples/helloworld.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld.cc>`_ |
| gives us |
| |
| .. code-block:: bash |
| |
| iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time |
| 0 4.512500e+01 0.00e+00 9.50e+00 0.00e+00 0.00e+00 1.00e+04 0 5.33e-04 3.46e-03 |
| 1 4.511598e-07 4.51e+01 9.50e-04 9.50e+00 1.00e+00 3.00e+04 1 5.00e-04 4.05e-03 |
| 2 5.012552e-16 4.51e-07 3.17e-08 9.50e-04 1.00e+00 9.00e+04 1 1.60e-05 4.09e-03 |
| Ceres Solver Report: Iterations: 2, Initial cost: 4.512500e+01, Final cost: 5.012552e-16, Termination: CONVERGENCE |
| x : 0.5 -> 10 |
| |
| Starting from a :math:`x=5`, the solver in two iterations goes to 10 |
| [#f2]_. The careful reader will note that this is a linear problem and |
| one linear solve should be enough to get the optimal value. The |
| default configuration of the solver is aimed at non-linear problems, |
| and for reasons of simplicity we did not change it in this example. It |
| is indeed possible to obtain the solution to this problem using Ceres |
| in one iteration. Also note that the solver did get very close to the |
| optimal function value of 0 in the very first iteration. We will |
| discuss these issues in greater detail when we talk about convergence |
| and parameter settings for Ceres. |
| |
| .. rubric:: Footnotes |
| |
| .. [#f1] `examples/helloworld.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld.cc>`_ |
| .. [#f2] Actually the solver ran for three iterations, and it was |
| by looking at the value returned by the linear solver in the third |
| iteration, it observed that the update to the parameter block was too |
| small and declared convergence. Ceres only prints out the display at |
| the end of an iteration, and terminates as soon as it detects |
| convergence, which is why you only see two iterations here and not |
| three. |
| |
| .. _section-derivatives: |
| |
| |
| Derivatives |
| =========== |
| |
| Ceres Solver like most optimization packages, depends on being able to |
| evaluate the value and the derivatives of each term in the objective |
| function at arbitrary parameter values. Doing so correctly and |
| efficiently is essential to getting good results. Ceres Solver |
| provides a number of ways of doing so. You have already seen one of |
| them in action -- |
| Automatic Differentiation in `examples/helloworld.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld.cc>`_ |
| |
| We now consider the other two possibilities. Analytic and numeric |
| derivatives. |
| |
| |
| Numeric Derivatives |
| ------------------- |
| |
| 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 can be used. The user defines a |
| functor which computes the residual value and construct a |
| :class:`NumericDiffCostFunction` using it. e.g., for :math:`f(x) = 10 - x` |
| the corresponding functor would be |
| |
| .. code-block:: c++ |
| |
| struct NumericDiffCostFunctor { |
| bool operator()(const double* const x, double* residual) const { |
| residual[0] = 10.0 - x[0]; |
| return true; |
| } |
| }; |
| |
| Which is added to the :class:`Problem` as: |
| |
| .. code-block:: c++ |
| |
| CostFunction* cost_function = |
| new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>( |
| new NumericDiffCostFunctor); |
| problem.AddResidualBlock(cost_function, NULL, &x); |
| |
| Notice the parallel from when we were using automatic differentiation |
| |
| .. code-block:: c++ |
| |
| CostFunction* cost_function = |
| new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); |
| problem.AddResidualBlock(cost_function, NULL, &x); |
| |
| The construction looks almost identical to the one used for automatic |
| differentiation, except for an extra template parameter that indicates |
| the kind of finite differencing scheme to be used for computing the |
| numerical derivatives [#f3]_. For more details see the documentation |
| for :class:`NumericDiffCostFunction`. |
| |
| **Generally speaking we recommend automatic differentiation instead of |
| numeric differentiation. The use of C++ templates makes automatic |
| differentiation efficient, whereas numeric differentiation is |
| expensive, prone to numeric errors, and leads to slower convergence.** |
| |
| |
| Analytic Derivatives |
| -------------------- |
| |
| In some cases, using automatic differentiation is not possible. For |
| example, it may be the case that it is more efficient to compute the |
| derivatives in closed form instead of relying on the chain rule used |
| by the automatic differentiation code. |
| |
| In such cases, it is possible to supply your own residual and jacobian |
| computation code. To do this, define a subclass of |
| :class:`CostFunction` or :class:`SizedCostFunction` if you know the |
| sizes of the parameters and residuals at compile time. Here for |
| example is ``SimpleCostFunction`` that implements :math:`f(x) = 10 - |
| x`. |
| |
| .. code-block:: c++ |
| |
| class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> { |
| public: |
| virtual ~QuadraticCostFunction() {} |
| virtual bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const { |
| const double x = parameters[0][0]; |
| residuals[0] = 10 - x; |
| |
| // Compute the Jacobian if asked for. |
| if (jacobians != NULL && jacobians[0] != NULL) { |
| jacobians[0][0] = -1; |
| } |
| return true; |
| } |
| }; |
| |
| |
| ``SimpleCostFunction::Evaluate`` is provided with an input array of |
| ``parameters``, an output array ``residuals`` for residuals and an |
| output array ``jacobians`` for Jacobians. The ``jacobians`` array is |
| optional, ``Evaluate`` is expected to check when it is non-null, and |
| if it is the case then fill it with the values of the derivative of |
| the residual function. In this case since the residual function is |
| linear, the Jacobian is constant [#f4]_ . |
| |
| As can be seen from the above code fragments, implementing |
| :class:`CostFunction` objects is a bit tedious. We recommend that |
| unless you have a good reason to manage the jacobian computation |
| yourself, you use :class:`AutoDiffCostFunction` or |
| :class:`NumericDiffCostFunction` to construct your residual blocks. |
| |
| More About Derivatives |
| ---------------------- |
| |
| Computing derivatives is by far the most complicated part of using |
| Ceres, and depending on the circumstance the user may need more |
| sophisticated ways of computing derivatives. This section just |
| scratches the surface of how derivatives can be supplied to |
| Ceres. Once you are comfortable with using |
| :class:`NumericDiffCostFunction` and :class:`AutoDiffCostFunction` we |
| recommend taking a look at :class:`DynamicAutoDiffCostFunction`, |
| :class:`CostFunctionToFunctor`, :class:`NumericDiffFunctor` and |
| :class:`ConditionedCostFunction` for more advanced ways of |
| constructing and computing cost functions. |
| |
| .. rubric:: Footnotes |
| |
| .. [#f3] `examples/helloworld_numeric_diff.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld_numeric_diff.cc>`_. |
| .. [#f4] `examples/helloworld_analytic_diff.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld_analytic_diff.cc>`_. |
| |
| |
| .. _section-powell: |
| |
| Powell's Function |
| ================= |
| |
| Consider now a slightly more complicated example -- the minimization |
| of Powell's function. Let :math:`x = \left[x_1, x_2, x_3, x_4 \right]` |
| and |
| |
| .. math:: |
| |
| \begin{align} |
| f_1(x) &= x_1 + 10x_2 \\ |
| f_2(x) &= \sqrt{5} (x_3 - x_4)\\ |
| f_3(x) &= (x_2 - 2x_3)^2\\ |
| f_4(x) &= \sqrt{10} (x_1 - x_4)^2\\ |
| F(x) &= \left[f_1(x),\ f_2(x),\ f_3(x),\ f_4(x) \right] |
| \end{align} |
| |
| |
| :math:`F(x)` is a function of four parameters, has four residuals |
| and we wish to find :math:`x` such that :math:`\frac{1}{2}\|F(x)\|^2` |
| is minimized. |
| |
| Again, the first step is to define functors that evaluate of the terms |
| in the objective functor. Here is the code for evaluating |
| :math:`f_4(x_1, x_4)`: |
| |
| .. code-block:: c++ |
| |
| struct F4 { |
| template <typename T> |
| bool operator()(const T* const x1, const T* const x4, T* residual) const { |
| residual[0] = T(sqrt(10.0)) * (x1[0] - x4[0]) * (x1[0] - x4[0]); |
| return true; |
| } |
| }; |
| |
| |
| Similarly, we can define classes ``F1``, ``F2`` and ``F3`` to evaluate |
| :math:`f_1(x_1, x_2)`, :math:`f_2(x_3, x_4)` and :math:`f_3(x_2, x_3)` |
| respectively. Using these, the problem can be constructed as follows: |
| |
| |
| .. code-block:: c++ |
| |
| double x1 = 3.0; double x2 = -1.0; double x3 = 0.0; double x4 = 1.0; |
| |
| Problem problem; |
| |
| // Add residual terms to the problem using the using the autodiff |
| // wrapper to get the derivatives automatically. |
| problem.AddResidualBlock( |
| new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), NULL, &x1, &x2); |
| problem.AddResidualBlock( |
| new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x3, &x4); |
| problem.AddResidualBlock( |
| new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x2, &x3) |
| problem.AddResidualBlock( |
| new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x1, &x4); |
| |
| |
| Note that each ``ResidualBlock`` only depends on the two parameters |
| that the corresponding residual object depends on and not on all four |
| parameters. Compiling and running `examples/powell.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/powell.cc>`_ |
| gives us: |
| |
| .. code-block:: bash |
| |
| Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1 |
| iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time |
| 0 1.075000e+02 0.00e+00 1.55e+02 0.00e+00 0.00e+00 1.00e+04 0 4.95e-04 2.30e-03 |
| 1 5.036190e+00 1.02e+02 2.00e+01 2.16e+00 9.53e-01 3.00e+04 1 4.39e-05 2.40e-03 |
| 2 3.148168e-01 4.72e+00 2.50e+00 6.23e-01 9.37e-01 9.00e+04 1 9.06e-06 2.43e-03 |
| 3 1.967760e-02 2.95e-01 3.13e-01 3.08e-01 9.37e-01 2.70e+05 1 8.11e-06 2.45e-03 |
| 4 1.229900e-03 1.84e-02 3.91e-02 1.54e-01 9.37e-01 8.10e+05 1 6.91e-06 2.48e-03 |
| 5 7.687123e-05 1.15e-03 4.89e-03 7.69e-02 9.37e-01 2.43e+06 1 7.87e-06 2.50e-03 |
| 6 4.804625e-06 7.21e-05 6.11e-04 3.85e-02 9.37e-01 7.29e+06 1 5.96e-06 2.52e-03 |
| 7 3.003028e-07 4.50e-06 7.64e-05 1.92e-02 9.37e-01 2.19e+07 1 5.96e-06 2.55e-03 |
| 8 1.877006e-08 2.82e-07 9.54e-06 9.62e-03 9.37e-01 6.56e+07 1 5.96e-06 2.57e-03 |
| 9 1.173223e-09 1.76e-08 1.19e-06 4.81e-03 9.37e-01 1.97e+08 1 7.87e-06 2.60e-03 |
| 10 7.333425e-11 1.10e-09 1.49e-07 2.40e-03 9.37e-01 5.90e+08 1 6.20e-06 2.63e-03 |
| 11 4.584044e-12 6.88e-11 1.86e-08 1.20e-03 9.37e-01 1.77e+09 1 6.91e-06 2.65e-03 |
| 12 2.865573e-13 4.30e-12 2.33e-09 6.02e-04 9.37e-01 5.31e+09 1 5.96e-06 2.67e-03 |
| 13 1.791438e-14 2.69e-13 2.91e-10 3.01e-04 9.37e-01 1.59e+10 1 7.15e-06 2.69e-03 |
| |
| Ceres Solver v1.12.0 Solve Report |
| ---------------------------------- |
| Original Reduced |
| Parameter blocks 4 4 |
| Parameters 4 4 |
| Residual blocks 4 4 |
| Residual 4 4 |
| |
| Minimizer TRUST_REGION |
| |
| Dense linear algebra library EIGEN |
| Trust region strategy LEVENBERG_MARQUARDT |
| |
| Given Used |
| Linear solver DENSE_QR DENSE_QR |
| Threads 1 1 |
| Linear solver threads 1 1 |
| |
| Cost: |
| Initial 1.075000e+02 |
| Final 1.791438e-14 |
| Change 1.075000e+02 |
| |
| Minimizer iterations 14 |
| Successful steps 14 |
| Unsuccessful steps 0 |
| |
| Time (in seconds): |
| Preprocessor 0.002 |
| |
| Residual evaluation 0.000 |
| Jacobian evaluation 0.000 |
| Linear solver 0.000 |
| Minimizer 0.001 |
| |
| Postprocessor 0.000 |
| Total 0.005 |
| |
| Termination: CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.642190e-11 <= 1.000000e-10) |
| |
| Final x1 = 0.000292189, x2 = -2.92189e-05, x3 = 4.79511e-05, x4 = 4.79511e-05 |
| |
| It is easy to see that the optimal solution to this problem is at |
| :math:`x_1=0, x_2=0, x_3=0, x_4=0` with an objective function value of |
| :math:`0`. In 10 iterations, Ceres finds a solution with an objective |
| function value of :math:`4\times 10^{-12}`. |
| |
| .. rubric:: Footnotes |
| |
| .. [#f5] `examples/powell.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/powell.cc>`_. |
| |
| |
| .. _section-fitting: |
| |
| Curve Fitting |
| ============= |
| |
| The examples we have seen until now are simple optimization problems |
| with no data. The original purpose of least squares and non-linear |
| least squares analysis was fitting curves to data. It is only |
| appropriate that we now consider an example of such a problem |
| [#f6]_. It contains data generated by sampling the curve :math:`y = |
| e^{0.3x + 0.1}` and adding Gaussian noise with standard deviation |
| :math:`\sigma = 0.2`. Let us fit some data to the curve |
| |
| .. math:: y = e^{mx + c}. |
| |
| We begin by defining a templated object to evaluate the |
| residual. There will be a residual for each observation. |
| |
| .. code-block:: c++ |
| |
| struct ExponentialResidual { |
| ExponentialResidual(double x, double y) |
| : x_(x), y_(y) {} |
| |
| template <typename T> |
| bool operator()(const T* const m, const T* const c, T* residual) const { |
| residual[0] = T(y_) - exp(m[0] * T(x_) + c[0]); |
| return true; |
| } |
| |
| private: |
| // Observations for a sample. |
| const double x_; |
| const double y_; |
| }; |
| |
| Assuming the observations are in a :math:`2n` sized array called |
| ``data`` the problem construction is a simple matter of creating a |
| :class:`CostFunction` for every observation. |
| |
| |
| .. code-block:: c++ |
| |
| double m = 0.0; |
| double c = 0.0; |
| |
| Problem problem; |
| for (int i = 0; i < kNumObservations; ++i) { |
| CostFunction* cost_function = |
| new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>( |
| new ExponentialResidual(data[2 * i], data[2 * i + 1])); |
| problem.AddResidualBlock(cost_function, NULL, &m, &c); |
| } |
| |
| Compiling and running `examples/curve_fitting.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/curve_fitting.cc>`_ |
| gives us: |
| |
| .. code-block:: bash |
| |
| iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time |
| 0 1.211734e+02 0.00e+00 3.61e+02 0.00e+00 0.00e+00 1.00e+04 0 5.34e-04 2.56e-03 |
| 1 1.211734e+02 -2.21e+03 0.00e+00 7.52e-01 -1.87e+01 5.00e+03 1 4.29e-05 3.25e-03 |
| 2 1.211734e+02 -2.21e+03 0.00e+00 7.51e-01 -1.86e+01 1.25e+03 1 1.10e-05 3.28e-03 |
| 3 1.211734e+02 -2.19e+03 0.00e+00 7.48e-01 -1.85e+01 1.56e+02 1 1.41e-05 3.31e-03 |
| 4 1.211734e+02 -2.02e+03 0.00e+00 7.22e-01 -1.70e+01 9.77e+00 1 1.00e-05 3.34e-03 |
| 5 1.211734e+02 -7.34e+02 0.00e+00 5.78e-01 -6.32e+00 3.05e-01 1 1.00e-05 3.36e-03 |
| 6 3.306595e+01 8.81e+01 4.10e+02 3.18e-01 1.37e+00 9.16e-01 1 2.79e-05 3.41e-03 |
| 7 6.426770e+00 2.66e+01 1.81e+02 1.29e-01 1.10e+00 2.75e+00 1 2.10e-05 3.45e-03 |
| 8 3.344546e+00 3.08e+00 5.51e+01 3.05e-02 1.03e+00 8.24e+00 1 2.10e-05 3.48e-03 |
| 9 1.987485e+00 1.36e+00 2.33e+01 8.87e-02 9.94e-01 2.47e+01 1 2.10e-05 3.52e-03 |
| 10 1.211585e+00 7.76e-01 8.22e+00 1.05e-01 9.89e-01 7.42e+01 1 2.10e-05 3.56e-03 |
| 11 1.063265e+00 1.48e-01 1.44e+00 6.06e-02 9.97e-01 2.22e+02 1 2.60e-05 3.61e-03 |
| 12 1.056795e+00 6.47e-03 1.18e-01 1.47e-02 1.00e+00 6.67e+02 1 2.10e-05 3.64e-03 |
| 13 1.056751e+00 4.39e-05 3.79e-03 1.28e-03 1.00e+00 2.00e+03 1 2.10e-05 3.68e-03 |
| Ceres Solver Report: Iterations: 13, Initial cost: 1.211734e+02, Final cost: 1.056751e+00, Termination: CONVERGENCE |
| Initial m: 0 c: 0 |
| Final m: 0.291861 c: 0.131439 |
| |
| Starting from parameter values :math:`m = 0, c=0` with an initial |
| objective function value of :math:`121.173` Ceres finds a solution |
| :math:`m= 0.291861, c = 0.131439` with an objective function value of |
| :math:`1.05675`. These values are a bit different than the |
| parameters of the original model :math:`m=0.3, c= 0.1`, but this is |
| expected. When reconstructing a curve from noisy data, we expect to |
| see such deviations. Indeed, if you were to evaluate the objective |
| function for :math:`m=0.3, c=0.1`, the fit is worse with an objective |
| function value of :math:`1.082425`. The figure below illustrates the fit. |
| |
| .. figure:: least_squares_fit.png |
| :figwidth: 500px |
| :height: 400px |
| :align: center |
| |
| Least squares curve fitting. |
| |
| |
| .. rubric:: Footnotes |
| |
| .. [#f6] `examples/curve_fitting.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/curve_fitting.cc>`_ |
| |
| |
| Robust Curve Fitting |
| ===================== |
| |
| Now suppose the data we are given has some outliers, i.e., we have |
| some points that do not obey the noise model. If we were to use the |
| code above to fit such data, we would get a fit that looks as |
| below. Notice how the fitted curve deviates from the ground truth. |
| |
| .. figure:: non_robust_least_squares_fit.png |
| :figwidth: 500px |
| :height: 400px |
| :align: center |
| |
| To deal with outliers, a standard technique is to use a |
| :class:`LossFunction`. Loss functions reduce the influence of |
| residual blocks with high residuals, usually the ones corresponding to |
| outliers. To associate a loss function with a residual block, we change |
| |
| .. code-block:: c++ |
| |
| problem.AddResidualBlock(cost_function, NULL , &m, &c); |
| |
| to |
| |
| .. code-block:: c++ |
| |
| problem.AddResidualBlock(cost_function, new CauchyLoss(0.5) , &m, &c); |
| |
| :class:`CauchyLoss` is one of the loss functions that ships with Ceres |
| Solver. The argument :math:`0.5` specifies the scale of the loss |
| function. As a result, we get the fit below [#f7]_. Notice how the |
| fitted curve moves back closer to the ground truth curve. |
| |
| .. figure:: robust_least_squares_fit.png |
| :figwidth: 500px |
| :height: 400px |
| :align: center |
| |
| Using :class:`LossFunction` to reduce the effect of outliers on a |
| least squares fit. |
| |
| |
| .. rubric:: Footnotes |
| |
| .. [#f7] `examples/robust_curve_fitting.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/robust_curve_fitting.cc>`_ |
| |
| |
| Bundle Adjustment |
| ================= |
| |
| One of the main reasons for writing Ceres was our need to solve large |
| scale bundle adjustment problems [HartleyZisserman]_, [Triggs]_. |
| |
| Given a set of measured image feature locations and correspondences, |
| the goal of bundle adjustment is to find 3D point positions and camera |
| parameters that minimize the reprojection error. This optimization |
| problem is usually formulated as a non-linear least squares problem, |
| where the error is the squared :math:`L_2` norm of the difference between |
| the observed feature location and the projection of the corresponding |
| 3D point on the image plane of the camera. Ceres has extensive support |
| for solving bundle adjustment problems. |
| |
| Let us solve a problem from the `BAL |
| <http://grail.cs.washington.edu/projects/bal/>`_ dataset [#f8]_. |
| |
| The first step as usual is to define a templated functor that computes |
| the reprojection error/residual. The structure of the functor is |
| similar to the ``ExponentialResidual``, in that there is an |
| instance of this object responsible for each image observation. |
| |
| Each residual in a BAL problem depends on a three dimensional point |
| and a nine parameter camera. The nine parameters defining the camera |
| are: three for rotation as a Rodriques' axis-angle vector, three |
| for translation, one for focal length and two for radial distortion. |
| The details of this camera model can be found the `Bundler homepage |
| <http://phototour.cs.washington.edu/bundler/>`_ and the `BAL homepage |
| <http://grail.cs.washington.edu/projects/bal/>`_. |
| |
| .. code-block:: c++ |
| |
| struct SnavelyReprojectionError { |
| SnavelyReprojectionError(double observed_x, double observed_y) |
| : observed_x(observed_x), observed_y(observed_y) {} |
| |
| template <typename T> |
| bool operator()(const T* const camera, |
| const T* const point, |
| T* residuals) const { |
| // camera[0,1,2] are the angle-axis rotation. |
| T p[3]; |
| ceres::AngleAxisRotatePoint(camera, point, p); |
| // camera[3,4,5] are the translation. |
| p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5]; |
| |
| // Compute the center of distortion. The sign change comes from |
| // the camera model that Noah Snavely's Bundler assumes, whereby |
| // the camera coordinate system has a negative z axis. |
| T xp = - p[0] / p[2]; |
| T yp = - p[1] / p[2]; |
| |
| // Apply second and fourth order radial distortion. |
| const T& l1 = camera[7]; |
| const T& l2 = camera[8]; |
| T r2 = xp*xp + yp*yp; |
| T distortion = T(1.0) + r2 * (l1 + l2 * r2); |
| |
| // Compute final projected point position. |
| const T& focal = camera[6]; |
| T predicted_x = focal * distortion * xp; |
| T predicted_y = focal * distortion * yp; |
| |
| // The error is the difference between the predicted and observed position. |
| residuals[0] = predicted_x - T(observed_x); |
| residuals[1] = predicted_y - T(observed_y); |
| return true; |
| } |
| |
| // Factory to hide the construction of the CostFunction object from |
| // the client code. |
| static ceres::CostFunction* Create(const double observed_x, |
| const double observed_y) { |
| return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>( |
| new SnavelyReprojectionError(observed_x, observed_y))); |
| } |
| |
| double observed_x; |
| double observed_y; |
| }; |
| |
| |
| Note that unlike the examples before, this is a non-trivial function |
| and computing its analytic Jacobian is a bit of a pain. Automatic |
| differentiation makes life much simpler. The function |
| :func:`AngleAxisRotatePoint` and other functions for manipulating |
| rotations can be found in ``include/ceres/rotation.h``. |
| |
| Given this functor, the bundle adjustment problem can be constructed |
| as follows: |
| |
| .. code-block:: c++ |
| |
| ceres::Problem problem; |
| for (int i = 0; i < bal_problem.num_observations(); ++i) { |
| ceres::CostFunction* cost_function = |
| SnavelyReprojectionError::Create( |
| bal_problem.observations()[2 * i + 0], |
| bal_problem.observations()[2 * i + 1]); |
| problem.AddResidualBlock(cost_function, |
| NULL /* squared loss */, |
| bal_problem.mutable_camera_for_observation(i), |
| bal_problem.mutable_point_for_observation(i)); |
| } |
| |
| |
| Notice that the problem construction for bundle adjustment is very |
| similar to the curve fitting example -- one term is added to the |
| objective function per observation. |
| |
| Since this is a large sparse problem (well large for ``DENSE_QR`` |
| anyways), one way to solve this problem is to set |
| :member:`Solver::Options::linear_solver_type` to |
| ``SPARSE_NORMAL_CHOLESKY`` and call :func:`Solve`. And while this is |
| a reasonable thing to do, bundle adjustment problems have a special |
| sparsity structure that can be exploited to solve them much more |
| efficiently. Ceres provides three specialized solvers (collectively |
| known as Schur-based solvers) for this task. The example code uses the |
| simplest of them ``DENSE_SCHUR``. |
| |
| .. code-block:: c++ |
| |
| ceres::Solver::Options options; |
| options.linear_solver_type = ceres::DENSE_SCHUR; |
| options.minimizer_progress_to_stdout = true; |
| ceres::Solver::Summary summary; |
| ceres::Solve(options, &problem, &summary); |
| std::cout << summary.FullReport() << "\n"; |
| |
| For a more sophisticated bundle adjustment example which demonstrates |
| the use of Ceres' more advanced features including its various linear |
| solvers, robust loss functions and local parameterizations see |
| `examples/bundle_adjuster.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/bundle_adjuster.cc>`_ |
| |
| |
| .. rubric:: Footnotes |
| |
| .. [#f8] `examples/simple_bundle_adjuster.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/simple_bundle_adjuster.cc>`_ |
| |
| Other Examples |
| ============== |
| |
| Besides the examples in this chapter, the `example |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/>`_ |
| directory contains a number of other examples: |
| |
| #. `bundle_adjuster.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/bundle_adjuster.cc>`_ |
| shows how to use the various features of Ceres to solve bundle |
| adjustment problems. |
| |
| #. `circle_fit.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/circle_fit.cc>`_ |
| shows how to fit data to a circle. |
| |
| #. `ellipse_approximation.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/ellipse_approximation.cc>`_ |
| fits points randomly distributed on an ellipse with an approximate |
| line segment contour. This is done by jointly optimizing the |
| control points of the line segment contour along with the preimage |
| positions for the data points. The purpose of this example is to |
| show an example use case for ``Solver::Options::dynamic_sparsity``, |
| and how it can benefit problems which are numerically dense but |
| dynamically sparse. |
| |
| #. `denoising.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/denoising.cc>`_ |
| implements image denoising using the `Fields of Experts |
| <http://www.gris.informatik.tu-darmstadt.de/~sroth/research/foe/index.html>`_ |
| model. |
| |
| #. `nist.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/nist.cc>`_ |
| implements and attempts to solves the `NIST |
| <http://www.itl.nist.gov/div898/strd/nls/nls_main.shtm>`_ |
| non-linear regression problems. |
| |
| #. `more_garbow_hillstrom.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/more_garbow_hillstrom.cc>`_ |
| A subset of the test problems from the paper |
| |
| Testing Unconstrained Optimization Software |
| Jorge J. More, Burton S. Garbow and Kenneth E. Hillstrom |
| ACM Transactions on Mathematical Software, 7(1), pp. 17-41, 1981 |
| |
| which were augmented with bounds and used for testing bounds |
| constrained optimization algorithms by |
| |
| A Trust Region Approach to Linearly Constrained Optimization |
| David M. Gay |
| Numerical Analysis (Griffiths, D.F., ed.), pp. 72-105 |
| Lecture Notes in Mathematics 1066, Springer Verlag, 1984. |
| |
| |
| #. `libmv_bundle_adjuster.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/libmv_bundle_adjuster.cc>`_ |
| is the bundle adjustment algorithm used by `Blender <www.blender.org>`_/libmv. |
| |
| #. `libmv_homography.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/libmv_homography.cc>`_ |
| This file demonstrates solving for a homography between two sets of |
| points and using a custom exit criterion by having a callback check |
| for image-space error. |
| |
| #. `robot_pose_mle.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/robot_pose_mle.cc>`_ |
| This example demonstrates how to use the ``DynamicAutoDiffCostFunction`` |
| variant of CostFunction. The ``DynamicAutoDiffCostFunction`` is meant to |
| be used in cases where the number of parameter blocks or the sizes are not |
| known at compile time. |
| |
| This example simulates a robot traversing down a 1-dimension hallway with |
| noise odometry readings and noisy range readings of the end of the hallway. |
| By fusing the noisy odometry and sensor readings this example demonstrates |
| how to compute the maximum likelihood estimate (MLE) of the robot's pose at |
| each timestep. |
| |
| #. `slam/pose_graph_2d/pose_graph_2d.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_2d/pose_graph_2d.cc>`_ |
| The Simultaneous Localization and Mapping (SLAM) problem consists of building |
| a map of an unknown environment while simultaneously localizing against this |
| map. The main difficulty of this problem stems from not having any additional |
| external aiding information such as GPS. SLAM has been considered one of the |
| fundamental challenges of robotics. There are many resources on SLAM |
| [#f9]_. A pose graph optimization problem is one example of a SLAM |
| problem. The following explains how to formulate the pose graph based SLAM |
| problem in 2-Dimensions with relative pose constraints. |
| |
| Consider a robot moving in a 2-Dimensional plane. The robot has access to a |
| set of sensors such as wheel odometry or a laser range scanner. From these |
| raw measurements, we want to estimate the trajectory of the robot as well as |
| build a map of the environment. In order to reduce the computational |
| complexity of the problem, the pose graph approach abstracts the raw |
| measurements away. Specifically, it creates a graph of nodes which represent |
| the pose of the robot, and edges which represent the relative transformation |
| (delta position and orientation) between the two nodes. The edges are virtual |
| measurements derived from the raw sensor measurements, e.g. by integrating |
| the raw wheel odometry or aligning the laser range scans acquired from the |
| robot. A visualization of the resulting graph is shown below. |
| |
| .. figure:: slam2d.png |
| :figwidth: 500px |
| :height: 400px |
| :align: center |
| |
| Visual representation of a graph SLAM problem. |
| |
| The figure depicts the pose of the robot as the triangles, the measurements |
| are indicated by the connecting lines, and the loop closure measurements are |
| shown as dotted lines. Loop closures are measurements between non-sequential |
| robot states and they reduce the accumulation of error over time. The |
| following will describe the mathematical formulation of the pose graph |
| problem. |
| |
| The robot at timestamp :math:`t` has state :math:`x_t = [p^T, \psi]^T` where |
| :math:`p` is a 2D vector that represents the position in the plane and |
| :math:`\psi` is the orientation in radians. The measurement of the relative |
| transform between the robot state at two timestamps :math:`a` and :math:`b` |
| is given as: :math:`z_{ab} = [\hat{p}_{ab}^T, \hat{\psi}_{ab}]`. The residual |
| implemented in the Ceres cost function which computes the error between the |
| measurement and the predicted measurement is: |
| |
| .. math:: r_{ab} = |
| \left[ |
| \begin{array}{c} |
| R_a^T\left(p_b - p_a\right) - \hat{p}_{ab} \\ |
| \mathrm{Normalize}\left(\psi_b - \psi_a - \hat{\psi}_{ab}\right) |
| \end{array} |
| \right] |
| |
| where the function :math:`\mathrm{Normalize}()` normalizes the angle in the range |
| :math:`[-\pi,\pi)`, and :math:`R` is the rotation matrix given by |
| |
| .. math:: R_a = |
| \left[ |
| \begin{array}{cc} |
| \cos \psi_a & -\sin \psi_a \\ |
| \sin \psi_a & \cos \psi_a \\ |
| \end{array} |
| \right] |
| |
| To finish the cost function, we need to weight the residual by the |
| uncertainty of the measurement. Hence, we pre-multiply the residual by the |
| inverse square root of the covariance matrix for the measurement, |
| i.e. :math:`\Sigma_{ab}^{-\frac{1}{2}} r_{ab}` where :math:`\Sigma_{ab}` is |
| the covariance. |
| |
| Lastly, we use a local parameterization to normalize the orientation in the |
| range which is normalized between :math:`[-\pi,\pi)`. Specially, we define |
| the :member:`AngleLocalParameterization::operator()` function to be: |
| :math:`\mathrm{Normalize}(\psi + \delta \psi)`. |
| |
| This package includes an executable :member:`pose_graph_2d` that will read a |
| problem definition file. This executable can work with any 2D problem |
| definition that uses the g2o format. It would be relatively straightforward |
| to implement a new reader for a different format such as TORO or |
| others. :member:`pose_graph_2d` will print the Ceres solver full summary and |
| then output to disk the original and optimized poses (``poses_original.txt`` |
| and ``poses_optimized.txt``, respectively) of the robot in the following |
| format: |
| |
| .. code-block:: bash |
| |
| pose_id x y yaw_radians |
| pose_id x y yaw_radians |
| pose_id x y yaw_radians |
| |
| where ``pose_id`` is the corresponding integer ID from the file |
| definition. Note, the file will be sorted in ascending order for the |
| ``pose_id``. |
| |
| The executable :member:`pose_graph_2d` expects the first argument to be |
| the path to the problem definition. To run the executable, |
| |
| .. code-block:: bash |
| |
| /path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o |
| |
| A python script is provided to visualize the resulting output files. |
| |
| .. code-block:: bash |
| |
| /path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt |
| |
| As an example, a standard synthetic benchmark dataset [#f10]_ created by |
| Edwin Olson which has 3500 nodes in a grid world with a total of 5598 edges |
| was solved. Visualizing the results with the provided script produces: |
| |
| .. figure:: manhattan_olson_3500_result.png |
| :figwidth: 600px |
| :height: 600px |
| :align: center |
| |
| with the original poses in green and the optimized poses in blue. As shown, |
| the optimized poses more closely match the underlying grid world. Note, the |
| left side of the graph has a small yaw drift due to a lack of relative |
| constraints to provide enough information to reconstruct the trajectory. |
| |
| .. rubric:: Footnotes |
| |
| .. [#f9] Giorgio Grisetti, Rainer Kummerle, Cyrill Stachniss, Wolfram |
| Burgard. A Tutorial on Graph-Based SLAM. IEEE Intelligent Transportation |
| Systems Magazine, 52(3):199–222, 2010. |
| |
| .. [#f10] E. Olson, J. Leonard, and S. Teller, “Fast iterative optimization of |
| pose graphs with poor initial estimates,” in Robotics and Automation |
| (ICRA), IEEE International Conference on, 2006, pp. 2262–2269. |
| |
| #. `slam/pose_graph_3d/pose_graph_3d.cc |
| <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_3d/pose_graph_3d.cc>`_ |
| The following explains how to formulate the pose graph based SLAM problem in |
| 3-Dimensions with relative pose constraints. The example also illustrates how |
| to use Eigen's geometry module with Ceres's automatic differentiation |
| functionality. |
| |
| The robot at timestamp :math:`t` has state :math:`x_t = [p^T, q^T]^T` where |
| :math:`p` is a 3D vector that represents the position and :math:`q` is the |
| orientation represented as an Eigen quaternion. The measurement of the |
| relative transform between the robot state at two timestamps :math:`a` and |
| :math:`b` is given as: :math:`z_{ab} = [\hat{p}_{ab}^T, \hat{q}_{ab}^T]^T`. |
| The residual implemented in the Ceres cost function which computes the error |
| between the measurement and the predicted measurement is: |
| |
| .. math:: r_{ab} = |
| \left[ |
| \begin{array}{c} |
| R(q_a)^{T} (p_b - p_a) - \hat{p}_{ab} \\ |
| 2.0 \mathrm{vec}\left((q_a^{-1} q_b) \hat{q}_{ab}^{-1}\right) |
| \end{array} |
| \right] |
| |
| where the function :math:`\mathrm{vec}()` returns the vector part of the |
| quaternion, i.e. :math:`[q_x, q_y, q_z]`, and :math:`R(q)` is the rotation |
| matrix for the quaternion. |
| |
| To finish the cost function, we need to weight the residual by the |
| uncertainty of the measurement. Hence, we pre-multiply the residual by the |
| inverse square root of the covariance matrix for the measurement, |
| i.e. :math:`\Sigma_{ab}^{-\frac{1}{2}} r_{ab}` where :math:`\Sigma_{ab}` is |
| the covariance. |
| |
| Given that we are using a quaternion to represent the orientation, we need to |
| use a local parameterization (:class:`EigenQuaternionParameterization`) to |
| only apply updates orthogonal to the 4-vector defining the |
| quaternion. Eigen's quaternion 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 :math:`[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 |
| :math:`w`, :math:`x`, :math:`y`, :math:`z` order. Since Ceres operates on |
| parameter blocks which are raw double pointers this difference is important |
| and requires a different parameterization. |
| |
| This package includes an executable :member:`pose_graph_3d` that will read a |
| problem definition file. This executable can work with any 3D problem |
| definition that uses the g2o format with quaternions used for the orientation |
| representation. It would be relatively straightforward to implement a new |
| reader for a different format such as TORO or others. :member:`pose_graph_3d` |
| will print the Ceres solver full summary and then output to disk the original |
| and optimized poses (``poses_original.txt`` and ``poses_optimized.txt``, |
| respectively) of the robot in the following format: |
| |
| .. code-block:: bash |
| |
| pose_id x y z q_x q_y q_z q_w |
| pose_id x y z q_x q_y q_z q_w |
| pose_id x y z q_x q_y q_z q_w |
| ... |
| |
| where ``pose_id`` is the corresponding integer ID from the file |
| definition. Note, the file will be sorted in ascending order for the |
| ``pose_id``. |
| |
| The executable :member:`pose_graph_3d` expects the first argument to be the |
| path to the problem definition. The executable can be run via |
| |
| .. code-block:: bash |
| |
| /path/to/bin/pose_graph_3d /path/to/dataset/dataset.g2o |
| |
| A script is provided to visualize the resulting output files. There is also |
| an option to enable equal axes using ``--axes_equal`` |
| |
| .. code-block:: bash |
| |
| /path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt |
| |
| As an example, a standard synthetic benchmark dataset [#f9]_ where the robot is |
| traveling on the surface of a sphere which has 2500 nodes with a total of |
| 4949 edges was solved. Visualizing the results with the provided script |
| produces: |
| |
| .. figure:: pose_graph_3d_ex.png |
| :figwidth: 600px |
| :height: 300px |
| :align: center |