Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc
new file mode 100644
index 0000000..5619c52
--- /dev/null
+++ b/examples/bundle_adjuster.cc
@@ -0,0 +1,346 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// An example of solving a dynamically sized problem with various
+// solvers and loss functions.
+//
+// For a simpler bare bones example of doing bundle adjustment with
+// Ceres, please see simple_bundle_adjuster.cc.
+//
+// NOTE: This example will not compile without gflags and SuiteSparse.
+//
+// The problem being solved here is known as a Bundle Adjustment
+// problem in computer vision. Given a set of 3d points X_1, ..., X_n,
+// a set of cameras P_1, ..., P_m. If the point X_i is visible in
+// image j, then there is a 2D observation u_ij that is the expected
+// projection of X_i using P_j. The aim of this optimization is to
+// find values of X_i and P_j such that the reprojection error
+//
+// E(X,P) = sum_ij |u_ij - P_j X_i|^2
+//
+// is minimized.
+//
+// The problem used here comes from a collection of bundle adjustment
+// problems published at University of Washington.
+// http://grail.cs.washington.edu/projects/bal
+
+#include <algorithm>
+#include <cmath>
+#include <cstdio>
+#include <cstdlib>
+#include <string>
+#include <vector>
+
+#include "bal_problem.h"
+#include "ceres/ceres.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "snavely_reprojection_error.h"
+
+DEFINE_string(input, "", "Input File name");
+DEFINE_string(trust_region_strategy, "levenberg_marquardt",
+ "Options are: levenberg_marquardt, dogleg.");
+DEFINE_string(dogleg, "traditional_dogleg", "Options are: traditional_dogleg,"
+ "subspace_dogleg.");
+
+DEFINE_bool(inner_iterations, false, "Use inner iterations to non-linearly "
+ "refine each successful trust region step.");
+
+DEFINE_string(blocks_for_inner_iterations, "automatic", "Options are: "
+ "automatic, cameras, points, cameras,points, points,cameras");
+
+DEFINE_string(linear_solver, "sparse_schur", "Options are: "
+ "sparse_schur, dense_schur, iterative_schur, sparse_normal_cholesky, "
+ "dense_qr, dense_normal_cholesky and cgnr.");
+DEFINE_bool(explicit_schur_complement, false, "If using ITERATIVE_SCHUR "
+ "then explicitly compute the Schur complement.");
+DEFINE_string(preconditioner, "jacobi", "Options are: "
+ "identity, jacobi, schur_jacobi, cluster_jacobi, "
+ "cluster_tridiagonal.");
+DEFINE_string(visibility_clustering, "canonical_views",
+ "single_linkage, canonical_views");
+
+DEFINE_string(sparse_linear_algebra_library, "suite_sparse",
+ "Options are: suite_sparse and cx_sparse.");
+DEFINE_string(dense_linear_algebra_library, "eigen",
+ "Options are: eigen and lapack.");
+DEFINE_string(ordering, "automatic", "Options are: automatic, user.");
+
+DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent "
+ "rotations. If false, angle axis is used.");
+DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local "
+ "parameterization.");
+DEFINE_bool(robustify, false, "Use a robust loss function.");
+
+DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the "
+ "accuracy of each linear solve of the truncated newton step. "
+ "Changing this parameter can affect solve performance.");
+
+DEFINE_int32(num_threads, 1, "Number of threads.");
+DEFINE_int32(num_iterations, 5, "Number of iterations.");
+DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds.");
+DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
+ " nonmonotic steps.");
+
+DEFINE_double(rotation_sigma, 0.0, "Standard deviation of camera rotation "
+ "perturbation.");
+DEFINE_double(translation_sigma, 0.0, "Standard deviation of the camera "
+ "translation perturbation.");
+DEFINE_double(point_sigma, 0.0, "Standard deviation of the point "
+ "perturbation.");
+DEFINE_int32(random_seed, 38401, "Random seed used to set the state "
+ "of the pseudo random number generator used to generate "
+ "the pertubations.");
+DEFINE_bool(line_search, false, "Use a line search instead of trust region "
+ "algorithm.");
+DEFINE_bool(mixed_precision_solves, false, "Use mixed precision solves.");
+DEFINE_int32(max_num_refinement_iterations, 0, "Iterative refinement iterations");
+DEFINE_string(initial_ply, "", "Export the BAL file data as a PLY file.");
+DEFINE_string(final_ply, "", "Export the refined BAL file data as a PLY "
+ "file.");
+
+namespace ceres {
+namespace examples {
+
+void SetLinearSolver(Solver::Options* options) {
+ CHECK(StringToLinearSolverType(FLAGS_linear_solver,
+ &options->linear_solver_type));
+ CHECK(StringToPreconditionerType(FLAGS_preconditioner,
+ &options->preconditioner_type));
+ CHECK(StringToVisibilityClusteringType(FLAGS_visibility_clustering,
+ &options->visibility_clustering_type));
+ CHECK(StringToSparseLinearAlgebraLibraryType(
+ FLAGS_sparse_linear_algebra_library,
+ &options->sparse_linear_algebra_library_type));
+ CHECK(StringToDenseLinearAlgebraLibraryType(
+ FLAGS_dense_linear_algebra_library,
+ &options->dense_linear_algebra_library_type));
+ options->use_explicit_schur_complement = FLAGS_explicit_schur_complement;
+ options->use_mixed_precision_solves = FLAGS_mixed_precision_solves;
+ options->max_num_refinement_iterations = FLAGS_max_num_refinement_iterations;
+}
+
+void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {
+ const int num_points = bal_problem->num_points();
+ const int point_block_size = bal_problem->point_block_size();
+ double* points = bal_problem->mutable_points();
+
+ const int num_cameras = bal_problem->num_cameras();
+ const int camera_block_size = bal_problem->camera_block_size();
+ double* cameras = bal_problem->mutable_cameras();
+
+ if (options->use_inner_iterations) {
+ if (FLAGS_blocks_for_inner_iterations == "cameras") {
+ LOG(INFO) << "Camera blocks for inner iterations";
+ options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ for (int i = 0; i < num_cameras; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
+ }
+ } else if (FLAGS_blocks_for_inner_iterations == "points") {
+ LOG(INFO) << "Point blocks for inner iterations";
+ options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ for (int i = 0; i < num_points; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
+ }
+ } else if (FLAGS_blocks_for_inner_iterations == "cameras,points") {
+ LOG(INFO) << "Camera followed by point blocks for inner iterations";
+ options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ for (int i = 0; i < num_cameras; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
+ }
+ for (int i = 0; i < num_points; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1);
+ }
+ } else if (FLAGS_blocks_for_inner_iterations == "points,cameras") {
+ LOG(INFO) << "Point followed by camera blocks for inner iterations";
+ options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ for (int i = 0; i < num_cameras; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
+ }
+ for (int i = 0; i < num_points; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
+ }
+ } else if (FLAGS_blocks_for_inner_iterations == "automatic") {
+ LOG(INFO) << "Choosing automatic blocks for inner iterations";
+ } else {
+ LOG(FATAL) << "Unknown block type for inner iterations: "
+ << FLAGS_blocks_for_inner_iterations;
+ }
+ }
+
+ // Bundle adjustment problems have a sparsity structure that makes
+ // them amenable to more specialized and much more efficient
+ // solution strategies. The SPARSE_SCHUR, DENSE_SCHUR and
+ // ITERATIVE_SCHUR solvers make use of this specialized
+ // structure.
+ //
+ // This can either be done by specifying Options::ordering_type =
+ // ceres::SCHUR, in which case Ceres will automatically determine
+ // the right ParameterBlock ordering, or by manually specifying a
+ // suitable ordering vector and defining
+ // Options::num_eliminate_blocks.
+ if (FLAGS_ordering == "automatic") {
+ return;
+ }
+
+ ceres::ParameterBlockOrdering* ordering =
+ new ceres::ParameterBlockOrdering;
+
+ // The points come before the cameras.
+ for (int i = 0; i < num_points; ++i) {
+ ordering->AddElementToGroup(points + point_block_size * i, 0);
+ }
+
+ for (int i = 0; i < num_cameras; ++i) {
+ // When using axis-angle, there is a single parameter block for
+ // the entire camera.
+ ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
+ }
+
+ options->linear_solver_ordering.reset(ordering);
+}
+
+void SetMinimizerOptions(Solver::Options* options) {
+ options->max_num_iterations = FLAGS_num_iterations;
+ options->minimizer_progress_to_stdout = true;
+ options->num_threads = FLAGS_num_threads;
+ options->eta = FLAGS_eta;
+ options->max_solver_time_in_seconds = FLAGS_max_solver_time;
+ options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
+ if (FLAGS_line_search) {
+ options->minimizer_type = ceres::LINE_SEARCH;
+ }
+
+ CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
+ &options->trust_region_strategy_type));
+ CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));
+ options->use_inner_iterations = FLAGS_inner_iterations;
+}
+
+void SetSolverOptionsFromFlags(BALProblem* bal_problem,
+ Solver::Options* options) {
+ SetMinimizerOptions(options);
+ SetLinearSolver(options);
+ SetOrdering(bal_problem, options);
+}
+
+void BuildProblem(BALProblem* bal_problem, Problem* problem) {
+ const int point_block_size = bal_problem->point_block_size();
+ const int camera_block_size = bal_problem->camera_block_size();
+ double* points = bal_problem->mutable_points();
+ double* cameras = bal_problem->mutable_cameras();
+
+ // Observations is 2*num_observations long array observations =
+ // [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x
+ // and y positions of the observation.
+ const double* observations = bal_problem->observations();
+ for (int i = 0; i < bal_problem->num_observations(); ++i) {
+ CostFunction* cost_function;
+ // Each Residual block takes a point and a camera as input and
+ // outputs a 2 dimensional residual.
+ cost_function =
+ (FLAGS_use_quaternions)
+ ? SnavelyReprojectionErrorWithQuaternions::Create(
+ observations[2 * i + 0],
+ observations[2 * i + 1])
+ : SnavelyReprojectionError::Create(
+ observations[2 * i + 0],
+ observations[2 * i + 1]);
+
+ // If enabled use Huber's loss function.
+ LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;
+
+ // Each observation correponds to a pair of a camera and a point
+ // which are identified by camera_index()[i] and point_index()[i]
+ // respectively.
+ double* camera =
+ cameras + camera_block_size * bal_problem->camera_index()[i];
+ double* point = points + point_block_size * bal_problem->point_index()[i];
+ problem->AddResidualBlock(cost_function, loss_function, camera, point);
+ }
+
+ if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
+ LocalParameterization* camera_parameterization =
+ new ProductParameterization(
+ new QuaternionParameterization(),
+ new IdentityParameterization(6));
+ for (int i = 0; i < bal_problem->num_cameras(); ++i) {
+ problem->SetParameterization(cameras + camera_block_size * i,
+ camera_parameterization);
+ }
+ }
+}
+
+void SolveProblem(const char* filename) {
+ BALProblem bal_problem(filename, FLAGS_use_quaternions);
+
+ if (!FLAGS_initial_ply.empty()) {
+ bal_problem.WriteToPLYFile(FLAGS_initial_ply);
+ }
+
+ Problem problem;
+
+ srand(FLAGS_random_seed);
+ bal_problem.Normalize();
+ bal_problem.Perturb(FLAGS_rotation_sigma,
+ FLAGS_translation_sigma,
+ FLAGS_point_sigma);
+
+ BuildProblem(&bal_problem, &problem);
+ Solver::Options options;
+ SetSolverOptionsFromFlags(&bal_problem, &options);
+ options.gradient_tolerance = 1e-16;
+ options.function_tolerance = 1e-16;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ std::cout << summary.FullReport() << "\n";
+
+ if (!FLAGS_final_ply.empty()) {
+ bal_problem.WriteToPLYFile(FLAGS_final_ply);
+ }
+}
+
+} // namespace examples
+} // namespace ceres
+
+int main(int argc, char** argv) {
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+ google::InitGoogleLogging(argv[0]);
+ if (FLAGS_input.empty()) {
+ LOG(ERROR) << "Usage: bundle_adjuster --input=bal_problem";
+ return 1;
+ }
+
+ CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization)
+ << "--use_local_parameterization can only be used with "
+ << "--use_quaternions.";
+ ceres::examples::SolveProblem(FLAGS_input.c_str());
+ return 0;
+}