1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3// http://code.google.com/p/ceres-solver/
4//
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are met:
7//
8// * Redistributions of source code must retain the above copyright notice,
9//   this list of conditions and the following disclaimer.
10// * Redistributions in binary form must reproduce the above copyright notice,
11//   this list of conditions and the following disclaimer in the documentation
12//   and/or other materials provided with the distribution.
13// * Neither the name of Google Inc. nor the names of its contributors may be
14//   used to endorse or promote products derived from this software without
15//   specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28//
29// Author: sameeragarwal@google.com (Sameer Agarwal)
30//
31// An example of solving a dynamically sized problem with various
32// solvers and loss functions.
33//
34// For a simpler bare bones example of doing bundle adjustment with
35// Ceres, please see simple_bundle_adjuster.cc.
36//
37// NOTE: This example will not compile without gflags and SuiteSparse.
38//
39// The problem being solved here is known as a Bundle Adjustment
40// problem in computer vision. Given a set of 3d points X_1, ..., X_n,
41// a set of cameras P_1, ..., P_m. If the point X_i is visible in
42// image j, then there is a 2D observation u_ij that is the expected
43// projection of X_i using P_j. The aim of this optimization is to
44// find values of X_i and P_j such that the reprojection error
45//
46//    E(X,P) =  sum_ij  |u_ij - P_j X_i|^2
47//
48// is minimized.
49//
50// The problem used here comes from a collection of bundle adjustment
51// problems published at University of Washington.
52// http://grail.cs.washington.edu/projects/bal
53
54#include <algorithm>
55#include <cmath>
56#include <cstdio>
57#include <cstdlib>
58#include <string>
59#include <vector>
60
61#include "bal_problem.h"
62#include "ceres/ceres.h"
63#include "gflags/gflags.h"
64#include "glog/logging.h"
65#include "snavely_reprojection_error.h"
66
67DEFINE_string(input, "", "Input File name");
68DEFINE_string(trust_region_strategy, "levenberg_marquardt",
69              "Options are: levenberg_marquardt, dogleg.");
70DEFINE_string(dogleg, "traditional_dogleg", "Options are: traditional_dogleg,"
71              "subspace_dogleg.");
72
73DEFINE_bool(inner_iterations, false, "Use inner iterations to non-linearly "
74            "refine each successful trust region step.");
75
76DEFINE_string(blocks_for_inner_iterations, "automatic", "Options are: "
77            "automatic, cameras, points, cameras,points, points,cameras");
78
79DEFINE_string(linear_solver, "sparse_schur", "Options are: "
80              "sparse_schur, dense_schur, iterative_schur, sparse_normal_cholesky, "
81              "dense_qr, dense_normal_cholesky and cgnr.");
82DEFINE_string(preconditioner, "jacobi", "Options are: "
83              "identity, jacobi, schur_jacobi, cluster_jacobi, "
84              "cluster_tridiagonal.");
85DEFINE_string(visibility_clustering, "canonical_views",
86              "single_linkage, canonical_views");
87
88DEFINE_string(sparse_linear_algebra_library, "suite_sparse",
89              "Options are: suite_sparse and cx_sparse.");
90DEFINE_string(dense_linear_algebra_library, "eigen",
91              "Options are: eigen and lapack.");
92DEFINE_string(ordering, "automatic", "Options are: automatic, user.");
93
94DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent "
95            "rotations. If false, angle axis is used.");
96DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local "
97            "parameterization.");
98DEFINE_bool(robustify, false, "Use a robust loss function.");
99
100DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the "
101             "accuracy of each linear solve of the truncated newton step. "
102             "Changing this parameter can affect solve performance.");
103
104DEFINE_int32(num_threads, 1, "Number of threads.");
105DEFINE_int32(num_iterations, 5, "Number of iterations.");
106DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds.");
107DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
108            " nonmonotic steps.");
109
110DEFINE_double(rotation_sigma, 0.0, "Standard deviation of camera rotation "
111              "perturbation.");
112DEFINE_double(translation_sigma, 0.0, "Standard deviation of the camera "
113              "translation perturbation.");
114DEFINE_double(point_sigma, 0.0, "Standard deviation of the point "
115              "perturbation.");
116DEFINE_int32(random_seed, 38401, "Random seed used to set the state "
117             "of the pseudo random number generator used to generate "
118             "the pertubations.");
119DEFINE_bool(line_search, false, "Use a line search instead of trust region "
120            "algorithm.");
121
122namespace ceres {
123namespace examples {
124
125void SetLinearSolver(Solver::Options* options) {
126  CHECK(StringToLinearSolverType(FLAGS_linear_solver,
127                                 &options->linear_solver_type));
128  CHECK(StringToPreconditionerType(FLAGS_preconditioner,
129                                   &options->preconditioner_type));
130  CHECK(StringToVisibilityClusteringType(FLAGS_visibility_clustering,
131                                         &options->visibility_clustering_type));
132  CHECK(StringToSparseLinearAlgebraLibraryType(
133            FLAGS_sparse_linear_algebra_library,
134            &options->sparse_linear_algebra_library_type));
135  CHECK(StringToDenseLinearAlgebraLibraryType(
136            FLAGS_dense_linear_algebra_library,
137            &options->dense_linear_algebra_library_type));
138  options->num_linear_solver_threads = FLAGS_num_threads;
139}
140
141void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {
142  const int num_points = bal_problem->num_points();
143  const int point_block_size = bal_problem->point_block_size();
144  double* points = bal_problem->mutable_points();
145
146  const int num_cameras = bal_problem->num_cameras();
147  const int camera_block_size = bal_problem->camera_block_size();
148  double* cameras = bal_problem->mutable_cameras();
149
150  if (options->use_inner_iterations) {
151    if (FLAGS_blocks_for_inner_iterations == "cameras") {
152      LOG(INFO) << "Camera blocks for inner iterations";
153      options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
154      for (int i = 0; i < num_cameras; ++i) {
155        options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
156      }
157    } else if (FLAGS_blocks_for_inner_iterations == "points") {
158      LOG(INFO) << "Point blocks for inner iterations";
159      options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
160      for (int i = 0; i < num_points; ++i) {
161        options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
162      }
163    } else if (FLAGS_blocks_for_inner_iterations == "cameras,points") {
164      LOG(INFO) << "Camera followed by point blocks for inner iterations";
165      options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
166      for (int i = 0; i < num_cameras; ++i) {
167        options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
168      }
169      for (int i = 0; i < num_points; ++i) {
170        options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1);
171      }
172    } else if (FLAGS_blocks_for_inner_iterations == "points,cameras") {
173      LOG(INFO) << "Point followed by camera blocks for inner iterations";
174      options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
175      for (int i = 0; i < num_cameras; ++i) {
176        options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
177      }
178      for (int i = 0; i < num_points; ++i) {
179        options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
180      }
181    } else if (FLAGS_blocks_for_inner_iterations == "automatic") {
182      LOG(INFO) << "Choosing automatic blocks for inner iterations";
183    } else {
184      LOG(FATAL) << "Unknown block type for inner iterations: "
185                 << FLAGS_blocks_for_inner_iterations;
186    }
187  }
188
189  // Bundle adjustment problems have a sparsity structure that makes
190  // them amenable to more specialized and much more efficient
191  // solution strategies. The SPARSE_SCHUR, DENSE_SCHUR and
192  // ITERATIVE_SCHUR solvers make use of this specialized
193  // structure.
194  //
195  // This can either be done by specifying Options::ordering_type =
196  // ceres::SCHUR, in which case Ceres will automatically determine
197  // the right ParameterBlock ordering, or by manually specifying a
198  // suitable ordering vector and defining
199  // Options::num_eliminate_blocks.
200  if (FLAGS_ordering == "automatic") {
201    return;
202  }
203
204  ceres::ParameterBlockOrdering* ordering =
205      new ceres::ParameterBlockOrdering;
206
207  // The points come before the cameras.
208  for (int i = 0; i < num_points; ++i) {
209    ordering->AddElementToGroup(points + point_block_size * i, 0);
210  }
211
212  for (int i = 0; i < num_cameras; ++i) {
213    // When using axis-angle, there is a single parameter block for
214    // the entire camera.
215    ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
216    // If quaternions are used, there are two blocks, so add the
217    // second block to the ordering.
218    if (FLAGS_use_quaternions) {
219      ordering->AddElementToGroup(cameras + camera_block_size * i + 4, 1);
220    }
221  }
222
223  options->linear_solver_ordering.reset(ordering);
224}
225
226void SetMinimizerOptions(Solver::Options* options) {
227  options->max_num_iterations = FLAGS_num_iterations;
228  options->minimizer_progress_to_stdout = true;
229  options->num_threads = FLAGS_num_threads;
230  options->eta = FLAGS_eta;
231  options->max_solver_time_in_seconds = FLAGS_max_solver_time;
232  options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
233  if (FLAGS_line_search) {
234    options->minimizer_type = ceres::LINE_SEARCH;
235  }
236
237  CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
238                                        &options->trust_region_strategy_type));
239  CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));
240  options->use_inner_iterations = FLAGS_inner_iterations;
241}
242
243void SetSolverOptionsFromFlags(BALProblem* bal_problem,
244                               Solver::Options* options) {
245  SetMinimizerOptions(options);
246  SetLinearSolver(options);
247  SetOrdering(bal_problem, options);
248}
249
250void BuildProblem(BALProblem* bal_problem, Problem* problem) {
251  const int point_block_size = bal_problem->point_block_size();
252  const int camera_block_size = bal_problem->camera_block_size();
253  double* points = bal_problem->mutable_points();
254  double* cameras = bal_problem->mutable_cameras();
255
256  // Observations is 2*num_observations long array observations =
257  // [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x
258  // and y positions of the observation.
259  const double* observations = bal_problem->observations();
260
261  for (int i = 0; i < bal_problem->num_observations(); ++i) {
262    CostFunction* cost_function;
263    // Each Residual block takes a point and a camera as input and
264    // outputs a 2 dimensional residual.
265    cost_function =
266        (FLAGS_use_quaternions)
267        ? SnavelyReprojectionErrorWithQuaternions::Create(
268            observations[2 * i + 0],
269            observations[2 * i + 1])
270        : SnavelyReprojectionError::Create(
271            observations[2 * i + 0],
272            observations[2 * i + 1]);
273
274    // If enabled use Huber's loss function.
275    LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;
276
277    // Each observation correponds to a pair of a camera and a point
278    // which are identified by camera_index()[i] and point_index()[i]
279    // respectively.
280    double* camera =
281        cameras + camera_block_size * bal_problem->camera_index()[i];
282    double* point = points + point_block_size * bal_problem->point_index()[i];
283
284    if (FLAGS_use_quaternions) {
285      // When using quaternions, we split the camera into two
286      // parameter blocks. One of size 4 for the quaternion and the
287      // other of size 6 containing the translation, focal length and
288      // the radial distortion parameters.
289      problem->AddResidualBlock(cost_function,
290                                loss_function,
291                                camera,
292                                camera + 4,
293                                point);
294    } else {
295      problem->AddResidualBlock(cost_function, loss_function, camera, point);
296    }
297  }
298
299  if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
300    LocalParameterization* quaternion_parameterization =
301         new QuaternionParameterization;
302    for (int i = 0; i < bal_problem->num_cameras(); ++i) {
303      problem->SetParameterization(cameras + camera_block_size * i,
304                                   quaternion_parameterization);
305    }
306  }
307}
308
309void SolveProblem(const char* filename) {
310  BALProblem bal_problem(filename, FLAGS_use_quaternions);
311  Problem problem;
312
313  srand(FLAGS_random_seed);
314  bal_problem.Normalize();
315  bal_problem.Perturb(FLAGS_rotation_sigma,
316                      FLAGS_translation_sigma,
317                      FLAGS_point_sigma);
318
319  BuildProblem(&bal_problem, &problem);
320  Solver::Options options;
321  SetSolverOptionsFromFlags(&bal_problem, &options);
322  options.gradient_tolerance = 1e-16;
323  options.function_tolerance = 1e-16;
324  Solver::Summary summary;
325  Solve(options, &problem, &summary);
326  std::cout << summary.FullReport() << "\n";
327}
328
329}  // namespace examples
330}  // namespace ceres
331
332int main(int argc, char** argv) {
333  google::ParseCommandLineFlags(&argc, &argv, true);
334  google::InitGoogleLogging(argv[0]);
335  if (FLAGS_input.empty()) {
336    LOG(ERROR) << "Usage: bundle_adjustment_example --input=bal_problem";
337    return 1;
338  }
339
340  CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization)
341      << "--use_local_parameterization can only be used with "
342      << "--use_quaternions.";
343  ceres::examples::SolveProblem(FLAGS_input.c_str());
344  return 0;
345}
346