docs/bundleadjustment.tex

%!TEX root = ceres-solver.tex
\chapter{Bundle Adjustment}
\label{chapter:tutorial:bundleadjustment}
One of the main reasons for writing Ceres was our need to solve large scale bundle adjustment problems~\cite{hartley-zisserman-book-2004,triggs-etal-1999}.

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 $L_2$ norm of the difference between the observed feature location and the projection of the corresponding 3D point on the image plane of the camera. Ceres has extensive support for solving bundle adjustment problems. 

Let us consider the solution of a problem from the BAL~\cite{Agarwal10bal} dataset~\footnote{The code for this example can be found in \texttt{examples/simple\_bundle\_adjuster.cc}.}. 

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 \texttt{ExponentialResidual}, in that there is an instance of this object responsible for each image observation.


Each residual in a BAL problem depends on a three dimensional point and a nine parameter
camera. The nine parameters defining the camera can are: Three for rotation as a Rodriquez axis-angle vector, three for translation, one for focal length and two for radial distortion.  The details of this camera model can be found on Noah
Snavely's Bundler
homepage~\footnote{\url{http://phototour.cs.washington.edu/bundler/}}
and the BAL
homepage~\footnote{\url{http://grail.cs.washington.edu/projects/bal/}}.


%\begin{listing}[ht]
\clearpage
\begin{minted}[mathescape]{c++}
struct SnavelyReprojectionError {
  SnavelyReprojectionError(double observed_x, double observed_y)
      : observed_x(observed_x), observed_y(observed_y) {}
  template <typename T>
  bool operator()(const T* const camera,
                  const T* const point,
                  T* residuals) const {
    // camera[0,1,2] are the angle-axis rotation.
    T p[3];
    ceres::AngleAxisRotatePoint(camera, point, p);
    // camera[3,4,5] are the translation.
    p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];

    // Compute the center of distortion. The sign change comes from
    // the camera model that Noah Snavely's Bundler assumes, whereby
    // the camera coordinate system has a negative z axis.
    T xp = - p[0] / p[2];
    T yp = - p[1] / p[2];

    // Apply second and fourth order radial distortion.
    const T& l1 = camera[7];
    const T& l2 = camera[8];
    T r2 = xp*xp + yp*yp;
    T distortion = T(1.0) + r2  * (l1 + l2  * r2);

    // Compute final projected point position.
    const T& focal = camera[6];
    T predicted_x = focal * distortion * xp;
    T predicted_y = focal * distortion * yp;

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);
    return true;
  }
  double observed_x;
  double observed_y;
};
\end{minted}

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
our life very simple here. The function \texttt{AngleAxisRotatePoint} and other functions for manipulating rotations can be found in \texttt{include/ceres/rotation.h}.  

Given this functor, the bundle adjustment problem can be constructed as follows:
\begin{minted}{c++}
// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
  // Each Residual block takes a point and a camera as input and outputs a 2
  // dimensional residual. Internally, the cost function stores the observed
  // image location and compares the reprojection against the observation.
  ceres::CostFunction* cost_function =
      new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
          new SnavelyReprojectionError(
              bal_problem.observations()[2 * i + 0],
              bal_problem.observations()[2 * i + 1]));
  problem.AddResidualBlock(cost_function,
                           NULL /* squared loss */,
                           bal_problem.mutable_camera_for_observation(i),
                           bal_problem.mutable_point_for_observation(i));
}
\end{minted}
Again note that that the problem construction for bundle adjustment is very similar to the curve fitting example.

One way to solve this problem is to set \texttt{Solver::Options::linear\_solver\_type} to \texttt{SPARSE\_NORMAL\_CHOLESKY} and call \texttt{Solve}. And while this is a reasonable thing to do, bundle adjustment problems have a special sparsity structure that can be exploited to solve them much more efficiently. Ceres provides three specialized solvers (collectively known as Schur based solvers) for this task. The example code uses the simplest of them \texttt{DENSE\_SCHUR}. 
\begin{minted}{c++}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
\end{minted}

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 \texttt{examples/bundle\_adjuster.cc}.