Initial commit of Ceres Solver.
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..b07a3dd --- /dev/null +++ b/examples/CMakeLists.txt
@@ -0,0 +1,53 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +# http://code.google.com/p/ceres-solver/ +# +# 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: keir@google.com (Keir Mierle) + +ADD_EXECUTABLE(quadratic quadratic.cc) +TARGET_LINK_LIBRARIES(quadratic ceres) + +ADD_EXECUTABLE(quadratic_auto_diff quadratic_auto_diff.cc) +TARGET_LINK_LIBRARIES(quadratic_auto_diff ceres) + +ADD_EXECUTABLE(quadratic_numeric_diff quadratic_numeric_diff.cc) +TARGET_LINK_LIBRARIES(quadratic_numeric_diff ceres) + +ADD_EXECUTABLE(powell powell.cc) +TARGET_LINK_LIBRARIES(powell ceres) + +ADD_EXECUTABLE(circle_fit circle_fit.cc) +TARGET_LINK_LIBRARIES(circle_fit ceres) + +ADD_EXECUTABLE(bundle_adjuster + bundle_adjuster.cc + bal_problem.cc) +TARGET_LINK_LIBRARIES(bundle_adjuster ceres) + +ADD_EXECUTABLE(simple_bundle_adjuster + simple_bundle_adjuster.cc) +TARGET_LINK_LIBRARIES(simple_bundle_adjuster ceres)
diff --git a/examples/bal_problem.cc b/examples/bal_problem.cc new file mode 100644 index 0000000..2ab35e4 --- /dev/null +++ b/examples/bal_problem.cc
@@ -0,0 +1,119 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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) + +#include "bal_problem.h" + +#include <cstdio> +#include <cstdlib> +#include <string> +#include <glog/logging.h> +#include "ceres/rotation.h" + +namespace ceres { +namespace examples { + +template<typename T> +void FscanfOrDie(FILE *fptr, const char *format, T *value) { + int num_scanned = fscanf(fptr, format, value); + if (num_scanned != 1) { + LOG(FATAL) << "Invalid UW data file."; + } +} + +BALProblem::BALProblem(const std::string filename, bool use_quaternions) { + FILE* fptr = fopen(filename.c_str(), "r"); + + if (!fptr) { + LOG(FATAL) << "Error: unable to open file " << filename; + return; + }; + + // This wil die horribly on invalid files. Them's the breaks. + FscanfOrDie(fptr, "%d", &num_cameras_); + FscanfOrDie(fptr, "%d", &num_points_); + FscanfOrDie(fptr, "%d", &num_observations_); + + VLOG(1) << "Header: " << num_cameras_ + << " " << num_points_ + << " " << num_observations_; + + point_index_ = new int[num_observations_]; + camera_index_ = new int[num_observations_]; + observations_ = new double[2 * num_observations_]; + + num_parameters_ = 9 * num_cameras_ + 3 * num_points_; + parameters_ = new double[num_parameters_]; + + for (int i = 0; i < num_observations_; ++i) { + FscanfOrDie(fptr, "%d", camera_index_ + i); + FscanfOrDie(fptr, "%d", point_index_ + i); + for (int j = 0; j < 2; ++j) { + FscanfOrDie(fptr, "%lf", observations_ + 2*i + j); + } + } + + for (int i = 0; i < num_parameters_; ++i) { + FscanfOrDie(fptr, "%lf", parameters_ + i); + } + + use_quaternions_ = use_quaternions; + if (use_quaternions) { + // Switch the angle-axis rotations to quaternions. + num_parameters_ = 10 * num_cameras_ + 3 * num_points_; + double* quaternion_parameters = new double[num_parameters_]; + double* original_cursor = parameters_; + double* quaternion_cursor = quaternion_parameters; + for (int i = 0; i < num_cameras_; ++i) { + AngleAxisToQuaternion(original_cursor, quaternion_cursor); + quaternion_cursor += 4; + original_cursor += 3; + for (int j = 4; j < 10; ++j) { + *quaternion_cursor++ = *original_cursor++; + } + } + // Copy the rest of the points. + for (int i = 0; i < 3 * num_points_; ++i) { + *quaternion_cursor++ = *original_cursor++; + } + // Swap in the quaternion parameters. + delete []parameters_; + parameters_ = quaternion_parameters; + } +} + +BALProblem::~BALProblem() { + delete []point_index_; + delete []camera_index_; + delete []observations_; + delete []parameters_; +} + +} // namespace examples +} // namespace ceres
diff --git a/examples/bal_problem.h b/examples/bal_problem.h new file mode 100644 index 0000000..ff411b8 --- /dev/null +++ b/examples/bal_problem.h
@@ -0,0 +1,83 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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) +// +// Class for loading and holding in memory bundle adjustment problems +// from the BAL (Bundle Adjustment in the Large) dataset from the +// University of Washington. +// +// For more details see http://grail.cs.washington.edu/projects/bal/ + +#ifndef CERES_EXAMPLES_BAL_PROBLEM_H_ +#define CERES_EXAMPLES_BAL_PROBLEM_H_ + +#include <string> + +namespace ceres { +namespace examples { + +class BALProblem { + public: + explicit BALProblem(const std::string filename, bool use_quaternions); + ~BALProblem(); + + int camera_block_size() const { return use_quaternions_ ? 10 : 9; } + int point_block_size() const { return 3; } + int num_cameras() const { return num_cameras_; } + int num_points() const { return num_points_; } + int num_observations() const { return num_observations_; } + int num_parameters() const { return num_parameters_; } + const int* point_index() const { return point_index_; } + const int* camera_index() const { return camera_index_; } + const double* observations() const { return observations_; } + const double* parameters() const { return parameters_; } + double* mutable_cameras() { return parameters_; } + double* mutable_points() { + return parameters_ + camera_block_size() * num_cameras_; + } + + private: + int num_cameras_; + int num_points_; + int num_observations_; + int num_parameters_; + bool use_quaternions_; + + int* point_index_; + int* camera_index_; + double* observations_; + // The parameter vector is laid out as follows + // [camera_1, ..., camera_n, point_1, ..., point_m] + double* parameters_; +}; + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_BAL_PROBLEM_H_
diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc new file mode 100644 index 0000000..bafdf25 --- /dev/null +++ b/examples/bundle_adjuster.cc
@@ -0,0 +1,281 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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 <string> +#include <vector> + +#include <gflags/gflags.h> +#include <glog/logging.h> +#include "bal_problem.h" +#include "snavely_reprojection_error.h" +#include "ceres/ceres.h" + +DEFINE_string(input, "", "Input File name"); + +DEFINE_string(solver_type, "sparse_schur", "Options are: " + "sparse_schur, dense_schur, iterative_schur, cholesky " + "and dense_qr"); + +DEFINE_string(preconditioner_type, "jacobi", "Options are: " + "identity, jacobi, schur_jacobi, cluster_jacobi, " + "cluster_tridiagonal"); + +DEFINE_int32(num_iterations, 5, "Number of iterations"); +DEFINE_int32(num_threads, 1, "Number of threads"); +DEFINE_bool(use_schur_ordering, false, "Use automatic Schur ordering."); +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"); + +namespace ceres { +namespace examples { + +void SetLinearSolver(Solver::Options* options) { + if (FLAGS_solver_type == "sparse_schur") { + options->linear_solver_type = ceres::SPARSE_SCHUR; + } else if (FLAGS_solver_type == "dense_schur") { + options->linear_solver_type = ceres::DENSE_SCHUR; + } else if (FLAGS_solver_type == "iterative_schur") { + options->linear_solver_type = ceres::ITERATIVE_SCHUR; + } else if (FLAGS_solver_type == "cholesky") { + options->linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; + } else if (FLAGS_solver_type == "dense_qr") { + // DENSE_QR is included here for completeness, but actually using + // this opttion is a bad idea due to the amount of memory needed + // to store even the smallest of the bundle adjustment jacobian + // arrays + options->linear_solver_type = ceres::DENSE_QR; + } else { + LOG(FATAL) << "Unknown ceres solver type: " + << FLAGS_solver_type; + } + + if (options->linear_solver_type == ceres::ITERATIVE_SCHUR) { + options->linear_solver_min_num_iterations = 5; + if (FLAGS_preconditioner_type == "identity") { + options->preconditioner_type = ceres::IDENTITY; + } else if (FLAGS_preconditioner_type == "jacobi") { + options->preconditioner_type = ceres::JACOBI; + } else if (FLAGS_preconditioner_type == "schur_jacobi") { + options->preconditioner_type = ceres::SCHUR_JACOBI; + } else if (FLAGS_preconditioner_type == "cluster_jacobi") { + options->preconditioner_type = ceres::CLUSTER_JACOBI; + } else if (FLAGS_preconditioner_type == "cluster_tridiagonal") { + options->preconditioner_type = ceres::CLUSTER_TRIDIAGONAL; + } else { + LOG(FATAL) << "Unknown ceres preconditioner type: " + << FLAGS_preconditioner_type; + } + } + + options->num_linear_solver_threads = FLAGS_num_threads; +} + +void SetOrdering(BALProblem* bal_problem, Solver::Options* options) { + // 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. Using them however requires that the ParameterBlocks + // are in a particular order (points before cameras) and + // Solver::Options::num_eliminate_blocks is set to the number of + // points. + // + // 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_use_schur_ordering) { + options->ordering_type = ceres::SCHUR; + return; + } + + options->ordering_type = ceres::USER; + 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(); + + // The points come before the cameras. + for (int i = 0; i < num_points; ++i) { + options->ordering.push_back(points + point_block_size * i); + } + + for (int i = 0; i < num_cameras; ++i) { + // When using axis-angle, there is a single parameter block for + // the entire camera. + options->ordering.push_back(cameras + camera_block_size * i); + + // If quaternions are used, there are two blocks, so add the + // second block to the ordering. + if (FLAGS_use_quaternions) { + options->ordering.push_back(cameras + camera_block_size * i + 4); + } + } + + options->num_eliminate_blocks = num_points; +} + +void SetMinimizerOptions(Solver::Options* options) { + options->max_num_iterations = FLAGS_num_iterations; + options->minimizer_progress_to_stdout = true; + options->num_threads = FLAGS_num_threads; +} + +void SetSolverOptionsFromFlags(BALProblem* bal_problem, + Solver::Options* options) { + SetLinearSolver(options); + SetOrdering(bal_problem, options); + SetMinimizerOptions(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. + if (FLAGS_use_quaternions) { + cost_function = new AutoDiffCostFunction< + SnavelyReprojectionErrorWitQuaternions, 2, 4, 6, 3>( + new SnavelyReprojectionErrorWitQuaternions( + observations[2 * i + 0], + observations[2 * i + 1])); + } else { + cost_function = + new AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>( + new SnavelyReprojectionError(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); + Problem problem; + BuildProblem(&bal_problem, &problem); + Solver::Options options; + SetSolverOptionsFromFlags(&bal_problem, &options); + + Solver::Summary summary; + Solve(options, &problem, &summary); + std::cout << summary.FullReport() << "\n"; +} + +} // namespace examples +} // namespace ceres + +int main(int argc, char** argv) { + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + if (FLAGS_input.empty()) { + LOG(ERROR) << "Usage: bundle_adjustment_example --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; +}
diff --git a/examples/circle_fit.cc b/examples/circle_fit.cc new file mode 100644 index 0000000..a044134 --- /dev/null +++ b/examples/circle_fit.cc
@@ -0,0 +1,163 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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: keir@google.com (Keir Mierle) +// +// This fits circles to a collection of points, where the error is related to +// the distance of a point from the circle. This uses auto-differentiation to +// take the derivatives. +// +// The input format is simple text. Feed on standard in: +// +// x_initial y_initial r_initial +// x1 y1 +// x2 y2 +// y3 y3 +// ... +// +// And the result after solving will be printed to stdout: +// +// x y r +// +// There are closed form solutions [1] to this problem which you may want to +// consider instead of using this one. If you already have a decent guess, Ceres +// can squeeze down the last bit of error. +// +// [1] http://www.mathworks.com/matlabcentral/fileexchange/5557-circle-fit/content/circfit.m + +#include <cstdio> +#include <vector> + +#include <gflags/gflags.h> +#include "ceres/ceres.h" + +using ceres::AutoDiffCostFunction; +using ceres::CauchyLoss; +using ceres::CostFunction; +using ceres::LossFunction; +using ceres::Problem; +using ceres::Solve; +using ceres::Solver; + +DEFINE_double(robust_threshold, -1.0, "Robust loss parameter. Set to -1 for " + "normal squared error (no robustification)."); + +// The cost for a single sample. The returned residual is related to the +// distance of the point from the circle (passed in as x, y, m parameters). +// +// Note that the radius is parameterized as r = m^2 to constrain the radius to +// positive values. +class DistanceFromCircleCost { + public: + DistanceFromCircleCost(double xx, double yy) : xx_(xx), yy_(yy) {} + template <typename T> bool operator()(const T* const x, + const T* const y, + const T* const m, // r = m^2 + T* residual) const { + // Since the radius is parameterized as m^2, unpack m to get r. + T r = *m * *m; + + // Get the position of the sample in the circle's coordinate system. + T xp = xx_ - *x; + T yp = yy_ - *y; + + // It is tempting to use the following cost: + // + // residual[0] = *r - sqrt(xp*xp + yp*yp); + // + // which is the distance of the sample from the circle. This works + // reasonably well, but the sqrt() adds strong nonlinearities to the cost + // function. Instead, a different cost is used, which while not strictly a + // distance in the metric sense (it has units distance^2) it produces more + // robust fits when there are outliers. This is because the cost surface is + // more convex. + residual[0] = r*r - xp*xp - yp*yp; + return true; + } + + private: + // The measured x,y coordinate that should be on the circle. + double xx_, yy_; +}; + +int main(int argc, char** argv) { + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + + double x, y, r; + if (scanf("%lg %lg %lg", &x, &y, &r) != 3) { + fprintf(stderr, "Couldn't read first line.\n"); + return 1; + } + fprintf(stderr, "Got x, y, r %lg, %lg, %lg\n", x, y, r); + + // Save initial values for comparison. + double initial_x = x; + double initial_y = y; + double initial_r = r; + + // Parameterize r as m^2 so that it can't be negative. + double m = sqrt(r); + + Problem problem; + + // Configure the loss function. + LossFunction* loss = NULL; + if (FLAGS_robust_threshold) { + loss = new CauchyLoss(FLAGS_robust_threshold); + } + + // Add the residuals. + double xx, yy; + int num_points = 0; + while (scanf("%lf %lf\n", &xx, &yy) == 2) { + CostFunction *cost = + new AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>( + new DistanceFromCircleCost(xx, yy)); + problem.AddResidualBlock(cost, loss, &x, &y, &m); + num_points++; + } + + std::cout << "Got " << num_points << " points.\n"; + + // Build and solve the problem. + Solver::Options options; + options.max_num_iterations = 500; + options.linear_solver_type = ceres::DENSE_QR; + Solver::Summary summary; + Solve(options, &problem, &summary); + + // Recover r from m. + r = m * m; + + std::cout << summary.BriefReport() << "\n"; + std::cout << "x : " << initial_x << " -> " << x << "\n"; + std::cout << "y : " << initial_y << " -> " << y << "\n"; + std::cout << "r : " << initial_r << " -> " << r << "\n"; + return 0; +}
diff --git a/examples/data_fitting.cc b/examples/data_fitting.cc new file mode 100644 index 0000000..f097172 --- /dev/null +++ b/examples/data_fitting.cc
@@ -0,0 +1,164 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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) + +#include "ceres/ceres.h" + +using ceres::AutoDiffCostFunction; +using ceres::CostFunction; +using ceres::Problem; +using ceres::Solver; +using ceres::Solve; + +// Data generated using the following octave code. +// randn('seed', 23497); +// m = 0.3; +// c = 0.1; +// x=[0:0.075:5]; +// y = exp(m * x + c); +// noise = randn(size(x)) * 0.2; +// y_observed = y + noise; +// data = [x', y_observed']; + +const int kNumObservations = 67; +const double data[] = { + 0.000000e+00, 1.133898e+00, + 7.500000e-02, 1.334902e+00, + 1.500000e-01, 1.213546e+00, + 2.250000e-01, 1.252016e+00, + 3.000000e-01, 1.392265e+00, + 3.750000e-01, 1.314458e+00, + 4.500000e-01, 1.472541e+00, + 5.250000e-01, 1.536218e+00, + 6.000000e-01, 1.355679e+00, + 6.750000e-01, 1.463566e+00, + 7.500000e-01, 1.490201e+00, + 8.250000e-01, 1.658699e+00, + 9.000000e-01, 1.067574e+00, + 9.750000e-01, 1.464629e+00, + 1.050000e+00, 1.402653e+00, + 1.125000e+00, 1.713141e+00, + 1.200000e+00, 1.527021e+00, + 1.275000e+00, 1.702632e+00, + 1.350000e+00, 1.423899e+00, + 1.425000e+00, 1.543078e+00, + 1.500000e+00, 1.664015e+00, + 1.575000e+00, 1.732484e+00, + 1.650000e+00, 1.543296e+00, + 1.725000e+00, 1.959523e+00, + 1.800000e+00, 1.685132e+00, + 1.875000e+00, 1.951791e+00, + 1.950000e+00, 2.095346e+00, + 2.025000e+00, 2.361460e+00, + 2.100000e+00, 2.169119e+00, + 2.175000e+00, 2.061745e+00, + 2.250000e+00, 2.178641e+00, + 2.325000e+00, 2.104346e+00, + 2.400000e+00, 2.584470e+00, + 2.475000e+00, 1.914158e+00, + 2.550000e+00, 2.368375e+00, + 2.625000e+00, 2.686125e+00, + 2.700000e+00, 2.712395e+00, + 2.775000e+00, 2.499511e+00, + 2.850000e+00, 2.558897e+00, + 2.925000e+00, 2.309154e+00, + 3.000000e+00, 2.869503e+00, + 3.075000e+00, 3.116645e+00, + 3.150000e+00, 3.094907e+00, + 3.225000e+00, 2.471759e+00, + 3.300000e+00, 3.017131e+00, + 3.375000e+00, 3.232381e+00, + 3.450000e+00, 2.944596e+00, + 3.525000e+00, 3.385343e+00, + 3.600000e+00, 3.199826e+00, + 3.675000e+00, 3.423039e+00, + 3.750000e+00, 3.621552e+00, + 3.825000e+00, 3.559255e+00, + 3.900000e+00, 3.530713e+00, + 3.975000e+00, 3.561766e+00, + 4.050000e+00, 3.544574e+00, + 4.125000e+00, 3.867945e+00, + 4.200000e+00, 4.049776e+00, + 4.275000e+00, 3.885601e+00, + 4.350000e+00, 4.110505e+00, + 4.425000e+00, 4.345320e+00, + 4.500000e+00, 4.161241e+00, + 4.575000e+00, 4.363407e+00, + 4.650000e+00, 4.161576e+00, + 4.725000e+00, 4.619728e+00, + 4.800000e+00, 4.737410e+00, + 4.875000e+00, 4.727863e+00, + 4.950000e+00, 4.669206e+00, +}; + +class ExponentialResidual { + public: + ExponentialResidual(double x, double y) + : x_(x), y_(y) {} + + template <typename T> bool operator()(const T* const m, + const T* const c, + T* residual) const { + residual[0] = T(y_) - exp(m[0] * T(x_) + c[0]); + return true; + } + + private: + const double x_; + const double y_; +}; + +int main(int argc, char** argv) { + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + + double m = 0.0; + double c = 0.0; + + Problem problem; + for (int i = 0; i < kNumObservations; ++i) { + problem.AddResidualBlock( + new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>( + new ExponentialResidual(data[2 * i], data[2 * i + 1])), + NULL, + &m, &c); + } + + Solver::Options options; + options.max_num_iterations = 25; + options.linear_solver_type = ceres::DENSE_QR; + options.minimizer_progress_to_stdout = true; + + Solver::Summary summary; + Solve(options, &problem, &summary); + std::cout << summary.BriefReport() << "\n"; + std::cout << "Initial m: " << 0.0 << " c: " << 0.0 << "\n"; + std::cout << "Final m: " << m << " c: " << c << "\n"; + return 0; +}
diff --git a/examples/powell.cc b/examples/powell.cc new file mode 100644 index 0000000..1c7cc68 --- /dev/null +++ b/examples/powell.cc
@@ -0,0 +1,150 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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 program that minimizes Powell's singular function. +// +// F = 1/2 (f1^2 + f2^2 + f3^2 + f4^2) +// +// f1 = x1 + 10*x2; +// f2 = sqrt(5) * (x3 - x4) +// f3 = (x2 - 2*x3)^2 +// f4 = sqrt(10) * (x1 - x4)^2 +// +// The starting values are x1 = 3, x2 = -1, x3 = 0, x4 = 1. +// The minimum is 0 at (x1, x2, x3, x4) = 0. +// +// From: Testing Unconstrained Optimization Software by Jorge J. More, Burton S. +// Garbow and Kenneth E. Hillstrom in ACM Transactions on Mathematical Software, +// Vol 7(1), March 1981. + +#include <vector> + +#include "ceres/ceres.h" + +using ceres::AutoDiffCostFunction; +using ceres::CostFunction; +using ceres::Problem; +using ceres::Solver; +using ceres::Solve; + +class F1 { + public: + template <typename T> bool operator()(const T* const x1, + const T* const x2, + T* residual) const { + // f1 = x1 + 10 * x2; + residual[0] = x1[0] + T(10.0) * x2[0]; + return true; + } +}; + +class F2 { + public: + template <typename T> bool operator()(const T* const x3, + const T* const x4, + T* residual) const { + // f2 = sqrt(5) (x3 - x4) + residual[0] = T(sqrt(5.0)) * (x3[0] - x4[0]); + return true; + } +}; + +class F3 { + public: + template <typename T> bool operator()(const T* const x2, + const T* const x4, + T* residual) const { + // f3 = (x2 - 2 x3)^2 + residual[0] = (x2[0] - T(2.0) * x4[0]) * (x2[0] - T(2.0) * x4[0]); + return true; + } +}; + +class F4 { + public: + template <typename T> bool operator()(const T* const x1, + const T* const x4, + T* residual) const { + // f4 = sqrt(10) (x1 - x4)^2 + residual[0] = T(sqrt(10.0)) * (x1[0] - x4[0]) * (x1[0] - x4[0]); + return true; + } +}; + +int main(int argc, char** argv) { + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + + double x1 = 3.0; + double x2 = -1.0; + double x3 = 0.0; + double x4 = 1.0; + + Problem problem; + // Add residual terms to the problem using the using the autodiff + // wrapper to get the derivatives automatically. The parameters, x1 through + // x4, are modified in place. + problem.AddResidualBlock(new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), + NULL, + &x1, &x2); + problem.AddResidualBlock(new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), + NULL, + &x3, &x4); + problem.AddResidualBlock(new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), + NULL, + &x2, &x3); + problem.AddResidualBlock(new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), + NULL, + &x1, &x4); + + // Run the solver! + Solver::Options options; + options.max_num_iterations = 30; + options.linear_solver_type = ceres::DENSE_QR; + options.minimizer_progress_to_stdout = true; + + Solver::Summary summary; + + std::cout << "Initial x1 = " << x1 + << ", x2 = " << x2 + << ", x3 = " << x3 + << ", x4 = " << x4 + << "\n"; + + Solve(options, &problem, &summary); + + std::cout << summary.BriefReport() << "\n"; + std::cout << "Final x1 = " << x1 + << ", x2 = " << x2 + << ", x3 = " << x3 + << ", x4 = " << x4 + << "\n"; + return 0; +}
diff --git a/examples/quadratic.cc b/examples/quadratic.cc new file mode 100644 index 0000000..81ba4f9 --- /dev/null +++ b/examples/quadratic.cc
@@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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: keir@google.com (Keir Mierle) +// +// A simple example of using the Ceres minimizer. +// +// Minimize 0.5 (10 - x)^2 using analytic jacobian matrix. + +#include <vector> + +#include "ceres/ceres.h" + +using ceres::SizedCostFunction; +using ceres::Problem; +using ceres::Solver; +using ceres::Solve; + +class SimpleCostFunction + : public SizedCostFunction<1 /* number of residuals */, + 1 /* size of first parameter */> { + public: + virtual ~SimpleCostFunction() {} + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + double x = parameters[0][0]; + + // f(x) = 10 - x. + residuals[0] = 10 - x; + + // f'(x) = -1. Since there's only 1 parameter and that parameter + // has 1 dimension, there is only 1 element to fill in the + // jacobians. + if (jacobians != NULL && jacobians[0] != NULL) { + jacobians[0][0] = -1; + } + return true; + } +}; + +int main(int argc, char** argv) { + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + + // The variable with its initial value that we will be solving for. + double x = 5.0; + + // Build the problem. + Problem problem; + // Set up the only cost function (also known as residual). + problem.AddResidualBlock(new SimpleCostFunction, NULL, &x); + + // Run the solver! + Solver::Options options; + options.max_num_iterations = 10; + options.linear_solver_type = ceres::DENSE_QR; + options.minimizer_progress_to_stdout = true; + Solver::Summary summary; + Solve(options, &problem, &summary); + std::cout << summary.BriefReport() << "\n"; + std::cout << "x : 5.0 -> " << x << "\n"; + return 0; +}
diff --git a/examples/quadratic_auto_diff.cc b/examples/quadratic_auto_diff.cc new file mode 100644 index 0000000..71b216b --- /dev/null +++ b/examples/quadratic_auto_diff.cc
@@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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: keir@google.com (Keir Mierle) +// +// A simple example of using the Ceres minimizer. +// +// Minimize 0.5 (10 - x)^2 using jacobian matrix computed using +// automatic differentiation. + +#include <vector> + +#include "ceres/ceres.h" + +using ceres::AutoDiffCostFunction; +using ceres::CostFunction; +using ceres::Problem; +using ceres::Solver; +using ceres::Solve; + +// A templated cost function that implements the residual r = 10 - x. The method +// Map is templated so that we can then use an automatic differentiation wrapper +// around it to generate its derivatives. +class QuadraticCostFunction { + public: + template <typename T> bool operator()(const T* const x, T* residual) const { + residual[0] = T(10.0) - x[0]; + return true; + } +}; + +int main(int argc, char** argv) { + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + + // The variable to solve for with its initial value. + double initial_x = 5.0; + double x = initial_x; + + // Build the problem. + Problem problem; + + // Set up the only cost function (also known as residual). This uses + // auto-differentiation to obtain the derivative (jacobian). + problem.AddResidualBlock( + new AutoDiffCostFunction<QuadraticCostFunction, 1, 1>( + new QuadraticCostFunction), + NULL, + &x); + + // Run the solver! + Solver::Options options; + options.max_num_iterations = 10; + options.linear_solver_type = ceres::DENSE_QR; + options.minimizer_progress_to_stdout = true; + Solver::Summary summary; + Solve(options, &problem, &summary); + std::cout << summary.BriefReport() << "\n"; + std::cout << "x : " << initial_x + << " -> " << x << "\n"; + return 0; +}
diff --git a/examples/quadratic_numeric_diff.cc b/examples/quadratic_numeric_diff.cc new file mode 100644 index 0000000..933dbc7 --- /dev/null +++ b/examples/quadratic_numeric_diff.cc
@@ -0,0 +1,91 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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: keir@google.com (Keir Mierle) +// +// Minimize 0.5 (10 - x)^2 using jacobian matrix computed using +// numeric differentiation. + +#include <vector> + +#include "ceres/ceres.h" + +using ceres::NumericDiffCostFunction; +using ceres::CENTRAL; +using ceres::SizedCostFunction; +using ceres::CostFunction; +using ceres::Problem; +using ceres::Solver; +using ceres::Solve; + +class ResidualWithNoDerivative + : public SizedCostFunction<1 /* number of residuals */, + 1 /* size of first parameter */> { + public: + virtual ~ResidualWithNoDerivative() {} + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + (void) jacobians; // Ignored; filled in by numeric differentiation. + + // f(x) = 10 - x. + residuals[0] = 10 - parameters[0][0]; + return true; + } +}; + +int main(int argc, char** argv) { + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + + // The variable to solve for with its initial value. + double initial_x = 5.0; + double x = initial_x; + + // Set up the only cost function (also known as residual). This uses + // numeric differentiation to obtain the derivative (jacobian). + CostFunction* cost = + new NumericDiffCostFunction<ResidualWithNoDerivative, CENTRAL, 1, 1> ( + new ResidualWithNoDerivative, ceres::TAKE_OWNERSHIP); + + // Build the problem. + Problem problem; + problem.AddResidualBlock(cost, NULL, &x); + + // Run the solver! + Solver::Options options; + options.max_num_iterations = 10; + options.linear_solver_type = ceres::DENSE_QR; + options.minimizer_progress_to_stdout = true; + Solver::Summary summary; + Solve(options, &problem, &summary); + std::cout << summary.BriefReport() << "\n"; + std::cout << "x : " << initial_x + << " -> " << x << "\n"; + return 0; +}
diff --git a/examples/simple_bundle_adjuster.cc b/examples/simple_bundle_adjuster.cc new file mode 100644 index 0000000..cb1143c --- /dev/null +++ b/examples/simple_bundle_adjuster.cc
@@ -0,0 +1,210 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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: keir@google.com (Keir Mierle) +// +// A minimal, self-contained bundle adjuster using Ceres, that reads +// files from University of Washington' Bundle Adjustment in the Large dataset: +// http://grail.cs.washington.edu/projects/bal +// +// This does not use the best configuration for solving; see the more involved +// bundle_adjuster.cc file for details. + +#include <cmath> +#include <cstdio> +#include <iostream> + +#include "ceres/ceres.h" +#include "ceres/rotation.h" + +// Read a Bundle Adjustment in the Large dataset. +class BALProblem { + public: + ~BALProblem() { + delete[] point_index_; + delete[] camera_index_; + delete[] observations_; + delete[] parameters_; + } + + int num_observations() const { return num_observations_; } + const double* observations() const { return observations_; } + double* mutable_cameras() { return parameters_; } + double* mutable_points() { return parameters_ + 9 * num_cameras_; } + + double* mutable_camera_for_observation(int i) { + return mutable_cameras() + camera_index_[i] * 9; + } + double* mutable_point_for_observation(int i) { + return mutable_points() + point_index_[i] * 3; + } + + bool LoadFile(const char* filename) { + FILE* fptr = fopen(filename, "r"); + if (fptr == NULL) { + return false; + }; + + FscanfOrDie(fptr, "%d", &num_cameras_); + FscanfOrDie(fptr, "%d", &num_points_); + FscanfOrDie(fptr, "%d", &num_observations_); + + point_index_ = new int[num_observations_]; + camera_index_ = new int[num_observations_]; + observations_ = new double[2 * num_observations_]; + + num_parameters_ = 9 * num_cameras_ + 3 * num_points_; + parameters_ = new double[num_parameters_]; + + for (int i = 0; i < num_observations_; ++i) { + FscanfOrDie(fptr, "%d", camera_index_ + i); + FscanfOrDie(fptr, "%d", point_index_ + i); + for (int j = 0; j < 2; ++j) { + FscanfOrDie(fptr, "%lf", observations_ + 2*i + j); + } + } + + for (int i = 0; i < num_parameters_; ++i) { + FscanfOrDie(fptr, "%lf", parameters_ + i); + } + return true; + } + + private: + template<typename T> + void FscanfOrDie(FILE *fptr, const char *format, T *value) { + int num_scanned = fscanf(fptr, format, value); + if (num_scanned != 1) { + LOG(FATAL) << "Invalid UW data file."; + } + } + + int num_cameras_; + int num_points_; + int num_observations_; + int num_parameters_; + + int* point_index_; + int* camera_index_; + double* observations_; + double* parameters_; +}; + +// Templated pinhole camera model for used with Ceres. The camera is +// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for +// focal length and 2 for radial distortion. The principal point is not modeled +// (i.e. it is assumed be located at the image center). +struct SnavelyReprojectionError { + SnavelyReprojectionError(double observed_x, double observed_y) + : observed_x(observed_x), observed_y(observed_y) {} + + template <typename T> + bool operator()(const T* const camera, + const T* const point, + T* residuals) const { + // camera[0,1,2] are the angle-axis rotation. + T p[3]; + ceres::AngleAxisRotatePoint(camera, point, p); + + // camera[3,4,5] are the translation. + p[0] += camera[3]; + p[1] += camera[4]; + p[2] += camera[5]; + + // Compute the center of distortion. The sign change comes from + // the camera model that Noah Snavely's Bundler assumes, whereby + // the camera coordinate system has a negative z axis. + const T& focal = camera[6]; + T xp = - focal * p[0] / p[2]; + T yp = - focal * p[1] / p[2]; + + // Apply second and fourth order radial distortion. + const T& l1 = camera[7]; + const T& l2 = camera[8]; + T r2 = xp*xp + yp*yp; + T distortion = T(1.0) + r2 * (l1 + l2 * r2); + + // Compute final projected point position. + T predicted_x = distortion * xp; + T predicted_y = distortion * yp; + + // The error is the difference between the predicted and observed position. + residuals[0] = predicted_x - T(observed_x); + residuals[1] = predicted_y - T(observed_y); + + return true; + } + + double observed_x; + double observed_y; +}; + +int main(int argc, char** argv) { + if (argc != 2) { + std::cerr << "usage: simple_bundle_adjuster <bal_problem>\n"; + return 1; + } + + BALProblem bal_problem; + if (!bal_problem.LoadFile(argv[1])) { + std::cerr << "ERROR: unable to open file " << argv[1] << "\n"; + return 1; + } + + // Create residuals for each observation in the bundle adjustment problem. The + // parameters for cameras and points are added automatically. + ceres::Problem problem; + for (int i = 0; i < bal_problem.num_observations(); ++i) { + // Each Residual block takes a point and a camera as input and outputs a 2 + // dimensional residual. Internally, the cost function stores the observed + // image location and compares the reprojection against the observation. + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>( + new SnavelyReprojectionError( + bal_problem.observations()[2 * i + 0], + bal_problem.observations()[2 * i + 1])); + + problem.AddResidualBlock(cost_function, + NULL /* squared loss */, + bal_problem.mutable_camera_for_observation(i), + bal_problem.mutable_point_for_observation(i)); + } + + // Make Ceres automatically detect the bundle structure. Note that the + // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower + // for standard bundle adjustment problems. + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + options.ordering_type = ceres::SCHUR; + options.minimizer_progress_to_stdout = true; + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + std::cout << summary.FullReport() << "\n"; + return 0; +}
diff --git a/examples/snavely_reprojection_error.h b/examples/snavely_reprojection_error.h new file mode 100644 index 0000000..e43538f --- /dev/null +++ b/examples/snavely_reprojection_error.h
@@ -0,0 +1,156 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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) +// +// Templated struct implementing the camera model and residual +// computation for bundle adjustment used by Noah Snavely's Bundler +// SfM system. This is also the camera model/residual for the bundle +// adjustment problems in the BAL dataset. It is templated so that we +// can use Ceres's automatic differentiation to compute analytic +// jacobians. +// +// For details see: http://phototour.cs.washington.edu/bundler/ +// and http://grail.cs.washington.edu/projects/bal/ + +#ifndef CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_ +#define CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_ + +#include "ceres/rotation.h" + +namespace ceres { +namespace examples { + +// Templated pinhole camera model for used with Ceres. The camera is +// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for +// focal length and 2 for radial distortion. The principal point is not modeled +// (i.e. it is assumed be located at the image center). +struct SnavelyReprojectionError { + SnavelyReprojectionError(double observed_x, double observed_y) + : observed_x(observed_x), observed_y(observed_y) {} + + template <typename T> + bool operator()(const T* const camera, + const T* const point, + T* residuals) const { + // camera[0,1,2] are the angle-axis rotation. + T p[3]; + ceres::AngleAxisRotatePoint(camera, point, p); + + // camera[3,4,5] are the translation. + p[0] += camera[3]; + p[1] += camera[4]; + p[2] += camera[5]; + + // Compute the center of distortion. The sign change comes from + // the camera model that Noah Snavely's Bundler assumes, whereby + // the camera coordinate system has a negative z axis. + const T& focal = camera[6]; + T xp = - focal * p[0] / p[2]; + T yp = - focal * p[1] / p[2]; + + // Apply second and fourth order radial distortion. + const T& l1 = camera[7]; + const T& l2 = camera[8]; + T r2 = xp*xp + yp*yp; + T distortion = T(1.0) + r2 * (l1 + l2 * r2); + + // Compute final projected point position. + T predicted_x = distortion * xp; + T predicted_y = distortion * yp; + + // The error is the difference between the predicted and observed position. + residuals[0] = predicted_x - T(observed_x); + residuals[1] = predicted_y - T(observed_y); + + return true; + } + + double observed_x; + double observed_y; +}; + +// Templated pinhole camera model for used with Ceres. The camera is +// parameterized using 10 parameters. 4 for rotation, 3 for +// translation, 1 for focal length and 2 for radial distortion. The +// principal point is not modeled (i.e. it is assumed be located at +// the image center). +struct SnavelyReprojectionErrorWitQuaternions { + // (u, v): the position of the observation with respect to the image + // center point. + SnavelyReprojectionErrorWitQuaternions(double observed_x, double observed_y) + : observed_x(observed_x), observed_y(observed_y) {} + + template <typename T> + bool operator()(const T* const camera_rotation, + const T* const camera_translation_and_intrinsics, + const T* const point, + T* residuals) const { + const T& focal = camera_translation_and_intrinsics[3]; + const T& l1 = camera_translation_and_intrinsics[4]; + const T& l2 = camera_translation_and_intrinsics[5]; + + // Use a quaternion rotation that doesn't assume the quaternion is + // normalized, since one of the ways to run the bundler is to let Ceres + // optimize all 4 quaternion parameters unconstrained. + T p[3]; + QuaternionRotatePoint(camera_rotation, point, p); + + p[0] += camera_translation_and_intrinsics[0]; + p[1] += camera_translation_and_intrinsics[1]; + p[2] += camera_translation_and_intrinsics[2]; + + // Compute the center of distortion. The sign change comes from + // the camera model that Noah Snavely's Bundler assumes, whereby + // the camera coordinate system has a negative z axis. + T xp = - focal * p[0] / p[2]; + T yp = - focal * p[1] / p[2]; + + // Apply second and fourth order radial distortion. + T r2 = xp*xp + yp*yp; + T distortion = T(1.0) + r2 * (l1 + l2 * r2); + + // Compute final projected point position. + T predicted_x = distortion * xp; + T predicted_y = distortion * yp; + + // The error is the difference between the predicted and observed position. + residuals[0] = predicted_x - T(observed_x); + residuals[1] = predicted_y - T(observed_y); + + return true; + } + + double observed_x; + double observed_y; +}; + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_