blob: 074931fac845ac49508f4a175bda0b5b03616e5b [file] [log] [blame]
Austin Schuh70cc9552019-01-21 19:46:48 -08001// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2018 Google Inc. All rights reserved.
3// http://ceres-solver.org/
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: keir@google.com (Keir Mierle)
30//
31// End-to-end bundle adjustment test utilities for Ceres. This base is used in
32// the generated bundle adjustment test binaries. The reason to split the
33// bundle tests into separate binaries is so the tests can get parallelized.
34
35#include <cmath>
36#include <cstdio>
37#include <cstdlib>
38#include <string>
39
Austin Schuh70cc9552019-01-21 19:46:48 -080040#include "ceres/autodiff_cost_function.h"
Austin Schuh1d1e6ea2020-12-23 21:56:30 -080041#include "ceres/internal/port.h"
Austin Schuh70cc9552019-01-21 19:46:48 -080042#include "ceres/ordered_groups.h"
43#include "ceres/problem.h"
44#include "ceres/rotation.h"
45#include "ceres/solver.h"
46#include "ceres/stringprintf.h"
47#include "ceres/test_util.h"
48#include "ceres/types.h"
49#include "gflags/gflags.h"
50#include "glog/logging.h"
51#include "gtest/gtest.h"
52
53namespace ceres {
54namespace internal {
55
56using std::string;
57using std::vector;
58
59const bool kAutomaticOrdering = true;
60const bool kUserOrdering = false;
61
62// This class implements the SystemTestProblem interface and provides
63// access to a bundle adjustment problem. It is based on
64// examples/bundle_adjustment_example.cc. Currently a small 16 camera
65// problem is hard coded in the constructor.
66class BundleAdjustmentProblem {
67 public:
68 BundleAdjustmentProblem() {
69 const string input_file = TestFileAbsolutePath("problem-16-22106-pre.txt");
70 ReadData(input_file);
71 BuildProblem();
72 }
73
74 ~BundleAdjustmentProblem() {
Austin Schuh1d1e6ea2020-12-23 21:56:30 -080075 delete[] point_index_;
76 delete[] camera_index_;
77 delete[] observations_;
78 delete[] parameters_;
Austin Schuh70cc9552019-01-21 19:46:48 -080079 }
80
81 Problem* mutable_problem() { return &problem_; }
82 Solver::Options* mutable_solver_options() { return &options_; }
83
Austin Schuh1d1e6ea2020-12-23 21:56:30 -080084 // clang-format off
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_; }
Austin Schuh70cc9552019-01-21 19:46:48 -080089 const int* camera_index() const { return camera_index_; }
90 const double* observations() const { return observations_; }
Austin Schuh1d1e6ea2020-12-23 21:56:30 -080091 double* mutable_cameras() { return parameters_; }
92 double* mutable_points() { return parameters_ + 9 * num_cameras_; }
93 // clang-format on
Austin Schuh70cc9552019-01-21 19:46:48 -080094
95 static double kResidualTolerance;
96
97 private:
98 void ReadData(const string& filename) {
Austin Schuh1d1e6ea2020-12-23 21:56:30 -080099 FILE* fptr = fopen(filename.c_str(), "r");
Austin Schuh70cc9552019-01-21 19:46:48 -0800100
101 if (!fptr) {
102 LOG(FATAL) << "File Error: unable to open file " << filename;
103 }
104
105 // This will die horribly on invalid files. Them's the breaks.
106 FscanfOrDie(fptr, "%d", &num_cameras_);
107 FscanfOrDie(fptr, "%d", &num_points_);
108 FscanfOrDie(fptr, "%d", &num_observations_);
109
Austin Schuh1d1e6ea2020-12-23 21:56:30 -0800110 VLOG(1) << "Header: " << num_cameras_ << " " << num_points_ << " "
111 << num_observations_;
Austin Schuh70cc9552019-01-21 19:46:48 -0800112
113 point_index_ = new int[num_observations_];
114 camera_index_ = new int[num_observations_];
115 observations_ = new double[2 * num_observations_];
116
117 num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
118 parameters_ = new double[num_parameters_];
119
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) {
Austin Schuh1d1e6ea2020-12-23 21:56:30 -0800124 FscanfOrDie(fptr, "%lf", observations_ + 2 * i + j);
Austin Schuh70cc9552019-01-21 19:46:48 -0800125 }
126 }
127
128 for (int i = 0; i < num_parameters_; ++i) {
129 FscanfOrDie(fptr, "%lf", parameters_ + i);
130 }
131
132 fclose(fptr);
133 }
134
135 void BuildProblem() {
136 double* points = mutable_points();
137 double* cameras = mutable_cameras();
138
139 for (int i = 0; i < num_observations(); ++i) {
140 // Each Residual block takes a point and a camera as input and
141 // outputs a 2 dimensional residual.
142 CostFunction* cost_function =
143 new AutoDiffCostFunction<BundlerResidual, 2, 9, 3>(
Austin Schuh1d1e6ea2020-12-23 21:56:30 -0800144 new BundlerResidual(observations_[2 * i + 0],
145 observations_[2 * i + 1]));
Austin Schuh70cc9552019-01-21 19:46:48 -0800146
147 // Each observation corresponds to a pair of a camera and a point
148 // which are identified by camera_index()[i] and
149 // point_index()[i] respectively.
150 double* camera = cameras + 9 * camera_index_[i];
151 double* point = points + 3 * point_index()[i];
152 problem_.AddResidualBlock(cost_function, NULL, camera, point);
153 }
154
155 options_.linear_solver_ordering.reset(new ParameterBlockOrdering);
156
157 // The points come before the cameras.
158 for (int i = 0; i < num_points_; ++i) {
159 options_.linear_solver_ordering->AddElementToGroup(points + 3 * i, 0);
160 }
161
162 for (int i = 0; i < num_cameras_; ++i) {
163 options_.linear_solver_ordering->AddElementToGroup(cameras + 9 * i, 1);
164 }
165
166 options_.linear_solver_type = DENSE_SCHUR;
167 options_.max_num_iterations = 25;
168 options_.function_tolerance = 1e-10;
169 options_.gradient_tolerance = 1e-10;
170 options_.parameter_tolerance = 1e-10;
171 }
172
Austin Schuh1d1e6ea2020-12-23 21:56:30 -0800173 template <typename T>
174 void FscanfOrDie(FILE* fptr, const char* format, T* value) {
Austin Schuh70cc9552019-01-21 19:46:48 -0800175 int num_scanned = fscanf(fptr, format, value);
176 if (num_scanned != 1) {
177 LOG(FATAL) << "Invalid UW data file.";
178 }
179 }
180
181 // Templated pinhole camera model. The camera is parameterized
182 // using 9 parameters. 3 for rotation, 3 for translation, 1 for
183 // focal length and 2 for radial distortion. The principal point is
184 // not modeled (i.e. it is assumed to be located at the image
185 // center).
186 struct BundlerResidual {
187 // (u, v): the position of the observation with respect to the image
188 // center point.
Austin Schuh1d1e6ea2020-12-23 21:56:30 -0800189 BundlerResidual(double u, double v) : u(u), v(v) {}
Austin Schuh70cc9552019-01-21 19:46:48 -0800190
191 template <typename T>
192 bool operator()(const T* const camera,
193 const T* const point,
194 T* residuals) const {
195 T p[3];
196 AngleAxisRotatePoint(camera, point, p);
197
198 // Add the translation vector
199 p[0] += camera[3];
200 p[1] += camera[4];
201 p[2] += camera[5];
202
203 const T& focal = camera[6];
204 const T& l1 = camera[7];
205 const T& l2 = camera[8];
206
207 // Compute the center of distortion. The sign change comes from
208 // the camera model that Noah Snavely's Bundler assumes, whereby
209 // the camera coordinate system has a negative z axis.
Austin Schuh1d1e6ea2020-12-23 21:56:30 -0800210 T xp = -focal * p[0] / p[2];
211 T yp = -focal * p[1] / p[2];
Austin Schuh70cc9552019-01-21 19:46:48 -0800212
213 // Apply second and fourth order radial distortion.
Austin Schuh1d1e6ea2020-12-23 21:56:30 -0800214 T r2 = xp * xp + yp * yp;
215 T distortion = T(1.0) + r2 * (l1 + l2 * r2);
Austin Schuh70cc9552019-01-21 19:46:48 -0800216
217 residuals[0] = distortion * xp - u;
218 residuals[1] = distortion * yp - v;
219
220 return true;
221 }
222
223 double u;
224 double v;
225 };
226
227 Problem problem_;
228 Solver::Options options_;
229
230 int num_cameras_;
231 int num_points_;
232 int num_observations_;
233 int num_parameters_;
234
235 int* point_index_;
236 int* camera_index_;
237 double* observations_;
238 // The parameter vector is laid out as follows
239 // [camera_1, ..., camera_n, point_1, ..., point_m]
240 double* parameters_;
241};
242
243double BundleAdjustmentProblem::kResidualTolerance = 1e-4;
244typedef SystemTest<BundleAdjustmentProblem> BundleAdjustmentTest;
245
246} // namespace internal
247} // namespace ceres