1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
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.
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.
29 // Author: keir@google.com (Keir Mierle)
30 // sameeragarwal@google.com (Sameer Agarwal)
32 // End-to-end bundle adjustment tests for Ceres. It uses a bundle
33 // adjustment problem with 16 cameras and two thousand points.
40 #include "ceres/internal/port.h"
42 #include "ceres/autodiff_cost_function.h"
43 #include "ceres/ordered_groups.h"
44 #include "ceres/problem.h"
45 #include "ceres/rotation.h"
46 #include "ceres/solver.h"
47 #include "ceres/stringprintf.h"
48 #include "ceres/test_util.h"
49 #include "ceres/types.h"
50 #include "gflags/gflags.h"
51 #include "glog/logging.h"
52 #include "gtest/gtest.h"
60 const bool kAutomaticOrdering = true;
61 const bool kUserOrdering = false;
63 // This class implements the SystemTestProblem interface and provides
64 // access to a bundle adjustment problem. It is based on
65 // examples/bundle_adjustment_example.cc. Currently a small 16 camera
66 // problem is hard coded in the constructor.
67 class BundleAdjustmentProblem {
69 BundleAdjustmentProblem() {
70 const string input_file = TestFileAbsolutePath("problem-16-22106-pre.txt");
75 ~BundleAdjustmentProblem() {
76 delete []point_index_;
77 delete []camera_index_;
78 delete []observations_;
82 Problem* mutable_problem() { return &problem_; }
83 Solver::Options* mutable_solver_options() { return &options_; }
85 int num_cameras() const { return num_cameras_; }
86 int num_points() const { return num_points_; }
87 int num_observations() const { return num_observations_; }
88 const int* point_index() const { return point_index_; }
89 const int* camera_index() const { return camera_index_; }
90 const double* observations() const { return observations_; }
91 double* mutable_cameras() { return parameters_; }
92 double* mutable_points() { return parameters_ + 9 * num_cameras_; }
94 static double kResidualTolerance;
97 void ReadData(const string& filename) {
98 FILE * fptr = fopen(filename.c_str(), "r");
101 LOG(FATAL) << "File Error: unable to open file " << filename;
104 // This will die horribly on invalid files. Them's the breaks.
105 FscanfOrDie(fptr, "%d", &num_cameras_);
106 FscanfOrDie(fptr, "%d", &num_points_);
107 FscanfOrDie(fptr, "%d", &num_observations_);
109 VLOG(1) << "Header: " << num_cameras_
110 << " " << num_points_
111 << " " << num_observations_;
113 point_index_ = new int[num_observations_];
114 camera_index_ = new int[num_observations_];
115 observations_ = new double[2 * num_observations_];
117 num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
118 parameters_ = new double[num_parameters_];
120 for (int i = 0; i < num_observations_; ++i) {
121 FscanfOrDie(fptr, "%d", camera_index_ + i);
122 FscanfOrDie(fptr, "%d", point_index_ + i);
123 for (int j = 0; j < 2; ++j) {
124 FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
128 for (int i = 0; i < num_parameters_; ++i) {
129 FscanfOrDie(fptr, "%lf", parameters_ + i);
133 void BuildProblem() {
134 double* points = mutable_points();
135 double* cameras = mutable_cameras();
137 for (int i = 0; i < num_observations(); ++i) {
138 // Each Residual block takes a point and a camera as input and
139 // outputs a 2 dimensional residual.
140 CostFunction* cost_function =
141 new AutoDiffCostFunction<BundlerResidual, 2, 9, 3>(
142 new BundlerResidual(observations_[2*i + 0],
143 observations_[2*i + 1]));
145 // Each observation correponds to a pair of a camera and a point
146 // which are identified by camera_index()[i] and
147 // point_index()[i] respectively.
148 double* camera = cameras + 9 * camera_index_[i];
149 double* point = points + 3 * point_index()[i];
150 problem_.AddResidualBlock(cost_function, NULL, camera, point);
153 options_.linear_solver_ordering.reset(new ParameterBlockOrdering);
155 // The points come before the cameras.
156 for (int i = 0; i < num_points_; ++i) {
157 options_.linear_solver_ordering->AddElementToGroup(points + 3 * i, 0);
160 for (int i = 0; i < num_cameras_; ++i) {
161 options_.linear_solver_ordering->AddElementToGroup(cameras + 9 * i, 1);
164 options_.linear_solver_type = DENSE_SCHUR;
165 options_.max_num_iterations = 25;
166 options_.function_tolerance = 1e-10;
167 options_.gradient_tolerance = 1e-10;
168 options_.parameter_tolerance = 1e-10;
172 void FscanfOrDie(FILE *fptr, const char *format, T *value) {
173 int num_scanned = fscanf(fptr, format, value);
174 if (num_scanned != 1) {
175 LOG(FATAL) << "Invalid UW data file.";
179 // Templated pinhole camera model. The camera is parameterized
180 // using 9 parameters. 3 for rotation, 3 for translation, 1 for
181 // focal length and 2 for radial distortion. The principal point is
182 // not modeled (i.e. it is assumed to be located at the image
184 struct BundlerResidual {
185 // (u, v): the position of the observation with respect to the image
187 BundlerResidual(double u, double v): u(u), v(v) {}
189 template <typename T>
190 bool operator()(const T* const camera,
191 const T* const point,
192 T* residuals) const {
194 AngleAxisRotatePoint(camera, point, p);
196 // Add the translation vector
201 const T& focal = camera[6];
202 const T& l1 = camera[7];
203 const T& l2 = camera[8];
205 // Compute the center of distortion. The sign change comes from
206 // the camera model that Noah Snavely's Bundler assumes, whereby
207 // the camera coordinate system has a negative z axis.
208 T xp = - focal * p[0] / p[2];
209 T yp = - focal * p[1] / p[2];
211 // Apply second and fourth order radial distortion.
212 T r2 = xp*xp + yp*yp;
213 T distortion = T(1.0) + r2 * (l1 + l2 * r2);
215 residuals[0] = distortion * xp - u;
216 residuals[1] = distortion * yp - v;
226 Solver::Options options_;
230 int num_observations_;
235 double* observations_;
236 // The parameter vector is laid out as follows
237 // [camera_1, ..., camera_n, point_1, ..., point_m]
241 double BundleAdjustmentProblem::kResidualTolerance = 1e-4;
242 typedef SystemTest<BundleAdjustmentProblem> BundleAdjustmentTest;
244 TEST_F(BundleAdjustmentTest, DenseSchurWithAutomaticOrdering) {
245 RunSolverForConfigAndExpectResidualsMatch(
246 SolverConfig(DENSE_SCHUR, NO_SPARSE, kAutomaticOrdering));
249 TEST_F(BundleAdjustmentTest, DenseSchurWithUserOrdering) {
250 RunSolverForConfigAndExpectResidualsMatch(
251 SolverConfig(DENSE_SCHUR, NO_SPARSE, kUserOrdering));
254 TEST_F(BundleAdjustmentTest, IterativeSchurWithJacobiAndAutomaticOrdering) {
255 RunSolverForConfigAndExpectResidualsMatch(
256 SolverConfig(ITERATIVE_SCHUR, NO_SPARSE, kAutomaticOrdering, JACOBI));
259 TEST_F(BundleAdjustmentTest, IterativeSchurWithJacobiAndUserOrdering) {
260 RunSolverForConfigAndExpectResidualsMatch(
261 SolverConfig(ITERATIVE_SCHUR, NO_SPARSE, kUserOrdering, JACOBI));
264 TEST_F(BundleAdjustmentTest,
265 IterativeSchurWithSchurJacobiAndAutomaticOrdering) {
266 RunSolverForConfigAndExpectResidualsMatch(
267 SolverConfig(ITERATIVE_SCHUR,
273 TEST_F(BundleAdjustmentTest, IterativeSchurWithSchurJacobiAndUserOrdering) {
274 RunSolverForConfigAndExpectResidualsMatch(
275 SolverConfig(ITERATIVE_SCHUR, NO_SPARSE, kUserOrdering, SCHUR_JACOBI));
278 #ifndef CERES_NO_SUITESPARSE
279 TEST_F(BundleAdjustmentTest,
280 SparseNormalCholeskyWithAutomaticOrderingUsingSuiteSparse) {
281 RunSolverForConfigAndExpectResidualsMatch(
282 SolverConfig(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, kAutomaticOrdering));
285 TEST_F(BundleAdjustmentTest,
286 SparseNormalCholeskyWithUserOrderingUsingSuiteSparse) {
287 RunSolverForConfigAndExpectResidualsMatch(
288 SolverConfig(SPARSE_NORMAL_CHOLESKY, SUITE_SPARSE, kUserOrdering));
291 TEST_F(BundleAdjustmentTest,
292 SparseSchurWithAutomaticOrderingUsingSuiteSparse) {
293 RunSolverForConfigAndExpectResidualsMatch(
294 SolverConfig(SPARSE_SCHUR, SUITE_SPARSE, kAutomaticOrdering));
297 TEST_F(BundleAdjustmentTest, SparseSchurWithUserOrderingUsingSuiteSparse) {
298 RunSolverForConfigAndExpectResidualsMatch(
299 SolverConfig(SPARSE_SCHUR, SUITE_SPARSE, kUserOrdering));
302 TEST_F(BundleAdjustmentTest,
303 IterativeSchurWithClusterJacobiAndAutomaticOrderingUsingSuiteSparse) {
304 RunSolverForConfigAndExpectResidualsMatch(
305 SolverConfig(ITERATIVE_SCHUR,
311 TEST_F(BundleAdjustmentTest,
312 IterativeSchurWithClusterJacobiAndUserOrderingUsingSuiteSparse) {
313 RunSolverForConfigAndExpectResidualsMatch(
314 SolverConfig(ITERATIVE_SCHUR,
320 TEST_F(BundleAdjustmentTest,
321 IterativeSchurWithClusterTridiagonalAndAutomaticOrderingUsingSuiteSparse) {
322 RunSolverForConfigAndExpectResidualsMatch(
323 SolverConfig(ITERATIVE_SCHUR,
326 CLUSTER_TRIDIAGONAL));
329 TEST_F(BundleAdjustmentTest,
330 IterativeSchurWithClusterTridiagonalAndUserOrderingUsingSuiteSparse) {
331 RunSolverForConfigAndExpectResidualsMatch(
332 SolverConfig(ITERATIVE_SCHUR,
335 CLUSTER_TRIDIAGONAL));
337 #endif // CERES_NO_SUITESPARSE
339 #ifndef CERES_NO_CXSPARSE
340 TEST_F(BundleAdjustmentTest,
341 SparseNormalCholeskyWithAutomaticOrderingUsingCXSparse) {
342 RunSolverForConfigAndExpectResidualsMatch(
343 SolverConfig(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, kAutomaticOrdering));
346 TEST_F(BundleAdjustmentTest,
347 SparseNormalCholeskyWithUserOrderingUsingCXSparse) {
348 RunSolverForConfigAndExpectResidualsMatch(
349 SolverConfig(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, kUserOrdering));
352 TEST_F(BundleAdjustmentTest, SparseSchurWithAutomaticOrderingUsingCXSparse) {
353 RunSolverForConfigAndExpectResidualsMatch(
354 SolverConfig(SPARSE_SCHUR, CX_SPARSE, kAutomaticOrdering));
357 TEST_F(BundleAdjustmentTest, SparseSchurWithUserOrderingUsingCXSparse) {
358 RunSolverForConfigAndExpectResidualsMatch(
359 SolverConfig(SPARSE_SCHUR, CX_SPARSE, kUserOrdering));
361 #endif // CERES_NO_CXSPARSE
363 #ifdef CERES_USE_EIGEN_SPARSE
364 TEST_F(BundleAdjustmentTest,
365 SparseNormalCholeskyWithAutomaticOrderingUsingEigenSparse) {
366 RunSolverForConfigAndExpectResidualsMatch(
367 SolverConfig(SPARSE_NORMAL_CHOLESKY, EIGEN_SPARSE, kAutomaticOrdering));
370 TEST_F(BundleAdjustmentTest,
371 SparseNormalCholeskyWithUserOrderingUsingEigenSparse) {
372 RunSolverForConfigAndExpectResidualsMatch(
373 SolverConfig(SPARSE_NORMAL_CHOLESKY, EIGEN_SPARSE, kUserOrdering));
376 TEST_F(BundleAdjustmentTest,
377 SparseSchurWithAutomaticOrderingUsingEigenSparse) {
378 RunSolverForConfigAndExpectResidualsMatch(
379 SolverConfig(SPARSE_SCHUR, EIGEN_SPARSE, kAutomaticOrdering));
382 TEST_F(BundleAdjustmentTest, SparseSchurWithUserOrderingUsingEigenSparse) {
383 RunSolverForConfigAndExpectResidualsMatch(
384 SolverConfig(SPARSE_SCHUR, EIGEN_SPARSE, kUserOrdering));
386 #endif // CERES_USE_EIGEN_SPARSE
388 #ifdef CERES_USE_OPENMP
390 TEST_F(BundleAdjustmentTest, MultiThreadedDenseSchurWithAutomaticOrdering) {
391 RunSolverForConfigAndExpectResidualsMatch(
392 ThreadedSolverConfig(DENSE_SCHUR, NO_SPARSE, kAutomaticOrdering));
395 TEST_F(BundleAdjustmentTest, MultiThreadedDenseSchurWithUserOrdering) {
396 RunSolverForConfigAndExpectResidualsMatch(
397 ThreadedSolverConfig(DENSE_SCHUR, NO_SPARSE, kUserOrdering));
400 TEST_F(BundleAdjustmentTest,
401 MultiThreadedIterativeSchurWithJacobiAndAutomaticOrdering) {
402 RunSolverForConfigAndExpectResidualsMatch(
403 ThreadedSolverConfig(ITERATIVE_SCHUR,
409 TEST_F(BundleAdjustmentTest,
410 MultiThreadedIterativeSchurWithJacobiAndUserOrdering) {
411 RunSolverForConfigAndExpectResidualsMatch(
412 ThreadedSolverConfig(ITERATIVE_SCHUR, NO_SPARSE, kUserOrdering, JACOBI));
415 TEST_F(BundleAdjustmentTest,
416 MultiThreadedIterativeSchurWithSchurJacobiAndAutomaticOrdering) {
417 RunSolverForConfigAndExpectResidualsMatch(
418 ThreadedSolverConfig(ITERATIVE_SCHUR,
424 TEST_F(BundleAdjustmentTest,
425 MultiThreadedIterativeSchurWithSchurJacobiAndUserOrdering) {
426 RunSolverForConfigAndExpectResidualsMatch(
427 ThreadedSolverConfig(ITERATIVE_SCHUR,
433 #ifndef CERES_NO_SUITESPARSE
434 TEST_F(BundleAdjustmentTest,
435 MultiThreadedSparseNormalCholeskyWithAutomaticOrderingUsingSuiteSparse) {
436 RunSolverForConfigAndExpectResidualsMatch(
437 ThreadedSolverConfig(SPARSE_NORMAL_CHOLESKY,
439 kAutomaticOrdering));
442 TEST_F(BundleAdjustmentTest,
443 MultiThreadedSparseNormalCholeskyWithUserOrderingUsingSuiteSparse) {
444 RunSolverForConfigAndExpectResidualsMatch(
445 ThreadedSolverConfig(SPARSE_NORMAL_CHOLESKY,
450 TEST_F(BundleAdjustmentTest,
451 MultiThreadedSparseSchurWithAutomaticOrderingUsingSuiteSparse) {
452 RunSolverForConfigAndExpectResidualsMatch(
453 ThreadedSolverConfig(SPARSE_SCHUR,
455 kAutomaticOrdering));
458 TEST_F(BundleAdjustmentTest,
459 MultiThreadedSparseSchurWithUserOrderingUsingSuiteSparse) {
460 RunSolverForConfigAndExpectResidualsMatch(
461 ThreadedSolverConfig(SPARSE_SCHUR, SUITE_SPARSE, kUserOrdering));
464 TEST_F(BundleAdjustmentTest,
465 MultiThreadedIterativeSchurWithClusterJacobiAndAutomaticOrderingUsingSuiteSparse) { // NOLINT
466 RunSolverForConfigAndExpectResidualsMatch(
467 ThreadedSolverConfig(ITERATIVE_SCHUR,
473 TEST_F(BundleAdjustmentTest,
474 MultiThreadedIterativeSchurWithClusterJacobiAndUserOrderingUsingSuiteSparse) { // NOLINT
475 RunSolverForConfigAndExpectResidualsMatch(
476 ThreadedSolverConfig(ITERATIVE_SCHUR,
482 TEST_F(BundleAdjustmentTest,
483 MultiThreadedIterativeSchurWithClusterTridiagonalAndAutomaticOrderingUsingSuiteSparse) { // NOLINT
484 RunSolverForConfigAndExpectResidualsMatch(
485 ThreadedSolverConfig(ITERATIVE_SCHUR,
488 CLUSTER_TRIDIAGONAL));
491 TEST_F(BundleAdjustmentTest,
492 MultiThreadedIterativeSchurWithClusterTridiagonalAndUserOrderingUsingSuiteSparse) { // NOTLINT
493 RunSolverForConfigAndExpectResidualsMatch(
494 ThreadedSolverConfig(ITERATIVE_SCHUR,
497 CLUSTER_TRIDIAGONAL));
499 #endif // CERES_NO_SUITESPARSE
501 #ifndef CERES_NO_CXSPARSE
502 TEST_F(BundleAdjustmentTest,
503 MultiThreadedSparseNormalCholeskyWithAutomaticOrderingUsingCXSparse) {
504 RunSolverForConfigAndExpectResidualsMatch(
505 ThreadedSolverConfig(SPARSE_NORMAL_CHOLESKY,
507 kAutomaticOrdering));
510 TEST_F(BundleAdjustmentTest,
511 MultiThreadedSparseNormalCholeskyWithUserOrderingUsingCXSparse) {
512 RunSolverForConfigAndExpectResidualsMatch(
513 ThreadedSolverConfig(SPARSE_NORMAL_CHOLESKY, CX_SPARSE, kUserOrdering));
516 TEST_F(BundleAdjustmentTest,
517 MultiThreadedSparseSchurWithAutomaticOrderingUsingCXSparse) {
518 RunSolverForConfigAndExpectResidualsMatch(
519 ThreadedSolverConfig(SPARSE_SCHUR, CX_SPARSE, kAutomaticOrdering));
522 TEST_F(BundleAdjustmentTest,
523 MultiThreadedSparseSchurWithUserOrderingUsingCXSparse) {
524 RunSolverForConfigAndExpectResidualsMatch(
525 ThreadedSolverConfig(SPARSE_SCHUR, CX_SPARSE, kUserOrdering));
527 #endif // CERES_NO_CXSPARSE
529 #ifdef CERES_USE_EIGEN_SPARSE
530 TEST_F(BundleAdjustmentTest,
531 MultiThreadedSparseNormalCholeskyWithAutomaticOrderingUsingEigenSparse) {
532 RunSolverForConfigAndExpectResidualsMatch(
533 ThreadedSolverConfig(SPARSE_NORMAL_CHOLESKY,
535 kAutomaticOrdering));
538 TEST_F(BundleAdjustmentTest,
539 MultiThreadedSparseNormalCholeskyWithUserOrderingUsingEigenSparse) {
540 RunSolverForConfigAndExpectResidualsMatch(
541 ThreadedSolverConfig(SPARSE_NORMAL_CHOLESKY,
546 TEST_F(BundleAdjustmentTest,
547 MultiThreadedSparseSchurWithAutomaticOrderingUsingEigenSparse) {
548 RunSolverForConfigAndExpectResidualsMatch(
549 ThreadedSolverConfig(SPARSE_SCHUR, EIGEN_SPARSE, kAutomaticOrdering));
552 TEST_F(BundleAdjustmentTest,
553 MultiThreadedSparseSchurWithUserOrderingUsingEigenSparse) {
554 RunSolverForConfigAndExpectResidualsMatch(
555 ThreadedSolverConfig(SPARSE_SCHUR, EIGEN_SPARSE, kUserOrdering));
557 #endif // CERES_USE_EIGEN_SPARSE
558 #endif // CERES_USE_OPENMP
560 } // namespace internal