3 .. default-domain:: cpp
5 .. _chapter-nnls_tutorial:
7 ========================
8 Non-linear Least Squares
9 ========================
14 Ceres can solve bounds constrained robustified non-linear least
15 squares problems of the form
17 .. math:: :label: ceresproblem
19 \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) \\
20 \text{s.t.} &\quad l_j \le x_j \le u_j
22 Problems of this form comes up in a broad range of areas across
23 science and engineering - from `fitting curves`_ in statistics, to
24 constructing `3D models from photographs`_ in computer vision.
26 .. _fitting curves: http://en.wikipedia.org/wiki/Nonlinear_regression
27 .. _3D models from photographs: http://en.wikipedia.org/wiki/Bundle_adjustment
29 In this chapter we will learn how to solve :eq:`ceresproblem` using
30 Ceres Solver. Full working code for all the examples described in this
31 chapter and more can be found in the `examples
32 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/>`_
36 :math:`\rho_i\left(\left\|f_i\left(x_{i_1},...,x_{i_k}\right)\right\|^2\right)`
37 is known as a ``ResidualBlock``, where :math:`f_i(\cdot)` is a
38 :class:`CostFunction` that depends on the parameter blocks
39 :math:`\left[x_{i_1},... , x_{i_k}\right]`. In most optimization
40 problems small groups of scalars occur together. For example the three
41 components of a translation vector and the four components of the
42 quaternion that define the pose of a camera. We refer to such a group
43 of small scalars as a ``ParameterBlock``. Of course a
44 ``ParameterBlock`` can just be a single parameter. :math:`l_j` and
45 :math:`u_j` are bounds on the parameter block :math:`x_j`.
47 :math:`\rho_i` is a :class:`LossFunction`. A :class:`LossFunction` is
48 a scalar function that is used to reduce the influence of outliers on
49 the solution of non-linear least squares problems.
51 As a special case, when :math:`\rho_i(x) = x`, i.e., the identity
52 function, and :math:`l_j = -\infty` and :math:`u_j = \infty` we get
53 the more familiar `non-linear least squares problem
54 <http://en.wikipedia.org/wiki/Non-linear_least_squares>`_.
56 .. math:: \frac{1}{2}\sum_{i} \left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2.
59 .. _section-hello-world:
64 To get started, consider the problem of finding the minimum of the
67 .. math:: \frac{1}{2}(10 -x)^2.
69 This is a trivial problem, whose minimum is located at :math:`x = 10`,
70 but it is a good place to start to illustrate the basics of solving a
71 problem with Ceres [#f1]_.
73 The first step is to write a functor that will evaluate this the
74 function :math:`f(x) = 10 - x`:
80 bool operator()(const T* const x, T* residual) const {
81 residual[0] = T(10.0) - x[0];
86 The important thing to note here is that ``operator()`` is a templated
87 method, which assumes that all its inputs and outputs are of some type
88 ``T``. The use of templating here allows Ceres to call
89 ``CostFunctor::operator<T>()``, with ``T=double`` when just the value
90 of the residual is needed, and with a special type ``T=Jet`` when the
91 Jacobians are needed. In :ref:`section-derivatives` we will discuss the
92 various ways of supplying derivatives to Ceres in more detail.
94 Once we have a way of computing the residual function, it is now time
95 to construct a non-linear least squares problem using it and have
100 int main(int argc, char** argv) {
101 google::InitGoogleLogging(argv[0]);
103 // The variable to solve for with its initial value.
104 double initial_x = 5.0;
105 double x = initial_x;
107 // Build the problem.
110 // Set up the only cost function (also known as residual). This uses
111 // auto-differentiation to obtain the derivative (jacobian).
112 CostFunction* cost_function =
113 new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
114 problem.AddResidualBlock(cost_function, NULL, &x);
117 Solver::Options options;
118 options.linear_solver_type = ceres::DENSE_QR;
119 options.minimizer_progress_to_stdout = true;
120 Solver::Summary summary;
121 Solve(options, &problem, &summary);
123 std::cout << summary.BriefReport() << "\n";
124 std::cout << "x : " << initial_x
125 << " -> " << x << "\n";
129 :class:`AutoDiffCostFunction` takes a ``CostFunctor`` as input,
130 automatically differentiates it and gives it a :class:`CostFunction`
133 Compiling and running `examples/helloworld.cc
134 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld.cc>`_
139 iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
140 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
141 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
142 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
143 Ceres Solver Report: Iterations: 2, Initial cost: 4.512500e+01, Final cost: 5.012552e-16, Termination: CONVERGENCE
146 Starting from a :math:`x=5`, the solver in two iterations goes to 10
147 [#f2]_. The careful reader will note that this is a linear problem and
148 one linear solve should be enough to get the optimal value. The
149 default configuration of the solver is aimed at non-linear problems,
150 and for reasons of simplicity we did not change it in this example. It
151 is indeed possible to obtain the solution to this problem using Ceres
152 in one iteration. Also note that the solver did get very close to the
153 optimal function value of 0 in the very first iteration. We will
154 discuss these issues in greater detail when we talk about convergence
155 and parameter settings for Ceres.
157 .. rubric:: Footnotes
159 .. [#f1] `examples/helloworld.cc
160 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld.cc>`_
161 .. [#f2] Actually the solver ran for three iterations, and it was
162 by looking at the value returned by the linear solver in the third
163 iteration, it observed that the update to the parameter block was too
164 small and declared convergence. Ceres only prints out the display at
165 the end of an iteration, and terminates as soon as it detects
166 convergence, which is why you only see two iterations here and not
169 .. _section-derivatives:
175 Ceres Solver like most optimization packages, depends on being able to
176 evaluate the value and the derivatives of each term in the objective
177 function at arbitrary parameter values. Doing so correctly and
178 efficiently is essential to getting good results. Ceres Solver
179 provides a number of ways of doing so. You have already seen one of
181 Automatic Differentiation in `examples/helloworld.cc
182 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld.cc>`_
184 We now consider the other two possibilities. Analytic and numeric
191 In some cases, its not possible to define a templated cost functor,
192 for example when the evaluation of the residual involves a call to a
193 library function that you do not have control over. In such a
194 situation, numerical differentiation can be used. The user defines a
195 functor which computes the residual value and construct a
196 :class:`NumericDiffCostFunction` using it. e.g., for :math:`f(x) = 10 - x`
197 the corresponding functor would be
201 struct NumericDiffCostFunctor {
202 bool operator()(const double* const x, double* residual) const {
203 residual[0] = 10.0 - x[0];
208 Which is added to the :class:`Problem` as:
212 CostFunction* cost_function =
213 new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(
214 new NumericDiffCostFunctor);
215 problem.AddResidualBlock(cost_function, NULL, &x);
217 Notice the parallel from when we were using automatic differentiation
221 CostFunction* cost_function =
222 new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
223 problem.AddResidualBlock(cost_function, NULL, &x);
225 The construction looks almost identical to the one used for automatic
226 differentiation, except for an extra template parameter that indicates
227 the kind of finite differencing scheme to be used for computing the
228 numerical derivatives [#f3]_. For more details see the documentation
229 for :class:`NumericDiffCostFunction`.
231 **Generally speaking we recommend automatic differentiation instead of
232 numeric differentiation. The use of C++ templates makes automatic
233 differentiation efficient, whereas numeric differentiation is
234 expensive, prone to numeric errors, and leads to slower convergence.**
240 In some cases, using automatic differentiation is not possible. For
241 example, it may be the case that it is more efficient to compute the
242 derivatives in closed form instead of relying on the chain rule used
243 by the automatic differentiation code.
245 In such cases, it is possible to supply your own residual and jacobian
246 computation code. To do this, define a subclass of
247 :class:`CostFunction` or :class:`SizedCostFunction` if you know the
248 sizes of the parameters and residuals at compile time. Here for
249 example is ``SimpleCostFunction`` that implements :math:`f(x) = 10 -
254 class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> {
256 virtual ~QuadraticCostFunction() {}
257 virtual bool Evaluate(double const* const* parameters,
259 double** jacobians) const {
260 const double x = parameters[0][0];
261 residuals[0] = 10 - x;
263 // Compute the Jacobian if asked for.
264 if (jacobians != NULL && jacobians[0] != NULL) {
265 jacobians[0][0] = -1;
272 ``SimpleCostFunction::Evaluate`` is provided with an input array of
273 ``parameters``, an output array ``residuals`` for residuals and an
274 output array ``jacobians`` for Jacobians. The ``jacobians`` array is
275 optional, ``Evaluate`` is expected to check when it is non-null, and
276 if it is the case then fill it with the values of the derivative of
277 the residual function. In this case since the residual function is
278 linear, the Jacobian is constant [#f4]_ .
280 As can be seen from the above code fragments, implementing
281 :class:`CostFunction` objects is a bit tedious. We recommend that
282 unless you have a good reason to manage the jacobian computation
283 yourself, you use :class:`AutoDiffCostFunction` or
284 :class:`NumericDiffCostFunction` to construct your residual blocks.
286 More About Derivatives
287 ----------------------
289 Computing derivatives is by far the most complicated part of using
290 Ceres, and depending on the circumstance the user may need more
291 sophisticated ways of computing derivatives. This section just
292 scratches the surface of how derivatives can be supplied to
293 Ceres. Once you are comfortable with using
294 :class:`NumericDiffCostFunction` and :class:`AutoDiffCostFunction` we
295 recommend taking a look at :class:`DynamicAutoDiffCostFunction`,
296 :class:`CostFunctionToFunctor`, :class:`NumericDiffFunctor` and
297 :class:`ConditionedCostFunction` for more advanced ways of
298 constructing and computing cost functions.
300 .. rubric:: Footnotes
302 .. [#f3] `examples/helloworld_numeric_diff.cc
303 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld_numeric_diff.cc>`_.
304 .. [#f4] `examples/helloworld_analytic_diff.cc
305 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld_analytic_diff.cc>`_.
313 Consider now a slightly more complicated example -- the minimization
314 of Powell's function. Let :math:`x = \left[x_1, x_2, x_3, x_4 \right]`
320 f_1(x) &= x_1 + 10x_2 \\
321 f_2(x) &= \sqrt{5} (x_3 - x_4)\\
322 f_3(x) &= (x_2 - 2x_3)^2\\
323 f_4(x) &= \sqrt{10} (x_1 - x_4)^2\\
324 F(x) &= \left[f_1(x),\ f_2(x),\ f_3(x),\ f_4(x) \right]
328 :math:`F(x)` is a function of four parameters, has four residuals
329 and we wish to find :math:`x` such that :math:`\frac{1}{2}\|F(x)\|^2`
332 Again, the first step is to define functors that evaluate of the terms
333 in the objective functor. Here is the code for evaluating
334 :math:`f_4(x_1, x_4)`:
339 template <typename T>
340 bool operator()(const T* const x1, const T* const x4, T* residual) const {
341 residual[0] = T(sqrt(10.0)) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
347 Similarly, we can define classes ``F1``, ``F2`` and ``F3`` to evaluate
348 :math:`f_1(x_1, x_2)`, :math:`f_2(x_3, x_4)` and :math:`f_3(x_2, x_3)`
349 respectively. Using these, the problem can be constructed as follows:
354 double x1 = 3.0; double x2 = -1.0; double x3 = 0.0; double x4 = 1.0;
358 // Add residual terms to the problem using the using the autodiff
359 // wrapper to get the derivatives automatically.
360 problem.AddResidualBlock(
361 new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), NULL, &x1, &x2);
362 problem.AddResidualBlock(
363 new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x3, &x4);
364 problem.AddResidualBlock(
365 new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x2, &x3)
366 problem.AddResidualBlock(
367 new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x1, &x4);
370 Note that each ``ResidualBlock`` only depends on the two parameters
371 that the corresponding residual object depends on and not on all four
372 parameters. Compiling and running `examples/powell.cc
373 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/powell.cc>`_
378 Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1
379 iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
380 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
381 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
382 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
383 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
384 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
385 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
386 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
387 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
388 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
389 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
390 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
391 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
392 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
393 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
395 Ceres Solver v1.12.0 Solve Report
396 ----------------------------------
403 Minimizer TRUST_REGION
405 Dense linear algebra library EIGEN
406 Trust region strategy LEVENBERG_MARQUARDT
409 Linear solver DENSE_QR DENSE_QR
411 Linear solver threads 1 1
418 Minimizer iterations 14
425 Residual evaluation 0.000
426 Jacobian evaluation 0.000
433 Termination: CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.642190e-11 <= 1.000000e-10)
435 Final x1 = 0.000292189, x2 = -2.92189e-05, x3 = 4.79511e-05, x4 = 4.79511e-05
437 It is easy to see that the optimal solution to this problem is at
438 :math:`x_1=0, x_2=0, x_3=0, x_4=0` with an objective function value of
439 :math:`0`. In 10 iterations, Ceres finds a solution with an objective
440 function value of :math:`4\times 10^{-12}`.
442 .. rubric:: Footnotes
444 .. [#f5] `examples/powell.cc
445 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/powell.cc>`_.
453 The examples we have seen until now are simple optimization problems
454 with no data. The original purpose of least squares and non-linear
455 least squares analysis was fitting curves to data. It is only
456 appropriate that we now consider an example of such a problem
457 [#f6]_. It contains data generated by sampling the curve :math:`y =
458 e^{0.3x + 0.1}` and adding Gaussian noise with standard deviation
459 :math:`\sigma = 0.2`. Let us fit some data to the curve
461 .. math:: y = e^{mx + c}.
463 We begin by defining a templated object to evaluate the
464 residual. There will be a residual for each observation.
468 struct ExponentialResidual {
469 ExponentialResidual(double x, double y)
472 template <typename T>
473 bool operator()(const T* const m, const T* const c, T* residual) const {
474 residual[0] = T(y_) - exp(m[0] * T(x_) + c[0]);
479 // Observations for a sample.
484 Assuming the observations are in a :math:`2n` sized array called
485 ``data`` the problem construction is a simple matter of creating a
486 :class:`CostFunction` for every observation.
495 for (int i = 0; i < kNumObservations; ++i) {
496 CostFunction* cost_function =
497 new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
498 new ExponentialResidual(data[2 * i], data[2 * i + 1]));
499 problem.AddResidualBlock(cost_function, NULL, &m, &c);
502 Compiling and running `examples/curve_fitting.cc
503 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/curve_fitting.cc>`_
508 iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
509 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
510 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
511 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
512 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
513 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
514 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
515 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
516 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
517 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
518 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
519 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
520 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
521 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
522 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
523 Ceres Solver Report: Iterations: 13, Initial cost: 1.211734e+02, Final cost: 1.056751e+00, Termination: CONVERGENCE
525 Final m: 0.291861 c: 0.131439
527 Starting from parameter values :math:`m = 0, c=0` with an initial
528 objective function value of :math:`121.173` Ceres finds a solution
529 :math:`m= 0.291861, c = 0.131439` with an objective function value of
530 :math:`1.05675`. These values are a bit different than the
531 parameters of the original model :math:`m=0.3, c= 0.1`, but this is
532 expected. When reconstructing a curve from noisy data, we expect to
533 see such deviations. Indeed, if you were to evaluate the objective
534 function for :math:`m=0.3, c=0.1`, the fit is worse with an objective
535 function value of :math:`1.082425`. The figure below illustrates the fit.
537 .. figure:: least_squares_fit.png
542 Least squares curve fitting.
545 .. rubric:: Footnotes
547 .. [#f6] `examples/curve_fitting.cc
548 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/curve_fitting.cc>`_
552 =====================
554 Now suppose the data we are given has some outliers, i.e., we have
555 some points that do not obey the noise model. If we were to use the
556 code above to fit such data, we would get a fit that looks as
557 below. Notice how the fitted curve deviates from the ground truth.
559 .. figure:: non_robust_least_squares_fit.png
564 To deal with outliers, a standard technique is to use a
565 :class:`LossFunction`. Loss functions reduce the influence of
566 residual blocks with high residuals, usually the ones corresponding to
567 outliers. To associate a loss function with a residual block, we change
571 problem.AddResidualBlock(cost_function, NULL , &m, &c);
577 problem.AddResidualBlock(cost_function, new CauchyLoss(0.5) , &m, &c);
579 :class:`CauchyLoss` is one of the loss functions that ships with Ceres
580 Solver. The argument :math:`0.5` specifies the scale of the loss
581 function. As a result, we get the fit below [#f7]_. Notice how the
582 fitted curve moves back closer to the ground truth curve.
584 .. figure:: robust_least_squares_fit.png
589 Using :class:`LossFunction` to reduce the effect of outliers on a
593 .. rubric:: Footnotes
595 .. [#f7] `examples/robust_curve_fitting.cc
596 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/robust_curve_fitting.cc>`_
602 One of the main reasons for writing Ceres was our need to solve large
603 scale bundle adjustment problems [HartleyZisserman]_, [Triggs]_.
605 Given a set of measured image feature locations and correspondences,
606 the goal of bundle adjustment is to find 3D point positions and camera
607 parameters that minimize the reprojection error. This optimization
608 problem is usually formulated as a non-linear least squares problem,
609 where the error is the squared :math:`L_2` norm of the difference between
610 the observed feature location and the projection of the corresponding
611 3D point on the image plane of the camera. Ceres has extensive support
612 for solving bundle adjustment problems.
614 Let us solve a problem from the `BAL
615 <http://grail.cs.washington.edu/projects/bal/>`_ dataset [#f8]_.
617 The first step as usual is to define a templated functor that computes
618 the reprojection error/residual. The structure of the functor is
619 similar to the ``ExponentialResidual``, in that there is an
620 instance of this object responsible for each image observation.
622 Each residual in a BAL problem depends on a three dimensional point
623 and a nine parameter camera. The nine parameters defining the camera
624 are: three for rotation as a Rodriques' axis-angle vector, three
625 for translation, one for focal length and two for radial distortion.
626 The details of this camera model can be found the `Bundler homepage
627 <http://phototour.cs.washington.edu/bundler/>`_ and the `BAL homepage
628 <http://grail.cs.washington.edu/projects/bal/>`_.
632 struct SnavelyReprojectionError {
633 SnavelyReprojectionError(double observed_x, double observed_y)
634 : observed_x(observed_x), observed_y(observed_y) {}
636 template <typename T>
637 bool operator()(const T* const camera,
638 const T* const point,
639 T* residuals) const {
640 // camera[0,1,2] are the angle-axis rotation.
642 ceres::AngleAxisRotatePoint(camera, point, p);
643 // camera[3,4,5] are the translation.
644 p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
646 // Compute the center of distortion. The sign change comes from
647 // the camera model that Noah Snavely's Bundler assumes, whereby
648 // the camera coordinate system has a negative z axis.
649 T xp = - p[0] / p[2];
650 T yp = - p[1] / p[2];
652 // Apply second and fourth order radial distortion.
653 const T& l1 = camera[7];
654 const T& l2 = camera[8];
655 T r2 = xp*xp + yp*yp;
656 T distortion = T(1.0) + r2 * (l1 + l2 * r2);
658 // Compute final projected point position.
659 const T& focal = camera[6];
660 T predicted_x = focal * distortion * xp;
661 T predicted_y = focal * distortion * yp;
663 // The error is the difference between the predicted and observed position.
664 residuals[0] = predicted_x - T(observed_x);
665 residuals[1] = predicted_y - T(observed_y);
669 // Factory to hide the construction of the CostFunction object from
671 static ceres::CostFunction* Create(const double observed_x,
672 const double observed_y) {
673 return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
674 new SnavelyReprojectionError(observed_x, observed_y)));
682 Note that unlike the examples before, this is a non-trivial function
683 and computing its analytic Jacobian is a bit of a pain. Automatic
684 differentiation makes life much simpler. The function
685 :func:`AngleAxisRotatePoint` and other functions for manipulating
686 rotations can be found in ``include/ceres/rotation.h``.
688 Given this functor, the bundle adjustment problem can be constructed
693 ceres::Problem problem;
694 for (int i = 0; i < bal_problem.num_observations(); ++i) {
695 ceres::CostFunction* cost_function =
696 SnavelyReprojectionError::Create(
697 bal_problem.observations()[2 * i + 0],
698 bal_problem.observations()[2 * i + 1]);
699 problem.AddResidualBlock(cost_function,
700 NULL /* squared loss */,
701 bal_problem.mutable_camera_for_observation(i),
702 bal_problem.mutable_point_for_observation(i));
706 Notice that the problem construction for bundle adjustment is very
707 similar to the curve fitting example -- one term is added to the
708 objective function per observation.
710 Since this is a large sparse problem (well large for ``DENSE_QR``
711 anyways), one way to solve this problem is to set
712 :member:`Solver::Options::linear_solver_type` to
713 ``SPARSE_NORMAL_CHOLESKY`` and call :member:`Solve`. And while this is
714 a reasonable thing to do, bundle adjustment problems have a special
715 sparsity structure that can be exploited to solve them much more
716 efficiently. Ceres provides three specialized solvers (collectively
717 known as Schur-based solvers) for this task. The example code uses the
718 simplest of them ``DENSE_SCHUR``.
722 ceres::Solver::Options options;
723 options.linear_solver_type = ceres::DENSE_SCHUR;
724 options.minimizer_progress_to_stdout = true;
725 ceres::Solver::Summary summary;
726 ceres::Solve(options, &problem, &summary);
727 std::cout << summary.FullReport() << "\n";
729 For a more sophisticated bundle adjustment example which demonstrates
730 the use of Ceres' more advanced features including its various linear
731 solvers, robust loss functions and local parameterizations see
732 `examples/bundle_adjuster.cc
733 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/bundle_adjuster.cc>`_
736 .. rubric:: Footnotes
738 .. [#f8] `examples/simple_bundle_adjuster.cc
739 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/simple_bundle_adjuster.cc>`_
744 Besides the examples in this chapter, the `example
745 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/>`_
746 directory contains a number of other examples:
748 #. `bundle_adjuster.cc
749 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/bundle_adjuster.cc>`_
750 shows how to use the various features of Ceres to solve bundle
754 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/circle_fit.cc>`_
755 shows how to fit data to a circle.
757 #. `ellipse_approximation.cc
758 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/ellipse_approximation.cc>`_
759 fits points randomly distributed on an ellipse with an approximate
760 line segment contour. This is done by jointly optimizing the
761 control points of the line segment contour along with the preimage
762 positions for the data points. The purpose of this example is to
763 show an example use case for ``Solver::Options::dynamic_sparsity``,
764 and how it can benefit problems which are numerically dense but
768 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/denoising.cc>`_
769 implements image denoising using the `Fields of Experts
770 <http://www.gris.informatik.tu-darmstadt.de/~sroth/research/foe/index.html>`_
774 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/nist.cc>`_
775 implements and attempts to solves the `NIST
776 <http://www.itl.nist.gov/div898/strd/nls/nls_main.shtm>`_
777 non-linear regression problems.
779 #. `more_garbow_hillstrom.cc
780 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/more_garbow_hillstrom.cc>`_
781 A subset of the test problems from the paper
783 Testing Unconstrained Optimization Software
784 Jorge J. More, Burton S. Garbow and Kenneth E. Hillstrom
785 ACM Transactions on Mathematical Software, 7(1), pp. 17-41, 1981
787 which were augmented with bounds and used for testing bounds
788 constrained optimization algorithms by
790 A Trust Region Approach to Linearly Constrained Optimization
792 Numerical Analysis (Griffiths, D.F., ed.), pp. 72-105
793 Lecture Notes in Mathematics 1066, Springer Verlag, 1984.
796 #. `libmv_bundle_adjuster.cc
797 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/libmv_bundle_adjuster.cc>`_
798 is the bundle adjustment algorithm used by `Blender <www.blender.org>`_/libmv.
800 #. `libmv_homography.cc
801 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/libmv_homography.cc>`_
802 This file demonstrates solving for a homography between two sets of
803 points and using a custom exit criterion by having a callback check
804 for image-space error.
806 #. `robot_pose_mle.cc
807 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/robot_pose_mle.cc>`_
808 This example demonstrates how to use the ``DynamicAutoDiffCostFunction``
809 variant of CostFunction. The ``DynamicAutoDiffCostFunction`` is meant to
810 be used in cases where the number of parameter blocks or the sizes are not
811 known at compile time.
813 This example simulates a robot traversing down a 1-dimension hallway with
814 noise odometry readings and noisy range readings of the end of the hallway.
815 By fusing the noisy odometry and sensor readings this example demonstrates
816 how to compute the maximum likelihood estimate (MLE) of the robot's pose at
819 #. `slam/pose_graph_2d/pose_graph_2d.cc
820 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_2d/pose_graph_2d.cc>`_
821 The Simultaneous Localization and Mapping (SLAM) problem consists of building
822 a map of an unknown environment while simultaneously localizing against this
823 map. The main difficulty of this problem stems from not having any additional
824 external aiding information such as GPS. SLAM has been considered one of the
825 fundamental challenges of robotics. There are many resources on SLAM
826 [#f9]_. A pose graph optimization problem is one example of a SLAM
827 problem. The following explains how to formulate the pose graph based SLAM
828 problem in 2-Dimensions with relative pose constraints.
830 Consider a robot moving in a 2-Dimensional plane. The robot has access to a
831 set of sensors such as wheel odometry or a laser range scanner. From these
832 raw measurements, we want to estimate the trajectory of the robot as well as
833 build a map of the environment. In order to reduce the computational
834 complexity of the problem, the pose graph approach abstracts the raw
835 measurements away. Specifically, it creates a graph of nodes which represent
836 the pose of the robot, and edges which represent the relative transformation
837 (delta position and orientation) between the two nodes. The edges are virtual
838 measurements derived from the raw sensor measurements, e.g. by integrating
839 the raw wheel odometry or aligning the laser range scans acquired from the
840 robot. A visualization of the resulting graph is shown below.
842 .. figure:: slam2d.png
847 Visual representation of a graph SLAM problem.
849 The figure depicts the pose of the robot as the triangles, the measurements
850 are indicated by the connecting lines, and the loop closure measurements are
851 shown as dotted lines. Loop closures are measurements between non-sequential
852 robot states and they reduce the accumulation of error over time. The
853 following will describe the mathematical formulation of the pose graph
856 The robot at timestamp :math:`t` has state :math:`x_t = [p^T, \psi]^T` where
857 :math:`p` is a 2D vector that represents the position in the plane and
858 :math:`\psi` is the orientation in radians. The measurement of the relative
859 transform between the robot state at two timestamps :math:`a` and :math:`b`
860 is given as: :math:`z_{ab} = [\hat{p}_{ab}^T, \hat{\psi}_{ab}]`. The residual
861 implemented in the Ceres cost function which computes the error between the
862 measurement and the predicted measurement is:
867 R_a^T\left(p_b - p_a\right) - \hat{p}_{ab} \\
868 \mathrm{Normalize}\left(\psi_b - \psi_a - \hat{\psi}_{ab}\right)
872 where the function :math:`\mathrm{Normalize}()` normalizes the angle in the range
873 :math:`[-\pi,\pi)`, and :math:`R` is the rotation matrix given by
878 \cos \psi_a & -\sin \psi_a \\
879 \sin \psi_a & \cos \psi_a \\
883 To finish the cost function, we need to weight the residual by the
884 uncertainty of the measurement. Hence, we pre-multiply the residual by the
885 inverse square root of the covariance matrix for the measurement,
886 i.e. :math:`\Sigma_{ab}^{-\frac{1}{2}} r_{ab}` where :math:`\Sigma_{ab}` is
889 Lastly, we use a local parameterization to normalize the orientation in the
890 range which is normalized between :math:`[-\pi,\pi)`. Specially, we define
891 the :member:`AngleLocalParameterization::operator()` function to be:
892 :math:`\mathrm{Normalize}(\psi + \delta \psi)`.
894 This package includes an executable :member:`pose_graph_2d` that will read a
895 problem definition file. This executable can work with any 2D problem
896 definition that uses the g2o format. It would be relatively straightforward
897 to implement a new reader for a different format such as TORO or
898 others. :member:`pose_graph_2d` will print the Ceres solver full summary and
899 then output to disk the original and optimized poses (``poses_original.txt``
900 and ``poses_optimized.txt``, respectively) of the robot in the following
905 pose_id x y yaw_radians
906 pose_id x y yaw_radians
907 pose_id x y yaw_radians
909 where ``pose_id`` is the corresponding integer ID from the file
910 definition. Note, the file will be sorted in ascending order for the
913 The executable :member:`pose_graph_2d` expects the first argument to be
914 the path to the problem definition. To run the executable,
918 /path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o
920 A python script is provided to visualize the resulting output files.
924 /path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
926 As an example, a standard synthetic benchmark dataset [#f10]_ created by
927 Edwin Olson which has 3500 nodes in a grid world with a total of 5598 edges
928 was solved. Visualizing the results with the provided script produces:
930 .. figure:: manhattan_olson_3500_result.png
935 with the original poses in green and the optimized poses in blue. As shown,
936 the optimized poses more closely match the underlying grid world. Note, the
937 left side of the graph has a small yaw drift due to a lack of relative
938 constraints to provide enough information to reconstruct the trajectory.
940 .. rubric:: Footnotes
942 .. [#f9] Giorgio Grisetti, Rainer Kummerle, Cyrill Stachniss, Wolfram
943 Burgard. A Tutorial on Graph-Based SLAM. IEEE Intelligent Transportation
944 Systems Magazine, 52(3):199–222, 2010.
946 .. [#f10] E. Olson, J. Leonard, and S. Teller, “Fast iterative optimization of
947 pose graphs with poor initial estimates,” in Robotics and Automation
948 (ICRA), IEEE International Conference on, 2006, pp. 2262–2269.
950 #. `slam/pose_graph_3d/pose_graph_3d.cc
951 <https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_3d/pose_graph_3d.cc>`_
952 The following explains how to formulate the pose graph based SLAM problem in
953 3-Dimensions with relative pose constraints. The example also illustrates how
954 to use Eigen's geometry module with Ceres's automatic differentiation
957 The robot at timestamp :math:`t` has state :math:`x_t = [p^T, q^T]^T` where
958 :math:`p` is a 3D vector that represents the position and :math:`q` is the
959 orientation represented as an Eigen quaternion. The measurement of the
960 relative transform between the robot state at two timestamps :math:`a` and
961 :math:`b` is given as: :math:`z_{ab} = [\hat{p}_{ab}^T, \hat{q}_{ab}^T]^T`.
962 The residual implemented in the Ceres cost function which computes the error
963 between the measurement and the predicted measurement is:
968 R(q_a)^{T} (p_b - p_a) - \hat{p}_{ab} \\
969 2.0 \mathrm{vec}\left((q_a^{-1} q_b) \hat{q}_{ab}^{-1}\right)
973 where the function :math:`\mathrm{vec}()` returns the vector part of the
974 quaternion, i.e. :math:`[q_x, q_y, q_z]`, and :math:`R(q)` is the rotation
975 matrix for the quaternion.
977 To finish the cost function, we need to weight the residual by the
978 uncertainty of the measurement. Hence, we pre-multiply the residual by the
979 inverse square root of the covariance matrix for the measurement,
980 i.e. :math:`\Sigma_{ab}^{-\frac{1}{2}} r_{ab}` where :math:`\Sigma_{ab}` is
983 Given that we are using a quaternion to represent the orientation, we need to
984 use a local parameterization (:class:`EigenQuaternionParameterization`) to
985 only apply updates orthogonal to the 4-vector defining the
986 quaternion. Eigen's quaternion uses a different internal memory layout for
987 the elements of the quaternion than what is commonly used. Specifically,
988 Eigen stores the elements in memory as :math:`[x, y, z, w]` where the real
989 part is last whereas it is typically stored first. Note, when creating an
990 Eigen quaternion through the constructor the elements are accepted in
991 :math:`w`, :math:`x`, :math:`y`, :math:`z` order. Since Ceres operates on
992 parameter blocks which are raw double pointers this difference is important
993 and requires a different parameterization.
995 This package includes an executable :member:`pose_graph_3d` that will read a
996 problem definition file. This executable can work with any 3D problem
997 definition that uses the g2o format with quaternions used for the orientation
998 representation. It would be relatively straightforward to implement a new
999 reader for a different format such as TORO or others. :member:`pose_graph_3d`
1000 will print the Ceres solver full summary and then output to disk the original
1001 and optimized poses (``poses_original.txt`` and ``poses_optimized.txt``,
1002 respectively) of the robot in the following format:
1004 .. code-block:: bash
1006 pose_id x y z q_x q_y q_z q_w
1007 pose_id x y z q_x q_y q_z q_w
1008 pose_id x y z q_x q_y q_z q_w
1011 where ``pose_id`` is the corresponding integer ID from the file
1012 definition. Note, the file will be sorted in ascending order for the
1015 The executable :member:`pose_graph_3d` expects the first argument to be the
1016 path to the problem definition. The executable can be run via
1018 .. code-block:: bash
1020 /path/to/bin/pose_graph_3d /path/to/dataset/dataset.g2o
1022 A script is provided to visualize the resulting output files. There is also
1023 an option to enable equal axes using ``--axes_equal``
1025 .. code-block:: bash
1027 /path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
1029 As an example, a standard synthetic benchmark dataset [#f9]_ where the robot is
1030 traveling on the surface of a sphere which has 2500 nodes with a total of
1031 4949 edges was solved. Visualizing the results with the provided script
1034 .. figure:: pose_graph_3d_ex.png