361 lines
15 KiB
C++
361 lines
15 KiB
C++
|
// Ceres Solver - A fast non-linear least squares minimizer
|
||
|
|
// Copyright 2015 Google Inc. All rights reserved.
|
||
|
|
// http://ceres-solver.org/
|
||
|
|
//
|
||
|
|
// Redistribution and use in source and binary forms, with or without
|
||
|
|
// modification, are permitted provided that the following conditions are met:
|
||
|
|
//
|
||
|
|
// * Redistributions of source code must retain the above copyright notice,
|
||
|
|
// this list of conditions and the following disclaimer.
|
||
|
|
// * Redistributions in binary form must reproduce the above copyright notice,
|
||
|
|
// this list of conditions and the following disclaimer in the documentation
|
||
|
|
// and/or other materials provided with the distribution.
|
||
|
|
// * Neither the name of Google Inc. nor the names of its contributors may be
|
||
|
|
// used to endorse or promote products derived from this software without
|
||
|
|
// specific prior written permission.
|
||
|
|
//
|
||
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||
|
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||
|
|
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||
|
|
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||
|
|
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||
|
|
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||
|
|
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||
|
|
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||
|
|
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||
|
|
// POSSIBILITY OF SUCH DAMAGE.
|
||
|
|
//
|
||
|
|
// Author: sameeragarwal@google.com (Sameer Agarwal)
|
||
|
|
//
|
||
|
|
// An example of solving a dynamically sized problem with various
|
||
|
|
// solvers and loss functions.
|
||
|
|
//
|
||
|
|
// For a simpler bare bones example of doing bundle adjustment with
|
||
|
|
// Ceres, please see simple_bundle_adjuster.cc.
|
||
|
|
//
|
||
|
|
// NOTE: This example will not compile without gflags and SuiteSparse.
|
||
|
|
//
|
||
|
|
// The problem being solved here is known as a Bundle Adjustment
|
||
|
|
// problem in computer vision. Given a set of 3d points X_1, ..., X_n,
|
||
|
|
// a set of cameras P_1, ..., P_m. If the point X_i is visible in
|
||
|
|
// image j, then there is a 2D observation u_ij that is the expected
|
||
|
|
// projection of X_i using P_j. The aim of this optimization is to
|
||
|
|
// find values of X_i and P_j such that the reprojection error
|
||
|
|
//
|
||
|
|
// E(X,P) = sum_ij |u_ij - P_j X_i|^2
|
||
|
|
//
|
||
|
|
// is minimized.
|
||
|
|
//
|
||
|
|
// The problem used here comes from a collection of bundle adjustment
|
||
|
|
// problems published at University of Washington.
|
||
|
|
// http://grail.cs.washington.edu/projects/bal
|
||
|
|
|
||
|
|
#include <algorithm>
|
||
|
|
#include <cmath>
|
||
|
|
#include <cstdio>
|
||
|
|
#include <cstdlib>
|
||
|
|
#include <string>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
#include "bal_problem.h"
|
||
|
|
#include "ceres/ceres.h"
|
||
|
|
#include "gflags/gflags.h"
|
||
|
|
#include "glog/logging.h"
|
||
|
|
#include "snavely_reprojection_error.h"
|
||
|
|
|
||
|
|
DEFINE_string(input, "", "Input File name");
|
||
|
|
DEFINE_string(trust_region_strategy, "levenberg_marquardt",
|
||
|
|
"Options are: levenberg_marquardt, dogleg.");
|
||
|
|
DEFINE_string(dogleg, "traditional_dogleg", "Options are: traditional_dogleg,"
|
||
|
|
"subspace_dogleg.");
|
||
|
|
|
||
|
|
DEFINE_bool(inner_iterations, false, "Use inner iterations to non-linearly "
|
||
|
|
"refine each successful trust region step.");
|
||
|
|
|
||
|
|
DEFINE_string(blocks_for_inner_iterations, "automatic", "Options are: "
|
||
|
|
"automatic, cameras, points, cameras,points, points,cameras");
|
||
|
|
|
||
|
|
DEFINE_string(linear_solver, "sparse_schur", "Options are: "
|
||
|
|
"sparse_schur, dense_schur, iterative_schur, sparse_normal_cholesky, "
|
||
|
|
"dense_qr, dense_normal_cholesky and cgnr.");
|
||
|
|
DEFINE_bool(explicit_schur_complement, false, "If using ITERATIVE_SCHUR "
|
||
|
|
"then explicitly compute the Schur complement.");
|
||
|
|
DEFINE_string(preconditioner, "jacobi", "Options are: "
|
||
|
|
"identity, jacobi, schur_jacobi, cluster_jacobi, "
|
||
|
|
"cluster_tridiagonal.");
|
||
|
|
DEFINE_string(visibility_clustering, "canonical_views",
|
||
|
|
"single_linkage, canonical_views");
|
||
|
|
|
||
|
|
DEFINE_string(sparse_linear_algebra_library, "suite_sparse",
|
||
|
|
"Options are: suite_sparse and cx_sparse.");
|
||
|
|
DEFINE_string(dense_linear_algebra_library, "eigen",
|
||
|
|
"Options are: eigen and lapack.");
|
||
|
|
DEFINE_string(ordering, "automatic", "Options are: automatic, user.");
|
||
|
|
|
||
|
|
DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent "
|
||
|
|
"rotations. If false, angle axis is used.");
|
||
|
|
DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local "
|
||
|
|
"parameterization.");
|
||
|
|
DEFINE_bool(robustify, false, "Use a robust loss function.");
|
||
|
|
|
||
|
|
DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the "
|
||
|
|
"accuracy of each linear solve of the truncated newton step. "
|
||
|
|
"Changing this parameter can affect solve performance.");
|
||
|
|
|
||
|
|
DEFINE_int32(num_threads, 1, "Number of threads.");
|
||
|
|
DEFINE_int32(num_iterations, 5, "Number of iterations.");
|
||
|
|
DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds.");
|
||
|
|
DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
|
||
|
|
" nonmonotic steps.");
|
||
|
|
|
||
|
|
DEFINE_double(rotation_sigma, 0.0, "Standard deviation of camera rotation "
|
||
|
|
"perturbation.");
|
||
|
|
DEFINE_double(translation_sigma, 0.0, "Standard deviation of the camera "
|
||
|
|
"translation perturbation.");
|
||
|
|
DEFINE_double(point_sigma, 0.0, "Standard deviation of the point "
|
||
|
|
"perturbation.");
|
||
|
|
DEFINE_int32(random_seed, 38401, "Random seed used to set the state "
|
||
|
|
"of the pseudo random number generator used to generate "
|
||
|
|
"the pertubations.");
|
||
|
|
DEFINE_bool(line_search, false, "Use a line search instead of trust region "
|
||
|
|
"algorithm.");
|
||
|
|
DEFINE_string(initial_ply, "", "Export the BAL file data as a PLY file.");
|
||
|
|
DEFINE_string(final_ply, "", "Export the refined BAL file data as a PLY "
|
||
|
|
"file.");
|
||
|
|
|
||
|
|
namespace ceres {
|
||
|
|
namespace examples {
|
||
|
|
|
||
|
|
void SetLinearSolver(Solver::Options* options) {
|
||
|
|
CHECK(StringToLinearSolverType(FLAGS_linear_solver,
|
||
|
|
&options->linear_solver_type));
|
||
|
|
CHECK(StringToPreconditionerType(FLAGS_preconditioner,
|
||
|
|
&options->preconditioner_type));
|
||
|
|
CHECK(StringToVisibilityClusteringType(FLAGS_visibility_clustering,
|
||
|
|
&options->visibility_clustering_type));
|
||
|
|
CHECK(StringToSparseLinearAlgebraLibraryType(
|
||
|
|
FLAGS_sparse_linear_algebra_library,
|
||
|
|
&options->sparse_linear_algebra_library_type));
|
||
|
|
CHECK(StringToDenseLinearAlgebraLibraryType(
|
||
|
|
FLAGS_dense_linear_algebra_library,
|
||
|
|
&options->dense_linear_algebra_library_type));
|
||
|
|
options->num_linear_solver_threads = FLAGS_num_threads;
|
||
|
|
options->use_explicit_schur_complement = FLAGS_explicit_schur_complement;
|
||
|
|
}
|
||
|
|
|
||
|
|
void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {
|
||
|
|
const int num_points = bal_problem->num_points();
|
||
|
|
const int point_block_size = bal_problem->point_block_size();
|
||
|
|
double* points = bal_problem->mutable_points();
|
||
|
|
|
||
|
|
const int num_cameras = bal_problem->num_cameras();
|
||
|
|
const int camera_block_size = bal_problem->camera_block_size();
|
||
|
|
double* cameras = bal_problem->mutable_cameras();
|
||
|
|
|
||
|
|
if (options->use_inner_iterations) {
|
||
|
|
if (FLAGS_blocks_for_inner_iterations == "cameras") {
|
||
|
|
LOG(INFO) << "Camera blocks for inner iterations";
|
||
|
|
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
|
||
|
|
for (int i = 0; i < num_cameras; ++i) {
|
||
|
|
options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
|
||
|
|
}
|
||
|
|
} else if (FLAGS_blocks_for_inner_iterations == "points") {
|
||
|
|
LOG(INFO) << "Point blocks for inner iterations";
|
||
|
|
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
|
||
|
|
for (int i = 0; i < num_points; ++i) {
|
||
|
|
options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
|
||
|
|
}
|
||
|
|
} else if (FLAGS_blocks_for_inner_iterations == "cameras,points") {
|
||
|
|
LOG(INFO) << "Camera followed by point blocks for inner iterations";
|
||
|
|
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
|
||
|
|
for (int i = 0; i < num_cameras; ++i) {
|
||
|
|
options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
|
||
|
|
}
|
||
|
|
for (int i = 0; i < num_points; ++i) {
|
||
|
|
options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1);
|
||
|
|
}
|
||
|
|
} else if (FLAGS_blocks_for_inner_iterations == "points,cameras") {
|
||
|
|
LOG(INFO) << "Point followed by camera blocks for inner iterations";
|
||
|
|
options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
|
||
|
|
for (int i = 0; i < num_cameras; ++i) {
|
||
|
|
options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
|
||
|
|
}
|
||
|
|
for (int i = 0; i < num_points; ++i) {
|
||
|
|
options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
|
||
|
|
}
|
||
|
|
} else if (FLAGS_blocks_for_inner_iterations == "automatic") {
|
||
|
|
LOG(INFO) << "Choosing automatic blocks for inner iterations";
|
||
|
|
} else {
|
||
|
|
LOG(FATAL) << "Unknown block type for inner iterations: "
|
||
|
|
<< FLAGS_blocks_for_inner_iterations;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Bundle adjustment problems have a sparsity structure that makes
|
||
|
|
// them amenable to more specialized and much more efficient
|
||
|
|
// solution strategies. The SPARSE_SCHUR, DENSE_SCHUR and
|
||
|
|
// ITERATIVE_SCHUR solvers make use of this specialized
|
||
|
|
// structure.
|
||
|
|
//
|
||
|
|
// This can either be done by specifying Options::ordering_type =
|
||
|
|
// ceres::SCHUR, in which case Ceres will automatically determine
|
||
|
|
// the right ParameterBlock ordering, or by manually specifying a
|
||
|
|
// suitable ordering vector and defining
|
||
|
|
// Options::num_eliminate_blocks.
|
||
|
|
if (FLAGS_ordering == "automatic") {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
ceres::ParameterBlockOrdering* ordering =
|
||
|
|
new ceres::ParameterBlockOrdering;
|
||
|
|
|
||
|
|
// The points come before the cameras.
|
||
|
|
for (int i = 0; i < num_points; ++i) {
|
||
|
|
ordering->AddElementToGroup(points + point_block_size * i, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int i = 0; i < num_cameras; ++i) {
|
||
|
|
// When using axis-angle, there is a single parameter block for
|
||
|
|
// the entire camera.
|
||
|
|
ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
|
||
|
|
// If quaternions are used, there are two blocks, so add the
|
||
|
|
// second block to the ordering.
|
||
|
|
if (FLAGS_use_quaternions) {
|
||
|
|
ordering->AddElementToGroup(cameras + camera_block_size * i + 4, 1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
options->linear_solver_ordering.reset(ordering);
|
||
|
|
}
|
||
|
|
|
||
|
|
void SetMinimizerOptions(Solver::Options* options) {
|
||
|
|
options->max_num_iterations = FLAGS_num_iterations;
|
||
|
|
options->minimizer_progress_to_stdout = true;
|
||
|
|
options->num_threads = FLAGS_num_threads;
|
||
|
|
options->eta = FLAGS_eta;
|
||
|
|
options->max_solver_time_in_seconds = FLAGS_max_solver_time;
|
||
|
|
options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
|
||
|
|
if (FLAGS_line_search) {
|
||
|
|
options->minimizer_type = ceres::LINE_SEARCH;
|
||
|
|
}
|
||
|
|
|
||
|
|
CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
|
||
|
|
&options->trust_region_strategy_type));
|
||
|
|
CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));
|
||
|
|
options->use_inner_iterations = FLAGS_inner_iterations;
|
||
|
|
}
|
||
|
|
|
||
|
|
void SetSolverOptionsFromFlags(BALProblem* bal_problem,
|
||
|
|
Solver::Options* options) {
|
||
|
|
SetMinimizerOptions(options);
|
||
|
|
SetLinearSolver(options);
|
||
|
|
SetOrdering(bal_problem, options);
|
||
|
|
}
|
||
|
|
|
||
|
|
void BuildProblem(BALProblem* bal_problem, Problem* problem) {
|
||
|
|
const int point_block_size = bal_problem->point_block_size();
|
||
|
|
const int camera_block_size = bal_problem->camera_block_size();
|
||
|
|
double* points = bal_problem->mutable_points();
|
||
|
|
double* cameras = bal_problem->mutable_cameras();
|
||
|
|
|
||
|
|
// Observations is 2*num_observations long array observations =
|
||
|
|
// [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x
|
||
|
|
// and y positions of the observation.
|
||
|
|
const double* observations = bal_problem->observations();
|
||
|
|
|
||
|
|
for (int i = 0; i < bal_problem->num_observations(); ++i) {
|
||
|
|
CostFunction* cost_function;
|
||
|
|
// Each Residual block takes a point and a camera as input and
|
||
|
|
// outputs a 2 dimensional residual.
|
||
|
|
cost_function =
|
||
|
|
(FLAGS_use_quaternions)
|
||
|
|
? SnavelyReprojectionErrorWithQuaternions::Create(
|
||
|
|
observations[2 * i + 0],
|
||
|
|
observations[2 * i + 1])
|
||
|
|
: SnavelyReprojectionError::Create(
|
||
|
|
observations[2 * i + 0],
|
||
|
|
observations[2 * i + 1]);
|
||
|
|
|
||
|
|
// If enabled use Huber's loss function.
|
||
|
|
LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;
|
||
|
|
|
||
|
|
// Each observation correponds to a pair of a camera and a point
|
||
|
|
// which are identified by camera_index()[i] and point_index()[i]
|
||
|
|
// respectively.
|
||
|
|
double* camera =
|
||
|
|
cameras + camera_block_size * bal_problem->camera_index()[i];
|
||
|
|
double* point = points + point_block_size * bal_problem->point_index()[i];
|
||
|
|
|
||
|
|
if (FLAGS_use_quaternions) {
|
||
|
|
// When using quaternions, we split the camera into two
|
||
|
|
// parameter blocks. One of size 4 for the quaternion and the
|
||
|
|
// other of size 6 containing the translation, focal length and
|
||
|
|
// the radial distortion parameters.
|
||
|
|
problem->AddResidualBlock(cost_function,
|
||
|
|
loss_function,
|
||
|
|
camera,
|
||
|
|
camera + 4,
|
||
|
|
point);
|
||
|
|
} else {
|
||
|
|
problem->AddResidualBlock(cost_function, loss_function, camera, point);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
|
||
|
|
LocalParameterization* quaternion_parameterization =
|
||
|
|
new QuaternionParameterization;
|
||
|
|
for (int i = 0; i < bal_problem->num_cameras(); ++i) {
|
||
|
|
problem->SetParameterization(cameras + camera_block_size * i,
|
||
|
|
quaternion_parameterization);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void SolveProblem(const char* filename) {
|
||
|
|
BALProblem bal_problem(filename, FLAGS_use_quaternions);
|
||
|
|
|
||
|
|
if (!FLAGS_initial_ply.empty()) {
|
||
|
|
bal_problem.WriteToPLYFile(FLAGS_initial_ply);
|
||
|
|
}
|
||
|
|
|
||
|
|
Problem problem;
|
||
|
|
|
||
|
|
srand(FLAGS_random_seed);
|
||
|
|
bal_problem.Normalize();
|
||
|
|
bal_problem.Perturb(FLAGS_rotation_sigma,
|
||
|
|
FLAGS_translation_sigma,
|
||
|
|
FLAGS_point_sigma);
|
||
|
|
|
||
|
|
BuildProblem(&bal_problem, &problem);
|
||
|
|
Solver::Options options;
|
||
|
|
SetSolverOptionsFromFlags(&bal_problem, &options);
|
||
|
|
options.gradient_tolerance = 1e-16;
|
||
|
|
options.function_tolerance = 1e-16;
|
||
|
|
Solver::Summary summary;
|
||
|
|
Solve(options, &problem, &summary);
|
||
|
|
std::cout << summary.FullReport() << "\n";
|
||
|
|
|
||
|
|
if (!FLAGS_final_ply.empty()) {
|
||
|
|
bal_problem.WriteToPLYFile(FLAGS_final_ply);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace examples
|
||
|
|
} // namespace ceres
|
||
|
|
|
||
|
|
int main(int argc, char** argv) {
|
||
|
|
CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
|
||
|
|
google::InitGoogleLogging(argv[0]);
|
||
|
|
if (FLAGS_input.empty()) {
|
||
|
|
LOG(ERROR) << "Usage: bundle_adjuster --input=bal_problem";
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization)
|
||
|
|
<< "--use_local_parameterization can only be used with "
|
||
|
|
<< "--use_quaternions.";
|
||
|
|
ceres::examples::SolveProblem(FLAGS_input.c_str());
|
||
|
|
return 0;
|
||
|
|
}
|