diff options
author | Angus Kong <shkong@google.com> | 2013-02-13 14:56:04 -0800 |
---|---|---|
committer | Angus Kong <shkong@google.com> | 2013-02-14 13:33:05 -0800 |
commit | 0ae28bd5885b5daa526898fcf7c323dc2c3e1963 (patch) | |
tree | 487c4897ae7509fb4e05751b0a673a8090e26d41 /examples | |
parent | e9e9be9383b479ac1573370ab221ffbac4de8c90 (diff) | |
download | ceres-solver-0ae28bd5885b5daa526898fcf7c323dc2c3e1963.tar.gz |
Initial import of ceres-solver 1.4.0android-4.3_r3.1android-4.3_r3android-4.3_r2.3android-4.3_r2.2android-4.3_r2.1android-4.3_r2android-4.3_r1.1android-4.3_r1android-4.3_r0.9.1android-4.3_r0.9android-4.3.1_r1tools_r22.2jb-mr2.0.0-releasejb-mr2.0-releasejb-mr2-releasejb-mr2-dev
Added a NOTICE and a MODULE_LICENSE_BSD file.
Added Android.mk for master build and unbundled build.
Added CleanSpec.mk to optimize Android build.
Change-Id: I6cd82bcabc1a94b10239f9fca017de0afd20e769
Diffstat (limited to 'examples')
-rw-r--r-- | examples/CMakeLists.txt | 66 | ||||
-rw-r--r-- | examples/Makefile.example | 82 | ||||
-rw-r--r-- | examples/bal_problem.cc | 301 | ||||
-rw-r--r-- | examples/bal_problem.h | 107 | ||||
-rw-r--r-- | examples/bundle_adjuster.cc | 341 | ||||
-rw-r--r-- | examples/circle_fit.cc | 164 | ||||
-rw-r--r-- | examples/data_fitting.cc | 165 | ||||
-rw-r--r-- | examples/denoising.cc | 214 | ||||
-rw-r--r-- | examples/fields_of_experts.cc | 152 | ||||
-rw-r--r-- | examples/fields_of_experts.h | 145 | ||||
-rw-r--r-- | examples/nist.cc | 479 | ||||
-rw-r--r-- | examples/pgm_image.h | 319 | ||||
-rw-r--r-- | examples/powell.cc | 151 | ||||
-rw-r--r-- | examples/quadratic.cc | 90 | ||||
-rw-r--r-- | examples/quadratic_auto_diff.cc | 88 | ||||
-rw-r--r-- | examples/quadratic_numeric_diff.cc | 92 | ||||
-rw-r--r-- | examples/simple_bundle_adjuster.cc | 210 | ||||
-rw-r--r-- | examples/snavely_reprojection_error.h | 156 |
18 files changed, 3322 insertions, 0 deletions
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..2307a03 --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1,66 @@ +# 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) + +IF (${GFLAGS}) + ADD_EXECUTABLE(quadratic quadratic.cc) + TARGET_LINK_LIBRARIES(quadratic ceres) + + ADD_EXECUTABLE(nist nist.cc) + TARGET_LINK_LIBRARIES(nist 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(data_fitting data_fitting.cc) + TARGET_LINK_LIBRARIES(data_fitting ceres) + + ADD_EXECUTABLE(bundle_adjuster + bundle_adjuster.cc + bal_problem.cc) + TARGET_LINK_LIBRARIES(bundle_adjuster ceres) + + ADD_EXECUTABLE(denoising + denoising.cc + fields_of_experts.cc) + TARGET_LINK_LIBRARIES(denoising ceres) +ENDIF (${GFLAGS}) + +ADD_EXECUTABLE(simple_bundle_adjuster + simple_bundle_adjuster.cc) +TARGET_LINK_LIBRARIES(simple_bundle_adjuster ceres) diff --git a/examples/Makefile.example b/examples/Makefile.example new file mode 100644 index 0000000..3f303d6 --- /dev/null +++ b/examples/Makefile.example @@ -0,0 +1,82 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 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 is an example Makefile for using Ceres. In practice, the Ceres authors +# suggest using CMake instead, but if Make is needed for some reason, this +# example serves to make it easy to do so. + +# This should point to place where you unpacked or cloned Ceres. +CERES_SRC_DIR := /home/keir/wrk/ceres-extra + +# This should point to the place where you built Ceres. If you got Ceres by +# installing it, then this will likely be /usr/local/lib. +CERES_BIN_DIR := /home/keir/wrk/ceres-extra-bin + +# The place you unpacked or cloned Eigen. If Eigen was installed from packages, +# this will likely be /usr/local/include. +EIGEN_SRC_DIR := /home/keir/src/eigen-3.0.5 + +INCLUDES := -I$(CERES_SRC_DIR)/include \ + -I$(EIGEN_SRC_DIR) + +CERES_LIBRARY := -lceres +CERES_LIBRARY_PATH := -L$(CERES_BIN_DIR)/lib +CERES_LIBRARY_DEPENDENCIES = -lgflags -lglog + +# If Ceres was built with Suitesparse: +CERES_LIBRARY_DEPENDENCIES += -llapack -lcamd -lamd -lccolamd -lcolamd -lcholmod + +# If Ceres was built with CXSparse: +CERES_LIBRARY_DEPENDENCIES += -lcxsparse + +# If Ceres was built with OpenMP: +CERES_LIBRARY_DEPENDENCIES += -fopenmp -lpthread -lgomp -lm + +# The set of object files for your application. +APPLICATION_OBJS := simple_bundle_adjuster.o + +all: simple_bundle_adjuster + +simple_bundle_adjuster: $(APPLICATION_OBJS) + g++ \ + $(APPLICATION_OBJS) \ + $(CERES_LIBRARY_PATH) \ + $(CERES_LIBRARY) \ + $(CERES_LIBRARY_DEPENDENCIES) \ + -o simple_bundle_adjuster + +# Disabling debug asserts via -DNDEBUG helps make Eigen faster, at the cost of +# not getting handy assert failures when there are issues in your code. +CFLAGS := -O2 -DNDEBUG + +# If you have files ending in .cpp instead of .cc, fix the next line +# appropriately. +%.o: %.cc $(DEPS) + g++ -c -o $@ $< $(CFLAGS) $(INCLUDES) diff --git a/examples/bal_problem.cc b/examples/bal_problem.cc new file mode 100644 index 0000000..5733f46 --- /dev/null +++ b/examples/bal_problem.cc @@ -0,0 +1,301 @@ +// 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 <vector> +#include "Eigen/Core" +#include "ceres/random.h" +#include "ceres/rotation.h" +#include "glog/logging.h" + +namespace ceres { +namespace examples { +namespace { +typedef Eigen::Map<Eigen::VectorXd> VectorRef; +typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef; + +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."; + } +} + +void PerturbPoint3(const double sigma, double* point) { + for (int i = 0; i < 3; ++i) { + point[i] += RandNormal() * sigma; + } +} + +double Median(std::vector<double>* data) { + int n = data->size(); + std::vector<double>::iterator mid_point = data->begin() + n / 2; + std::nth_element(data->begin(), mid_point, data->end()); + return *mid_point; +} + +} // namespace + +BALProblem::BALProblem(const std::string& filename, bool use_quaternions) { + FILE* fptr = fopen(filename.c_str(), "r"); + + if (fptr == NULL) { + 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); + } + + fclose(fptr); + + 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; + } +} + +// This function writes the problem to a file in the same format that +// is read by the constructor. +void BALProblem::WriteToFile(const std::string& filename) const { + FILE* fptr = fopen(filename.c_str(), "w"); + + if (fptr == NULL) { + LOG(FATAL) << "Error: unable to open file " << filename; + return; + }; + + fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_); + + for (int i = 0; i < num_observations_; ++i) { + fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]); + for (int j = 0; j < 2; ++j) { + fprintf(fptr, " %g", observations_[2 * i + j]); + } + fprintf(fptr, "\n"); + } + + for (int i = 0; i < num_cameras(); ++i) { + double angleaxis[9]; + if (use_quaternions_) { + // Output in angle-axis format. + QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis); + memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double)); + } else { + memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double)); + } + for (int j = 0; j < 9; ++j) { + fprintf(fptr, "%.16g\n", angleaxis[j]); + } + } + + const double* points = parameters_ + camera_block_size() * num_cameras_; + for (int i = 0; i < num_points(); ++i) { + const double* point = points + i * point_block_size(); + for (int j = 0; j < point_block_size(); ++j) { + fprintf(fptr, "%.16g\n", point[j]); + } + } + + fclose(fptr); +} + +void BALProblem::CameraToAngleAxisAndCenter(const double* camera, + double* angle_axis, + double* center) { + VectorRef angle_axis_ref(angle_axis, 3); + if (use_quaternions_) { + QuaternionToAngleAxis(camera, angle_axis); + } else { + angle_axis_ref = ConstVectorRef(camera, 3); + } + + // c = -R't + Eigen::VectorXd inverse_rotation = -angle_axis_ref; + AngleAxisRotatePoint(inverse_rotation.data(), + camera + camera_block_size() - 6, + center); + VectorRef(center, 3) *= -1.0; +} + +void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis, + const double* center, + double* camera) { + ConstVectorRef angle_axis_ref(angle_axis, 3); + if (use_quaternions_) { + AngleAxisToQuaternion(angle_axis, camera); + } else { + VectorRef(camera, 3) = angle_axis_ref; + } + + // t = -R * c + AngleAxisRotatePoint(angle_axis, + center, + camera + camera_block_size() - 6); + VectorRef(camera + camera_block_size() - 6, 3) *= -1.0; +} + + +void BALProblem::Normalize() { + // Compute the marginal median of the geometry. + std::vector<double> tmp(num_points_); + Eigen::Vector3d median; + double* points = mutable_points(); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < num_points_; ++j) { + tmp[j] = points[3 * j + i]; + } + median(i) = Median(&tmp); + } + + for (int i = 0; i < num_points_; ++i) { + VectorRef point(points + 3 * i, 3); + tmp[i] = (point - median).lpNorm<1>(); + } + + const double median_absolute_deviation = Median(&tmp); + + // Scale so that the median absolute deviation of the resulting + // reconstruction is 100. + const double scale = 100.0 / median_absolute_deviation; + + VLOG(2) << "median: " << median.transpose(); + VLOG(2) << "median absolute deviation: " << median_absolute_deviation; + VLOG(2) << "scale: " << scale; + + // X = scale * (X - median) + for (int i = 0; i < num_points_; ++i) { + VectorRef point(points + 3 * i, 3); + point = scale * (point - median); + } + + double* cameras = mutable_cameras(); + double angle_axis[3]; + double center[3]; + for (int i = 0; i < num_cameras_; ++i) { + double* camera = cameras + camera_block_size() * i; + CameraToAngleAxisAndCenter(camera, angle_axis, center); + // center = scale * (center - median) + VectorRef(center, 3) = scale * (VectorRef(center, 3) - median); + AngleAxisAndCenterToCamera(angle_axis, center, camera); + } +} + +void BALProblem::Perturb(const double rotation_sigma, + const double translation_sigma, + const double point_sigma) { + CHECK_GE(point_sigma, 0.0); + CHECK_GE(rotation_sigma, 0.0); + CHECK_GE(translation_sigma, 0.0); + + double* points = mutable_points(); + if (point_sigma > 0) { + for (int i = 0; i < num_points_; ++i) { + PerturbPoint3(point_sigma, points + 3 * i); + } + } + + for (int i = 0; i < num_cameras_; ++i) { + double* camera = mutable_cameras() + camera_block_size() * i; + + double angle_axis[3]; + double center[3]; + // Perturb in the rotation of the camera in the angle-axis + // representation. + CameraToAngleAxisAndCenter(camera, angle_axis, center); + if (rotation_sigma > 0.0) { + PerturbPoint3(rotation_sigma, angle_axis); + } + AngleAxisAndCenterToCamera(angle_axis, center, camera); + + if (translation_sigma > 0.0) { + PerturbPoint3(translation_sigma, camera + camera_block_size() - 6); + } + } +} + +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..9173246 --- /dev/null +++ b/examples/bal_problem.h @@ -0,0 +1,107 @@ +// 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(); + + void WriteToFile(const std::string& filename) const; + + // Move the "center" of the reconstruction to the origin, where the + // center is determined by computing the marginal median of the + // points. The reconstruction is then scaled so that the median + // absolute deviation of the points measured from the origin is + // 100.0. + // + // The reprojection error of the problem remains the same. + void Normalize(); + + // Perturb the camera pose and the geometry with random normal + // numbers with corresponding standard deviations. + void Perturb(const double rotation_sigma, + const double translation_sigma, + const double point_sigma); + + 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: + void CameraToAngleAxisAndCenter(const double* camera, + double* angle_axis, + double* center); + + void AngleAxisAndCenterToCamera(const double* angle_axis, + const double* center, + double* camera); + 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..78dbd01 --- /dev/null +++ b/examples/bundle_adjuster.cc @@ -0,0 +1,341 @@ +// 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 <cstdlib> +#include <string> +#include <vector> + +#include "bal_problem.h" +#include "ceres/ceres.h" +#include "ceres/random.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_string(preconditioner, "jacobi", "Options are: " + "identity, jacobi, schur_jacobi, cluster_jacobi, " + "cluster_tridiagonal."); +DEFINE_string(sparse_linear_algebra_library, "suite_sparse", + "Options are: suite_sparse and cx_sparse."); +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_bool(use_block_amd, true, "Use a block oriented fill reducing " + "ordering."); + +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_string(solver_log, "", "File to record the solver execution to."); + +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(StringToSparseLinearAlgebraLibraryType( + FLAGS_sparse_linear_algebra_library, + &options->sparse_linear_algebra_library)); + options->num_linear_solver_threads = FLAGS_num_threads; +} + +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(); + + options->use_block_amd = FLAGS_use_block_amd; + + if (options->use_inner_iterations) { + if (FLAGS_blocks_for_inner_iterations == "cameras") { + LOG(INFO) << "Camera blocks for inner iterations"; + options->inner_iteration_ordering = 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 = 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 = 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 = 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 = 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; + 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. + if (FLAGS_use_quaternions) { + cost_function = new AutoDiffCostFunction< + SnavelyReprojectionErrorWithQuaternions, 2, 4, 6, 3>( + new SnavelyReprojectionErrorWithQuaternions( + 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; + + SetRandomState(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.solver_log = FLAGS_solver_log; + options.gradient_tolerance = 1e-16; + options.function_tolerance = 1e-16; + 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..0763806 --- /dev/null +++ b/examples/circle_fit.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: 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 "ceres/ceres.h" +#include "gflags/gflags.h" +#include "glog/logging.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, 0.0, "Robust loss parameter. Set to 0 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..5d54123 --- /dev/null +++ b/examples/data_fitting.cc @@ -0,0 +1,165 @@ +// 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" +#include "gflags/gflags.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/denoising.cc b/examples/denoising.cc new file mode 100644 index 0000000..086be00 --- /dev/null +++ b/examples/denoising.cc @@ -0,0 +1,214 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 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: strandmark@google.com (Petter Strandmark) +// +// Denoising using Fields of Experts and the Ceres minimizer. +// +// Note that for good denoising results the weighting between the data term +// and the Fields of Experts term needs to be adjusted. This is discussed +// in [1]. This program assumes Gaussian noise. The noise model can be changed +// by substituing another function for QuadraticCostFunction. +// +// [1] S. Roth and M.J. Black. "Fields of Experts." International Journal of +// Computer Vision, 82(2):205--229, 2009. + +#include <algorithm> +#include <cmath> +#include <iostream> +#include <vector> +#include <sstream> +#include <string> + +#include "ceres/ceres.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +#include "fields_of_experts.h" +#include "pgm_image.h" + +DEFINE_string(input, "", "File to which the output image should be written"); +DEFINE_string(foe_file, "", "FoE file to use"); +DEFINE_string(output, "", "File to which the output image should be written"); +DEFINE_double(sigma, 20.0, "Standard deviation of noise"); +DEFINE_bool(verbose, false, "Prints information about the solver progress."); + +namespace ceres { +namespace examples { + +// This cost function is used to build the data term. +// +// f_i(x) = a * (x_i - b)^2 +// +class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> { + public: + QuadraticCostFunction(double a, double b) + : sqrta_(std::sqrt(a)), b_(b) {} + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + const double x = parameters[0][0]; + residuals[0] = sqrta_ * (x - b_); + if (jacobians != NULL && jacobians[0] != NULL) { + jacobians[0][0] = sqrta_; + } + return true; + } + private: + double sqrta_, b_; +}; + +// Creates a Fields of Experts MAP inference problem. +void CreateProblem(const FieldsOfExperts& foe, + const PGMImage<double>& image, + Problem* problem, + PGMImage<double>* solution) { + // Create the data term + CHECK_GT(FLAGS_sigma, 0.0); + const double coefficient = 1 / (2.0 * FLAGS_sigma * FLAGS_sigma); + for (unsigned index = 0; index < image.NumPixels(); ++index) { + ceres::CostFunction* cost_function = + new QuadraticCostFunction(coefficient, + image.PixelFromLinearIndex(index)); + problem->AddResidualBlock(cost_function, + NULL, + solution->MutablePixelFromLinearIndex(index)); + } + + // Create Ceres cost and loss functions for regularization. One is needed for + // each filter. + std::vector<ceres::LossFunction*> loss_function(foe.NumFilters()); + std::vector<ceres::CostFunction*> cost_function(foe.NumFilters()); + for (int alpha_index = 0; alpha_index < foe.NumFilters(); ++alpha_index) { + loss_function[alpha_index] = foe.NewLossFunction(alpha_index); + cost_function[alpha_index] = foe.NewCostFunction(alpha_index); + } + + // Add FoE regularization for each patch in the image. + for (int x = 0; x < image.width() - (foe.Size() - 1); ++x) { + for (int y = 0; y < image.height() - (foe.Size() - 1); ++y) { + // Build a vector with the pixel indices of this patch. + std::vector<double*> pixels; + const std::vector<int>& x_delta_indices = foe.GetXDeltaIndices(); + const std::vector<int>& y_delta_indices = foe.GetYDeltaIndices(); + for (int i = 0; i < foe.NumVariables(); ++i) { + double* pixel = solution->MutablePixel(x + x_delta_indices[i], + y + y_delta_indices[i]); + pixels.push_back(pixel); + } + // For this patch with coordinates (x, y), we will add foe.NumFilters() + // terms to the objective function. + for (int alpha_index = 0; alpha_index < foe.NumFilters(); ++alpha_index) { + problem->AddResidualBlock(cost_function[alpha_index], + loss_function[alpha_index], + pixels); + } + } + } +} + +// Solves the FoE problem using Ceres and post-processes it to make sure the +// solution stays within [0, 255]. +void SolveProblem(Problem* problem, PGMImage<double>* solution) { + // These parameters may be experimented with. For example, ceres::DOGLEG tends + // to be faster for 2x2 filters, but gives solutions with slightly higher + // objective function value. + ceres::Solver::Options options; + options.max_num_iterations = 100; + if (FLAGS_verbose) { + options.minimizer_progress_to_stdout = true; + } + options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; + options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; + options.function_tolerance = 1e-3; // Enough for denoising. + + ceres::Solver::Summary summary; + ceres::Solve(options, problem, &summary); + if (FLAGS_verbose) { + std::cout << summary.FullReport() << "\n"; + } + + // Make the solution stay in [0, 255]. + for (int x = 0; x < solution->width(); ++x) { + for (int y = 0; y < solution->height(); ++y) { + *solution->MutablePixel(x, y) = + std::min(255.0, std::max(0.0, solution->Pixel(x, y))); + } + } +} +} // namespace examples +} // namespace ceres + +int main(int argc, char** argv) { + using namespace ceres::examples; + std::string + usage("This program denoises an image using Ceres. Sample usage:\n"); + usage += argv[0]; + usage += " --input=<noisy image PGM file> --foe_file=<FoE file name>"; + google::SetUsageMessage(usage); + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + + if (FLAGS_input.empty()) { + std::cerr << "Please provide an image file name.\n"; + return 1; + } + + if (FLAGS_foe_file.empty()) { + std::cerr << "Please provide a Fields of Experts file name.\n"; + return 1; + } + + // Load the Fields of Experts filters from file. + FieldsOfExperts foe; + if (!foe.LoadFromFile(FLAGS_foe_file)) { + std::cerr << "Loading \"" << FLAGS_foe_file << "\" failed.\n"; + return 2; + } + + // Read the images + PGMImage<double> image(FLAGS_input); + if (image.width() == 0) { + std::cerr << "Reading \"" << FLAGS_input << "\" failed.\n"; + return 3; + } + PGMImage<double> solution(image.width(), image.height()); + solution.Set(0.0); + + ceres::Problem problem; + CreateProblem(foe, image, &problem, &solution); + + SolveProblem(&problem, &solution); + + if (!FLAGS_output.empty()) { + CHECK(solution.WriteToFile(FLAGS_output)) + << "Writing \"" << FLAGS_output << "\" failed."; + } + + return 0; +} diff --git a/examples/fields_of_experts.cc b/examples/fields_of_experts.cc new file mode 100644 index 0000000..0cee40b --- /dev/null +++ b/examples/fields_of_experts.cc @@ -0,0 +1,152 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 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: strandmark@google.com (Petter Strandmark) +// +// Class for loading the data required for descibing a Fields of Experts (FoE) +// model. + +#include "fields_of_experts.h" + +#include <fstream> +#include <cmath> + +#include "pgm_image.h" + +namespace ceres { +namespace examples { + +FieldsOfExpertsCost::FieldsOfExpertsCost(const std::vector<double>& filter) + : filter_(filter) { + set_num_residuals(1); + for (int i = 0; i < filter_.size(); ++i) { + mutable_parameter_block_sizes()->push_back(1); + } +} + +// This is a dot product between a the scalar parameters and a vector of filter +// coefficients. +bool FieldsOfExpertsCost::Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + int num_variables = filter_.size(); + residuals[0] = 0; + for (int i = 0; i < num_variables; ++i) { + residuals[0] += filter_[i] * parameters[i][0]; + } + + if (jacobians != NULL) { + for (int i = 0; i < num_variables; ++i) { + if (jacobians[i] != NULL) { + jacobians[i][0] = filter_[i]; + } + } + } + + return true; +} + +// This loss function builds the FoE terms and is equal to +// +// f(x) = alpha_i * log(1 + (1/2)s) +// +void FieldsOfExpertsLoss::Evaluate(double sq_norm, double rho[3]) const { + const double c = 0.5; + const double sum = 1.0 + sq_norm * c; + const double inv = 1.0 / sum; + // 'sum' and 'inv' are always positive, assuming that 's' is. + rho[0] = alpha_ * log(sum); + rho[1] = alpha_ * c * inv; + rho[2] = - alpha_ * c * c * inv * inv; +} + +FieldsOfExperts::FieldsOfExperts() + : size_(0), num_filters_(0) { +} + +bool FieldsOfExperts::LoadFromFile(const std::string& filename) { + std::ifstream foe_file(filename.c_str()); + foe_file >> size_; + foe_file >> num_filters_; + if (size_ < 0 || num_filters_ < 0) { + return false; + } + const int num_variables = NumVariables(); + + x_delta_indices_.resize(num_variables); + for (int i = 0; i < num_variables; ++i) { + foe_file >> x_delta_indices_[i]; + } + + y_delta_indices_.resize(NumVariables()); + for (int i = 0; i < num_variables; ++i) { + foe_file >> y_delta_indices_[i]; + } + + alpha_.resize(num_filters_); + for (int i = 0; i < num_filters_; ++i) { + foe_file >> alpha_[i]; + } + + filters_.resize(num_filters_); + for (int i = 0; i < num_filters_; ++i) { + filters_[i].resize(num_variables); + for (int j = 0; j < num_variables; ++j) { + foe_file >> filters_[i][j]; + } + } + + // If any read failed, return failure. + if (!foe_file) { + size_ = 0; + return false; + } + + // There cannot be anything else in the file. Try reading another number and + // return failure if that succeeded. + double temp; + foe_file >> temp; + if (foe_file) { + size_ = 0; + return false; + } + + return true; +} + +ceres::CostFunction* FieldsOfExperts::NewCostFunction(int alpha_index) const { + return new FieldsOfExpertsCost(filters_[alpha_index]); +} + +ceres::LossFunction* FieldsOfExperts::NewLossFunction(int alpha_index) const { + return new FieldsOfExpertsLoss(alpha_[alpha_index]); +} + + +} // namespace examples +} // namespace ceres diff --git a/examples/fields_of_experts.h b/examples/fields_of_experts.h new file mode 100644 index 0000000..845a4cf --- /dev/null +++ b/examples/fields_of_experts.h @@ -0,0 +1,145 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 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: strandmark@google.com (Petter Strandmark) +// +// Class for loading the data required for descibing a Fields of Experts (FoE) +// model. The Fields of Experts regularization consists of terms of the type +// +// alpha * log(1 + (1/2)*sum(F .* X)^2), +// +// where F is a d-by-d image patch and alpha is a constant. This is implemented +// by a FieldsOfExpertsSum object which represents the dot product between the +// image patches and a FieldsOfExpertsLoss which implements the log(1 + (1/2)s) +// part. +// +// [1] S. Roth and M.J. Black. "Fields of Experts." International Journal of +// Computer Vision, 82(2):205--229, 2009. + +#ifndef CERES_EXAMPLES_FIELDS_OF_EXPERTS_H_ +#define CERES_EXAMPLES_FIELDS_OF_EXPERTS_H_ + +#include <iostream> +#include <vector> + +#include "ceres/loss_function.h" +#include "ceres/cost_function.h" +#include "ceres/sized_cost_function.h" + +#include "pgm_image.h" + +namespace ceres { +namespace examples { + +// One sum in the FoE regularizer. This is a dot product between a filter and an +// image patch. It simply calculates the dot product between the filter +// coefficients given in the constructor and the scalar parameters passed to it. +class FieldsOfExpertsCost : public ceres::CostFunction { + public: + explicit FieldsOfExpertsCost(const std::vector<double>& filter); + // The number of scalar parameters passed to Evaluate must equal the number of + // filter coefficients passed to the constructor. + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const; + + private: + const std::vector<double>& filter_; +}; + +// The loss function used to build the correct regularization. See above. +// +// f(x) = alpha_i * log(1 + (1/2)s) +// +class FieldsOfExpertsLoss : public ceres::LossFunction { + public: + explicit FieldsOfExpertsLoss(double alpha) : alpha_(alpha) { } + virtual void Evaluate(double, double*) const; + + private: + const double alpha_; +}; + +// This class loads a set of filters and coefficients from file. Then the users +// obtains the correct loss and cost functions through NewCostFunction and +// NewLossFunction. +class FieldsOfExperts { + public: + // Creates an empty object with size() == 0. + FieldsOfExperts(); + // Attempts to load filters from a file. If unsuccessful it returns false and + // sets size() == 0. + bool LoadFromFile(const std::string& filename); + + // Side length of a square filter in this FoE. They are all of the same size. + int Size() const { + return size_; + } + + // Total number of pixels the filter covers. + int NumVariables() const { + return size_ * size_; + } + + // Number of filters used by the FoE. + int NumFilters() const { + return num_filters_; + } + + // Creates a new cost function. The caller is responsible for deallocating the + // memory. alpha_index specifies which filter is used in the cost function. + ceres::CostFunction* NewCostFunction(int alpha_index) const; + // Creates a new loss function. The caller is responsible for deallocating the + // memory. alpha_index specifies which filter this loss function is for. + ceres::LossFunction* NewLossFunction(int alpha_index) const; + + // Gets the delta pixel indices for all pixels in a patch. + const std::vector<int>& GetXDeltaIndices() const { + return x_delta_indices_; + } + const std::vector<int>& GetYDeltaIndices() const { + return y_delta_indices_; + } + + private: + // The side length of a square filter. + int size_; + // The number of different filters used. + int num_filters_; + // Pixel offsets for all variables. + std::vector<int> x_delta_indices_, y_delta_indices_; + // The coefficients in front of each term. + std::vector<double> alpha_; + // The filters used for the dot product with image patches. + std::vector<std::vector<double> > filters_; +}; + +} // namespace examples +} // namespace ceres + +#endif // CERES_EXAMPLES_FIELDS_OF_EXPERTS_H_ diff --git a/examples/nist.cc b/examples/nist.cc new file mode 100644 index 0000000..440ab5c --- /dev/null +++ b/examples/nist.cc @@ -0,0 +1,479 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 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) +// +// NIST non-linear regression problems solved using Ceres. +// +// The data was obtained from +// http://www.itl.nist.gov/div898/strd/nls/nls_main.shtml, where more +// background on these problems can also be found. +// +// Currently not all problems are solved successfully. Some of the +// failures are due to convergence to a local minimum, and some fail +// because of numerical issues. +// +// TODO(sameeragarwal): Fix numerical issues so that all the problems +// converge and then look at convergence to the wrong solution issues. + +#include <iostream> +#include <fstream> +#include "ceres/ceres.h" +#include "ceres/split.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "Eigen/Core" + +DEFINE_string(nist_data_dir, "", "Directory containing the NIST non-linear" + "regression examples"); +DEFINE_string(trust_region_strategy, "levenberg_marquardt", + "Options are: levenberg_marquardt, dogleg"); +DEFINE_string(dogleg, "traditional_dogleg", + "Options are: traditional_dogleg, subspace_dogleg"); +DEFINE_string(linear_solver, "dense_qr", "Options are: " + "sparse_cholesky, dense_qr, dense_normal_cholesky and" + "cgnr"); +DEFINE_string(preconditioner, "jacobi", "Options are: " + "identity, jacobi"); +DEFINE_int32(num_iterations, 10000, "Number of iterations"); +DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use" + " nonmonotic steps"); +DEFINE_double(initial_trust_region_radius, 1e4, "Initial trust region radius"); + +using Eigen::Dynamic; +using Eigen::RowMajor; +typedef Eigen::Matrix<double, Dynamic, 1> Vector; +typedef Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> Matrix; + +bool GetAndSplitLine(std::ifstream& ifs, std::vector<std::string>* pieces) { + pieces->clear(); + char buf[256]; + ifs.getline(buf, 256); + ceres::SplitStringUsing(std::string(buf), " ", pieces); + return true; +} + +void SkipLines(std::ifstream& ifs, int num_lines) { + char buf[256]; + for (int i = 0; i < num_lines; ++i) { + ifs.getline(buf, 256); + } +} + +bool IsSuccessfulTermination(ceres::SolverTerminationType status) { + return + (status == ceres::FUNCTION_TOLERANCE) || + (status == ceres::GRADIENT_TOLERANCE) || + (status == ceres::PARAMETER_TOLERANCE) || + (status == ceres::USER_SUCCESS); +} + +class NISTProblem { + public: + explicit NISTProblem(const std::string& filename) { + std::ifstream ifs(filename.c_str(), std::ifstream::in); + + std::vector<std::string> pieces; + SkipLines(ifs, 24); + GetAndSplitLine(ifs, &pieces); + const int kNumResponses = std::atoi(pieces[1].c_str()); + + GetAndSplitLine(ifs, &pieces); + const int kNumPredictors = std::atoi(pieces[0].c_str()); + + GetAndSplitLine(ifs, &pieces); + const int kNumObservations = std::atoi(pieces[0].c_str()); + + SkipLines(ifs, 4); + GetAndSplitLine(ifs, &pieces); + const int kNumParameters = std::atoi(pieces[0].c_str()); + SkipLines(ifs, 8); + + // Get the first line of initial and final parameter values to + // determine the number of tries. + GetAndSplitLine(ifs, &pieces); + const int kNumTries = pieces.size() - 4; + + predictor_.resize(kNumObservations, kNumPredictors); + response_.resize(kNumObservations, kNumResponses); + initial_parameters_.resize(kNumTries, kNumParameters); + final_parameters_.resize(1, kNumParameters); + + // Parse the line for parameter b1. + int parameter_id = 0; + for (int i = 0; i < kNumTries; ++i) { + initial_parameters_(i, parameter_id) = std::atof(pieces[i + 2].c_str()); + } + final_parameters_(0, parameter_id) = std::atof(pieces[2 + kNumTries].c_str()); + + // Parse the remaining parameter lines. + for (int parameter_id = 1; parameter_id < kNumParameters; ++parameter_id) { + GetAndSplitLine(ifs, &pieces); + // b2, b3, .... + for (int i = 0; i < kNumTries; ++i) { + initial_parameters_(i, parameter_id) = std::atof(pieces[i + 2].c_str()); + } + final_parameters_(0, parameter_id) = std::atof(pieces[2 + kNumTries].c_str()); + } + + // Certfied cost + SkipLines(ifs, 1); + GetAndSplitLine(ifs, &pieces); + certified_cost_ = std::atof(pieces[4].c_str()) / 2.0; + + // Read the observations. + SkipLines(ifs, 18 - kNumParameters); + for (int i = 0; i < kNumObservations; ++i) { + GetAndSplitLine(ifs, &pieces); + // Response. + for (int j = 0; j < kNumResponses; ++j) { + response_(i, j) = std::atof(pieces[j].c_str()); + } + + // Predictor variables. + for (int j = 0; j < kNumPredictors; ++j) { + predictor_(i, j) = std::atof(pieces[j + kNumResponses].c_str()); + } + } + } + + Matrix initial_parameters(int start) const { return initial_parameters_.row(start); } + Matrix final_parameters() const { return final_parameters_; } + Matrix predictor() const { return predictor_; } + Matrix response() const { return response_; } + int predictor_size() const { return predictor_.cols(); } + int num_observations() const { return predictor_.rows(); } + int response_size() const { return response_.cols(); } + int num_parameters() const { return initial_parameters_.cols(); } + int num_starts() const { return initial_parameters_.rows(); } + double certified_cost() const { return certified_cost_; } + + private: + Matrix predictor_; + Matrix response_; + Matrix initial_parameters_; + Matrix final_parameters_; + double certified_cost_; +}; + +#define NIST_BEGIN(CostFunctionName) \ + struct CostFunctionName { \ + CostFunctionName(const double* const x, \ + const double* const y) \ + : x_(*x), y_(*y) {} \ + double x_; \ + double y_; \ + template <typename T> \ + bool operator()(const T* const b, T* residual) const { \ + const T y(y_); \ + const T x(x_); \ + residual[0] = y - ( + +#define NIST_END ); return true; }}; + +// y = b1 * (b2+x)**(-1/b3) + e +NIST_BEGIN(Bennet5) + b[0] * pow(b[1] + x, T(-1.0) / b[2]) +NIST_END + +// y = b1*(1-exp[-b2*x]) + e +NIST_BEGIN(BoxBOD) + b[0] * (T(1.0) - exp(-b[1] * x)) +NIST_END + +// y = exp[-b1*x]/(b2+b3*x) + e +NIST_BEGIN(Chwirut) + exp(-b[0] * x) / (b[1] + b[2] * x) +NIST_END + +// y = b1*x**b2 + e +NIST_BEGIN(DanWood) + b[0] * pow(x, b[1]) +NIST_END + +// y = b1*exp( -b2*x ) + b3*exp( -(x-b4)**2 / b5**2 ) +// + b6*exp( -(x-b7)**2 / b8**2 ) + e +NIST_BEGIN(Gauss) + b[0] * exp(-b[1] * x) + + b[2] * exp(-pow((x - b[3])/b[4], 2)) + + b[5] * exp(-pow((x - b[6])/b[7],2)) +NIST_END + +// y = b1*exp(-b2*x) + b3*exp(-b4*x) + b5*exp(-b6*x) + e +NIST_BEGIN(Lanczos) + b[0] * exp(-b[1] * x) + b[2] * exp(-b[3] * x) + b[4] * exp(-b[5] * x) +NIST_END + +// y = (b1+b2*x+b3*x**2+b4*x**3) / +// (1+b5*x+b6*x**2+b7*x**3) + e +NIST_BEGIN(Hahn1) + (b[0] + b[1] * x + b[2] * x * x + b[3] * x * x * x) / + (T(1.0) + b[4] * x + b[5] * x * x + b[6] * x * x * x) +NIST_END + +// y = (b1 + b2*x + b3*x**2) / +// (1 + b4*x + b5*x**2) + e +NIST_BEGIN(Kirby2) + (b[0] + b[1] * x + b[2] * x * x) / + (T(1.0) + b[3] * x + b[4] * x * x) +NIST_END + +// y = b1*(x**2+x*b2) / (x**2+x*b3+b4) + e +NIST_BEGIN(MGH09) + b[0] * (x * x + x * b[1]) / (x * x + x * b[2] + b[3]) +NIST_END + +// y = b1 * exp[b2/(x+b3)] + e +NIST_BEGIN(MGH10) + b[0] * exp(b[1] / (x + b[2])) +NIST_END + +// y = b1 + b2*exp[-x*b4] + b3*exp[-x*b5] +NIST_BEGIN(MGH17) + b[0] + b[1] * exp(-x * b[3]) + b[2] * exp(-x * b[4]) +NIST_END + +// y = b1*(1-exp[-b2*x]) + e +NIST_BEGIN(Misra1a) + b[0] * (T(1.0) - exp(-b[1] * x)) +NIST_END + +// y = b1 * (1-(1+b2*x/2)**(-2)) + e +NIST_BEGIN(Misra1b) + b[0] * (T(1.0) - T(1.0)/ ((T(1.0) + b[1] * x / 2.0) * (T(1.0) + b[1] * x / 2.0))) +NIST_END + +// y = b1 * (1-(1+2*b2*x)**(-.5)) + e +NIST_BEGIN(Misra1c) + b[0] * (T(1.0) - pow(T(1.0) + T(2.0) * b[1] * x, -0.5)) +NIST_END + +// y = b1*b2*x*((1+b2*x)**(-1)) + e +NIST_BEGIN(Misra1d) + b[0] * b[1] * x / (T(1.0) + b[1] * x) +NIST_END + +const double kPi = 3.141592653589793238462643383279; +// pi = 3.141592653589793238462643383279E0 +// y = b1 - b2*x - arctan[b3/(x-b4)]/pi + e +NIST_BEGIN(Roszman1) + b[0] - b[1] * x - atan2(b[2], (x - b[3]))/T(kPi) +NIST_END + +// y = b1 / (1+exp[b2-b3*x]) + e +NIST_BEGIN(Rat42) + b[0] / (T(1.0) + exp(b[1] - b[2] * x)) +NIST_END + +// y = b1 / ((1+exp[b2-b3*x])**(1/b4)) + e +NIST_BEGIN(Rat43) + b[0] / pow(T(1.0) + exp(b[1] - b[2] * x), T(1.0) / b[3]) +NIST_END + +// y = (b1 + b2*x + b3*x**2 + b4*x**3) / +// (1 + b5*x + b6*x**2 + b7*x**3) + e +NIST_BEGIN(Thurber) + (b[0] + b[1] * x + b[2] * x * x + b[3] * x * x * x) / + (T(1.0) + b[4] * x + b[5] * x * x + b[6] * x * x * x) +NIST_END + +// y = b1 + b2*cos( 2*pi*x/12 ) + b3*sin( 2*pi*x/12 ) +// + b5*cos( 2*pi*x/b4 ) + b6*sin( 2*pi*x/b4 ) +// + b8*cos( 2*pi*x/b7 ) + b9*sin( 2*pi*x/b7 ) + e +NIST_BEGIN(ENSO) + b[0] + b[1] * cos(T(2.0 * kPi) * x / T(12.0)) + + b[2] * sin(T(2.0 * kPi) * x / T(12.0)) + + b[4] * cos(T(2.0 * kPi) * x / b[3]) + + b[5] * sin(T(2.0 * kPi) * x / b[3]) + + b[7] * cos(T(2.0 * kPi) * x / b[6]) + + b[8] * sin(T(2.0 * kPi) * x / b[6]) +NIST_END + +// y = (b1/b2) * exp[-0.5*((x-b3)/b2)**2] + e +NIST_BEGIN(Eckerle4) + b[0] / b[1] * exp(T(-0.5) * pow((x - b[2])/b[1], 2)) +NIST_END + +struct Nelson { + public: + Nelson(const double* const x, const double* const y) + : x1_(x[0]), x2_(x[1]), y_(y[0]) {} + + template <typename T> + bool operator()(const T* const b, T* residual) const { + // log[y] = b1 - b2*x1 * exp[-b3*x2] + e + residual[0] = T(log(y_)) - (b[0] - b[1] * T(x1_) * exp(-b[2] * T(x2_))); + return true; + } + + private: + double x1_; + double x2_; + double y_; +}; + +template <typename Model, int num_residuals, int num_parameters> +int RegressionDriver(const std::string& filename, + const ceres::Solver::Options& options) { + NISTProblem nist_problem(FLAGS_nist_data_dir + filename); + CHECK_EQ(num_residuals, nist_problem.response_size()); + CHECK_EQ(num_parameters, nist_problem.num_parameters()); + + Matrix predictor = nist_problem.predictor(); + Matrix response = nist_problem.response(); + Matrix final_parameters = nist_problem.final_parameters(); + std::vector<ceres::Solver::Summary> summaries(nist_problem.num_starts() + 1); + std::cerr << filename << std::endl; + + // Each NIST problem comes with multiple starting points, so we + // construct the problem from scratch for each case and solve it. + for (int start = 0; start < nist_problem.num_starts(); ++start) { + Matrix initial_parameters = nist_problem.initial_parameters(start); + + ceres::Problem problem; + for (int i = 0; i < nist_problem.num_observations(); ++i) { + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction<Model, num_residuals, num_parameters>( + new Model(predictor.data() + nist_problem.predictor_size() * i, + response.data() + nist_problem.response_size() * i)), + NULL, + initial_parameters.data()); + } + + Solve(options, &problem, &summaries[start]); + } + + const double certified_cost = nist_problem.certified_cost(); + + int num_success = 0; + const int kMinNumMatchingDigits = 4; + for (int start = 0; start < nist_problem.num_starts(); ++start) { + const ceres::Solver::Summary& summary = summaries[start]; + + int num_matching_digits = 0; + if (IsSuccessfulTermination(summary.termination_type) + && summary.final_cost < certified_cost) { + num_matching_digits = kMinNumMatchingDigits + 1; + } else { + num_matching_digits = + -std::log10(fabs(summary.final_cost - certified_cost) / certified_cost); + } + + std::cerr << "start " << start + 1 << " " ; + if (num_matching_digits <= kMinNumMatchingDigits) { + std::cerr << "FAILURE"; + } else { + std::cerr << "SUCCESS"; + ++num_success; + } + std::cerr << " summary: " + << summary.BriefReport() + << " Certified cost: " << certified_cost + << std::endl; + + } + + return num_success; +} + +void SetMinimizerOptions(ceres::Solver::Options* options) { + CHECK(ceres::StringToLinearSolverType(FLAGS_linear_solver, + &options->linear_solver_type)); + CHECK(ceres::StringToPreconditionerType(FLAGS_preconditioner, + &options->preconditioner_type)); + CHECK(ceres::StringToTrustRegionStrategyType( + FLAGS_trust_region_strategy, + &options->trust_region_strategy_type)); + CHECK(ceres::StringToDoglegType(FLAGS_dogleg, &options->dogleg_type)); + + options->max_num_iterations = FLAGS_num_iterations; + options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps; + options->initial_trust_region_radius = FLAGS_initial_trust_region_radius; + options->function_tolerance = 1e-18; + options->gradient_tolerance = 1e-18; + options->parameter_tolerance = 1e-18; +} + +void SolveNISTProblems() { + if (FLAGS_nist_data_dir.empty()) { + LOG(FATAL) << "Must specify the directory containing the NIST problems"; + } + + ceres::Solver::Options options; + SetMinimizerOptions(&options); + + std::cerr << "Lower Difficulty\n"; + int easy_success = 0; + easy_success += RegressionDriver<Misra1a, 1, 2>("Misra1a.dat", options); + easy_success += RegressionDriver<Chwirut, 1, 3>("Chwirut1.dat", options); + easy_success += RegressionDriver<Chwirut, 1, 3>("Chwirut2.dat", options); + easy_success += RegressionDriver<Lanczos, 1, 6>("Lanczos3.dat", options); + easy_success += RegressionDriver<Gauss, 1, 8>("Gauss1.dat", options); + easy_success += RegressionDriver<Gauss, 1, 8>("Gauss2.dat", options); + easy_success += RegressionDriver<DanWood, 1, 2>("DanWood.dat", options); + easy_success += RegressionDriver<Misra1b, 1, 2>("Misra1b.dat", options); + + std::cerr << "\nMedium Difficulty\n"; + int medium_success = 0; + medium_success += RegressionDriver<Kirby2, 1, 5>("Kirby2.dat", options); + medium_success += RegressionDriver<Hahn1, 1, 7>("Hahn1.dat", options); + medium_success += RegressionDriver<Nelson, 1, 3>("Nelson.dat", options); + medium_success += RegressionDriver<MGH17, 1, 5>("MGH17.dat", options); + medium_success += RegressionDriver<Lanczos, 1, 6>("Lanczos1.dat", options); + medium_success += RegressionDriver<Lanczos, 1, 6>("Lanczos2.dat", options); + medium_success += RegressionDriver<Gauss, 1, 8>("Gauss3.dat", options); + medium_success += RegressionDriver<Misra1c, 1, 2>("Misra1c.dat", options); + medium_success += RegressionDriver<Misra1d, 1, 2>("Misra1d.dat", options); + medium_success += RegressionDriver<Roszman1, 1, 4>("Roszman1.dat", options); + medium_success += RegressionDriver<ENSO, 1, 9>("ENSO.dat", options); + + std::cerr << "\nHigher Difficulty\n"; + int hard_success = 0; + hard_success += RegressionDriver<MGH09, 1, 4>("MGH09.dat", options); + hard_success += RegressionDriver<Thurber, 1, 7>("Thurber.dat", options); + hard_success += RegressionDriver<BoxBOD, 1, 2>("BoxBOD.dat", options); + hard_success += RegressionDriver<Rat42, 1, 3>("Rat42.dat", options); + hard_success += RegressionDriver<MGH10, 1, 3>("MGH10.dat", options); + + hard_success += RegressionDriver<Eckerle4, 1, 3>("Eckerle4.dat", options); + hard_success += RegressionDriver<Rat43, 1, 4>("Rat43.dat", options); + hard_success += RegressionDriver<Bennet5, 1, 3>("Bennett5.dat", options); + + std::cerr << "\n"; + std::cerr << "Easy : " << easy_success << "/16\n"; + std::cerr << "Medium : " << medium_success << "/22\n"; + std::cerr << "Hard : " << hard_success << "/16\n"; + std::cerr << "Total : " << easy_success + medium_success + hard_success << "/54\n"; +} + +int main(int argc, char** argv) { + google::ParseCommandLineFlags(&argc, &argv, true); + google::InitGoogleLogging(argv[0]); + SolveNISTProblems(); + return 0; +}; diff --git a/examples/pgm_image.h b/examples/pgm_image.h new file mode 100644 index 0000000..15e99e4 --- /dev/null +++ b/examples/pgm_image.h @@ -0,0 +1,319 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 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: strandmark@google.com (Petter Strandmark) +// +// Simple class for accessing PGM images. + +#ifndef CERES_EXAMPLES_PGM_IMAGE_H_ +#define CERES_EXAMPLES_PGM_IMAGE_H_ + +#include <algorithm> +#include <cstring> +#include <fstream> +#include <iostream> +#include <sstream> +#include <string> +#include <vector> + +#include "glog/logging.h" + +namespace ceres { +namespace examples { + +template<typename Real> +class PGMImage { + public: + // Create an empty image + PGMImage(int width, int height); + // Load an image from file + explicit PGMImage(std::string filename); + // Sets an image to a constant + void Set(double constant); + + // Reading dimensions + int width() const; + int height() const; + int NumPixels() const; + + // Get individual pixels + Real* MutablePixel(int x, int y); + Real Pixel(int x, int y) const; + Real* MutablePixelFromLinearIndex(int index); + Real PixelFromLinearIndex(int index) const; + int LinearIndex(int x, int y) const; + + // Adds an image to another + void operator+=(const PGMImage& image); + // Adds a constant to an image + void operator+=(Real a); + // Multiplies an image by a constant + void operator*=(Real a); + + // File access + bool WriteToFile(std::string filename) const; + bool ReadFromFile(std::string filename); + + // Accessing the image data directly + bool SetData(const std::vector<Real>& new_data); + const std::vector<Real>& data() const; + + protected: + int height_, width_; + std::vector<Real> data_; +}; + +// --- IMPLEMENTATION + +template<typename Real> +PGMImage<Real>::PGMImage(int width, int height) + : height_(height), width_(width), data_(width*height, 0.0) { +} + +template<typename Real> +PGMImage<Real>::PGMImage(std::string filename) { + if (!ReadFromFile(filename)) { + height_ = 0; + width_ = 0; + } +} + +template<typename Real> +void PGMImage<Real>::Set(double constant) { + for (int i = 0; i < data_.size(); ++i) { + data_[i] = constant; + } +} + +template<typename Real> +int PGMImage<Real>::width() const { + return width_; +} + +template<typename Real> +int PGMImage<Real>::height() const { + return height_; +} + +template<typename Real> +int PGMImage<Real>::NumPixels() const { + return width_ * height_; +} + +template<typename Real> +Real* PGMImage<Real>::MutablePixel(int x, int y) { + return MutablePixelFromLinearIndex(LinearIndex(x, y)); +} + +template<typename Real> +Real PGMImage<Real>::Pixel(int x, int y) const { + return PixelFromLinearIndex(LinearIndex(x, y)); +} + +template<typename Real> +Real* PGMImage<Real>::MutablePixelFromLinearIndex(int index) { + CHECK(index >= 0); + CHECK(index < width_ * height_); + CHECK(index < data_.size()); + return &data_[index]; +} + +template<typename Real> +Real PGMImage<Real>::PixelFromLinearIndex(int index) const { + CHECK(index >= 0); + CHECK(index < width_ * height_); + CHECK(index < data_.size()); + return data_[index]; +} + +template<typename Real> +int PGMImage<Real>::LinearIndex(int x, int y) const { + return x + width_*y; +} + +// Adds an image to another +template<typename Real> +void PGMImage<Real>::operator+= (const PGMImage<Real>& image) { + CHECK(data_.size() == image.data_.size()); + for (int i = 0; i < data_.size(); ++i) { + data_[i] += image.data_[i]; + } +} + +// Adds a constant to an image +template<typename Real> +void PGMImage<Real>::operator+= (Real a) { + for (int i = 0; i < data_.size(); ++i) { + data_[i] += a; + } +} + +// Multiplies an image by a constant +template<typename Real> +void PGMImage<Real>::operator*= (Real a) { + for (int i = 0; i < data_.size(); ++i) { + data_[i] *= a; + } +} + +template<typename Real> +bool PGMImage<Real>::WriteToFile(std::string filename) const { + std::ofstream outputfile(filename.c_str()); + outputfile << "P2" << std::endl; + outputfile << "# PGM format" << std::endl; + outputfile << " # <width> <height> <levels> " << std::endl; + outputfile << " # <data> ... " << std::endl; + outputfile << width_ << ' ' << height_ << " 255 " << std::endl; + + // Write data + int num_pixels = width_*height_; + for (int i = 0; i < num_pixels; ++i) { + // Convert to integer by rounding when writing file + outputfile << static_cast<int>(data_[i] + 0.5) << ' '; + } + + return outputfile; // Returns true/false +} + +namespace { + +// Helper function to read data from a text file, ignoring "#" comments. +template<typename T> +bool GetIgnoreComment(std::istream* in, T& t) { + std::string word; + bool ok; + do { + ok = true; + (*in) >> word; + if (word.length() > 0 && word[0] == '#') { + // Comment; read the whole line + ok = false; + std::getline(*in, word); + } + } while (!ok); + + // Convert the string + std::stringstream sin(word); + sin >> t; + + // Check for success + if (!in || !sin) { + return false; + } + return true; +} +} // namespace + +template<typename Real> +bool PGMImage<Real>::ReadFromFile(std::string filename) { + std::ifstream inputfile(filename.c_str()); + + // File must start with "P2" + char ch1, ch2; + inputfile >> ch1 >> ch2; + if (!inputfile || ch1 != 'P' || (ch2 != '2' && ch2 != '5')) { + return false; + } + + // Read the image header + int two_fifty_five; + if (!GetIgnoreComment(&inputfile, width_) || + !GetIgnoreComment(&inputfile, height_) || + !GetIgnoreComment(&inputfile, two_fifty_five) ) { + return false; + } + // Assert that the number of grey levels is 255. + if (two_fifty_five != 255) { + return false; + } + + // Now read the data + int num_pixels = width_*height_; + data_.resize(num_pixels); + if (ch2 == '2') { + // Ascii file + for (int i = 0; i < num_pixels; ++i) { + int pixel_data; + bool res = GetIgnoreComment(&inputfile, pixel_data); + if (!res) { + return false; + } + data_[i] = pixel_data; + } + // There cannot be anything else in the file (except comments). Try reading + // another number and return failure if that succeeded. + int temp; + bool res = GetIgnoreComment(&inputfile, temp); + if (res) { + return false; + } + } else { + // Read the line feed character + if (inputfile.get() != '\n') { + return false; + } + // Binary file + // TODO(strandmark): Will not work on Windows (linebreak conversion). + for (int i = 0; i < num_pixels; ++i) { + unsigned char pixel_data = inputfile.get(); + if (!inputfile) { + return false; + } + data_[i] = pixel_data; + } + // There cannot be anything else in the file. Try reading another byte + // and return failure if that succeeded. + inputfile.get(); + if (inputfile) { + return false; + } + } + + return true; +} + +template<typename Real> +bool PGMImage<Real>::SetData(const std::vector<Real>& new_data) { + // This function cannot change the dimensions + if (new_data.size() != data_.size()) { + return false; + } + std::copy(new_data.begin(), new_data.end(), data_.begin()); + return true; +} + +template<typename Real> +const std::vector<Real>& PGMImage<Real>::data() const { + return data_; +} + +} // namespace examples +} // namespace ceres + + +#endif // CERES_EXAMPLES_PGM_IMAGE_H_ diff --git a/examples/powell.cc b/examples/powell.cc new file mode 100644 index 0000000..6cd3611 --- /dev/null +++ b/examples/powell.cc @@ -0,0 +1,151 @@ +// 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" +#include "gflags/gflags.h" +#include "glog/logging.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..8527af3 --- /dev/null +++ b/examples/quadratic.cc @@ -0,0 +1,90 @@ +// 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" +#include "gflags/gflags.h" +#include "glog/logging.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..ea7fae9 --- /dev/null +++ b/examples/quadratic_auto_diff.cc @@ -0,0 +1,88 @@ +// 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" +#include "gflags/gflags.h" +#include "glog/logging.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..8ec88ef --- /dev/null +++ b/examples/quadratic_numeric_diff.cc @@ -0,0 +1,92 @@ +// 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" +#include "gflags/gflags.h" +#include "glog/logging.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..cc6f04a --- /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. + T xp = - p[0] / p[2]; + T yp = - 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. + const T& focal = camera[6]; + T predicted_x = focal * distortion * xp; + T predicted_y = focal * 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) { + google::InitGoogleLogging(argv[0]); + 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.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..0704217 --- /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 = - p[0] / p[2]; + T yp = - 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 = focal * distortion * xp; + T predicted_y = focal * 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 SnavelyReprojectionErrorWithQuaternions { + // (u, v): the position of the observation with respect to the image + // center point. + SnavelyReprojectionErrorWithQuaternions(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 = - p[0] / p[2]; + T yp = - 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 = focal * distortion * xp; + T predicted_y = focal * 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_ |