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