| // 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 <memory> |
| #include <string> |
| #include <thread> |
| #include <vector> |
| |
| #include "bal_problem.h" |
| #include "ceres/ceres.h" |
| #include "gflags/gflags.h" |
| #include "glog/logging.h" |
| #include "snavely_reprojection_error.h" |
| |
| // clang-format makes the gflags definitions too verbose |
| // clang-format off |
| |
| 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, schur_power_series_expansion, cluster_jacobi, " |
| "cluster_tridiagonal."); |
| DEFINE_string(visibility_clustering, "canonical_views", |
| "single_linkage, canonical_views"); |
| DEFINE_bool(use_spse_initialization, false, |
| "Use power series expansion to initialize the solution in ITERATIVE_SCHUR linear solver."); |
| |
| DEFINE_string(sparse_linear_algebra_library, "suite_sparse", |
| "Options are: suite_sparse, accelerate_sparse, eigen_sparse, and " |
| "cuda_sparse."); |
| DEFINE_string(dense_linear_algebra_library, "eigen", |
| "Options are: eigen, lapack, and cuda"); |
| DEFINE_string(ordering_type, "amd", "Options are: amd, nesdis"); |
| DEFINE_string(linear_solver_ordering, "user", |
| "Options are: automatic and user"); |
| |
| DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent " |
| "rotations. If false, angle axis is used."); |
| DEFINE_bool(use_manifolds, false, "For quaternions, use a manifold."); |
| 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. -1 = std::thread::hardware_concurrency."); |
| DEFINE_int32(num_iterations, 5, "Number of iterations."); |
| DEFINE_int32(max_linear_solver_iterations, 500, "Maximum number of iterations" |
| " for solution of linear system."); |
| DEFINE_double(spse_tolerance, 0.1, |
| "Tolerance to reach during the iterations of power series expansion initialization or preconditioning."); |
| DEFINE_int32(max_num_spse_iterations, 5, |
| "Maximum number of iterations for power series expansion initialization or preconditioning."); |
| 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 perturbations."); |
| 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."); |
| // clang-format on |
| |
| namespace ceres::examples { |
| namespace { |
| |
| void SetLinearSolver(Solver::Options* options) { |
| CHECK(StringToLinearSolverType(CERES_GET_FLAG(FLAGS_linear_solver), |
| &options->linear_solver_type)); |
| CHECK(StringToPreconditionerType(CERES_GET_FLAG(FLAGS_preconditioner), |
| &options->preconditioner_type)); |
| CHECK(StringToVisibilityClusteringType( |
| CERES_GET_FLAG(FLAGS_visibility_clustering), |
| &options->visibility_clustering_type)); |
| CHECK(StringToSparseLinearAlgebraLibraryType( |
| CERES_GET_FLAG(FLAGS_sparse_linear_algebra_library), |
| &options->sparse_linear_algebra_library_type)); |
| CHECK(StringToDenseLinearAlgebraLibraryType( |
| CERES_GET_FLAG(FLAGS_dense_linear_algebra_library), |
| &options->dense_linear_algebra_library_type)); |
| CHECK( |
| StringToLinearSolverOrderingType(CERES_GET_FLAG(FLAGS_ordering_type), |
| &options->linear_solver_ordering_type)); |
| options->use_explicit_schur_complement = |
| CERES_GET_FLAG(FLAGS_explicit_schur_complement); |
| options->use_mixed_precision_solves = |
| CERES_GET_FLAG(FLAGS_mixed_precision_solves); |
| options->max_num_refinement_iterations = |
| CERES_GET_FLAG(FLAGS_max_num_refinement_iterations); |
| options->max_linear_solver_iterations = |
| CERES_GET_FLAG(FLAGS_max_linear_solver_iterations); |
| options->use_spse_initialization = |
| CERES_GET_FLAG(FLAGS_use_spse_initialization); |
| options->spse_tolerance = CERES_GET_FLAG(FLAGS_spse_tolerance); |
| options->max_num_spse_iterations = |
| CERES_GET_FLAG(FLAGS_max_num_spse_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 (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == "cameras") { |
| LOG(INFO) << "Camera blocks for inner iterations"; |
| options->inner_iteration_ordering = |
| std::make_shared<ParameterBlockOrdering>(); |
| for (int i = 0; i < num_cameras; ++i) { |
| options->inner_iteration_ordering->AddElementToGroup( |
| cameras + camera_block_size * i, 0); |
| } |
| } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == "points") { |
| LOG(INFO) << "Point blocks for inner iterations"; |
| options->inner_iteration_ordering = |
| std::make_shared<ParameterBlockOrdering>(); |
| for (int i = 0; i < num_points; ++i) { |
| options->inner_iteration_ordering->AddElementToGroup( |
| points + point_block_size * i, 0); |
| } |
| } else if (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == |
| "cameras,points") { |
| LOG(INFO) << "Camera followed by point blocks for inner iterations"; |
| options->inner_iteration_ordering = |
| std::make_shared<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 (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == |
| "points,cameras") { |
| LOG(INFO) << "Point followed by camera blocks for inner iterations"; |
| options->inner_iteration_ordering = |
| std::make_shared<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 (CERES_GET_FLAG(FLAGS_blocks_for_inner_iterations) == |
| "automatic") { |
| LOG(INFO) << "Choosing automatic blocks for inner iterations"; |
| } else { |
| LOG(FATAL) << "Unknown block type for inner iterations: " |
| << CERES_GET_FLAG(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 a |
| // Options::linear_solver_ordering or having Ceres figure it out |
| // automatically using a greedy maximum independent set algorithm. |
| if (CERES_GET_FLAG(FLAGS_linear_solver_ordering) == "user") { |
| auto* 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 = CERES_GET_FLAG(FLAGS_num_iterations); |
| options->minimizer_progress_to_stdout = true; |
| if (CERES_GET_FLAG(FLAGS_num_threads) == -1) { |
| const int num_available_threads = |
| static_cast<int>(std::thread::hardware_concurrency()); |
| if (num_available_threads > 0) { |
| options->num_threads = num_available_threads; |
| } |
| } else { |
| options->num_threads = CERES_GET_FLAG(FLAGS_num_threads); |
| } |
| CHECK_GE(options->num_threads, 1); |
| |
| options->eta = CERES_GET_FLAG(FLAGS_eta); |
| options->max_solver_time_in_seconds = CERES_GET_FLAG(FLAGS_max_solver_time); |
| options->use_nonmonotonic_steps = CERES_GET_FLAG(FLAGS_nonmonotonic_steps); |
| if (CERES_GET_FLAG(FLAGS_line_search)) { |
| options->minimizer_type = ceres::LINE_SEARCH; |
| } |
| |
| CHECK(StringToTrustRegionStrategyType( |
| CERES_GET_FLAG(FLAGS_trust_region_strategy), |
| &options->trust_region_strategy_type)); |
| CHECK( |
| StringToDoglegType(CERES_GET_FLAG(FLAGS_dogleg), &options->dogleg_type)); |
| options->use_inner_iterations = CERES_GET_FLAG(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 = (CERES_GET_FLAG(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 = |
| CERES_GET_FLAG(FLAGS_robustify) ? new HuberLoss(1.0) : nullptr; |
| |
| // Each observation corresponds 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 (CERES_GET_FLAG(FLAGS_use_quaternions) && |
| CERES_GET_FLAG(FLAGS_use_manifolds)) { |
| Manifold* camera_manifold = |
| new ProductManifold<QuaternionManifold, EuclideanManifold<6>>{}; |
| for (int i = 0; i < bal_problem->num_cameras(); ++i) { |
| problem->SetManifold(cameras + camera_block_size * i, camera_manifold); |
| } |
| } |
| } |
| |
| void SolveProblem(const char* filename) { |
| BALProblem bal_problem(filename, CERES_GET_FLAG(FLAGS_use_quaternions)); |
| |
| if (!CERES_GET_FLAG(FLAGS_initial_ply).empty()) { |
| bal_problem.WriteToPLYFile(CERES_GET_FLAG(FLAGS_initial_ply)); |
| } |
| |
| Problem problem; |
| |
| srand(CERES_GET_FLAG(FLAGS_random_seed)); |
| bal_problem.Normalize(); |
| bal_problem.Perturb(CERES_GET_FLAG(FLAGS_rotation_sigma), |
| CERES_GET_FLAG(FLAGS_translation_sigma), |
| CERES_GET_FLAG(FLAGS_point_sigma)); |
| |
| BuildProblem(&bal_problem, &problem); |
| Solver::Options options; |
| SetSolverOptionsFromFlags(&bal_problem, &options); |
| options.gradient_tolerance = 1e-16; |
| options.function_tolerance = 1e-16; |
| options.parameter_tolerance = 1e-16; |
| Solver::Summary summary; |
| Solve(options, &problem, &summary); |
| std::cout << summary.FullReport() << "\n"; |
| |
| if (!CERES_GET_FLAG(FLAGS_final_ply).empty()) { |
| bal_problem.WriteToPLYFile(CERES_GET_FLAG(FLAGS_final_ply)); |
| } |
| } |
| |
| } // namespace |
| } // namespace ceres::examples |
| |
| int main(int argc, char** argv) { |
| GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); |
| google::InitGoogleLogging(argv[0]); |
| if (CERES_GET_FLAG(FLAGS_input).empty()) { |
| LOG(ERROR) << "Usage: bundle_adjuster --input=bal_problem"; |
| return 1; |
| } |
| |
| CHECK(CERES_GET_FLAG(FLAGS_use_quaternions) || |
| !CERES_GET_FLAG(FLAGS_use_manifolds)) |
| << "--use_manifolds can only be used with --use_quaternions."; |
| ceres::examples::SolveProblem(CERES_GET_FLAG(FLAGS_input).c_str()); |
| return 0; |
| } |