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 (at) 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_string(preconditioner, "jacobi", "Options are: " 83 "identity, jacobi, schur_jacobi, cluster_jacobi, " 84 "cluster_tridiagonal."); 85 DEFINE_string(sparse_linear_algebra_library, "suite_sparse", 86 "Options are: suite_sparse and cx_sparse."); 87 DEFINE_string(dense_linear_algebra_library, "eigen", 88 "Options are: eigen and lapack."); 89 DEFINE_string(ordering, "automatic", "Options are: automatic, user."); 90 91 DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent " 92 "rotations. If false, angle axis is used."); 93 DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local " 94 "parameterization."); 95 DEFINE_bool(robustify, false, "Use a robust loss function."); 96 97 DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the " 98 "accuracy of each linear solve of the truncated newton step. " 99 "Changing this parameter can affect solve performance."); 100 101 DEFINE_int32(num_threads, 1, "Number of threads."); 102 DEFINE_int32(num_iterations, 5, "Number of iterations."); 103 DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds."); 104 DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use" 105 " nonmonotic steps."); 106 107 DEFINE_double(rotation_sigma, 0.0, "Standard deviation of camera rotation " 108 "perturbation."); 109 DEFINE_double(translation_sigma, 0.0, "Standard deviation of the camera " 110 "translation perturbation."); 111 DEFINE_double(point_sigma, 0.0, "Standard deviation of the point " 112 "perturbation."); 113 DEFINE_int32(random_seed, 38401, "Random seed used to set the state " 114 "of the pseudo random number generator used to generate " 115 "the pertubations."); 116 DEFINE_string(solver_log, "", "File to record the solver execution to."); 117 DEFINE_bool(line_search, false, "Use a line search instead of trust region " 118 "algorithm."); 119 120 namespace ceres { 121 namespace examples { 122 123 void SetLinearSolver(Solver::Options* options) { 124 CHECK(StringToLinearSolverType(FLAGS_linear_solver, 125 &options->linear_solver_type)); 126 CHECK(StringToPreconditionerType(FLAGS_preconditioner, 127 &options->preconditioner_type)); 128 CHECK(StringToSparseLinearAlgebraLibraryType( 129 FLAGS_sparse_linear_algebra_library, 130 &options->sparse_linear_algebra_library_type)); 131 CHECK(StringToDenseLinearAlgebraLibraryType( 132 FLAGS_dense_linear_algebra_library, 133 &options->dense_linear_algebra_library_type)); 134 options->num_linear_solver_threads = FLAGS_num_threads; 135 } 136 137 void SetOrdering(BALProblem* bal_problem, Solver::Options* options) { 138 const int num_points = bal_problem->num_points(); 139 const int point_block_size = bal_problem->point_block_size(); 140 double* points = bal_problem->mutable_points(); 141 142 const int num_cameras = bal_problem->num_cameras(); 143 const int camera_block_size = bal_problem->camera_block_size(); 144 double* cameras = bal_problem->mutable_cameras(); 145 146 if (options->use_inner_iterations) { 147 if (FLAGS_blocks_for_inner_iterations == "cameras") { 148 LOG(INFO) << "Camera blocks for inner iterations"; 149 options->inner_iteration_ordering = new ParameterBlockOrdering; 150 for (int i = 0; i < num_cameras; ++i) { 151 options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0); 152 } 153 } else if (FLAGS_blocks_for_inner_iterations == "points") { 154 LOG(INFO) << "Point blocks for inner iterations"; 155 options->inner_iteration_ordering = new ParameterBlockOrdering; 156 for (int i = 0; i < num_points; ++i) { 157 options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0); 158 } 159 } else if (FLAGS_blocks_for_inner_iterations == "cameras,points") { 160 LOG(INFO) << "Camera followed by point blocks for inner iterations"; 161 options->inner_iteration_ordering = new ParameterBlockOrdering; 162 for (int i = 0; i < num_cameras; ++i) { 163 options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0); 164 } 165 for (int i = 0; i < num_points; ++i) { 166 options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1); 167 } 168 } else if (FLAGS_blocks_for_inner_iterations == "points,cameras") { 169 LOG(INFO) << "Point followed by camera blocks for inner iterations"; 170 options->inner_iteration_ordering = new ParameterBlockOrdering; 171 for (int i = 0; i < num_cameras; ++i) { 172 options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1); 173 } 174 for (int i = 0; i < num_points; ++i) { 175 options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0); 176 } 177 } else if (FLAGS_blocks_for_inner_iterations == "automatic") { 178 LOG(INFO) << "Choosing automatic blocks for inner iterations"; 179 } else { 180 LOG(FATAL) << "Unknown block type for inner iterations: " 181 << FLAGS_blocks_for_inner_iterations; 182 } 183 } 184 185 // Bundle adjustment problems have a sparsity structure that makes 186 // them amenable to more specialized and much more efficient 187 // solution strategies. The SPARSE_SCHUR, DENSE_SCHUR and 188 // ITERATIVE_SCHUR solvers make use of this specialized 189 // structure. 190 // 191 // This can either be done by specifying Options::ordering_type = 192 // ceres::SCHUR, in which case Ceres will automatically determine 193 // the right ParameterBlock ordering, or by manually specifying a 194 // suitable ordering vector and defining 195 // Options::num_eliminate_blocks. 196 if (FLAGS_ordering == "automatic") { 197 return; 198 } 199 200 ceres::ParameterBlockOrdering* ordering = 201 new ceres::ParameterBlockOrdering; 202 203 // The points come before the cameras. 204 for (int i = 0; i < num_points; ++i) { 205 ordering->AddElementToGroup(points + point_block_size * i, 0); 206 } 207 208 for (int i = 0; i < num_cameras; ++i) { 209 // When using axis-angle, there is a single parameter block for 210 // the entire camera. 211 ordering->AddElementToGroup(cameras + camera_block_size * i, 1); 212 // If quaternions are used, there are two blocks, so add the 213 // second block to the ordering. 214 if (FLAGS_use_quaternions) { 215 ordering->AddElementToGroup(cameras + camera_block_size * i + 4, 1); 216 } 217 } 218 219 options->linear_solver_ordering = ordering; 220 } 221 222 void SetMinimizerOptions(Solver::Options* options) { 223 options->max_num_iterations = FLAGS_num_iterations; 224 options->minimizer_progress_to_stdout = true; 225 options->num_threads = FLAGS_num_threads; 226 options->eta = FLAGS_eta; 227 options->max_solver_time_in_seconds = FLAGS_max_solver_time; 228 options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps; 229 if (FLAGS_line_search) { 230 options->minimizer_type = ceres::LINE_SEARCH; 231 } 232 233 CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy, 234 &options->trust_region_strategy_type)); 235 CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type)); 236 options->use_inner_iterations = FLAGS_inner_iterations; 237 } 238 239 void SetSolverOptionsFromFlags(BALProblem* bal_problem, 240 Solver::Options* options) { 241 SetMinimizerOptions(options); 242 SetLinearSolver(options); 243 SetOrdering(bal_problem, options); 244 } 245 246 void BuildProblem(BALProblem* bal_problem, Problem* problem) { 247 const int point_block_size = bal_problem->point_block_size(); 248 const int camera_block_size = bal_problem->camera_block_size(); 249 double* points = bal_problem->mutable_points(); 250 double* cameras = bal_problem->mutable_cameras(); 251 252 // Observations is 2*num_observations long array observations = 253 // [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x 254 // and y positions of the observation. 255 const double* observations = bal_problem->observations(); 256 257 for (int i = 0; i < bal_problem->num_observations(); ++i) { 258 CostFunction* cost_function; 259 // Each Residual block takes a point and a camera as input and 260 // outputs a 2 dimensional residual. 261 if (FLAGS_use_quaternions) { 262 cost_function = new AutoDiffCostFunction< 263 SnavelyReprojectionErrorWithQuaternions, 2, 4, 6, 3>( 264 new SnavelyReprojectionErrorWithQuaternions( 265 observations[2 * i + 0], 266 observations[2 * i + 1])); 267 } else { 268 cost_function = 269 new AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>( 270 new SnavelyReprojectionError(observations[2 * i + 0], 271 observations[2 * i + 1])); 272 } 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 309 void 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.solver_log = FLAGS_solver_log; 323 options.gradient_tolerance = 1e-16; 324 options.function_tolerance = 1e-16; 325 Solver::Summary summary; 326 Solve(options, &problem, &summary); 327 std::cout << summary.FullReport() << "\n"; 328 } 329 330 } // namespace examples 331 } // namespace ceres 332 333 int main(int argc, char** argv) { 334 google::ParseCommandLineFlags(&argc, &argv, true); 335 google::InitGoogleLogging(argv[0]); 336 if (FLAGS_input.empty()) { 337 LOG(ERROR) << "Usage: bundle_adjustment_example --input=bal_problem"; 338 return 1; 339 } 340 341 CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization) 342 << "--use_local_parameterization can only be used with " 343 << "--use_quaternions."; 344 ceres::examples::SolveProblem(FLAGS_input.c_str()); 345 return 0; 346 } 347