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/
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: sameeragarwal@google.com (Sameer Agarwal)
31
// An example of solving a dynamically sized problem with various
32
// solvers and loss functions.
34
// For a simpler bare bones example of doing bundle adjustment with
35
// Ceres, please see simple_bundle_adjuster.cc.
37
// NOTE: This example will not compile without gflags and SuiteSparse.
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
46
// E(X,P) = sum_ij |u_ij - P_j X_i|^2
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
60
#include <gflags/gflags.h>
61
#include <glog/logging.h>
62
#include "bal_problem.h"
63
#include "snavely_reprojection_error.h"
64
#include "ceres/ceres.h"
66
DEFINE_string(input, "", "Input File name");
68
DEFINE_string(solver_type, "sparse_schur", "Options are: "
69
"sparse_schur, dense_schur, iterative_schur, cholesky, "
70
"dense_qr, and conjugate_gradients");
72
DEFINE_string(preconditioner_type, "jacobi", "Options are: "
73
"identity, jacobi, schur_jacobi, cluster_jacobi, "
74
"cluster_tridiagonal");
76
DEFINE_int32(num_iterations, 5, "Number of iterations");
77
DEFINE_int32(num_threads, 1, "Number of threads");
78
DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the "
79
"accuracy of each linear solve of the truncated newton step. "
80
"Changing this parameter can affect solve performance ");
81
DEFINE_bool(use_schur_ordering, false, "Use automatic Schur ordering.");
82
DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent "
83
"rotations. If false, angle axis is used");
84
DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local "
86
DEFINE_bool(robustify, false, "Use a robust loss function");
91
void SetLinearSolver(Solver::Options* options) {
92
if (FLAGS_solver_type == "sparse_schur") {
93
options->linear_solver_type = ceres::SPARSE_SCHUR;
94
} else if (FLAGS_solver_type == "dense_schur") {
95
options->linear_solver_type = ceres::DENSE_SCHUR;
96
} else if (FLAGS_solver_type == "iterative_schur") {
97
options->linear_solver_type = ceres::ITERATIVE_SCHUR;
98
} else if (FLAGS_solver_type == "cholesky") {
99
options->linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
100
} else if (FLAGS_solver_type == "cgnr") {
101
options->linear_solver_type = ceres::CGNR;
102
} else if (FLAGS_solver_type == "dense_qr") {
103
// DENSE_QR is included here for completeness, but actually using
104
// this option is a bad idea due to the amount of memory needed
105
// to store even the smallest of the bundle adjustment jacobian
107
options->linear_solver_type = ceres::DENSE_QR;
109
LOG(FATAL) << "Unknown ceres solver type: "
110
<< FLAGS_solver_type;
113
if (options->linear_solver_type == ceres::CGNR) {
114
options->linear_solver_min_num_iterations = 5;
115
if (FLAGS_preconditioner_type == "identity") {
116
options->preconditioner_type = ceres::IDENTITY;
117
} else if (FLAGS_preconditioner_type == "jacobi") {
118
options->preconditioner_type = ceres::JACOBI;
120
LOG(FATAL) << "For CGNR, only identity and jacobian "
121
<< "preconditioners are supported. Got: "
122
<< FLAGS_preconditioner_type;
126
if (options->linear_solver_type == ceres::ITERATIVE_SCHUR) {
127
options->linear_solver_min_num_iterations = 5;
128
if (FLAGS_preconditioner_type == "identity") {
129
options->preconditioner_type = ceres::IDENTITY;
130
} else if (FLAGS_preconditioner_type == "jacobi") {
131
options->preconditioner_type = ceres::JACOBI;
132
} else if (FLAGS_preconditioner_type == "schur_jacobi") {
133
options->preconditioner_type = ceres::SCHUR_JACOBI;
134
} else if (FLAGS_preconditioner_type == "cluster_jacobi") {
135
options->preconditioner_type = ceres::CLUSTER_JACOBI;
136
} else if (FLAGS_preconditioner_type == "cluster_tridiagonal") {
137
options->preconditioner_type = ceres::CLUSTER_TRIDIAGONAL;
139
LOG(FATAL) << "Unknown ceres preconditioner type: "
140
<< FLAGS_preconditioner_type;
144
options->num_linear_solver_threads = FLAGS_num_threads;
147
void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {
148
// Bundle adjustment problems have a sparsity structure that makes
149
// them amenable to more specialized and much more efficient
150
// solution strategies. The SPARSE_SCHUR, DENSE_SCHUR and
151
// ITERATIVE_SCHUR solvers make use of this specialized
152
// structure. Using them however requires that the ParameterBlocks
153
// are in a particular order (points before cameras) and
154
// Solver::Options::num_eliminate_blocks is set to the number of
157
// This can either be done by specifying Options::ordering_type =
158
// ceres::SCHUR, in which case Ceres will automatically determine
159
// the right ParameterBlock ordering, or by manually specifying a
160
// suitable ordering vector and defining
161
// Options::num_eliminate_blocks.
162
if (FLAGS_use_schur_ordering) {
163
options->ordering_type = ceres::SCHUR;
167
options->ordering_type = ceres::USER;
168
const int num_points = bal_problem->num_points();
169
const int point_block_size = bal_problem->point_block_size();
170
double* points = bal_problem->mutable_points();
171
const int num_cameras = bal_problem->num_cameras();
172
const int camera_block_size = bal_problem->camera_block_size();
173
double* cameras = bal_problem->mutable_cameras();
175
// The points come before the cameras.
176
for (int i = 0; i < num_points; ++i) {
177
options->ordering.push_back(points + point_block_size * i);
180
for (int i = 0; i < num_cameras; ++i) {
181
// When using axis-angle, there is a single parameter block for
182
// the entire camera.
183
options->ordering.push_back(cameras + camera_block_size * i);
185
// If quaternions are used, there are two blocks, so add the
186
// second block to the ordering.
187
if (FLAGS_use_quaternions) {
188
options->ordering.push_back(cameras + camera_block_size * i + 4);
192
options->num_eliminate_blocks = num_points;
195
void SetMinimizerOptions(Solver::Options* options) {
196
options->max_num_iterations = FLAGS_num_iterations;
197
options->minimizer_progress_to_stdout = true;
198
options->num_threads = FLAGS_num_threads;
199
options->eta = FLAGS_eta;
202
void SetSolverOptionsFromFlags(BALProblem* bal_problem,
203
Solver::Options* options) {
204
SetLinearSolver(options);
205
SetOrdering(bal_problem, options);
206
SetMinimizerOptions(options);
209
void BuildProblem(BALProblem* bal_problem, Problem* problem) {
210
const int point_block_size = bal_problem->point_block_size();
211
const int camera_block_size = bal_problem->camera_block_size();
212
double* points = bal_problem->mutable_points();
213
double* cameras = bal_problem->mutable_cameras();
215
// Observations is 2*num_observations long array observations =
216
// [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x
217
// and y positions of the observation.
218
const double* observations = bal_problem->observations();
220
for (int i = 0; i < bal_problem->num_observations(); ++i) {
221
CostFunction* cost_function;
222
// Each Residual block takes a point and a camera as input and
223
// outputs a 2 dimensional residual.
224
if (FLAGS_use_quaternions) {
225
cost_function = new AutoDiffCostFunction<
226
SnavelyReprojectionErrorWitQuaternions, 2, 4, 6, 3>(
227
new SnavelyReprojectionErrorWitQuaternions(
228
observations[2 * i + 0],
229
observations[2 * i + 1]));
232
new AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
233
new SnavelyReprojectionError(observations[2 * i + 0],
234
observations[2 * i + 1]));
237
// If enabled use Huber's loss function.
238
LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;
240
// Each observation correponds to a pair of a camera and a point
241
// which are identified by camera_index()[i] and point_index()[i]
244
cameras + camera_block_size * bal_problem->camera_index()[i];
245
double* point = points + point_block_size * bal_problem->point_index()[i];
247
if (FLAGS_use_quaternions) {
248
// When using quaternions, we split the camera into two
249
// parameter blocks. One of size 4 for the quaternion and the
250
// other of size 6 containing the translation, focal length and
251
// the radial distortion parameters.
252
problem->AddResidualBlock(cost_function,
258
problem->AddResidualBlock(cost_function, loss_function, camera, point);
262
if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
263
LocalParameterization* quaternion_parameterization =
264
new QuaternionParameterization;
265
for (int i = 0; i < bal_problem->num_cameras(); ++i) {
266
problem->SetParameterization(cameras + camera_block_size * i,
267
quaternion_parameterization);
272
void SolveProblem(const char* filename) {
273
BALProblem bal_problem(filename, FLAGS_use_quaternions);
275
BuildProblem(&bal_problem, &problem);
276
Solver::Options options;
277
SetSolverOptionsFromFlags(&bal_problem, &options);
279
Solver::Summary summary;
280
Solve(options, &problem, &summary);
281
std::cout << summary.FullReport() << "\n";
284
} // namespace examples
287
int main(int argc, char** argv) {
288
google::ParseCommandLineFlags(&argc, &argv, true);
289
google::InitGoogleLogging(argv[0]);
290
if (FLAGS_input.empty()) {
291
LOG(ERROR) << "Usage: bundle_adjustment_example --input=bal_problem";
295
CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization)
296
<< "--use_local_parameterization can only be used with "
297
<< "--use_quaternions.";
298
ceres::examples::SolveProblem(FLAGS_input.c_str());
added
removed
examples/bundle_adjuster.cc
