<!DOCTYPE html>
<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]-->
<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]-->
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>Non-linear Least Squares — Ceres Solver</title>
<link rel="stylesheet" href="_static/css/theme.css" type="text/css" />
<link rel="top" title="Ceres Solver" href="index.html"/>
<link rel="up" title="Tutorial" href="tutorial.html"/>
<link rel="next" title="General Unconstrained Minimization" href="gradient_tutorial.html"/>
<link rel="prev" title="Tutorial" href="tutorial.html"/>
<script src="_static/js/modernizr.min.js"></script>
</head>
<body class="wy-body-for-nav" role="document">
<div class="wy-grid-for-nav">
<nav data-toggle="wy-nav-shift" class="wy-nav-side">
<div class="wy-side-scroll">
<div class="wy-side-nav-search">
<a href="index.html" class="icon icon-home"> Ceres Solver
</a>
<div class="version">
1.13
</div>
<div role="search">
<form id="rtd-search-form" class="wy-form" action="search.html" method="get">
<input type="text" name="q" placeholder="Search docs" />
<input type="hidden" name="check_keywords" value="yes" />
<input type="hidden" name="area" value="default" />
</form>
</div>
</div>
<div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
<ul class="current">
<li class="toctree-l1"><a class="reference internal" href="features.html">Why?</a></li>
<li class="toctree-l1"><a class="reference internal" href="installation.html">Installation</a></li>
<li class="toctree-l1 current"><a class="reference internal" href="tutorial.html">Tutorial</a><ul class="current">
<li class="toctree-l2 current"><a class="current reference internal" href="#">Non-linear Least Squares</a><ul>
<li class="toctree-l3"><a class="reference internal" href="#introduction">Introduction</a></li>
<li class="toctree-l3"><a class="reference internal" href="#hello-world">Hello World!</a></li>
<li class="toctree-l3"><a class="reference internal" href="#derivatives">Derivatives</a><ul>
<li class="toctree-l4"><a class="reference internal" href="#numeric-derivatives">Numeric Derivatives</a></li>
<li class="toctree-l4"><a class="reference internal" href="#analytic-derivatives">Analytic Derivatives</a></li>
<li class="toctree-l4"><a class="reference internal" href="#more-about-derivatives">More About Derivatives</a></li>
</ul>
</li>
<li class="toctree-l3"><a class="reference internal" href="#powell-s-function">Powell’s Function</a></li>
<li class="toctree-l3"><a class="reference internal" href="#curve-fitting">Curve Fitting</a></li>
<li class="toctree-l3"><a class="reference internal" href="#robust-curve-fitting">Robust Curve Fitting</a></li>
<li class="toctree-l3"><a class="reference internal" href="#bundle-adjustment">Bundle Adjustment</a></li>
<li class="toctree-l3"><a class="reference internal" href="#other-examples">Other Examples</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="gradient_tutorial.html">General Unconstrained Minimization</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="derivatives.html">On Derivatives</a></li>
<li class="toctree-l1"><a class="reference internal" href="nnls_modeling.html">Modeling Non-linear Least Squares</a></li>
<li class="toctree-l1"><a class="reference internal" href="nnls_solving.html">Solving Non-linear Least Squares</a></li>
<li class="toctree-l1"><a class="reference internal" href="nnls_covariance.html">Covariance Estimation</a></li>
<li class="toctree-l1"><a class="reference internal" href="gradient_solver.html">General Unconstrained Minimization</a></li>
<li class="toctree-l1"><a class="reference internal" href="faqs.html">FAQS, Tips & Tricks</a></li>
<li class="toctree-l1"><a class="reference internal" href="users.html">Users</a></li>
<li class="toctree-l1"><a class="reference internal" href="contributing.html">Contributing</a></li>
<li class="toctree-l1"><a class="reference internal" href="version_history.html">Version History</a></li>
<li class="toctree-l1"><a class="reference internal" href="bibliography.html">Bibliography</a></li>
<li class="toctree-l1"><a class="reference internal" href="license.html">License</a></li>
</ul>
</div>
</div>
</nav>
<section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">
<nav class="wy-nav-top" role="navigation" aria-label="top navigation">
<i data-toggle="wy-nav-top" class="fa fa-bars"></i>
<a href="index.html">Ceres Solver</a>
</nav>
<div class="wy-nav-content">
<div class="rst-content">
<div role="navigation" aria-label="breadcrumbs navigation">
<ul class="wy-breadcrumbs">
<li><a href="index.html">Docs</a> »</li>
<li><a href="tutorial.html">Tutorial</a> »</li>
<li>Non-linear Least Squares</li>
<li class="wy-breadcrumbs-aside">
</li>
</ul>
<hr/>
</div>
<div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
<div itemprop="articleBody">
<div class="section" id="non-linear-least-squares">
<span id="chapter-nnls-tutorial"></span><h1>Non-linear Least Squares<a class="headerlink" href="#non-linear-least-squares" title="Permalink to this headline">¶</a></h1>
<div class="section" id="introduction">
<h2>Introduction<a class="headerlink" href="#introduction" title="Permalink to this headline">¶</a></h2>
<p>Ceres can solve bounds constrained robustified non-linear least
squares problems of the form</p>
<div class="math" id="equation-ceresproblem">
<span class="eqno">(1)</span>\[\begin{split}\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\end{split}\]</div>
<p>Problems of this form comes up in a broad range of areas across
science and engineering - from <a class="reference external" href="http://en.wikipedia.org/wiki/Nonlinear_regression">fitting curves</a> in statistics, to
constructing <a class="reference external" href="http://en.wikipedia.org/wiki/Bundle_adjustment">3D models from photographs</a> in computer vision.</p>
<p>In this chapter we will learn how to solve <a href="#equation-ceresproblem">(1)</a> using
Ceres Solver. Full working code for all the examples described in this
chapter and more can be found in the <a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/">examples</a>
directory.</p>
<p>The expression
<span class="math">\(\rho_i\left(\left\|f_i\left(x_{i_1},...,x_{i_k}\right)\right\|^2\right)\)</span>
is known as a <code class="docutils literal"><span class="pre">ResidualBlock</span></code>, where <span class="math">\(f_i(\cdot)\)</span> is a
<code class="xref cpp cpp-class docutils literal"><span class="pre">CostFunction</span></code> that depends on the parameter blocks
<span class="math">\(\left[x_{i_1},... , x_{i_k}\right]\)</span>. 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 <code class="docutils literal"><span class="pre">ParameterBlock</span></code>. Of course a
<code class="docutils literal"><span class="pre">ParameterBlock</span></code> can just be a single parameter. <span class="math">\(l_j\)</span> and
<span class="math">\(u_j\)</span> are bounds on the parameter block <span class="math">\(x_j\)</span>.</p>
<p><span class="math">\(\rho_i\)</span> is a <code class="xref cpp cpp-class docutils literal"><span class="pre">LossFunction</span></code>. A <code class="xref cpp cpp-class docutils literal"><span class="pre">LossFunction</span></code> is
a scalar function that is used to reduce the influence of outliers on
the solution of non-linear least squares problems.</p>
<p>As a special case, when <span class="math">\(\rho_i(x) = x\)</span>, i.e., the identity
function, and <span class="math">\(l_j = -\infty\)</span> and <span class="math">\(u_j = \infty\)</span> we get
the more familiar <a class="reference external" href="http://en.wikipedia.org/wiki/Non-linear_least_squares">non-linear least squares problem</a>.</p>
<div class="math" id="equation-ceresproblem2">
<span class="eqno">(2)</span>\[\frac{1}{2}\sum_{i} \left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2.\]</div>
</div>
<div class="section" id="hello-world">
<span id="section-hello-world"></span><h2>Hello World!<a class="headerlink" href="#hello-world" title="Permalink to this headline">¶</a></h2>
<p>To get started, consider the problem of finding the minimum of the
function</p>
<div class="math">
\[\frac{1}{2}(10 -x)^2.\]</div>
<p>This is a trivial problem, whose minimum is located at <span class="math">\(x = 10\)</span>,
but it is a good place to start to illustrate the basics of solving a
problem with Ceres <a class="footnote-reference" href="#f1" id="id1">[1]</a>.</p>
<p>The first step is to write a functor that will evaluate this the
function <span class="math">\(f(x) = 10 - x\)</span>:</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="k">struct</span> <span class="n">CostFunctor</span> <span class="p">{</span>
<span class="k">template</span> <span class="o"><</span><span class="k">typename</span> <span class="n">T</span><span class="o">></span>
<span class="kt">bool</span> <span class="k">operator</span><span class="p">()(</span><span class="k">const</span> <span class="n">T</span><span class="o">*</span> <span class="k">const</span> <span class="n">x</span><span class="p">,</span> <span class="n">T</span><span class="o">*</span> <span class="n">residual</span><span class="p">)</span> <span class="k">const</span> <span class="p">{</span>
<span class="n">residual</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">T</span><span class="p">(</span><span class="mf">10.0</span><span class="p">)</span> <span class="o">-</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">];</span>
<span class="k">return</span> <span class="nb">true</span><span class="p">;</span>
<span class="p">}</span>
<span class="p">};</span>
</pre></div>
</div>
<p>The important thing to note here is that <code class="docutils literal"><span class="pre">operator()</span></code> is a templated
method, which assumes that all its inputs and outputs are of some type
<code class="docutils literal"><span class="pre">T</span></code>. The use of templating here allows Ceres to call
<code class="docutils literal"><span class="pre">CostFunctor::operator<T>()</span></code>, with <code class="docutils literal"><span class="pre">T=double</span></code> when just the value
of the residual is needed, and with a special type <code class="docutils literal"><span class="pre">T=Jet</span></code> when the
Jacobians are needed. In <a class="reference internal" href="#section-derivatives"><span class="std std-ref">Derivatives</span></a> we will discuss the
various ways of supplying derivatives to Ceres in more detail.</p>
<p>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.</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="kt">int</span> <span class="nf">main</span><span class="p">(</span><span class="kt">int</span> <span class="n">argc</span><span class="p">,</span> <span class="kt">char</span><span class="o">**</span> <span class="n">argv</span><span class="p">)</span> <span class="p">{</span>
<span class="n">google</span><span class="o">::</span><span class="n">InitGoogleLogging</span><span class="p">(</span><span class="n">argv</span><span class="p">[</span><span class="mi">0</span><span class="p">]);</span>
<span class="c1">// The variable to solve for with its initial value.</span>
<span class="kt">double</span> <span class="n">initial_x</span> <span class="o">=</span> <span class="mf">5.0</span><span class="p">;</span>
<span class="kt">double</span> <span class="n">x</span> <span class="o">=</span> <span class="n">initial_x</span><span class="p">;</span>
<span class="c1">// Build the problem.</span>
<span class="n">Problem</span> <span class="n">problem</span><span class="p">;</span>
<span class="c1">// Set up the only cost function (also known as residual). This uses</span>
<span class="c1">// auto-differentiation to obtain the derivative (jacobian).</span>
<span class="n">CostFunction</span><span class="o">*</span> <span class="n">cost_function</span> <span class="o">=</span>
<span class="k">new</span> <span class="n">AutoDiffCostFunction</span><span class="o"><</span><span class="n">CostFunctor</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="o">></span><span class="p">(</span><span class="k">new</span> <span class="n">CostFunctor</span><span class="p">);</span>
<span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span><span class="n">cost_function</span><span class="p">,</span> <span class="nb">NULL</span><span class="p">,</span> <span class="o">&</span><span class="n">x</span><span class="p">);</span>
<span class="c1">// Run the solver!</span>
<span class="n">Solver</span><span class="o">::</span><span class="n">Options</span> <span class="n">options</span><span class="p">;</span>
<span class="n">options</span><span class="p">.</span><span class="n">linear_solver_type</span> <span class="o">=</span> <span class="n">ceres</span><span class="o">::</span><span class="n">DENSE_QR</span><span class="p">;</span>
<span class="n">options</span><span class="p">.</span><span class="n">minimizer_progress_to_stdout</span> <span class="o">=</span> <span class="nb">true</span><span class="p">;</span>
<span class="n">Solver</span><span class="o">::</span><span class="n">Summary</span> <span class="n">summary</span><span class="p">;</span>
<span class="n">Solve</span><span class="p">(</span><span class="n">options</span><span class="p">,</span> <span class="o">&</span><span class="n">problem</span><span class="p">,</span> <span class="o">&</span><span class="n">summary</span><span class="p">);</span>
<span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="n">summary</span><span class="p">.</span><span class="n">BriefReport</span><span class="p">()</span> <span class="o"><<</span> <span class="s">"</span><span class="se">\n</span><span class="s">"</span><span class="p">;</span>
<span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="s">"x : "</span> <span class="o"><<</span> <span class="n">initial_x</span>
<span class="o"><<</span> <span class="s">" -> "</span> <span class="o"><<</span> <span class="n">x</span> <span class="o"><<</span> <span class="s">"</span><span class="se">\n</span><span class="s">"</span><span class="p">;</span>
<span class="k">return</span> <span class="mi">0</span><span class="p">;</span>
<span class="p">}</span>
</pre></div>
</div>
<p><code class="xref cpp cpp-class docutils literal"><span class="pre">AutoDiffCostFunction</span></code> takes a <code class="docutils literal"><span class="pre">CostFunctor</span></code> as input,
automatically differentiates it and gives it a <code class="xref cpp cpp-class docutils literal"><span class="pre">CostFunction</span></code>
interface.</p>
<p>Compiling and running <a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld.cc">examples/helloworld.cc</a>
gives us</p>
<div class="highlight-bash"><div class="highlight"><pre><span></span>iter cost cost_change <span class="p">|</span>gradient<span class="p">|</span> <span class="p">|</span>step<span class="p">|</span> tr_ratio tr_radius ls_iter iter_time total_time
<span class="m">0</span> 4.512500e+01 0.00e+00 9.50e+00 0.00e+00 0.00e+00 1.00e+04 <span class="m">0</span> 5.33e-04 3.46e-03
<span class="m">1</span> 4.511598e-07 4.51e+01 9.50e-04 9.50e+00 1.00e+00 3.00e+04 <span class="m">1</span> 5.00e-04 4.05e-03
<span class="m">2</span> 5.012552e-16 4.51e-07 3.17e-08 9.50e-04 1.00e+00 9.00e+04 <span class="m">1</span> 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
</pre></div>
</div>
<p>Starting from a <span class="math">\(x=5\)</span>, the solver in two iterations goes to 10
<a class="footnote-reference" href="#f2" id="id2">[2]</a>. 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.</p>
<p class="rubric">Footnotes</p>
<table class="docutils footnote" frame="void" id="f1" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label"><a class="fn-backref" href="#id1">[1]</a></td><td><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld.cc">examples/helloworld.cc</a></td></tr>
</tbody>
</table>
<table class="docutils footnote" frame="void" id="f2" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label"><a class="fn-backref" href="#id2">[2]</a></td><td>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.</td></tr>
</tbody>
</table>
</div>
<div class="section" id="derivatives">
<span id="section-derivatives"></span><h2>Derivatives<a class="headerlink" href="#derivatives" title="Permalink to this headline">¶</a></h2>
<p>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 <a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld.cc">examples/helloworld.cc</a></p>
<p>We now consider the other two possibilities. Analytic and numeric
derivatives.</p>
<div class="section" id="numeric-derivatives">
<h3>Numeric Derivatives<a class="headerlink" href="#numeric-derivatives" title="Permalink to this headline">¶</a></h3>
<p>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
<code class="xref cpp cpp-class docutils literal"><span class="pre">NumericDiffCostFunction</span></code> using it. e.g., for <span class="math">\(f(x) = 10 - x\)</span>
the corresponding functor would be</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="k">struct</span> <span class="n">NumericDiffCostFunctor</span> <span class="p">{</span>
<span class="kt">bool</span> <span class="k">operator</span><span class="p">()(</span><span class="k">const</span> <span class="kt">double</span><span class="o">*</span> <span class="k">const</span> <span class="n">x</span><span class="p">,</span> <span class="kt">double</span><span class="o">*</span> <span class="n">residual</span><span class="p">)</span> <span class="k">const</span> <span class="p">{</span>
<span class="n">residual</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mf">10.0</span> <span class="o">-</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">];</span>
<span class="k">return</span> <span class="nb">true</span><span class="p">;</span>
<span class="p">}</span>
<span class="p">};</span>
</pre></div>
</div>
<p>Which is added to the <code class="xref cpp cpp-class docutils literal"><span class="pre">Problem</span></code> as:</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="n">CostFunction</span><span class="o">*</span> <span class="n">cost_function</span> <span class="o">=</span>
<span class="k">new</span> <span class="n">NumericDiffCostFunction</span><span class="o"><</span><span class="n">NumericDiffCostFunctor</span><span class="p">,</span> <span class="n">ceres</span><span class="o">::</span><span class="n">CENTRAL</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="o">></span><span class="p">(</span>
<span class="k">new</span> <span class="n">NumericDiffCostFunctor</span><span class="p">);</span>
<span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span><span class="n">cost_function</span><span class="p">,</span> <span class="nb">NULL</span><span class="p">,</span> <span class="o">&</span><span class="n">x</span><span class="p">);</span>
</pre></div>
</div>
<p>Notice the parallel from when we were using automatic differentiation</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="n">CostFunction</span><span class="o">*</span> <span class="n">cost_function</span> <span class="o">=</span>
<span class="k">new</span> <span class="n">AutoDiffCostFunction</span><span class="o"><</span><span class="n">CostFunctor</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="o">></span><span class="p">(</span><span class="k">new</span> <span class="n">CostFunctor</span><span class="p">);</span>
<span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span><span class="n">cost_function</span><span class="p">,</span> <span class="nb">NULL</span><span class="p">,</span> <span class="o">&</span><span class="n">x</span><span class="p">);</span>
</pre></div>
</div>
<p>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 <a class="footnote-reference" href="#f3" id="id5">[3]</a>. For more details see the documentation
for <code class="xref cpp cpp-class docutils literal"><span class="pre">NumericDiffCostFunction</span></code>.</p>
<p><strong>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.</strong></p>
</div>
<div class="section" id="analytic-derivatives">
<h3>Analytic Derivatives<a class="headerlink" href="#analytic-derivatives" title="Permalink to this headline">¶</a></h3>
<p>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.</p>
<p>In such cases, it is possible to supply your own residual and jacobian
computation code. To do this, define a subclass of
<code class="xref cpp cpp-class docutils literal"><span class="pre">CostFunction</span></code> or <code class="xref cpp cpp-class docutils literal"><span class="pre">SizedCostFunction</span></code> if you know the
sizes of the parameters and residuals at compile time. Here for
example is <code class="docutils literal"><span class="pre">SimpleCostFunction</span></code> that implements <span class="math">\(f(x) = 10 -
x\)</span>.</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="k">class</span> <span class="nc">QuadraticCostFunction</span> <span class="o">:</span> <span class="k">public</span> <span class="n">ceres</span><span class="o">::</span><span class="n">SizedCostFunction</span><span class="o"><</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="o">></span> <span class="p">{</span>
<span class="k">public</span><span class="o">:</span>
<span class="k">virtual</span> <span class="o">~</span><span class="n">QuadraticCostFunction</span><span class="p">()</span> <span class="p">{}</span>
<span class="k">virtual</span> <span class="kt">bool</span> <span class="n">Evaluate</span><span class="p">(</span><span class="kt">double</span> <span class="k">const</span><span class="o">*</span> <span class="k">const</span><span class="o">*</span> <span class="n">parameters</span><span class="p">,</span>
<span class="kt">double</span><span class="o">*</span> <span class="n">residuals</span><span class="p">,</span>
<span class="kt">double</span><span class="o">**</span> <span class="n">jacobians</span><span class="p">)</span> <span class="k">const</span> <span class="p">{</span>
<span class="k">const</span> <span class="kt">double</span> <span class="n">x</span> <span class="o">=</span> <span class="n">parameters</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">0</span><span class="p">];</span>
<span class="n">residuals</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="mi">10</span> <span class="o">-</span> <span class="n">x</span><span class="p">;</span>
<span class="c1">// Compute the Jacobian if asked for.</span>
<span class="k">if</span> <span class="p">(</span><span class="n">jacobians</span> <span class="o">!=</span> <span class="nb">NULL</span> <span class="o">&&</span> <span class="n">jacobians</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">!=</span> <span class="nb">NULL</span><span class="p">)</span> <span class="p">{</span>
<span class="n">jacobians</span><span class="p">[</span><span class="mi">0</span><span class="p">][</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="mi">1</span><span class="p">;</span>
<span class="p">}</span>
<span class="k">return</span> <span class="nb">true</span><span class="p">;</span>
<span class="p">}</span>
<span class="p">};</span>
</pre></div>
</div>
<p><code class="docutils literal"><span class="pre">SimpleCostFunction::Evaluate</span></code> is provided with an input array of
<code class="docutils literal"><span class="pre">parameters</span></code>, an output array <code class="docutils literal"><span class="pre">residuals</span></code> for residuals and an
output array <code class="docutils literal"><span class="pre">jacobians</span></code> for Jacobians. The <code class="docutils literal"><span class="pre">jacobians</span></code> array is
optional, <code class="docutils literal"><span class="pre">Evaluate</span></code> 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 <a class="footnote-reference" href="#f4" id="id6">[4]</a> .</p>
<p>As can be seen from the above code fragments, implementing
<code class="xref cpp cpp-class docutils literal"><span class="pre">CostFunction</span></code> objects is a bit tedious. We recommend that
unless you have a good reason to manage the jacobian computation
yourself, you use <code class="xref cpp cpp-class docutils literal"><span class="pre">AutoDiffCostFunction</span></code> or
<code class="xref cpp cpp-class docutils literal"><span class="pre">NumericDiffCostFunction</span></code> to construct your residual blocks.</p>
</div>
<div class="section" id="more-about-derivatives">
<h3>More About Derivatives<a class="headerlink" href="#more-about-derivatives" title="Permalink to this headline">¶</a></h3>
<p>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
<code class="xref cpp cpp-class docutils literal"><span class="pre">NumericDiffCostFunction</span></code> and <code class="xref cpp cpp-class docutils literal"><span class="pre">AutoDiffCostFunction</span></code> we
recommend taking a look at <code class="xref cpp cpp-class docutils literal"><span class="pre">DynamicAutoDiffCostFunction</span></code>,
<code class="xref cpp cpp-class docutils literal"><span class="pre">CostFunctionToFunctor</span></code>, <code class="xref cpp cpp-class docutils literal"><span class="pre">NumericDiffFunctor</span></code> and
<code class="xref cpp cpp-class docutils literal"><span class="pre">ConditionedCostFunction</span></code> for more advanced ways of
constructing and computing cost functions.</p>
<p class="rubric">Footnotes</p>
<table class="docutils footnote" frame="void" id="f3" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label"><a class="fn-backref" href="#id5">[3]</a></td><td><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld_numeric_diff.cc">examples/helloworld_numeric_diff.cc</a>.</td></tr>
</tbody>
</table>
<table class="docutils footnote" frame="void" id="f4" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label"><a class="fn-backref" href="#id6">[4]</a></td><td><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/helloworld_analytic_diff.cc">examples/helloworld_analytic_diff.cc</a>.</td></tr>
</tbody>
</table>
</div>
</div>
<div class="section" id="powell-s-function">
<span id="section-powell"></span><h2>Powell’s Function<a class="headerlink" href="#powell-s-function" title="Permalink to this headline">¶</a></h2>
<p>Consider now a slightly more complicated example – the minimization
of Powell’s function. Let <span class="math">\(x = \left[x_1, x_2, x_3, x_4 \right]\)</span>
and</p>
<div class="math">
\[\begin{split}\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}\end{split}\]</div>
<p><span class="math">\(F(x)\)</span> is a function of four parameters, has four residuals
and we wish to find <span class="math">\(x\)</span> such that <span class="math">\(\frac{1}{2}\|F(x)\|^2\)</span>
is minimized.</p>
<p>Again, the first step is to define functors that evaluate of the terms
in the objective functor. Here is the code for evaluating
<span class="math">\(f_4(x_1, x_4)\)</span>:</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="k">struct</span> <span class="n">F4</span> <span class="p">{</span>
<span class="k">template</span> <span class="o"><</span><span class="k">typename</span> <span class="n">T</span><span class="o">></span>
<span class="kt">bool</span> <span class="k">operator</span><span class="p">()(</span><span class="k">const</span> <span class="n">T</span><span class="o">*</span> <span class="k">const</span> <span class="n">x1</span><span class="p">,</span> <span class="k">const</span> <span class="n">T</span><span class="o">*</span> <span class="k">const</span> <span class="n">x4</span><span class="p">,</span> <span class="n">T</span><span class="o">*</span> <span class="n">residual</span><span class="p">)</span> <span class="k">const</span> <span class="p">{</span>
<span class="n">residual</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">T</span><span class="p">(</span><span class="n">sqrt</span><span class="p">(</span><span class="mf">10.0</span><span class="p">))</span> <span class="o">*</span> <span class="p">(</span><span class="n">x1</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">x4</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">*</span> <span class="p">(</span><span class="n">x1</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">x4</span><span class="p">[</span><span class="mi">0</span><span class="p">]);</span>
<span class="k">return</span> <span class="nb">true</span><span class="p">;</span>
<span class="p">}</span>
<span class="p">};</span>
</pre></div>
</div>
<p>Similarly, we can define classes <code class="docutils literal"><span class="pre">F1</span></code>, <code class="docutils literal"><span class="pre">F2</span></code> and <code class="docutils literal"><span class="pre">F3</span></code> to evaluate
<span class="math">\(f_1(x_1, x_2)\)</span>, <span class="math">\(f_2(x_3, x_4)\)</span> and <span class="math">\(f_3(x_2, x_3)\)</span>
respectively. Using these, the problem can be constructed as follows:</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="kt">double</span> <span class="n">x1</span> <span class="o">=</span> <span class="mf">3.0</span><span class="p">;</span> <span class="kt">double</span> <span class="n">x2</span> <span class="o">=</span> <span class="o">-</span><span class="mf">1.0</span><span class="p">;</span> <span class="kt">double</span> <span class="n">x3</span> <span class="o">=</span> <span class="mf">0.0</span><span class="p">;</span> <span class="kt">double</span> <span class="n">x4</span> <span class="o">=</span> <span class="mf">1.0</span><span class="p">;</span>
<span class="n">Problem</span> <span class="n">problem</span><span class="p">;</span>
<span class="c1">// Add residual terms to the problem using the using the autodiff</span>
<span class="c1">// wrapper to get the derivatives automatically.</span>
<span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span>
<span class="k">new</span> <span class="n">AutoDiffCostFunction</span><span class="o"><</span><span class="n">F1</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="o">></span><span class="p">(</span><span class="k">new</span> <span class="n">F1</span><span class="p">),</span> <span class="nb">NULL</span><span class="p">,</span> <span class="o">&</span><span class="n">x1</span><span class="p">,</span> <span class="o">&</span><span class="n">x2</span><span class="p">);</span>
<span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span>
<span class="k">new</span> <span class="n">AutoDiffCostFunction</span><span class="o"><</span><span class="n">F2</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="o">></span><span class="p">(</span><span class="k">new</span> <span class="n">F2</span><span class="p">),</span> <span class="nb">NULL</span><span class="p">,</span> <span class="o">&</span><span class="n">x3</span><span class="p">,</span> <span class="o">&</span><span class="n">x4</span><span class="p">);</span>
<span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span>
<span class="k">new</span> <span class="n">AutoDiffCostFunction</span><span class="o"><</span><span class="n">F3</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="o">></span><span class="p">(</span><span class="k">new</span> <span class="n">F3</span><span class="p">),</span> <span class="nb">NULL</span><span class="p">,</span> <span class="o">&</span><span class="n">x2</span><span class="p">,</span> <span class="o">&</span><span class="n">x3</span><span class="p">)</span>
<span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span>
<span class="k">new</span> <span class="n">AutoDiffCostFunction</span><span class="o"><</span><span class="n">F4</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="o">></span><span class="p">(</span><span class="k">new</span> <span class="n">F4</span><span class="p">),</span> <span class="nb">NULL</span><span class="p">,</span> <span class="o">&</span><span class="n">x1</span><span class="p">,</span> <span class="o">&</span><span class="n">x4</span><span class="p">);</span>
</pre></div>
</div>
<p>Note that each <code class="docutils literal"><span class="pre">ResidualBlock</span></code> only depends on the two parameters
that the corresponding residual object depends on and not on all four
parameters. Compiling and running <a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/powell.cc">examples/powell.cc</a>
gives us:</p>
<div class="highlight-bash"><div class="highlight"><pre><span></span>Initial <span class="nv">x1</span> <span class="o">=</span> 3, <span class="nv">x2</span> <span class="o">=</span> -1, <span class="nv">x3</span> <span class="o">=</span> 0, <span class="nv">x4</span> <span class="o">=</span> 1
iter cost cost_change <span class="p">|</span>gradient<span class="p">|</span> <span class="p">|</span>step<span class="p">|</span> tr_ratio tr_radius ls_iter iter_time total_time
<span class="m">0</span> 1.075000e+02 0.00e+00 1.55e+02 0.00e+00 0.00e+00 1.00e+04 <span class="m">0</span> 4.95e-04 2.30e-03
<span class="m">1</span> 5.036190e+00 1.02e+02 2.00e+01 2.16e+00 9.53e-01 3.00e+04 <span class="m">1</span> 4.39e-05 2.40e-03
<span class="m">2</span> 3.148168e-01 4.72e+00 2.50e+00 6.23e-01 9.37e-01 9.00e+04 <span class="m">1</span> 9.06e-06 2.43e-03
<span class="m">3</span> 1.967760e-02 2.95e-01 3.13e-01 3.08e-01 9.37e-01 2.70e+05 <span class="m">1</span> 8.11e-06 2.45e-03
<span class="m">4</span> 1.229900e-03 1.84e-02 3.91e-02 1.54e-01 9.37e-01 8.10e+05 <span class="m">1</span> 6.91e-06 2.48e-03
<span class="m">5</span> 7.687123e-05 1.15e-03 4.89e-03 7.69e-02 9.37e-01 2.43e+06 <span class="m">1</span> 7.87e-06 2.50e-03
<span class="m">6</span> 4.804625e-06 7.21e-05 6.11e-04 3.85e-02 9.37e-01 7.29e+06 <span class="m">1</span> 5.96e-06 2.52e-03
<span class="m">7</span> 3.003028e-07 4.50e-06 7.64e-05 1.92e-02 9.37e-01 2.19e+07 <span class="m">1</span> 5.96e-06 2.55e-03
<span class="m">8</span> 1.877006e-08 2.82e-07 9.54e-06 9.62e-03 9.37e-01 6.56e+07 <span class="m">1</span> 5.96e-06 2.57e-03
<span class="m">9</span> 1.173223e-09 1.76e-08 1.19e-06 4.81e-03 9.37e-01 1.97e+08 <span class="m">1</span> 7.87e-06 2.60e-03
<span class="m">10</span> 7.333425e-11 1.10e-09 1.49e-07 2.40e-03 9.37e-01 5.90e+08 <span class="m">1</span> 6.20e-06 2.63e-03
<span class="m">11</span> 4.584044e-12 6.88e-11 1.86e-08 1.20e-03 9.37e-01 1.77e+09 <span class="m">1</span> 6.91e-06 2.65e-03
<span class="m">12</span> 2.865573e-13 4.30e-12 2.33e-09 6.02e-04 9.37e-01 5.31e+09 <span class="m">1</span> 5.96e-06 2.67e-03
<span class="m">13</span> 1.791438e-14 2.69e-13 2.91e-10 3.01e-04 9.37e-01 1.59e+10 <span class="m">1</span> 7.15e-06 2.69e-03
Ceres Solver v1.12.0 Solve Report
----------------------------------
Original Reduced
Parameter blocks <span class="m">4</span> 4
Parameters <span class="m">4</span> 4
Residual blocks <span class="m">4</span> 4
Residual <span class="m">4</span> 4
Minimizer TRUST_REGION
Dense linear algebra library EIGEN
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver DENSE_QR DENSE_QR
Threads <span class="m">1</span> 1
Linear solver threads <span class="m">1</span> 1
Cost:
Initial 1.075000e+02
Final 1.791438e-14
Change 1.075000e+02
Minimizer iterations 14
Successful steps 14
Unsuccessful steps 0
Time <span class="o">(</span>in seconds<span class="o">)</span>:
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 <span class="o">(</span>Gradient tolerance reached. Gradient max norm: 3.642190e-11 <<span class="o">=</span> 1.000000e-10<span class="o">)</span>
Final <span class="nv">x1</span> <span class="o">=</span> 0.000292189, <span class="nv">x2</span> <span class="o">=</span> -2.92189e-05, <span class="nv">x3</span> <span class="o">=</span> 4.79511e-05, <span class="nv">x4</span> <span class="o">=</span> 4.79511e-05
</pre></div>
</div>
<p>It is easy to see that the optimal solution to this problem is at
<span class="math">\(x_1=0, x_2=0, x_3=0, x_4=0\)</span> with an objective function value of
<span class="math">\(0\)</span>. In 10 iterations, Ceres finds a solution with an objective
function value of <span class="math">\(4\times 10^{-12}\)</span>.</p>
<p class="rubric">Footnotes</p>
<table class="docutils footnote" frame="void" id="f5" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label">[5]</td><td><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/powell.cc">examples/powell.cc</a>.</td></tr>
</tbody>
</table>
</div>
<div class="section" id="curve-fitting">
<span id="section-fitting"></span><h2>Curve Fitting<a class="headerlink" href="#curve-fitting" title="Permalink to this headline">¶</a></h2>
<p>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
<a class="footnote-reference" href="#f6" id="id8">[6]</a>. It contains data generated by sampling the curve <span class="math">\(y =
e^{0.3x + 0.1}\)</span> and adding Gaussian noise with standard deviation
<span class="math">\(\sigma = 0.2\)</span>. Let us fit some data to the curve</p>
<div class="math">
\[y = e^{mx + c}.\]</div>
<p>We begin by defining a templated object to evaluate the
residual. There will be a residual for each observation.</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="k">struct</span> <span class="n">ExponentialResidual</span> <span class="p">{</span>
<span class="n">ExponentialResidual</span><span class="p">(</span><span class="kt">double</span> <span class="n">x</span><span class="p">,</span> <span class="kt">double</span> <span class="n">y</span><span class="p">)</span>
<span class="o">:</span> <span class="n">x_</span><span class="p">(</span><span class="n">x</span><span class="p">),</span> <span class="n">y_</span><span class="p">(</span><span class="n">y</span><span class="p">)</span> <span class="p">{}</span>
<span class="k">template</span> <span class="o"><</span><span class="k">typename</span> <span class="n">T</span><span class="o">></span>
<span class="kt">bool</span> <span class="k">operator</span><span class="p">()(</span><span class="k">const</span> <span class="n">T</span><span class="o">*</span> <span class="k">const</span> <span class="n">m</span><span class="p">,</span> <span class="k">const</span> <span class="n">T</span><span class="o">*</span> <span class="k">const</span> <span class="n">c</span><span class="p">,</span> <span class="n">T</span><span class="o">*</span> <span class="n">residual</span><span class="p">)</span> <span class="k">const</span> <span class="p">{</span>
<span class="n">residual</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">T</span><span class="p">(</span><span class="n">y_</span><span class="p">)</span> <span class="o">-</span> <span class="n">exp</span><span class="p">(</span><span class="n">m</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">*</span> <span class="n">T</span><span class="p">(</span><span class="n">x_</span><span class="p">)</span> <span class="o">+</span> <span class="n">c</span><span class="p">[</span><span class="mi">0</span><span class="p">]);</span>
<span class="k">return</span> <span class="nb">true</span><span class="p">;</span>
<span class="p">}</span>
<span class="k">private</span><span class="o">:</span>
<span class="c1">// Observations for a sample.</span>
<span class="k">const</span> <span class="kt">double</span> <span class="n">x_</span><span class="p">;</span>
<span class="k">const</span> <span class="kt">double</span> <span class="n">y_</span><span class="p">;</span>
<span class="p">};</span>
</pre></div>
</div>
<p>Assuming the observations are in a <span class="math">\(2n\)</span> sized array called
<code class="docutils literal"><span class="pre">data</span></code> the problem construction is a simple matter of creating a
<code class="xref cpp cpp-class docutils literal"><span class="pre">CostFunction</span></code> for every observation.</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="kt">double</span> <span class="n">m</span> <span class="o">=</span> <span class="mf">0.0</span><span class="p">;</span>
<span class="kt">double</span> <span class="n">c</span> <span class="o">=</span> <span class="mf">0.0</span><span class="p">;</span>
<span class="n">Problem</span> <span class="n">problem</span><span class="p">;</span>
<span class="k">for</span> <span class="p">(</span><span class="kt">int</span> <span class="n">i</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="n">i</span> <span class="o"><</span> <span class="n">kNumObservations</span><span class="p">;</span> <span class="o">++</span><span class="n">i</span><span class="p">)</span> <span class="p">{</span>
<span class="n">CostFunction</span><span class="o">*</span> <span class="n">cost_function</span> <span class="o">=</span>
<span class="k">new</span> <span class="n">AutoDiffCostFunction</span><span class="o"><</span><span class="n">ExponentialResidual</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="o">></span><span class="p">(</span>
<span class="k">new</span> <span class="n">ExponentialResidual</span><span class="p">(</span><span class="n">data</span><span class="p">[</span><span class="mi">2</span> <span class="o">*</span> <span class="n">i</span><span class="p">],</span> <span class="n">data</span><span class="p">[</span><span class="mi">2</span> <span class="o">*</span> <span class="n">i</span> <span class="o">+</span> <span class="mi">1</span><span class="p">]));</span>
<span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span><span class="n">cost_function</span><span class="p">,</span> <span class="nb">NULL</span><span class="p">,</span> <span class="o">&</span><span class="n">m</span><span class="p">,</span> <span class="o">&</span><span class="n">c</span><span class="p">);</span>
<span class="p">}</span>
</pre></div>
</div>
<p>Compiling and running <a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/curve_fitting.cc">examples/curve_fitting.cc</a>
gives us:</p>
<div class="highlight-bash"><div class="highlight"><pre><span></span>iter cost cost_change <span class="p">|</span>gradient<span class="p">|</span> <span class="p">|</span>step<span class="p">|</span> tr_ratio tr_radius ls_iter iter_time total_time
<span class="m">0</span> 1.211734e+02 0.00e+00 3.61e+02 0.00e+00 0.00e+00 1.00e+04 <span class="m">0</span> 5.34e-04 2.56e-03
<span class="m">1</span> 1.211734e+02 -2.21e+03 0.00e+00 7.52e-01 -1.87e+01 5.00e+03 <span class="m">1</span> 4.29e-05 3.25e-03
<span class="m">2</span> 1.211734e+02 -2.21e+03 0.00e+00 7.51e-01 -1.86e+01 1.25e+03 <span class="m">1</span> 1.10e-05 3.28e-03
<span class="m">3</span> 1.211734e+02 -2.19e+03 0.00e+00 7.48e-01 -1.85e+01 1.56e+02 <span class="m">1</span> 1.41e-05 3.31e-03
<span class="m">4</span> 1.211734e+02 -2.02e+03 0.00e+00 7.22e-01 -1.70e+01 9.77e+00 <span class="m">1</span> 1.00e-05 3.34e-03
<span class="m">5</span> 1.211734e+02 -7.34e+02 0.00e+00 5.78e-01 -6.32e+00 3.05e-01 <span class="m">1</span> 1.00e-05 3.36e-03
<span class="m">6</span> 3.306595e+01 8.81e+01 4.10e+02 3.18e-01 1.37e+00 9.16e-01 <span class="m">1</span> 2.79e-05 3.41e-03
<span class="m">7</span> 6.426770e+00 2.66e+01 1.81e+02 1.29e-01 1.10e+00 2.75e+00 <span class="m">1</span> 2.10e-05 3.45e-03
<span class="m">8</span> 3.344546e+00 3.08e+00 5.51e+01 3.05e-02 1.03e+00 8.24e+00 <span class="m">1</span> 2.10e-05 3.48e-03
<span class="m">9</span> 1.987485e+00 1.36e+00 2.33e+01 8.87e-02 9.94e-01 2.47e+01 <span class="m">1</span> 2.10e-05 3.52e-03
<span class="m">10</span> 1.211585e+00 7.76e-01 8.22e+00 1.05e-01 9.89e-01 7.42e+01 <span class="m">1</span> 2.10e-05 3.56e-03
<span class="m">11</span> 1.063265e+00 1.48e-01 1.44e+00 6.06e-02 9.97e-01 2.22e+02 <span class="m">1</span> 2.60e-05 3.61e-03
<span class="m">12</span> 1.056795e+00 6.47e-03 1.18e-01 1.47e-02 1.00e+00 6.67e+02 <span class="m">1</span> 2.10e-05 3.64e-03
<span class="m">13</span> 1.056751e+00 4.39e-05 3.79e-03 1.28e-03 1.00e+00 2.00e+03 <span class="m">1</span> 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: <span class="m">0</span> c: 0
Final m: 0.291861 c: 0.131439
</pre></div>
</div>
<p>Starting from parameter values <span class="math">\(m = 0, c=0\)</span> with an initial
objective function value of <span class="math">\(121.173\)</span> Ceres finds a solution
<span class="math">\(m= 0.291861, c = 0.131439\)</span> with an objective function value of
<span class="math">\(1.05675\)</span>. These values are a bit different than the
parameters of the original model <span class="math">\(m=0.3, c= 0.1\)</span>, 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 <span class="math">\(m=0.3, c=0.1\)</span>, the fit is worse with an objective
function value of <span class="math">\(1.082425\)</span>. The figure below illustrates the fit.</p>
<div class="figure align-center" id="id17" style="width: 500px">
<a class="reference internal image-reference" href="_images/least_squares_fit.png"><img alt="_images/least_squares_fit.png" src="_images/least_squares_fit.png" style="height: 400px;" /></a>
<p class="caption"><span class="caption-text">Least squares curve fitting.</span></p>
</div>
<p class="rubric">Footnotes</p>
<table class="docutils footnote" frame="void" id="f6" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label"><a class="fn-backref" href="#id8">[6]</a></td><td><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/curve_fitting.cc">examples/curve_fitting.cc</a></td></tr>
</tbody>
</table>
</div>
<div class="section" id="robust-curve-fitting">
<h2>Robust Curve Fitting<a class="headerlink" href="#robust-curve-fitting" title="Permalink to this headline">¶</a></h2>
<p>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.</p>
<div class="figure align-center" style="width: 500px">
<a class="reference internal image-reference" href="_images/non_robust_least_squares_fit.png"><img alt="_images/non_robust_least_squares_fit.png" src="_images/non_robust_least_squares_fit.png" style="height: 400px;" /></a>
</div>
<p>To deal with outliers, a standard technique is to use a
<code class="xref cpp cpp-class docutils literal"><span class="pre">LossFunction</span></code>. 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</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span><span class="n">cost_function</span><span class="p">,</span> <span class="nb">NULL</span> <span class="p">,</span> <span class="o">&</span><span class="n">m</span><span class="p">,</span> <span class="o">&</span><span class="n">c</span><span class="p">);</span>
</pre></div>
</div>
<p>to</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span><span class="n">cost_function</span><span class="p">,</span> <span class="k">new</span> <span class="n">CauchyLoss</span><span class="p">(</span><span class="mf">0.5</span><span class="p">)</span> <span class="p">,</span> <span class="o">&</span><span class="n">m</span><span class="p">,</span> <span class="o">&</span><span class="n">c</span><span class="p">);</span>
</pre></div>
</div>
<p><code class="xref cpp cpp-class docutils literal"><span class="pre">CauchyLoss</span></code> is one of the loss functions that ships with Ceres
Solver. The argument <span class="math">\(0.5\)</span> specifies the scale of the loss
function. As a result, we get the fit below <a class="footnote-reference" href="#f7" id="id10">[7]</a>. Notice how the
fitted curve moves back closer to the ground truth curve.</p>
<div class="figure align-center" id="id18" style="width: 500px">
<a class="reference internal image-reference" href="_images/robust_least_squares_fit.png"><img alt="_images/robust_least_squares_fit.png" src="_images/robust_least_squares_fit.png" style="height: 400px;" /></a>
<p class="caption"><span class="caption-text">Using <code class="xref cpp cpp-class docutils literal"><span class="pre">LossFunction</span></code> to reduce the effect of outliers on a
least squares fit.</span></p>
</div>
<p class="rubric">Footnotes</p>
<table class="docutils footnote" frame="void" id="f7" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label"><a class="fn-backref" href="#id10">[7]</a></td><td><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/robust_curve_fitting.cc">examples/robust_curve_fitting.cc</a></td></tr>
</tbody>
</table>
</div>
<div class="section" id="bundle-adjustment">
<h2>Bundle Adjustment<a class="headerlink" href="#bundle-adjustment" title="Permalink to this headline">¶</a></h2>
<p>One of the main reasons for writing Ceres was our need to solve large
scale bundle adjustment problems <a class="reference internal" href="bibliography.html#hartleyzisserman" id="id11">[HartleyZisserman]</a>, <a class="reference internal" href="bibliography.html#triggs" id="id12">[Triggs]</a>.</p>
<p>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 <span class="math">\(L_2\)</span> 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.</p>
<p>Let us solve a problem from the <a class="reference external" href="http://grail.cs.washington.edu/projects/bal/">BAL</a> dataset <a class="footnote-reference" href="#f8" id="id13">[8]</a>.</p>
<p>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 <code class="docutils literal"><span class="pre">ExponentialResidual</span></code>, in that there is an
instance of this object responsible for each image observation.</p>
<p>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 <a class="reference external" href="http://phototour.cs.washington.edu/bundler/">Bundler homepage</a> and the <a class="reference external" href="http://grail.cs.washington.edu/projects/bal/">BAL homepage</a>.</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="k">struct</span> <span class="n">SnavelyReprojectionError</span> <span class="p">{</span>
<span class="n">SnavelyReprojectionError</span><span class="p">(</span><span class="kt">double</span> <span class="n">observed_x</span><span class="p">,</span> <span class="kt">double</span> <span class="n">observed_y</span><span class="p">)</span>
<span class="o">:</span> <span class="n">observed_x</span><span class="p">(</span><span class="n">observed_x</span><span class="p">),</span> <span class="n">observed_y</span><span class="p">(</span><span class="n">observed_y</span><span class="p">)</span> <span class="p">{}</span>
<span class="k">template</span> <span class="o"><</span><span class="k">typename</span> <span class="n">T</span><span class="o">></span>
<span class="kt">bool</span> <span class="k">operator</span><span class="p">()(</span><span class="k">const</span> <span class="n">T</span><span class="o">*</span> <span class="k">const</span> <span class="n">camera</span><span class="p">,</span>
<span class="k">const</span> <span class="n">T</span><span class="o">*</span> <span class="k">const</span> <span class="n">point</span><span class="p">,</span>
<span class="n">T</span><span class="o">*</span> <span class="n">residuals</span><span class="p">)</span> <span class="k">const</span> <span class="p">{</span>
<span class="c1">// camera[0,1,2] are the angle-axis rotation.</span>
<span class="n">T</span> <span class="n">p</span><span class="p">[</span><span class="mi">3</span><span class="p">];</span>
<span class="n">ceres</span><span class="o">::</span><span class="n">AngleAxisRotatePoint</span><span class="p">(</span><span class="n">camera</span><span class="p">,</span> <span class="n">point</span><span class="p">,</span> <span class="n">p</span><span class="p">);</span>
<span class="c1">// camera[3,4,5] are the translation.</span>
<span class="n">p</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">+=</span> <span class="n">camera</span><span class="p">[</span><span class="mi">3</span><span class="p">];</span> <span class="n">p</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">+=</span> <span class="n">camera</span><span class="p">[</span><span class="mi">4</span><span class="p">];</span> <span class="n">p</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span> <span class="o">+=</span> <span class="n">camera</span><span class="p">[</span><span class="mi">5</span><span class="p">];</span>
<span class="c1">// Compute the center of distortion. The sign change comes from</span>
<span class="c1">// the camera model that Noah Snavely's Bundler assumes, whereby</span>
<span class="c1">// the camera coordinate system has a negative z axis.</span>
<span class="n">T</span> <span class="n">xp</span> <span class="o">=</span> <span class="o">-</span> <span class="n">p</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">/</span> <span class="n">p</span><span class="p">[</span><span class="mi">2</span><span class="p">];</span>
<span class="n">T</span> <span class="n">yp</span> <span class="o">=</span> <span class="o">-</span> <span class="n">p</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">/</span> <span class="n">p</span><span class="p">[</span><span class="mi">2</span><span class="p">];</span>
<span class="c1">// Apply second and fourth order radial distortion.</span>
<span class="k">const</span> <span class="n">T</span><span class="o">&</span> <span class="n">l1</span> <span class="o">=</span> <span class="n">camera</span><span class="p">[</span><span class="mi">7</span><span class="p">];</span>
<span class="k">const</span> <span class="n">T</span><span class="o">&</span> <span class="n">l2</span> <span class="o">=</span> <span class="n">camera</span><span class="p">[</span><span class="mi">8</span><span class="p">];</span>
<span class="n">T</span> <span class="n">r2</span> <span class="o">=</span> <span class="n">xp</span><span class="o">*</span><span class="n">xp</span> <span class="o">+</span> <span class="n">yp</span><span class="o">*</span><span class="n">yp</span><span class="p">;</span>
<span class="n">T</span> <span class="n">distortion</span> <span class="o">=</span> <span class="n">T</span><span class="p">(</span><span class="mf">1.0</span><span class="p">)</span> <span class="o">+</span> <span class="n">r2</span> <span class="o">*</span> <span class="p">(</span><span class="n">l1</span> <span class="o">+</span> <span class="n">l2</span> <span class="o">*</span> <span class="n">r2</span><span class="p">);</span>
<span class="c1">// Compute final projected point position.</span>
<span class="k">const</span> <span class="n">T</span><span class="o">&</span> <span class="n">focal</span> <span class="o">=</span> <span class="n">camera</span><span class="p">[</span><span class="mi">6</span><span class="p">];</span>
<span class="n">T</span> <span class="n">predicted_x</span> <span class="o">=</span> <span class="n">focal</span> <span class="o">*</span> <span class="n">distortion</span> <span class="o">*</span> <span class="n">xp</span><span class="p">;</span>
<span class="n">T</span> <span class="n">predicted_y</span> <span class="o">=</span> <span class="n">focal</span> <span class="o">*</span> <span class="n">distortion</span> <span class="o">*</span> <span class="n">yp</span><span class="p">;</span>
<span class="c1">// The error is the difference between the predicted and observed position.</span>
<span class="n">residuals</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">predicted_x</span> <span class="o">-</span> <span class="n">T</span><span class="p">(</span><span class="n">observed_x</span><span class="p">);</span>
<span class="n">residuals</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">predicted_y</span> <span class="o">-</span> <span class="n">T</span><span class="p">(</span><span class="n">observed_y</span><span class="p">);</span>
<span class="k">return</span> <span class="nb">true</span><span class="p">;</span>
<span class="p">}</span>
<span class="c1">// Factory to hide the construction of the CostFunction object from</span>
<span class="c1">// the client code.</span>
<span class="k">static</span> <span class="n">ceres</span><span class="o">::</span><span class="n">CostFunction</span><span class="o">*</span> <span class="n">Create</span><span class="p">(</span><span class="k">const</span> <span class="kt">double</span> <span class="n">observed_x</span><span class="p">,</span>
<span class="k">const</span> <span class="kt">double</span> <span class="n">observed_y</span><span class="p">)</span> <span class="p">{</span>
<span class="k">return</span> <span class="p">(</span><span class="k">new</span> <span class="n">ceres</span><span class="o">::</span><span class="n">AutoDiffCostFunction</span><span class="o"><</span><span class="n">SnavelyReprojectionError</span><span class="p">,</span> <span class="mi">2</span><span class="p">,</span> <span class="mi">9</span><span class="p">,</span> <span class="mi">3</span><span class="o">></span><span class="p">(</span>
<span class="k">new</span> <span class="n">SnavelyReprojectionError</span><span class="p">(</span><span class="n">observed_x</span><span class="p">,</span> <span class="n">observed_y</span><span class="p">)));</span>
<span class="p">}</span>
<span class="kt">double</span> <span class="n">observed_x</span><span class="p">;</span>
<span class="kt">double</span> <span class="n">observed_y</span><span class="p">;</span>
<span class="p">};</span>
</pre></div>
</div>
<p>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
<code class="xref cpp cpp-func docutils literal"><span class="pre">AngleAxisRotatePoint()</span></code> and other functions for manipulating
rotations can be found in <code class="docutils literal"><span class="pre">include/ceres/rotation.h</span></code>.</p>
<p>Given this functor, the bundle adjustment problem can be constructed
as follows:</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="n">ceres</span><span class="o">::</span><span class="n">Problem</span> <span class="n">problem</span><span class="p">;</span>
<span class="k">for</span> <span class="p">(</span><span class="kt">int</span> <span class="n">i</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="n">i</span> <span class="o"><</span> <span class="n">bal_problem</span><span class="p">.</span><span class="n">num_observations</span><span class="p">();</span> <span class="o">++</span><span class="n">i</span><span class="p">)</span> <span class="p">{</span>
<span class="n">ceres</span><span class="o">::</span><span class="n">CostFunction</span><span class="o">*</span> <span class="n">cost_function</span> <span class="o">=</span>
<span class="n">SnavelyReprojectionError</span><span class="o">::</span><span class="n">Create</span><span class="p">(</span>
<span class="n">bal_problem</span><span class="p">.</span><span class="n">observations</span><span class="p">()[</span><span class="mi">2</span> <span class="o">*</span> <span class="n">i</span> <span class="o">+</span> <span class="mi">0</span><span class="p">],</span>
<span class="n">bal_problem</span><span class="p">.</span><span class="n">observations</span><span class="p">()[</span><span class="mi">2</span> <span class="o">*</span> <span class="n">i</span> <span class="o">+</span> <span class="mi">1</span><span class="p">]);</span>
<span class="n">problem</span><span class="p">.</span><span class="n">AddResidualBlock</span><span class="p">(</span><span class="n">cost_function</span><span class="p">,</span>
<span class="nb">NULL</span> <span class="cm">/* squared loss */</span><span class="p">,</span>
<span class="n">bal_problem</span><span class="p">.</span><span class="n">mutable_camera_for_observation</span><span class="p">(</span><span class="n">i</span><span class="p">),</span>
<span class="n">bal_problem</span><span class="p">.</span><span class="n">mutable_point_for_observation</span><span class="p">(</span><span class="n">i</span><span class="p">));</span>
<span class="p">}</span>
</pre></div>
</div>
<p>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.</p>
<p>Since this is a large sparse problem (well large for <code class="docutils literal"><span class="pre">DENSE_QR</span></code>
anyways), one way to solve this problem is to set
<code class="xref cpp cpp-member docutils literal"><span class="pre">Solver::Options::linear_solver_type</span></code> to
<code class="docutils literal"><span class="pre">SPARSE_NORMAL_CHOLESKY</span></code> and call <a class="reference internal" href="gradient_solver.html#_CPPv25SolveRKN21GradientProblemSolver7OptionsERK15GradientProblemPdPN21GradientProblemSolver7SummaryE" title="Solve"><code class="xref cpp cpp-member docutils literal"><span class="pre">Solve</span></code></a>. 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 <code class="docutils literal"><span class="pre">DENSE_SCHUR</span></code>.</p>
<div class="highlight-c++"><div class="highlight"><pre><span></span><span class="n">ceres</span><span class="o">::</span><span class="n">Solver</span><span class="o">::</span><span class="n">Options</span> <span class="n">options</span><span class="p">;</span>
<span class="n">options</span><span class="p">.</span><span class="n">linear_solver_type</span> <span class="o">=</span> <span class="n">ceres</span><span class="o">::</span><span class="n">DENSE_SCHUR</span><span class="p">;</span>
<span class="n">options</span><span class="p">.</span><span class="n">minimizer_progress_to_stdout</span> <span class="o">=</span> <span class="nb">true</span><span class="p">;</span>
<span class="n">ceres</span><span class="o">::</span><span class="n">Solver</span><span class="o">::</span><span class="n">Summary</span> <span class="n">summary</span><span class="p">;</span>
<span class="n">ceres</span><span class="o">::</span><span class="n">Solve</span><span class="p">(</span><span class="n">options</span><span class="p">,</span> <span class="o">&</span><span class="n">problem</span><span class="p">,</span> <span class="o">&</span><span class="n">summary</span><span class="p">);</span>
<span class="n">std</span><span class="o">::</span><span class="n">cout</span> <span class="o"><<</span> <span class="n">summary</span><span class="p">.</span><span class="n">FullReport</span><span class="p">()</span> <span class="o"><<</span> <span class="s">"</span><span class="se">\n</span><span class="s">"</span><span class="p">;</span>
</pre></div>
</div>
<p>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
<a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/bundle_adjuster.cc">examples/bundle_adjuster.cc</a></p>
<p class="rubric">Footnotes</p>
<table class="docutils footnote" frame="void" id="f8" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label"><a class="fn-backref" href="#id13">[8]</a></td><td><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/simple_bundle_adjuster.cc">examples/simple_bundle_adjuster.cc</a></td></tr>
</tbody>
</table>
</div>
<div class="section" id="other-examples">
<h2>Other Examples<a class="headerlink" href="#other-examples" title="Permalink to this headline">¶</a></h2>
<p>Besides the examples in this chapter, the <a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/">example</a>
directory contains a number of other examples:</p>
<ol class="arabic">
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/bundle_adjuster.cc">bundle_adjuster.cc</a>
shows how to use the various features of Ceres to solve bundle
adjustment problems.</p>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/circle_fit.cc">circle_fit.cc</a>
shows how to fit data to a circle.</p>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/ellipse_approximation.cc">ellipse_approximation.cc</a>
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 <code class="docutils literal"><span class="pre">Solver::Options::dynamic_sparsity</span></code>,
and how it can benefit problems which are numerically dense but
dynamically sparse.</p>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/denoising.cc">denoising.cc</a>
implements image denoising using the <a class="reference external" href="http://www.gris.informatik.tu-darmstadt.de/~sroth/research/foe/index.html">Fields of Experts</a>
model.</p>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/nist.cc">nist.cc</a>
implements and attempts to solves the <a class="reference external" href="http://www.itl.nist.gov/div898/strd/nls/nls_main.shtm">NIST</a>
non-linear regression problems.</p>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/more_garbow_hillstrom.cc">more_garbow_hillstrom.cc</a>
A subset of the test problems from the paper</p>
<p>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</p>
<p>which were augmented with bounds and used for testing bounds
constrained optimization algorithms by</p>
<p>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.</p>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/libmv_bundle_adjuster.cc">libmv_bundle_adjuster.cc</a>
is the bundle adjustment algorithm used by <a class="reference external" href="www.blender.org">Blender</a>/libmv.</p>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/libmv_homography.cc">libmv_homography.cc</a>
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.</p>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/robot_pose_mle.cc">robot_pose_mle.cc</a>
This example demonstrates how to use the <code class="docutils literal"><span class="pre">DynamicAutoDiffCostFunction</span></code>
variant of CostFunction. The <code class="docutils literal"><span class="pre">DynamicAutoDiffCostFunction</span></code> is meant to
be used in cases where the number of parameter blocks or the sizes are not
known at compile time.</p>
<p>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.</p>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_2d/pose_graph_2d.cc">slam/pose_graph_2d/pose_graph_2d.cc</a>
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
<a class="footnote-reference" href="#f9" id="id14">[9]</a>. 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.</p>
<p>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.</p>
<div class="figure align-center" id="id19" style="width: 500px">
<a class="reference internal image-reference" href="_images/slam2d.png"><img alt="_images/slam2d.png" src="_images/slam2d.png" style="height: 400px;" /></a>
<p class="caption"><span class="caption-text">Visual representation of a graph SLAM problem.</span></p>
</div>
<p>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.</p>
<p>The robot at timestamp <span class="math">\(t\)</span> has state <span class="math">\(x_t = [p^T, \psi]^T\)</span> where
<span class="math">\(p\)</span> is a 2D vector that represents the position in the plane and
<span class="math">\(\psi\)</span> is the orientation in radians. The measurement of the relative
transform between the robot state at two timestamps <span class="math">\(a\)</span> and <span class="math">\(b\)</span>
is given as: <span class="math">\(z_{ab} = [\hat{p}_{ab}^T, \hat{\psi}_{ab}]\)</span>. The residual
implemented in the Ceres cost function which computes the error between the
measurement and the predicted measurement is:</p>
<div class="math">
\[\begin{split}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]\end{split}\]</div>
<p>where the function <span class="math">\(\mathrm{Normalize}()\)</span> normalizes the angle in the range
<span class="math">\([-\pi,\pi)\)</span>, and <span class="math">\(R\)</span> is the rotation matrix given by</p>
<div class="math">
\[\begin{split}R_a =
\left[
\begin{array}{cc}
\cos \psi_a & -\sin \psi_a \\
\sin \psi_a & \cos \psi_a \\
\end{array}
\right]\end{split}\]</div>
<p>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. <span class="math">\(\Sigma_{ab}^{-\frac{1}{2}} r_{ab}\)</span> where <span class="math">\(\Sigma_{ab}\)</span> is
the covariance.</p>
<p>Lastly, we use a local parameterization to normalize the orientation in the
range which is normalized between <span class="math">\([-\pi,\pi)\)</span>. Specially, we define
the <code class="xref cpp cpp-member docutils literal"><span class="pre">AngleLocalParameterization::operator()</span></code> function to be:
<span class="math">\(\mathrm{Normalize}(\psi + \delta \psi)\)</span>.</p>
<p>This package includes an executable <code class="xref cpp cpp-member docutils literal"><span class="pre">pose_graph_2d</span></code> 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. <code class="xref cpp cpp-member docutils literal"><span class="pre">pose_graph_2d</span></code> will print the Ceres solver full summary and
then output to disk the original and optimized poses (<code class="docutils literal"><span class="pre">poses_original.txt</span></code>
and <code class="docutils literal"><span class="pre">poses_optimized.txt</span></code>, respectively) of the robot in the following
format:</p>
<div class="highlight-bash"><div class="highlight"><pre><span></span>pose_id x y yaw_radians
pose_id x y yaw_radians
pose_id x y yaw_radians
</pre></div>
</div>
<p>where <code class="docutils literal"><span class="pre">pose_id</span></code> is the corresponding integer ID from the file
definition. Note, the file will be sorted in ascending order for the
<code class="docutils literal"><span class="pre">pose_id</span></code>.</p>
<p>The executable <code class="xref cpp cpp-member docutils literal"><span class="pre">pose_graph_2d</span></code> expects the first argument to be
the path to the problem definition. To run the executable,</p>
<div class="highlight-bash"><div class="highlight"><pre><span></span>/path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o
</pre></div>
</div>
<p>A python script is provided to visualize the resulting output files.</p>
<div class="highlight-bash"><div class="highlight"><pre><span></span>/path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
</pre></div>
</div>
<p>As an example, a standard synthetic benchmark dataset <a class="footnote-reference" href="#f10" id="id15">[10]</a> 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:</p>
<div class="figure align-center" style="width: 600px">
<a class="reference internal image-reference" href="_images/manhattan_olson_3500_result.png"><img alt="_images/manhattan_olson_3500_result.png" src="_images/manhattan_olson_3500_result.png" style="height: 600px;" /></a>
</div>
<p>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.</p>
<p class="rubric">Footnotes</p>
<table class="docutils footnote" frame="void" id="f9" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label">[9]</td><td><em>(<a class="fn-backref" href="#id14">1</a>, <a class="fn-backref" href="#id16">2</a>)</em> <p class="last">Giorgio Grisetti, Rainer Kummerle, Cyrill Stachniss, Wolfram
Burgard. A Tutorial on Graph-Based SLAM. IEEE Intelligent Transportation
Systems Magazine, 52(3):199–222, 2010.</p>
</td></tr>
</tbody>
</table>
<table class="docutils footnote" frame="void" id="f10" rules="none">
<colgroup><col class="label" /><col /></colgroup>
<tbody valign="top">
<tr><td class="label"><a class="fn-backref" href="#id15">[10]</a></td><td><p class="first last">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.</p>
</td></tr>
</tbody>
</table>
</li>
<li><p class="first"><a class="reference external" href="https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_3d/pose_graph_3d.cc">slam/pose_graph_3d/pose_graph_3d.cc</a>
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.</p>
<p>The robot at timestamp <span class="math">\(t\)</span> has state <span class="math">\(x_t = [p^T, q^T]^T\)</span> where
<span class="math">\(p\)</span> is a 3D vector that represents the position and <span class="math">\(q\)</span> is the
orientation represented as an Eigen quaternion. The measurement of the
relative transform between the robot state at two timestamps <span class="math">\(a\)</span> and
<span class="math">\(b\)</span> is given as: <span class="math">\(z_{ab} = [\hat{p}_{ab}^T, \hat{q}_{ab}^T]^T\)</span>.
The residual implemented in the Ceres cost function which computes the error
between the measurement and the predicted measurement is:</p>
<div class="math">
\[\begin{split}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]\end{split}\]</div>
<p>where the function <span class="math">\(\mathrm{vec}()\)</span> returns the vector part of the
quaternion, i.e. <span class="math">\([q_x, q_y, q_z]\)</span>, and <span class="math">\(R(q)\)</span> is the rotation
matrix for the quaternion.</p>
<p>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. <span class="math">\(\Sigma_{ab}^{-\frac{1}{2}} r_{ab}\)</span> where <span class="math">\(\Sigma_{ab}\)</span> is
the covariance.</p>
<p>Given that we are using a quaternion to represent the orientation, we need to
use a local parameterization (<code class="xref cpp cpp-class docutils literal"><span class="pre">EigenQuaternionParameterization</span></code>) 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 <span class="math">\([x, y, z, w]\)</span> 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
<span class="math">\(w\)</span>, <span class="math">\(x\)</span>, <span class="math">\(y\)</span>, <span class="math">\(z\)</span> order. Since Ceres operates on
parameter blocks which are raw double pointers this difference is important
and requires a different parameterization.</p>
<p>This package includes an executable <code class="xref cpp cpp-member docutils literal"><span class="pre">pose_graph_3d</span></code> 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. <code class="xref cpp cpp-member docutils literal"><span class="pre">pose_graph_3d</span></code>
will print the Ceres solver full summary and then output to disk the original
and optimized poses (<code class="docutils literal"><span class="pre">poses_original.txt</span></code> and <code class="docutils literal"><span class="pre">poses_optimized.txt</span></code>,
respectively) of the robot in the following format:</p>
<div class="highlight-bash"><div class="highlight"><pre><span></span>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
...
</pre></div>
</div>
<p>where <code class="docutils literal"><span class="pre">pose_id</span></code> is the corresponding integer ID from the file
definition. Note, the file will be sorted in ascending order for the
<code class="docutils literal"><span class="pre">pose_id</span></code>.</p>
<p>The executable <code class="xref cpp cpp-member docutils literal"><span class="pre">pose_graph_3d</span></code> expects the first argument to be the
path to the problem definition. The executable can be run via</p>
<div class="highlight-bash"><div class="highlight"><pre><span></span>/path/to/bin/pose_graph_3d /path/to/dataset/dataset.g2o
</pre></div>
</div>
<p>A script is provided to visualize the resulting output files. There is also
an option to enable equal axes using <code class="docutils literal"><span class="pre">--axes_equal</span></code></p>
<div class="highlight-bash"><div class="highlight"><pre><span></span>/path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
</pre></div>
</div>
<p>As an example, a standard synthetic benchmark dataset <a class="footnote-reference" href="#f9" id="id16">[9]</a> 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:</p>
<div class="figure align-center" style="width: 600px">
<a class="reference internal image-reference" href="_images/pose_graph_3d_ex.png"><img alt="_images/pose_graph_3d_ex.png" src="_images/pose_graph_3d_ex.png" style="height: 300px;" /></a>
</div>
</li>
</ol>
</div>
</div>
</div>
</div>
<footer>
<div class="rst-footer-buttons" role="navigation" aria-label="footer navigation">
<a href="gradient_tutorial.html" class="btn btn-neutral float-right" title="General Unconstrained Minimization" accesskey="n">Next <span class="fa fa-arrow-circle-right"></span></a>
<a href="tutorial.html" class="btn btn-neutral" title="Tutorial" accesskey="p"><span class="fa fa-arrow-circle-left"></span> Previous</a>
</div>
<hr/>
<div role="contentinfo">
<p>
© Copyright 2016 Google Inc.
</p>
</div>
</footer>
</div>
</div>
</section>
</div>
<script type="text/javascript">
var DOCUMENTATION_OPTIONS = {
URL_ROOT:'./',
VERSION:'1.13.0',
COLLAPSE_INDEX:false,
FILE_SUFFIX:'.html',
HAS_SOURCE: true
};
</script>
<script type="text/javascript" src="_static/jquery.js"></script>
<script type="text/javascript" src="_static/underscore.js"></script>
<script type="text/javascript" src="_static/doctools.js"></script>
<script type="text/javascript" src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS_HTML">
MathJax.Hub.Config({
"HTML-CSS": {
availableFonts: ["TeX"]
}
});
</script>
<script type="text/javascript" src="_static/js/theme.js"></script>
<script type="text/javascript">
jQuery(function () {
SphinxRtdTheme.StickyNav.enable();
});
</script>
<script>
(function(i,s,o,g,r,a,m){i['GoogleAnalyticsObject']=r;i[r]=i[r]||function(){
(i[r].q=i[r].q||[]).push(arguments)},i[r].l=1*new Date();a=s.createElement(o),
m=s.getElementsByTagName(o)[0];a.async=1;a.src=g;m.parentNode.insertBefore(a,m)
})(window,document,'script','//www.google-analytics.com/analytics.js','ga');
ga('create', 'UA-49769510-1', 'ceres-solver.org');
ga('send', 'pageview');
</script>
</body>
</html>