GoogleGit

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