examples/bundle_adjuster.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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
  67 DEFINE_string(input, "", "Input File name");
  68 DEFINE_string(trust_region_strategy, "levenberg_marquardt",
  69               "Options are: levenberg_marquardt, dogleg.");
  70 DEFINE_string(dogleg, "traditional_dogleg", "Options are: traditional_dogleg,"
  71               "subspace_dogleg.");
  72
  73 DEFINE_bool(inner_iterations, false, "Use inner iterations to non-linearly "
  74             "refine each successful trust region step.");
  75
  76 DEFINE_string(blocks_for_inner_iterations, "automatic", "Options are: "
  77             "automatic, cameras, points, cameras,points, points,cameras");
  78
  79 DEFINE_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.");
  82 DEFINE_bool(explicit_schur_complement, false, "If using ITERATIVE_SCHUR "
  83             "then explicitly compute the Schur complement.");
  84 DEFINE_string(preconditioner, "jacobi", "Options are: "
  85               "identity, jacobi, schur_jacobi, cluster_jacobi, "
  86               "cluster_tridiagonal.");
  87 DEFINE_string(visibility_clustering, "canonical_views",
  88               "single_linkage, canonical_views");
  89
  90 DEFINE_string(sparse_linear_algebra_library, "suite_sparse",
  91               "Options are: suite_sparse and cx_sparse.");
  92 DEFINE_string(dense_linear_algebra_library, "eigen",
  93               "Options are: eigen and lapack.");
  94 DEFINE_string(ordering, "automatic", "Options are: automatic, user.");
  95
  96 DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent "
  97             "rotations. If false, angle axis is used.");
  98 DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local "
  99             "parameterization.");
 100 DEFINE_bool(robustify, false, "Use a robust loss function.");
 101
 102 DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the "
 103              "accuracy of each linear solve of the truncated newton step. "
 104              "Changing this parameter can affect solve performance.");
 105
 106 DEFINE_int32(num_threads, 1, "Number of threads.");
 107 DEFINE_int32(num_iterations, 5, "Number of iterations.");
 108 DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds.");
 109 DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
 110             " nonmonotic steps.");
 111
 112 DEFINE_double(rotation_sigma, 0.0, "Standard deviation of camera rotation "
 113               "perturbation.");
 114 DEFINE_double(translation_sigma, 0.0, "Standard deviation of the camera "
 115               "translation perturbation.");
 116 DEFINE_double(point_sigma, 0.0, "Standard deviation of the point "
 117               "perturbation.");
 118 DEFINE_int32(random_seed, 38401, "Random seed used to set the state "
 119              "of the pseudo random number generator used to generate "
 120              "the pertubations.");
 121 DEFINE_bool(line_search, false, "Use a line search instead of trust region "
 122             "algorithm.");
 123 DEFINE_string(initial_ply, "", "Export the BAL file data as a PLY file.");
 124 DEFINE_string(final_ply, "", "Export the refined BAL file data as a PLY "
 125               "file.");
 126
 127 namespace ceres {
 128 namespace examples {
 129
 130 void SetLinearSolver(Solver::Options* options) {
 131   CHECK(StringToLinearSolverType(FLAGS_linear_solver,
 132                                  &options->linear_solver_type));
 133   CHECK(StringToPreconditionerType(FLAGS_preconditioner,
 134                                    &options->preconditioner_type));
 135   CHECK(StringToVisibilityClusteringType(FLAGS_visibility_clustering,
 136                                          &options->visibility_clustering_type));
 137   CHECK(StringToSparseLinearAlgebraLibraryType(
 138             FLAGS_sparse_linear_algebra_library,
 139             &options->sparse_linear_algebra_library_type));
 140   CHECK(StringToDenseLinearAlgebraLibraryType(
 141             FLAGS_dense_linear_algebra_library,
 142             &options->dense_linear_algebra_library_type));
 143   options->num_linear_solver_threads = FLAGS_num_threads;
 144   options->use_explicit_schur_complement = FLAGS_explicit_schur_complement;
 145 }
 146
 147 void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {
 148   const int num_points = bal_problem->num_points();
 149   const int point_block_size = bal_problem->point_block_size();
 150   double* points = bal_problem->mutable_points();
 151
 152   const int num_cameras = bal_problem->num_cameras();
 153   const int camera_block_size = bal_problem->camera_block_size();
 154   double* cameras = bal_problem->mutable_cameras();
 155
 156   if (options->use_inner_iterations) {
 157     if (FLAGS_blocks_for_inner_iterations == "cameras") {
 158       LOG(INFO) << "Camera blocks for inner iterations";
 159       options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
 160       for (int i = 0; i < num_cameras; ++i) {
 161         options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
 162       }
 163     } else if (FLAGS_blocks_for_inner_iterations == "points") {
 164       LOG(INFO) << "Point blocks for inner iterations";
 165       options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
 166       for (int i = 0; i < num_points; ++i) {
 167         options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
 168       }
 169     } else if (FLAGS_blocks_for_inner_iterations == "cameras,points") {
 170       LOG(INFO) << "Camera followed by point blocks for inner iterations";
 171       options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
 172       for (int i = 0; i < num_cameras; ++i) {
 173         options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
 174       }
 175       for (int i = 0; i < num_points; ++i) {
 176         options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1);
 177       }
 178     } else if (FLAGS_blocks_for_inner_iterations == "points,cameras") {
 179       LOG(INFO) << "Point followed by camera blocks for inner iterations";
 180       options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
 181       for (int i = 0; i < num_cameras; ++i) {
 182         options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
 183       }
 184       for (int i = 0; i < num_points; ++i) {
 185         options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
 186       }
 187     } else if (FLAGS_blocks_for_inner_iterations == "automatic") {
 188       LOG(INFO) << "Choosing automatic blocks for inner iterations";
 189     } else {
 190       LOG(FATAL) << "Unknown block type for inner iterations: "
 191                  << FLAGS_blocks_for_inner_iterations;
 192     }
 193   }
 194
 195   // Bundle adjustment problems have a sparsity structure that makes
 196   // them amenable to more specialized and much more efficient
 197   // solution strategies. The SPARSE_SCHUR, DENSE_SCHUR and
 198   // ITERATIVE_SCHUR solvers make use of this specialized
 199   // structure.
 200   //
 201   // This can either be done by specifying Options::ordering_type =
 202   // ceres::SCHUR, in which case Ceres will automatically determine
 203   // the right ParameterBlock ordering, or by manually specifying a
 204   // suitable ordering vector and defining
 205   // Options::num_eliminate_blocks.
 206   if (FLAGS_ordering == "automatic") {
 207     return;
 208   }
 209
 210   ceres::ParameterBlockOrdering* ordering =
 211       new ceres::ParameterBlockOrdering;
 212
 213   // The points come before the cameras.
 214   for (int i = 0; i < num_points; ++i) {
 215     ordering->AddElementToGroup(points + point_block_size * i, 0);
 216   }
 217
 218   for (int i = 0; i < num_cameras; ++i) {
 219     // When using axis-angle, there is a single parameter block for
 220     // the entire camera.
 221     ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
 222   }
 223
 224   options->linear_solver_ordering.reset(ordering);
 225 }
 226
 227 void SetMinimizerOptions(Solver::Options* options) {
 228   options->max_num_iterations = FLAGS_num_iterations;
 229   options->minimizer_progress_to_stdout = true;
 230   options->num_threads = FLAGS_num_threads;
 231   options->eta = FLAGS_eta;
 232   options->max_solver_time_in_seconds = FLAGS_max_solver_time;
 233   options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
 234   if (FLAGS_line_search) {
 235     options->minimizer_type = ceres::LINE_SEARCH;
 236   }
 237
 238   CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
 239                                         &options->trust_region_strategy_type));
 240   CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));
 241   options->use_inner_iterations = FLAGS_inner_iterations;
 242 }
 243
 244 void SetSolverOptionsFromFlags(BALProblem* bal_problem,
 245                                Solver::Options* options) {
 246   SetMinimizerOptions(options);
 247   SetLinearSolver(options);
 248   SetOrdering(bal_problem, options);
 249 }
 250
 251 void BuildProblem(BALProblem* bal_problem, Problem* problem) {
 252   const int point_block_size = bal_problem->point_block_size();
 253   const int camera_block_size = bal_problem->camera_block_size();
 254   double* points = bal_problem->mutable_points();
 255   double* cameras = bal_problem->mutable_cameras();
 256
 257   // Observations is 2*num_observations long array observations =
 258   // [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x
 259   // and y positions of the observation.
 260   const double* observations = bal_problem->observations();
 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     problem->AddResidualBlock(cost_function, loss_function, camera, point);
 284   }
 285
 286   if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
 287     LocalParameterization* camera_parameterization =
 288         new ProductParameterization(
 289             new QuaternionParameterization(),
 290             new IdentityParameterization(6));
 291     for (int i = 0; i < bal_problem->num_cameras(); ++i) {
 292       problem->SetParameterization(cameras + camera_block_size * i,
 293                                    camera_parameterization);
 294     }
 295   }
 296 }
 297
 298 void SolveProblem(const char* filename) {
 299   BALProblem bal_problem(filename, FLAGS_use_quaternions);
 300
 301   if (!FLAGS_initial_ply.empty()) {
 302     bal_problem.WriteToPLYFile(FLAGS_initial_ply);
 303   }
 304
 305   Problem problem;
 306
 307   srand(FLAGS_random_seed);
 308   bal_problem.Normalize();
 309   bal_problem.Perturb(FLAGS_rotation_sigma,
 310                       FLAGS_translation_sigma,
 311                       FLAGS_point_sigma);
 312
 313   BuildProblem(&bal_problem, &problem);
 314   Solver::Options options;
 315   SetSolverOptionsFromFlags(&bal_problem, &options);
 316   options.gradient_tolerance = 1e-16;
 317   options.function_tolerance = 1e-16;
 318   Solver::Summary summary;
 319   Solve(options, &problem, &summary);
 320   std::cout << summary.FullReport() << "\n";
 321
 322   if (!FLAGS_final_ply.empty()) {
 323     bal_problem.WriteToPLYFile(FLAGS_final_ply);
 324   }
 325 }
 326
 327 }  // namespace examples
 328 }  // namespace ceres
 329
 330 int main(int argc, char** argv) {
 331   CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
 332   google::InitGoogleLogging(argv[0]);
 333   if (FLAGS_input.empty()) {
 334     LOG(ERROR) << "Usage: bundle_adjuster --input=bal_problem";
 335     return 1;
 336   }
 337
 338   CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization)
 339       << "--use_local_parameterization can only be used with "
 340       << "--use_quaternions.";
 341   ceres::examples::SolveProblem(FLAGS_input.c_str());
 342   return 0;
 343 }