Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/examples/BUILD b/examples/BUILD
new file mode 100644
index 0000000..4d8f1bb
--- /dev/null
+++ b/examples/BUILD
@@ -0,0 +1,126 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2018 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: mierle@gmail.com (Keir Mierle)
+
+EXAMPLE_COPTS = [
+ # Needed to silence GFlags complaints.
+ "-Wno-sign-compare",
+]
+
+EXAMPLE_DEPS = [
+ "//:ceres",
+ "@com_github_eigen_eigen//:eigen",
+ "@com_github_gflags_gflags//:gflags",
+]
+
+cc_binary(
+ name = "bundle_adjuster",
+ srcs = [
+ "bal_problem.cc",
+ "bal_problem.h",
+ "bundle_adjuster.cc",
+ "random.h",
+ "snavely_reprojection_error.h",
+ ],
+ copts = EXAMPLE_COPTS,
+ deps = EXAMPLE_DEPS,
+)
+
+cc_binary(
+ name = "denoising",
+ srcs = [
+ "denoising.cc",
+ "fields_of_experts.cc",
+ "fields_of_experts.h",
+ "pgm_image.h",
+ ],
+ copts = EXAMPLE_COPTS,
+ deps = EXAMPLE_DEPS,
+)
+
+cc_binary(
+ name = "robot_pose_mle",
+ srcs = [
+ "random.h",
+ "robot_pose_mle.cc",
+ ],
+ copts = EXAMPLE_COPTS,
+ deps = EXAMPLE_DEPS,
+)
+
+SLAM_COPTS = EXAMPLE_COPTS + ["-Iexamples/slam"]
+
+cc_binary(
+ name = "pose_graph_2d",
+ srcs = [
+ "slam/common/read_g2o.h",
+ "slam/pose_graph_2d/angle_local_parameterization.h",
+ "slam/pose_graph_2d/normalize_angle.h",
+ "slam/pose_graph_2d/pose_graph_2d.cc",
+ "slam/pose_graph_2d/pose_graph_2d_error_term.h",
+ "slam/pose_graph_2d/types.h",
+ ],
+ copts = SLAM_COPTS,
+ deps = EXAMPLE_DEPS,
+)
+
+cc_binary(
+ name = "pose_graph_3d",
+ srcs = [
+ "slam/common/read_g2o.h",
+ "slam/pose_graph_3d/pose_graph_3d.cc",
+ "slam/pose_graph_3d/pose_graph_3d_error_term.h",
+ "slam/pose_graph_3d/types.h",
+ ],
+ copts = SLAM_COPTS,
+ deps = EXAMPLE_DEPS,
+)
+
+[cc_binary(
+ name = example,
+ srcs = [example + ".cc"],
+ copts = EXAMPLE_COPTS,
+ deps = EXAMPLE_DEPS,
+) for example in [
+ "circle_fit",
+ "curve_fitting",
+ "ellipse_approximation",
+ "helloworld",
+ "helloworld_analytic_diff",
+ "helloworld_numeric_diff",
+ "libmv_bundle_adjuster",
+ "libmv_homography",
+ "more_garbow_hillstrom",
+ "nist",
+ "powell",
+ "robust_curve_fitting",
+ "rosenbrock",
+ "sampled_function/sampled_function",
+ "simple_bundle_adjuster",
+]]
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
new file mode 100644
index 0000000..3d86be8
--- /dev/null
+++ b/examples/CMakeLists.txt
@@ -0,0 +1,116 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2015 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: keir@google.com (Keir Mierle)
+
+# Only Ceres itself should be compiled with CERES_BUILDING_SHARED_LIBRARY
+# defined, any users of Ceres will have CERES_USING_SHARED_LIBRARY defined
+# for them in Ceres' config.h if appropriate.
+if (BUILD_SHARED_LIBS)
+ remove_definitions(-DCERES_BUILDING_SHARED_LIBRARY)
+endif()
+
+add_executable(helloworld helloworld.cc)
+target_link_libraries(helloworld ceres)
+
+add_executable(helloworld_numeric_diff helloworld_numeric_diff.cc)
+target_link_libraries(helloworld_numeric_diff ceres)
+
+add_executable(helloworld_analytic_diff helloworld_analytic_diff.cc)
+target_link_libraries(helloworld_analytic_diff ceres)
+
+add_executable(curve_fitting curve_fitting.cc)
+target_link_libraries(curve_fitting ceres)
+
+add_executable(rosenbrock rosenbrock.cc)
+target_link_libraries(rosenbrock ceres)
+
+add_executable(curve_fitting_c curve_fitting.c)
+target_link_libraries(curve_fitting_c ceres)
+# Force CMake to link curve_fitting_c using the C linker, this is important
+# when Ceres was compiled using C++11 to ensure that -std=c++11 is not passed
+# through.
+set_target_properties(curve_fitting_c PROPERTIES LINKER_LANGUAGE C)
+# As this is a C file #including <math.h> we have to explicitly add the math
+# library (libm). Although some compilers (dependent upon options) will accept
+# the indirect link to libm via Ceres, at least GCC 4.8 on pure Debian won't.
+if (NOT MSVC)
+ target_link_libraries(curve_fitting_c m)
+endif (NOT MSVC)
+
+add_executable(ellipse_approximation ellipse_approximation.cc)
+target_link_libraries(ellipse_approximation ceres)
+
+add_executable(robust_curve_fitting robust_curve_fitting.cc)
+target_link_libraries(robust_curve_fitting ceres)
+
+add_executable(simple_bundle_adjuster simple_bundle_adjuster.cc)
+target_link_libraries(simple_bundle_adjuster ceres)
+
+if (GFLAGS)
+ add_executable(powell powell.cc)
+ target_link_libraries(powell ceres ${GFLAGS_LIBRARIES})
+
+ add_executable(nist nist.cc)
+ target_link_libraries(nist ceres ${GFLAGS_LIBRARIES})
+ if (MSVC)
+ target_compile_options(nist PRIVATE "/bigobj")
+ endif()
+
+ add_executable(more_garbow_hillstrom more_garbow_hillstrom.cc)
+ target_link_libraries(more_garbow_hillstrom ceres ${GFLAGS_LIBRARIES})
+
+ add_executable(circle_fit circle_fit.cc)
+ target_link_libraries(circle_fit ceres ${GFLAGS_LIBRARIES})
+
+ add_executable(bundle_adjuster
+ bundle_adjuster.cc
+ bal_problem.cc)
+ target_link_libraries(bundle_adjuster ceres ${GFLAGS_LIBRARIES})
+
+ add_executable(libmv_bundle_adjuster
+ libmv_bundle_adjuster.cc)
+ target_link_libraries(libmv_bundle_adjuster ceres ${GFLAGS_LIBRARIES})
+
+ add_executable(libmv_homography
+ libmv_homography.cc)
+ target_link_libraries(libmv_homography ceres ${GFLAGS_LIBRARIES})
+
+ add_executable(denoising
+ denoising.cc
+ fields_of_experts.cc)
+ target_link_libraries(denoising ceres ${GFLAGS_LIBRARIES})
+
+ add_executable(robot_pose_mle
+ robot_pose_mle.cc)
+ target_link_libraries(robot_pose_mle ceres ${GFLAGS_LIBRARIES})
+
+endif (GFLAGS)
+
+add_subdirectory(sampled_function)
+add_subdirectory(slam)
diff --git a/examples/Makefile.example b/examples/Makefile.example
new file mode 100644
index 0000000..f2b0dc0
--- /dev/null
+++ b/examples/Makefile.example
@@ -0,0 +1,82 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2015 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: 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..1ee3c81
--- /dev/null
+++ b/examples/bal_problem.cc
@@ -0,0 +1,339 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "bal_problem.h"
+
+#include <cstdio>
+#include <cstdlib>
+#include <fstream>
+#include <string>
+#include <vector>
+#include "Eigen/Core"
+#include "ceres/rotation.h"
+#include "glog/logging.h"
+#include "random.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);
+}
+
+// Write the problem to a PLY file for inspection in Meshlab or CloudCompare.
+void BALProblem::WriteToPLYFile(const std::string& filename) const {
+ std::ofstream of(filename.c_str());
+
+ of << "ply"
+ << '\n' << "format ascii 1.0"
+ << '\n' << "element vertex " << num_cameras_ + num_points_
+ << '\n' << "property float x"
+ << '\n' << "property float y"
+ << '\n' << "property float z"
+ << '\n' << "property uchar red"
+ << '\n' << "property uchar green"
+ << '\n' << "property uchar blue"
+ << '\n' << "end_header" << std::endl;
+
+ // Export extrinsic data (i.e. camera centers) as green points.
+ double angle_axis[3];
+ double center[3];
+ for (int i = 0; i < num_cameras(); ++i) {
+ const double* camera = cameras() + camera_block_size() * i;
+ CameraToAngleAxisAndCenter(camera, angle_axis, center);
+ of << center[0] << ' ' << center[1] << ' ' << center[2]
+ << " 0 255 0" << '\n';
+ }
+
+ // Export the structure (i.e. 3D Points) as white points.
+ 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) {
+ of << point[j] << ' ';
+ }
+ of << "255 255 255\n";
+ }
+ of.close();
+}
+
+void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
+ double* angle_axis,
+ double* center) const {
+ 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) const {
+ 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..e9a348e
--- /dev/null
+++ b/examples/bal_problem.h
@@ -0,0 +1,109 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// 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;
+ void WriteToPLYFile(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_; }
+ const double* cameras() 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) const;
+
+ void AngleAxisAndCenterToCamera(const double* angle_axis,
+ const double* center,
+ double* camera) const;
+ 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..5619c52
--- /dev/null
+++ b/examples/bundle_adjuster.cc
@@ -0,0 +1,346 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// An example of solving a dynamically sized problem with various
+// solvers and loss functions.
+//
+// For a simpler bare bones example of doing bundle adjustment with
+// Ceres, please see simple_bundle_adjuster.cc.
+//
+// NOTE: This example will not compile without gflags and SuiteSparse.
+//
+// The problem being solved here is known as a Bundle Adjustment
+// problem in computer vision. Given a set of 3d points X_1, ..., X_n,
+// a set of cameras P_1, ..., P_m. If the point X_i is visible in
+// image j, then there is a 2D observation u_ij that is the expected
+// projection of X_i using P_j. The aim of this optimization is to
+// find values of X_i and P_j such that the reprojection error
+//
+// E(X,P) = sum_ij |u_ij - P_j X_i|^2
+//
+// is minimized.
+//
+// The problem used here comes from a collection of bundle adjustment
+// problems published at University of Washington.
+// http://grail.cs.washington.edu/projects/bal
+
+#include <algorithm>
+#include <cmath>
+#include <cstdio>
+#include <cstdlib>
+#include <string>
+#include <vector>
+
+#include "bal_problem.h"
+#include "ceres/ceres.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "snavely_reprojection_error.h"
+
+DEFINE_string(input, "", "Input File name");
+DEFINE_string(trust_region_strategy, "levenberg_marquardt",
+ "Options are: levenberg_marquardt, dogleg.");
+DEFINE_string(dogleg, "traditional_dogleg", "Options are: traditional_dogleg,"
+ "subspace_dogleg.");
+
+DEFINE_bool(inner_iterations, false, "Use inner iterations to non-linearly "
+ "refine each successful trust region step.");
+
+DEFINE_string(blocks_for_inner_iterations, "automatic", "Options are: "
+ "automatic, cameras, points, cameras,points, points,cameras");
+
+DEFINE_string(linear_solver, "sparse_schur", "Options are: "
+ "sparse_schur, dense_schur, iterative_schur, sparse_normal_cholesky, "
+ "dense_qr, dense_normal_cholesky and cgnr.");
+DEFINE_bool(explicit_schur_complement, false, "If using ITERATIVE_SCHUR "
+ "then explicitly compute the Schur complement.");
+DEFINE_string(preconditioner, "jacobi", "Options are: "
+ "identity, jacobi, schur_jacobi, cluster_jacobi, "
+ "cluster_tridiagonal.");
+DEFINE_string(visibility_clustering, "canonical_views",
+ "single_linkage, canonical_views");
+
+DEFINE_string(sparse_linear_algebra_library, "suite_sparse",
+ "Options are: suite_sparse and cx_sparse.");
+DEFINE_string(dense_linear_algebra_library, "eigen",
+ "Options are: eigen and lapack.");
+DEFINE_string(ordering, "automatic", "Options are: automatic, user.");
+
+DEFINE_bool(use_quaternions, false, "If true, uses quaternions to represent "
+ "rotations. If false, angle axis is used.");
+DEFINE_bool(use_local_parameterization, false, "For quaternions, use a local "
+ "parameterization.");
+DEFINE_bool(robustify, false, "Use a robust loss function.");
+
+DEFINE_double(eta, 1e-2, "Default value for eta. Eta determines the "
+ "accuracy of each linear solve of the truncated newton step. "
+ "Changing this parameter can affect solve performance.");
+
+DEFINE_int32(num_threads, 1, "Number of threads.");
+DEFINE_int32(num_iterations, 5, "Number of iterations.");
+DEFINE_double(max_solver_time, 1e32, "Maximum solve time in seconds.");
+DEFINE_bool(nonmonotonic_steps, false, "Trust region algorithm can use"
+ " nonmonotic steps.");
+
+DEFINE_double(rotation_sigma, 0.0, "Standard deviation of camera rotation "
+ "perturbation.");
+DEFINE_double(translation_sigma, 0.0, "Standard deviation of the camera "
+ "translation perturbation.");
+DEFINE_double(point_sigma, 0.0, "Standard deviation of the point "
+ "perturbation.");
+DEFINE_int32(random_seed, 38401, "Random seed used to set the state "
+ "of the pseudo random number generator used to generate "
+ "the pertubations.");
+DEFINE_bool(line_search, false, "Use a line search instead of trust region "
+ "algorithm.");
+DEFINE_bool(mixed_precision_solves, false, "Use mixed precision solves.");
+DEFINE_int32(max_num_refinement_iterations, 0, "Iterative refinement iterations");
+DEFINE_string(initial_ply, "", "Export the BAL file data as a PLY file.");
+DEFINE_string(final_ply, "", "Export the refined BAL file data as a PLY "
+ "file.");
+
+namespace ceres {
+namespace examples {
+
+void SetLinearSolver(Solver::Options* options) {
+ CHECK(StringToLinearSolverType(FLAGS_linear_solver,
+ &options->linear_solver_type));
+ CHECK(StringToPreconditionerType(FLAGS_preconditioner,
+ &options->preconditioner_type));
+ CHECK(StringToVisibilityClusteringType(FLAGS_visibility_clustering,
+ &options->visibility_clustering_type));
+ CHECK(StringToSparseLinearAlgebraLibraryType(
+ FLAGS_sparse_linear_algebra_library,
+ &options->sparse_linear_algebra_library_type));
+ CHECK(StringToDenseLinearAlgebraLibraryType(
+ FLAGS_dense_linear_algebra_library,
+ &options->dense_linear_algebra_library_type));
+ options->use_explicit_schur_complement = FLAGS_explicit_schur_complement;
+ options->use_mixed_precision_solves = FLAGS_mixed_precision_solves;
+ options->max_num_refinement_iterations = FLAGS_max_num_refinement_iterations;
+}
+
+void SetOrdering(BALProblem* bal_problem, Solver::Options* options) {
+ const int num_points = bal_problem->num_points();
+ const int point_block_size = bal_problem->point_block_size();
+ double* points = bal_problem->mutable_points();
+
+ const int num_cameras = bal_problem->num_cameras();
+ const int camera_block_size = bal_problem->camera_block_size();
+ double* cameras = bal_problem->mutable_cameras();
+
+ if (options->use_inner_iterations) {
+ if (FLAGS_blocks_for_inner_iterations == "cameras") {
+ LOG(INFO) << "Camera blocks for inner iterations";
+ options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ for (int i = 0; i < num_cameras; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
+ }
+ } else if (FLAGS_blocks_for_inner_iterations == "points") {
+ LOG(INFO) << "Point blocks for inner iterations";
+ options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ for (int i = 0; i < num_points; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
+ }
+ } else if (FLAGS_blocks_for_inner_iterations == "cameras,points") {
+ LOG(INFO) << "Camera followed by point blocks for inner iterations";
+ options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ for (int i = 0; i < num_cameras; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0);
+ }
+ for (int i = 0; i < num_points; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1);
+ }
+ } else if (FLAGS_blocks_for_inner_iterations == "points,cameras") {
+ LOG(INFO) << "Point followed by camera blocks for inner iterations";
+ options->inner_iteration_ordering.reset(new ParameterBlockOrdering);
+ for (int i = 0; i < num_cameras; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
+ }
+ for (int i = 0; i < num_points; ++i) {
+ options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0);
+ }
+ } else if (FLAGS_blocks_for_inner_iterations == "automatic") {
+ LOG(INFO) << "Choosing automatic blocks for inner iterations";
+ } else {
+ LOG(FATAL) << "Unknown block type for inner iterations: "
+ << FLAGS_blocks_for_inner_iterations;
+ }
+ }
+
+ // Bundle adjustment problems have a sparsity structure that makes
+ // them amenable to more specialized and much more efficient
+ // solution strategies. The SPARSE_SCHUR, DENSE_SCHUR and
+ // ITERATIVE_SCHUR solvers make use of this specialized
+ // structure.
+ //
+ // This can either be done by specifying Options::ordering_type =
+ // ceres::SCHUR, in which case Ceres will automatically determine
+ // the right ParameterBlock ordering, or by manually specifying a
+ // suitable ordering vector and defining
+ // Options::num_eliminate_blocks.
+ if (FLAGS_ordering == "automatic") {
+ return;
+ }
+
+ ceres::ParameterBlockOrdering* ordering =
+ new ceres::ParameterBlockOrdering;
+
+ // The points come before the cameras.
+ for (int i = 0; i < num_points; ++i) {
+ ordering->AddElementToGroup(points + point_block_size * i, 0);
+ }
+
+ for (int i = 0; i < num_cameras; ++i) {
+ // When using axis-angle, there is a single parameter block for
+ // the entire camera.
+ ordering->AddElementToGroup(cameras + camera_block_size * i, 1);
+ }
+
+ options->linear_solver_ordering.reset(ordering);
+}
+
+void SetMinimizerOptions(Solver::Options* options) {
+ options->max_num_iterations = FLAGS_num_iterations;
+ options->minimizer_progress_to_stdout = true;
+ options->num_threads = FLAGS_num_threads;
+ options->eta = FLAGS_eta;
+ options->max_solver_time_in_seconds = FLAGS_max_solver_time;
+ options->use_nonmonotonic_steps = FLAGS_nonmonotonic_steps;
+ if (FLAGS_line_search) {
+ options->minimizer_type = ceres::LINE_SEARCH;
+ }
+
+ CHECK(StringToTrustRegionStrategyType(FLAGS_trust_region_strategy,
+ &options->trust_region_strategy_type));
+ CHECK(StringToDoglegType(FLAGS_dogleg, &options->dogleg_type));
+ options->use_inner_iterations = FLAGS_inner_iterations;
+}
+
+void SetSolverOptionsFromFlags(BALProblem* bal_problem,
+ Solver::Options* options) {
+ SetMinimizerOptions(options);
+ SetLinearSolver(options);
+ SetOrdering(bal_problem, options);
+}
+
+void BuildProblem(BALProblem* bal_problem, Problem* problem) {
+ const int point_block_size = bal_problem->point_block_size();
+ const int camera_block_size = bal_problem->camera_block_size();
+ double* points = bal_problem->mutable_points();
+ double* cameras = bal_problem->mutable_cameras();
+
+ // Observations is 2*num_observations long array observations =
+ // [u_1, u_2, ... , u_n], where each u_i is two dimensional, the x
+ // and y positions of the observation.
+ const double* observations = bal_problem->observations();
+ for (int i = 0; i < bal_problem->num_observations(); ++i) {
+ CostFunction* cost_function;
+ // Each Residual block takes a point and a camera as input and
+ // outputs a 2 dimensional residual.
+ cost_function =
+ (FLAGS_use_quaternions)
+ ? SnavelyReprojectionErrorWithQuaternions::Create(
+ observations[2 * i + 0],
+ observations[2 * i + 1])
+ : SnavelyReprojectionError::Create(
+ observations[2 * i + 0],
+ observations[2 * i + 1]);
+
+ // If enabled use Huber's loss function.
+ LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;
+
+ // Each observation correponds to a pair of a camera and a point
+ // which are identified by camera_index()[i] and point_index()[i]
+ // respectively.
+ double* camera =
+ cameras + camera_block_size * bal_problem->camera_index()[i];
+ double* point = points + point_block_size * bal_problem->point_index()[i];
+ problem->AddResidualBlock(cost_function, loss_function, camera, point);
+ }
+
+ if (FLAGS_use_quaternions && FLAGS_use_local_parameterization) {
+ LocalParameterization* camera_parameterization =
+ new ProductParameterization(
+ new QuaternionParameterization(),
+ new IdentityParameterization(6));
+ for (int i = 0; i < bal_problem->num_cameras(); ++i) {
+ problem->SetParameterization(cameras + camera_block_size * i,
+ camera_parameterization);
+ }
+ }
+}
+
+void SolveProblem(const char* filename) {
+ BALProblem bal_problem(filename, FLAGS_use_quaternions);
+
+ if (!FLAGS_initial_ply.empty()) {
+ bal_problem.WriteToPLYFile(FLAGS_initial_ply);
+ }
+
+ Problem problem;
+
+ srand(FLAGS_random_seed);
+ bal_problem.Normalize();
+ bal_problem.Perturb(FLAGS_rotation_sigma,
+ FLAGS_translation_sigma,
+ FLAGS_point_sigma);
+
+ BuildProblem(&bal_problem, &problem);
+ Solver::Options options;
+ SetSolverOptionsFromFlags(&bal_problem, &options);
+ options.gradient_tolerance = 1e-16;
+ options.function_tolerance = 1e-16;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ std::cout << summary.FullReport() << "\n";
+
+ if (!FLAGS_final_ply.empty()) {
+ bal_problem.WriteToPLYFile(FLAGS_final_ply);
+ }
+}
+
+} // namespace examples
+} // namespace ceres
+
+int main(int argc, char** argv) {
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+ google::InitGoogleLogging(argv[0]);
+ if (FLAGS_input.empty()) {
+ LOG(ERROR) << "Usage: bundle_adjuster --input=bal_problem";
+ return 1;
+ }
+
+ CHECK(FLAGS_use_quaternions || !FLAGS_use_local_parameterization)
+ << "--use_local_parameterization can only be used with "
+ << "--use_quaternions.";
+ ceres::examples::SolveProblem(FLAGS_input.c_str());
+ return 0;
+}
diff --git a/examples/circle_fit.cc b/examples/circle_fit.cc
new file mode 100644
index 0000000..b58ce4e
--- /dev/null
+++ b/examples/circle_fit.cc
@@ -0,0 +1,164 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: 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) {
+ CERES_GFLAGS_NAMESPACE::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/curve_fitting.c b/examples/curve_fitting.c
new file mode 100644
index 0000000..0027aa8
--- /dev/null
+++ b/examples/curve_fitting.c
@@ -0,0 +1,187 @@
+/* Ceres Solver - A fast non-linear least squares minimizer
+ * Copyright 2015 Google Inc. All rights reserved.
+ * http://ceres-solver.org/
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * - Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * - Neither the name of Google Inc. nor the names of its contributors may be
+ * used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: mierle@gmail.com (Keir Mierle)
+ *
+ * This is a port of curve_fitting.cc to the minimal C API for Ceres.
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h> // For NULL
+#include "ceres/c_api.h"
+
+/* 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'];
+ *
+ */
+
+int num_observations = 67;
+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,
+};
+
+/* This is the equivalent of a use-defined CostFunction in the C++ Ceres API.
+ * This is passed as a callback to the Ceres C API, which internally converts
+ * the callback into a CostFunction. */
+int exponential_residual(void* user_data,
+ double** parameters,
+ double* residuals,
+ double** jacobians) {
+ double* measurement = (double*) user_data;
+ double x = measurement[0];
+ double y = measurement[1];
+ double m = parameters[0][0];
+ double c = parameters[1][0];
+
+ residuals[0] = y - exp(m * x + c);
+ if (jacobians == NULL) {
+ return 1;
+ }
+ if (jacobians[0] != NULL) {
+ jacobians[0][0] = - x * exp(m * x + c); /* dr/dm */
+ }
+ if (jacobians[1] != NULL) {
+ jacobians[1][0] = - exp(m * x + c); /* dr/dc */
+ }
+ return 1;
+}
+
+int main(int argc, char** argv) {
+ /* Note: Typically it is better to compact m and c into one block,
+ * but in this case use separate blocks to illustrate the use of
+ * multiple parameter blocks. */
+ double m = 0.0;
+ double c = 0.0;
+
+ double *parameter_pointers[] = { &m, &c };
+ int parameter_sizes[] = { 1, 1 };
+ int i;
+
+ ceres_problem_t* problem;
+
+ /* Ceres has some internal stuff that needs to get initialized. */
+ ceres_init();
+
+ problem = ceres_create_problem();
+
+ /* Add all the residuals. */
+ for (i = 0; i < num_observations; ++i) {
+ ceres_problem_add_residual_block(
+ problem,
+ exponential_residual, /* Cost function */
+ &data[2 * i], /* Points to the (x,y) measurement */
+ NULL, /* No loss function */
+ NULL, /* No loss function user data */
+ 1, /* Number of residuals */
+ 2, /* Number of parameter blocks */
+ parameter_sizes,
+ parameter_pointers);
+ }
+
+ ceres_solve(problem);
+ ceres_free_problem(problem);
+
+ printf("Initial m: 0.0, c: 0.0\n");
+ printf("Final m: %g, c: %g\n", m, c);
+ return 0;
+}
diff --git a/examples/curve_fitting.cc b/examples/curve_fitting.cc
new file mode 100644
index 0000000..3f577f8
--- /dev/null
+++ b/examples/curve_fitting.cc
@@ -0,0 +1,163 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/ceres.h"
+#include "glog/logging.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,
+};
+
+struct ExponentialResidual {
+ 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] = y_ - exp(m[0] * x_ + c[0]);
+ return true;
+ }
+
+ private:
+ const double x_;
+ const double y_;
+};
+
+int main(int argc, char** argv) {
+ 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..a8bdd7c
--- /dev/null
+++ b/examples/denoising.cc
@@ -0,0 +1,220 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: 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.");
+DEFINE_bool(line_search, false, "Use a line search instead of trust region "
+ "algorithm.");
+
+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;
+ }
+
+ if (FLAGS_line_search) {
+ options.minimizer_type = ceres::LINE_SEARCH;
+ }
+
+ 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>";
+ CERES_GFLAGS_NAMESPACE::SetUsageMessage(usage);
+ CERES_GFLAGS_NAMESPACE::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/ellipse_approximation.cc b/examples/ellipse_approximation.cc
new file mode 100644
index 0000000..f519f0c
--- /dev/null
+++ b/examples/ellipse_approximation.cc
@@ -0,0 +1,452 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: richie.stebbing@gmail.com (Richard Stebbing)
+//
+// This fits points randomly distributed on an ellipse with an approximate
+// line segment contour. This is done by jointly optimizing the control points
+// of the line segment contour along with the preimage positions for the data
+// points. The purpose of this example is to show an example use case for
+// dynamic_sparsity, and how it can benefit problems which are numerically
+// dense but dynamically sparse.
+
+#include <cmath>
+#include <vector>
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+// Data generated with the following Python code.
+// import numpy as np
+// np.random.seed(1337)
+// t = np.linspace(0.0, 2.0 * np.pi, 212, endpoint=False)
+// t += 2.0 * np.pi * 0.01 * np.random.randn(t.size)
+// theta = np.deg2rad(15)
+// a, b = np.cos(theta), np.sin(theta)
+// R = np.array([[a, -b],
+// [b, a]])
+// Y = np.dot(np.c_[4.0 * np.cos(t), np.sin(t)], R.T)
+
+const int kYRows = 212;
+const int kYCols = 2;
+const double kYData[kYRows * kYCols] = {
+ +3.871364e+00, +9.916027e-01,
+ +3.864003e+00, +1.034148e+00,
+ +3.850651e+00, +1.072202e+00,
+ +3.868350e+00, +1.014408e+00,
+ +3.796381e+00, +1.153021e+00,
+ +3.857138e+00, +1.056102e+00,
+ +3.787532e+00, +1.162215e+00,
+ +3.704477e+00, +1.227272e+00,
+ +3.564711e+00, +1.294959e+00,
+ +3.754363e+00, +1.191948e+00,
+ +3.482098e+00, +1.322725e+00,
+ +3.602777e+00, +1.279658e+00,
+ +3.585433e+00, +1.286858e+00,
+ +3.347505e+00, +1.356415e+00,
+ +3.220855e+00, +1.378914e+00,
+ +3.558808e+00, +1.297174e+00,
+ +3.403618e+00, +1.343809e+00,
+ +3.179828e+00, +1.384721e+00,
+ +3.054789e+00, +1.398759e+00,
+ +3.294153e+00, +1.366808e+00,
+ +3.247312e+00, +1.374813e+00,
+ +2.988547e+00, +1.404247e+00,
+ +3.114508e+00, +1.392698e+00,
+ +2.899226e+00, +1.409802e+00,
+ +2.533256e+00, +1.414778e+00,
+ +2.654773e+00, +1.415909e+00,
+ +2.565100e+00, +1.415313e+00,
+ +2.976456e+00, +1.405118e+00,
+ +2.484200e+00, +1.413640e+00,
+ +2.324751e+00, +1.407476e+00,
+ +1.930468e+00, +1.378221e+00,
+ +2.329017e+00, +1.407688e+00,
+ +1.760640e+00, +1.360319e+00,
+ +2.147375e+00, +1.396603e+00,
+ +1.741989e+00, +1.358178e+00,
+ +1.743859e+00, +1.358394e+00,
+ +1.557372e+00, +1.335208e+00,
+ +1.280551e+00, +1.295087e+00,
+ +1.429880e+00, +1.317546e+00,
+ +1.213485e+00, +1.284400e+00,
+ +9.168172e-01, +1.232870e+00,
+ +1.311141e+00, +1.299839e+00,
+ +1.231969e+00, +1.287382e+00,
+ +7.453773e-01, +1.200049e+00,
+ +6.151587e-01, +1.173683e+00,
+ +5.935666e-01, +1.169193e+00,
+ +2.538707e-01, +1.094227e+00,
+ +6.806136e-01, +1.187089e+00,
+ +2.805447e-01, +1.100405e+00,
+ +6.184807e-01, +1.174371e+00,
+ +1.170550e-01, +1.061762e+00,
+ +2.890507e-01, +1.102365e+00,
+ +3.834234e-01, +1.123772e+00,
+ +3.980161e-04, +1.033061e+00,
+ -3.651680e-01, +9.370367e-01,
+ -8.386351e-01, +7.987201e-01,
+ -8.105704e-01, +8.073702e-01,
+ -8.735139e-01, +7.878886e-01,
+ -9.913836e-01, +7.506100e-01,
+ -8.784011e-01, +7.863636e-01,
+ -1.181440e+00, +6.882566e-01,
+ -1.229556e+00, +6.720191e-01,
+ -1.035839e+00, +7.362765e-01,
+ -8.031520e-01, +8.096470e-01,
+ -1.539136e+00, +5.629549e-01,
+ -1.755423e+00, +4.817306e-01,
+ -1.337589e+00, +6.348763e-01,
+ -1.836966e+00, +4.499485e-01,
+ -1.913367e+00, +4.195617e-01,
+ -2.126467e+00, +3.314900e-01,
+ -1.927625e+00, +4.138238e-01,
+ -2.339862e+00, +2.379074e-01,
+ -1.881736e+00, +4.322152e-01,
+ -2.116753e+00, +3.356163e-01,
+ -2.255733e+00, +2.754930e-01,
+ -2.555834e+00, +1.368473e-01,
+ -2.770277e+00, +2.895711e-02,
+ -2.563376e+00, +1.331890e-01,
+ -2.826715e+00, -9.000818e-04,
+ -2.978191e+00, -8.457804e-02,
+ -3.115855e+00, -1.658786e-01,
+ -2.982049e+00, -8.678322e-02,
+ -3.307892e+00, -2.902083e-01,
+ -3.038346e+00, -1.194222e-01,
+ -3.190057e+00, -2.122060e-01,
+ -3.279086e+00, -2.705777e-01,
+ -3.322028e+00, -2.999889e-01,
+ -3.122576e+00, -1.699965e-01,
+ -3.551973e+00, -4.768674e-01,
+ -3.581866e+00, -5.032175e-01,
+ -3.497799e+00, -4.315203e-01,
+ -3.565384e+00, -4.885602e-01,
+ -3.699493e+00, -6.199815e-01,
+ -3.585166e+00, -5.061925e-01,
+ -3.758914e+00, -6.918275e-01,
+ -3.741104e+00, -6.689131e-01,
+ -3.688331e+00, -6.077239e-01,
+ -3.810425e+00, -7.689015e-01,
+ -3.791829e+00, -7.386911e-01,
+ -3.789951e+00, -7.358189e-01,
+ -3.823100e+00, -7.918398e-01,
+ -3.857021e+00, -8.727074e-01,
+ -3.858250e+00, -8.767645e-01,
+ -3.872100e+00, -9.563174e-01,
+ -3.864397e+00, -1.032630e+00,
+ -3.846230e+00, -1.081669e+00,
+ -3.834799e+00, -1.102536e+00,
+ -3.866684e+00, -1.022901e+00,
+ -3.808643e+00, -1.139084e+00,
+ -3.868840e+00, -1.011569e+00,
+ -3.791071e+00, -1.158615e+00,
+ -3.797999e+00, -1.151267e+00,
+ -3.696278e+00, -1.232314e+00,
+ -3.779007e+00, -1.170504e+00,
+ -3.622855e+00, -1.270793e+00,
+ -3.647249e+00, -1.259166e+00,
+ -3.655412e+00, -1.255042e+00,
+ -3.573218e+00, -1.291696e+00,
+ -3.638019e+00, -1.263684e+00,
+ -3.498409e+00, -1.317750e+00,
+ -3.304143e+00, -1.364970e+00,
+ -3.183001e+00, -1.384295e+00,
+ -3.202456e+00, -1.381599e+00,
+ -3.244063e+00, -1.375332e+00,
+ -3.233308e+00, -1.377019e+00,
+ -3.060112e+00, -1.398264e+00,
+ -3.078187e+00, -1.396517e+00,
+ -2.689594e+00, -1.415761e+00,
+ -2.947662e+00, -1.407039e+00,
+ -2.854490e+00, -1.411860e+00,
+ -2.660499e+00, -1.415900e+00,
+ -2.875955e+00, -1.410930e+00,
+ -2.675385e+00, -1.415848e+00,
+ -2.813155e+00, -1.413363e+00,
+ -2.417673e+00, -1.411512e+00,
+ -2.725461e+00, -1.415373e+00,
+ -2.148334e+00, -1.396672e+00,
+ -2.108972e+00, -1.393738e+00,
+ -2.029905e+00, -1.387302e+00,
+ -2.046214e+00, -1.388687e+00,
+ -2.057402e+00, -1.389621e+00,
+ -1.650250e+00, -1.347160e+00,
+ -1.806764e+00, -1.365469e+00,
+ -1.206973e+00, -1.283343e+00,
+ -8.029259e-01, -1.211308e+00,
+ -1.229551e+00, -1.286993e+00,
+ -1.101507e+00, -1.265754e+00,
+ -9.110645e-01, -1.231804e+00,
+ -1.110046e+00, -1.267211e+00,
+ -8.465274e-01, -1.219677e+00,
+ -7.594163e-01, -1.202818e+00,
+ -8.023823e-01, -1.211203e+00,
+ -3.732519e-01, -1.121494e+00,
+ -1.918373e-01, -1.079668e+00,
+ -4.671988e-01, -1.142253e+00,
+ -4.033645e-01, -1.128215e+00,
+ -1.920740e-01, -1.079724e+00,
+ -3.022157e-01, -1.105389e+00,
+ -1.652831e-01, -1.073354e+00,
+ +4.671625e-01, -9.085886e-01,
+ +5.940178e-01, -8.721832e-01,
+ +3.147557e-01, -9.508290e-01,
+ +6.383631e-01, -8.591867e-01,
+ +9.888923e-01, -7.514088e-01,
+ +7.076339e-01, -8.386023e-01,
+ +1.326682e+00, -6.386698e-01,
+ +1.149834e+00, -6.988221e-01,
+ +1.257742e+00, -6.624207e-01,
+ +1.492352e+00, -5.799632e-01,
+ +1.595574e+00, -5.421766e-01,
+ +1.240173e+00, -6.684113e-01,
+ +1.706612e+00, -5.004442e-01,
+ +1.873984e+00, -4.353002e-01,
+ +1.985633e+00, -3.902561e-01,
+ +1.722880e+00, -4.942329e-01,
+ +2.095182e+00, -3.447402e-01,
+ +2.018118e+00, -3.768991e-01,
+ +2.422702e+00, -1.999563e-01,
+ +2.370611e+00, -2.239326e-01,
+ +2.152154e+00, -3.205250e-01,
+ +2.525121e+00, -1.516499e-01,
+ +2.422116e+00, -2.002280e-01,
+ +2.842806e+00, +9.536372e-03,
+ +3.030128e+00, +1.146027e-01,
+ +2.888424e+00, +3.433444e-02,
+ +2.991609e+00, +9.226409e-02,
+ +2.924807e+00, +5.445844e-02,
+ +3.007772e+00, +1.015875e-01,
+ +2.781973e+00, -2.282382e-02,
+ +3.164737e+00, +1.961781e-01,
+ +3.237671e+00, +2.430139e-01,
+ +3.046123e+00, +1.240014e-01,
+ +3.414834e+00, +3.669060e-01,
+ +3.436591e+00, +3.833600e-01,
+ +3.626207e+00, +5.444311e-01,
+ +3.223325e+00, +2.336361e-01,
+ +3.511963e+00, +4.431060e-01,
+ +3.698380e+00, +6.187442e-01,
+ +3.670244e+00, +5.884943e-01,
+ +3.558833e+00, +4.828230e-01,
+ +3.661807e+00, +5.797689e-01,
+ +3.767261e+00, +7.030893e-01,
+ +3.801065e+00, +7.532650e-01,
+ +3.828523e+00, +8.024454e-01,
+ +3.840719e+00, +8.287032e-01,
+ +3.848748e+00, +8.485921e-01,
+ +3.865801e+00, +9.066551e-01,
+ +3.870983e+00, +9.404873e-01,
+ +3.870263e+00, +1.001884e+00,
+ +3.864462e+00, +1.032374e+00,
+ +3.870542e+00, +9.996121e-01,
+ +3.865424e+00, +1.028474e+00
+};
+ceres::ConstMatrixRef kY(kYData, kYRows, kYCols);
+
+class PointToLineSegmentContourCostFunction : public ceres::CostFunction {
+ public:
+ PointToLineSegmentContourCostFunction(const int num_segments,
+ const Eigen::Vector2d& y)
+ : num_segments_(num_segments), y_(y) {
+ // The first parameter is the preimage position.
+ mutable_parameter_block_sizes()->push_back(1);
+ // The next parameters are the control points for the line segment contour.
+ for (int i = 0; i < num_segments_; ++i) {
+ mutable_parameter_block_sizes()->push_back(2);
+ }
+ set_num_residuals(2);
+ }
+
+ virtual bool Evaluate(const double* const* x,
+ double* residuals,
+ double** jacobians) const {
+ // Convert the preimage position `t` into a segment index `i0` and the
+ // line segment interpolation parameter `u`. `i1` is the index of the next
+ // control point.
+ const double t = ModuloNumSegments(*x[0]);
+ CHECK_GE(t, 0.0);
+ CHECK_LT(t, num_segments_);
+ const int i0 = floor(t), i1 = (i0 + 1) % num_segments_;
+ const double u = t - i0;
+
+ // Linearly interpolate between control points `i0` and `i1`.
+ residuals[0] = y_[0] - ((1.0 - u) * x[1 + i0][0] + u * x[1 + i1][0]);
+ residuals[1] = y_[1] - ((1.0 - u) * x[1 + i0][1] + u * x[1 + i1][1]);
+
+ if (jacobians == NULL) {
+ return true;
+ }
+
+ if (jacobians[0] != NULL) {
+ jacobians[0][0] = x[1 + i0][0] - x[1 + i1][0];
+ jacobians[0][1] = x[1 + i0][1] - x[1 + i1][1];
+ }
+ for (int i = 0; i < num_segments_; ++i) {
+ if (jacobians[i + 1] != NULL) {
+ ceres::MatrixRef(jacobians[i + 1], 2, 2).setZero();
+ if (i == i0) {
+ jacobians[i + 1][0] = -(1.0 - u);
+ jacobians[i + 1][3] = -(1.0 - u);
+ } else if (i == i1) {
+ jacobians[i + 1][0] = -u;
+ jacobians[i + 1][3] = -u;
+ }
+ }
+ }
+ return true;
+ }
+
+ static ceres::CostFunction* Create(const int num_segments,
+ const Eigen::Vector2d& y) {
+ return new PointToLineSegmentContourCostFunction(num_segments, y);
+ }
+
+ private:
+ inline double ModuloNumSegments(const double t) const {
+ return t - num_segments_ * floor(t / num_segments_);
+ }
+
+ const int num_segments_;
+ const Eigen::Vector2d y_;
+};
+
+class EuclideanDistanceFunctor {
+ public:
+ explicit EuclideanDistanceFunctor(const double& sqrt_weight)
+ : sqrt_weight_(sqrt_weight) {}
+
+ template <typename T>
+ bool operator()(const T* x0, const T* x1, T* residuals) const {
+ residuals[0] = sqrt_weight_ * (x0[0] - x1[0]);
+ residuals[1] = sqrt_weight_ * (x0[1] - x1[1]);
+ return true;
+ }
+
+ static ceres::CostFunction* Create(const double sqrt_weight) {
+ return new ceres::AutoDiffCostFunction<EuclideanDistanceFunctor, 2, 2, 2>(
+ new EuclideanDistanceFunctor(sqrt_weight));
+ }
+
+ private:
+ const double sqrt_weight_;
+};
+
+bool SolveWithFullReport(ceres::Solver::Options options,
+ ceres::Problem* problem,
+ bool dynamic_sparsity) {
+ options.dynamic_sparsity = dynamic_sparsity;
+
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, problem, &summary);
+
+ std::cout << "####################" << std::endl;
+ std::cout << "dynamic_sparsity = " << dynamic_sparsity << std::endl;
+ std::cout << "####################" << std::endl;
+ std::cout << summary.FullReport() << std::endl;
+
+ return summary.termination_type == ceres::CONVERGENCE;
+}
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ // Problem configuration.
+ const int num_segments = 151;
+ const double regularization_weight = 1e-2;
+
+ // Eigen::MatrixXd is column major so we define our own MatrixXd which is
+ // row major. Eigen::VectorXd can be used directly.
+ typedef Eigen::Matrix<double,
+ Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor> MatrixXd;
+ using Eigen::VectorXd;
+
+ // `X` is the matrix of control points which make up the contour of line
+ // segments. The number of control points is equal to the number of line
+ // segments because the contour is closed.
+ //
+ // Initialize `X` to points on the unit circle.
+ VectorXd w(num_segments + 1);
+ w.setLinSpaced(num_segments + 1, 0.0, 2.0 * M_PI);
+ w.conservativeResize(num_segments);
+ MatrixXd X(num_segments, 2);
+ X.col(0) = w.array().cos();
+ X.col(1) = w.array().sin();
+
+ // Each data point has an associated preimage position on the line segment
+ // contour. For each data point we initialize the preimage positions to
+ // the index of the closest control point.
+ const int num_observations = kY.rows();
+ VectorXd t(num_observations);
+ for (int i = 0; i < num_observations; ++i) {
+ (X.rowwise() - kY.row(i)).rowwise().squaredNorm().minCoeff(&t[i]);
+ }
+
+ ceres::Problem problem;
+
+ // For each data point add a residual which measures its distance to its
+ // corresponding position on the line segment contour.
+ std::vector<double*> parameter_blocks(1 + num_segments);
+ parameter_blocks[0] = NULL;
+ for (int i = 0; i < num_segments; ++i) {
+ parameter_blocks[i + 1] = X.data() + 2 * i;
+ }
+ for (int i = 0; i < num_observations; ++i) {
+ parameter_blocks[0] = &t[i];
+ problem.AddResidualBlock(
+ PointToLineSegmentContourCostFunction::Create(num_segments, kY.row(i)),
+ NULL,
+ parameter_blocks);
+ }
+
+ // Add regularization to minimize the length of the line segment contour.
+ for (int i = 0; i < num_segments; ++i) {
+ problem.AddResidualBlock(
+ EuclideanDistanceFunctor::Create(sqrt(regularization_weight)),
+ NULL,
+ X.data() + 2 * i,
+ X.data() + 2 * ((i + 1) % num_segments));
+ }
+
+ ceres::Solver::Options options;
+ options.max_num_iterations = 100;
+ options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+
+ // First, solve `X` and `t` jointly with dynamic_sparsity = true.
+ MatrixXd X0 = X;
+ VectorXd t0 = t;
+ CHECK(SolveWithFullReport(options, &problem, true));
+
+ // Second, solve with dynamic_sparsity = false.
+ X = X0;
+ t = t0;
+ CHECK(SolveWithFullReport(options, &problem, false));
+
+ return 0;
+}
diff --git a/examples/fields_of_experts.cc b/examples/fields_of_experts.cc
new file mode 100644
index 0000000..60d179a
--- /dev/null
+++ b/examples/fields_of_experts.cc
@@ -0,0 +1,152 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: 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..210ca69
--- /dev/null
+++ b/examples/fields_of_experts.h
@@ -0,0 +1,145 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: 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/helloworld.cc b/examples/helloworld.cc
new file mode 100644
index 0000000..22dff55
--- /dev/null
+++ b/examples/helloworld.cc
@@ -0,0 +1,83 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: 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 "ceres/ceres.h"
+#include "glog/logging.h"
+
+using ceres::AutoDiffCostFunction;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+// A templated cost functor that implements the residual r = 10 -
+// x. The method operator() is templated so that we can then use an
+// automatic differentiation wrapper around it to generate its
+// derivatives.
+struct CostFunctor {
+ template <typename T> bool operator()(const T* const x, T* residual) const {
+ residual[0] = 10.0 - x[0];
+ return true;
+ }
+};
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ // The variable to solve for with its initial value. It will be
+ // mutated in place by the solver.
+ double x = 0.5;
+ const double initial_x = 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).
+ CostFunction* cost_function =
+ new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
+ problem.AddResidualBlock(cost_function, NULL, &x);
+
+ // Run the solver!
+ Solver::Options options;
+ 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/helloworld_analytic_diff.cc b/examples/helloworld_analytic_diff.cc
new file mode 100644
index 0000000..9dbbc76
--- /dev/null
+++ b/examples/helloworld_analytic_diff.cc
@@ -0,0 +1,107 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: 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 "glog/logging.h"
+
+using ceres::CostFunction;
+using ceres::SizedCostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+// A CostFunction implementing analytically derivatives for the
+// function f(x) = 10 - x.
+class QuadraticCostFunction
+ : public SizedCostFunction<1 /* number of residuals */,
+ 1 /* size of first parameter */> {
+ public:
+ virtual ~QuadraticCostFunction() {}
+
+ 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.
+ //
+ // Since the Evaluate function can be called with the jacobians
+ // pointer equal to NULL, the Evaluate function must check to see
+ // if jacobians need to be computed.
+ //
+ // For this simple problem it is overkill to check if jacobians[0]
+ // is NULL, but in general when writing more complex
+ // CostFunctions, it is possible that Ceres may only demand the
+ // derivatives w.r.t. a subset of the parameter blocks.
+ if (jacobians != NULL && jacobians[0] != NULL) {
+ jacobians[0][0] = -1;
+ }
+
+ return true;
+ }
+};
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ // The variable to solve for with its initial value. It will be
+ // mutated in place by the solver.
+ double x = 0.5;
+ const double initial_x = x;
+
+ // Build the problem.
+ Problem problem;
+
+ // Set up the only cost function (also known as residual).
+ CostFunction* cost_function = new QuadraticCostFunction;
+ problem.AddResidualBlock(cost_function, NULL, &x);
+
+ // Run the solver!
+ Solver::Options options;
+ 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/helloworld_numeric_diff.cc b/examples/helloworld_numeric_diff.cc
new file mode 100644
index 0000000..0810f47
--- /dev/null
+++ b/examples/helloworld_numeric_diff.cc
@@ -0,0 +1,79 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: keir@google.com (Keir Mierle)
+//
+// Minimize 0.5 (10 - x)^2 using jacobian matrix computed using
+// numeric differentiation.
+
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+using ceres::NumericDiffCostFunction;
+using ceres::CENTRAL;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+// A cost functor that implements the residual r = 10 - x.
+struct CostFunctor {
+ bool operator()(const double* const x, double* residual) const {
+ residual[0] = 10.0 - x[0];
+ return true;
+ }
+};
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ // The variable to solve for with its initial value. It will be
+ // mutated in place by the solver.
+ double x = 0.5;
+ const double initial_x = x;
+
+ // Build the problem.
+ Problem problem;
+
+ // Set up the only cost function (also known as residual). This uses
+ // numeric differentiation to obtain the derivative (jacobian).
+ CostFunction* cost_function =
+ new NumericDiffCostFunction<CostFunctor, CENTRAL, 1, 1> (new CostFunctor);
+ problem.AddResidualBlock(cost_function, NULL, &x);
+
+ // Run the solver!
+ Solver::Options options;
+ 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/libmv_bundle_adjuster.cc b/examples/libmv_bundle_adjuster.cc
new file mode 100644
index 0000000..47d0cd7
--- /dev/null
+++ b/examples/libmv_bundle_adjuster.cc
@@ -0,0 +1,843 @@
+// Copyright (c) 2013 libmv authors.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+//
+// Author: mierle@gmail.com (Keir Mierle)
+// sergey.vfx@gmail.com (Sergey Sharybin)
+//
+// This is an example application which contains bundle adjustment code used
+// in the Libmv library and Blender. It reads problems from files passed via
+// the command line and runs the bundle adjuster on the problem.
+//
+// File with problem a binary file, for which it is crucial to know in which
+// order bytes of float values are stored in. This information is provided
+// by a single character in the beginning of the file. There're two possible
+// values of this byte:
+// - V, which means values in the file are stored with big endian type
+// - v, which means values in the file are stored with little endian type
+//
+// The rest of the file contains data in the following order:
+// - Space in which markers' coordinates are stored in
+// - Camera intrinsics
+// - Number of cameras
+// - Cameras
+// - Number of 3D points
+// - 3D points
+// - Number of markers
+// - Markers
+//
+// Markers' space could either be normalized or image (pixels). This is defined
+// by the single character in the file. P means markers in the file is in image
+// space, and N means markers are in normalized space.
+//
+// Camera intrinsics are 8 described by 8 float 8.
+// This values goes in the following order:
+//
+// - Focal length, principal point X, principal point Y, k1, k2, k3, p1, p2
+//
+// Every camera is described by:
+//
+// - Image for which camera belongs to (single 4 bytes integer value).
+// - Column-major camera rotation matrix, 9 float values.
+// - Camera translation, 3-component vector of float values.
+//
+// Image number shall be greater or equal to zero. Order of cameras does not
+// matter and gaps are possible.
+//
+// Every 3D point is decribed by:
+//
+// - Track number point belongs to (single 4 bytes integer value).
+// - 3D position vector, 3-component vector of float values.
+//
+// Track number shall be greater or equal to zero. Order of tracks does not
+// matter and gaps are possible.
+//
+// Finally every marker is described by:
+//
+// - Image marker belongs to single 4 bytes integer value).
+// - Track marker belongs to single 4 bytes integer value).
+// - 2D marker position vector, (two float values).
+//
+// Marker's space is used a default value for refine_intrinsics command line
+// flag. This means if there's no refine_intrinsics flag passed via command
+// line, camera intrinsics will be refined if markers in the problem are
+// stored in image space and camera intrinsics will not be refined if markers
+// are in normalized space.
+//
+// Passing refine_intrinsics command line flag defines explicitly whether
+// refinement of intrinsics will happen. Currently, only none and all
+// intrinsics refinement is supported.
+//
+// There're existing problem files dumped from blender stored in folder
+// ../data/libmv-ba-problems.
+
+#include <cstdio>
+#include <fcntl.h>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#ifdef _MSC_VER
+# include <io.h>
+# define open _open
+# define close _close
+typedef unsigned __int32 uint32_t;
+#else
+#include <stdint.h>
+#include <unistd.h>
+
+// O_BINARY is not defined on unix like platforms, as there is no
+// difference between binary and text files.
+#define O_BINARY 0
+
+#endif
+
+#include "ceres/ceres.h"
+#include "ceres/rotation.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+typedef Eigen::Matrix<double, 3, 3> Mat3;
+typedef Eigen::Matrix<double, 6, 1> Vec6;
+typedef Eigen::Vector3d Vec3;
+typedef Eigen::Vector4d Vec4;
+
+using std::vector;
+
+DEFINE_string(input, "", "Input File name");
+DEFINE_string(refine_intrinsics, "", "Camera intrinsics to be refined. "
+ "Options are: none, radial.");
+
+namespace {
+
+// A EuclideanCamera is the location and rotation of the camera
+// viewing an image.
+//
+// image identifies which image this camera represents.
+// R is a 3x3 matrix representing the rotation of the camera.
+// t is a translation vector representing its positions.
+struct EuclideanCamera {
+ EuclideanCamera() : image(-1) {}
+ EuclideanCamera(const EuclideanCamera &c) : image(c.image), R(c.R), t(c.t) {}
+
+ int image;
+ Mat3 R;
+ Vec3 t;
+};
+
+// A Point is the 3D location of a track.
+//
+// track identifies which track this point corresponds to.
+// X represents the 3D position of the track.
+struct EuclideanPoint {
+ EuclideanPoint() : track(-1) {}
+ EuclideanPoint(const EuclideanPoint &p) : track(p.track), X(p.X) {}
+ int track;
+ Vec3 X;
+};
+
+// A Marker is the 2D location of a tracked point in an image.
+//
+// x and y is the position of the marker in pixels from the top left corner
+// in the image identified by an image. All markers for to the same target
+// form a track identified by a common track number.
+struct Marker {
+ int image;
+ int track;
+ double x, y;
+};
+
+// Cameras intrinsics to be bundled.
+//
+// BUNDLE_RADIAL actually implies bundling of k1 and k2 coefficients only,
+// no bundling of k3 is possible at this moment.
+enum BundleIntrinsics {
+ BUNDLE_NO_INTRINSICS = 0,
+ BUNDLE_FOCAL_LENGTH = 1,
+ BUNDLE_PRINCIPAL_POINT = 2,
+ BUNDLE_RADIAL_K1 = 4,
+ BUNDLE_RADIAL_K2 = 8,
+ BUNDLE_RADIAL = 12,
+ BUNDLE_TANGENTIAL_P1 = 16,
+ BUNDLE_TANGENTIAL_P2 = 32,
+ BUNDLE_TANGENTIAL = 48,
+};
+
+// Denotes which blocks to keep constant during bundling.
+// For example it is useful to keep camera translations constant
+// when bundling tripod motions.
+enum BundleConstraints {
+ BUNDLE_NO_CONSTRAINTS = 0,
+ BUNDLE_NO_TRANSLATION = 1,
+};
+
+// The intrinsics need to get combined into a single parameter block; use these
+// enums to index instead of numeric constants.
+enum {
+ OFFSET_FOCAL_LENGTH,
+ OFFSET_PRINCIPAL_POINT_X,
+ OFFSET_PRINCIPAL_POINT_Y,
+ OFFSET_K1,
+ OFFSET_K2,
+ OFFSET_K3,
+ OFFSET_P1,
+ OFFSET_P2,
+};
+
+// Returns a pointer to the camera corresponding to a image.
+EuclideanCamera *CameraForImage(vector<EuclideanCamera> *all_cameras,
+ const int image) {
+ if (image < 0 || image >= all_cameras->size()) {
+ return NULL;
+ }
+ EuclideanCamera *camera = &(*all_cameras)[image];
+ if (camera->image == -1) {
+ return NULL;
+ }
+ return camera;
+}
+
+const EuclideanCamera *CameraForImage(
+ const vector<EuclideanCamera> &all_cameras,
+ const int image) {
+ if (image < 0 || image >= all_cameras.size()) {
+ return NULL;
+ }
+ const EuclideanCamera *camera = &all_cameras[image];
+ if (camera->image == -1) {
+ return NULL;
+ }
+ return camera;
+}
+
+// Returns maximal image number at which marker exists.
+int MaxImage(const vector<Marker> &all_markers) {
+ if (all_markers.size() == 0) {
+ return -1;
+ }
+
+ int max_image = all_markers[0].image;
+ for (int i = 1; i < all_markers.size(); i++) {
+ max_image = std::max(max_image, all_markers[i].image);
+ }
+ return max_image;
+}
+
+// Returns a pointer to the point corresponding to a track.
+EuclideanPoint *PointForTrack(vector<EuclideanPoint> *all_points,
+ const int track) {
+ if (track < 0 || track >= all_points->size()) {
+ return NULL;
+ }
+ EuclideanPoint *point = &(*all_points)[track];
+ if (point->track == -1) {
+ return NULL;
+ }
+ return point;
+}
+
+// Reader of binary file which makes sure possibly needed endian
+// conversion happens when loading values like floats and integers.
+//
+// File's endian type is reading from a first character of file, which
+// could either be V for big endian or v for little endian. This
+// means you need to design file format assuming first character
+// denotes file endianness in this way.
+class EndianAwareFileReader {
+ public:
+ EndianAwareFileReader(void) : file_descriptor_(-1) {
+ // Get an endian type of the host machine.
+ union {
+ unsigned char bytes[4];
+ uint32_t value;
+ } endian_test = { { 0, 1, 2, 3 } };
+ host_endian_type_ = endian_test.value;
+ file_endian_type_ = host_endian_type_;
+ }
+
+ ~EndianAwareFileReader(void) {
+ if (file_descriptor_ > 0) {
+ close(file_descriptor_);
+ }
+ }
+
+ bool OpenFile(const std::string &file_name) {
+ file_descriptor_ = open(file_name.c_str(), O_RDONLY | O_BINARY);
+ if (file_descriptor_ < 0) {
+ return false;
+ }
+ // Get an endian tpye of data in the file.
+ unsigned char file_endian_type_flag = Read<unsigned char>();
+ if (file_endian_type_flag == 'V') {
+ file_endian_type_ = kBigEndian;
+ } else if (file_endian_type_flag == 'v') {
+ file_endian_type_ = kLittleEndian;
+ } else {
+ LOG(FATAL) << "Problem file is stored in unknown endian type.";
+ }
+ return true;
+ }
+
+ // Read value from the file, will switch endian if needed.
+ template <typename T>
+ T Read(void) const {
+ T value;
+ CHECK_GT(read(file_descriptor_, &value, sizeof(value)), 0);
+ // Switch endian type if file contains data in different type
+ // that current machine.
+ if (file_endian_type_ != host_endian_type_) {
+ value = SwitchEndian<T>(value);
+ }
+ return value;
+ }
+ private:
+ static const long int kLittleEndian = 0x03020100ul;
+ static const long int kBigEndian = 0x00010203ul;
+
+ // Switch endian type between big to little.
+ template <typename T>
+ T SwitchEndian(const T value) const {
+ if (sizeof(T) == 4) {
+ unsigned int temp_value = static_cast<unsigned int>(value);
+ return ((temp_value >> 24)) |
+ ((temp_value << 8) & 0x00ff0000) |
+ ((temp_value >> 8) & 0x0000ff00) |
+ ((temp_value << 24));
+ } else if (sizeof(T) == 1) {
+ return value;
+ } else {
+ LOG(FATAL) << "Entered non-implemented part of endian switching function.";
+ }
+ }
+
+ int host_endian_type_;
+ int file_endian_type_;
+ int file_descriptor_;
+};
+
+// Read 3x3 column-major matrix from the file
+void ReadMatrix3x3(const EndianAwareFileReader &file_reader,
+ Mat3 *matrix) {
+ for (int i = 0; i < 9; i++) {
+ (*matrix)(i % 3, i / 3) = file_reader.Read<float>();
+ }
+}
+
+// Read 3-vector from file
+void ReadVector3(const EndianAwareFileReader &file_reader,
+ Vec3 *vector) {
+ for (int i = 0; i < 3; i++) {
+ (*vector)(i) = file_reader.Read<float>();
+ }
+}
+
+// Reads a bundle adjustment problem from the file.
+//
+// file_name denotes from which file to read the problem.
+// camera_intrinsics will contain initial camera intrinsics values.
+//
+// all_cameras is a vector of all reconstructed cameras to be optimized,
+// vector element with number i will contain camera for image i.
+//
+// all_points is a vector of all reconstructed 3D points to be optimized,
+// vector element with number i will contain point for track i.
+//
+// all_markers is a vector of all tracked markers existing in
+// the problem. Only used for reprojection error calculation, stay
+// unchanged during optimization.
+//
+// Returns false if any kind of error happened during
+// reading.
+bool ReadProblemFromFile(const std::string &file_name,
+ double camera_intrinsics[8],
+ vector<EuclideanCamera> *all_cameras,
+ vector<EuclideanPoint> *all_points,
+ bool *is_image_space,
+ vector<Marker> *all_markers) {
+ EndianAwareFileReader file_reader;
+ if (!file_reader.OpenFile(file_name)) {
+ return false;
+ }
+
+ // Read markers' space flag.
+ unsigned char is_image_space_flag = file_reader.Read<unsigned char>();
+ if (is_image_space_flag == 'P') {
+ *is_image_space = true;
+ } else if (is_image_space_flag == 'N') {
+ *is_image_space = false;
+ } else {
+ LOG(FATAL) << "Problem file contains markers stored in unknown space.";
+ }
+
+ // Read camera intrinsics.
+ for (int i = 0; i < 8; i++) {
+ camera_intrinsics[i] = file_reader.Read<float>();
+ }
+
+ // Read all cameras.
+ int number_of_cameras = file_reader.Read<int>();
+ for (int i = 0; i < number_of_cameras; i++) {
+ EuclideanCamera camera;
+
+ camera.image = file_reader.Read<int>();
+ ReadMatrix3x3(file_reader, &camera.R);
+ ReadVector3(file_reader, &camera.t);
+
+ if (camera.image >= all_cameras->size()) {
+ all_cameras->resize(camera.image + 1);
+ }
+
+ (*all_cameras)[camera.image].image = camera.image;
+ (*all_cameras)[camera.image].R = camera.R;
+ (*all_cameras)[camera.image].t = camera.t;
+ }
+
+ LOG(INFO) << "Read " << number_of_cameras << " cameras.";
+
+ // Read all reconstructed 3D points.
+ int number_of_points = file_reader.Read<int>();
+ for (int i = 0; i < number_of_points; i++) {
+ EuclideanPoint point;
+
+ point.track = file_reader.Read<int>();
+ ReadVector3(file_reader, &point.X);
+
+ if (point.track >= all_points->size()) {
+ all_points->resize(point.track + 1);
+ }
+
+ (*all_points)[point.track].track = point.track;
+ (*all_points)[point.track].X = point.X;
+ }
+
+ LOG(INFO) << "Read " << number_of_points << " points.";
+
+ // And finally read all markers.
+ int number_of_markers = file_reader.Read<int>();
+ for (int i = 0; i < number_of_markers; i++) {
+ Marker marker;
+
+ marker.image = file_reader.Read<int>();
+ marker.track = file_reader.Read<int>();
+ marker.x = file_reader.Read<float>();
+ marker.y = file_reader.Read<float>();
+
+ all_markers->push_back(marker);
+ }
+
+ LOG(INFO) << "Read " << number_of_markers << " markers.";
+
+ return true;
+}
+
+// Apply camera intrinsics to the normalized point to get image coordinates.
+// This applies the radial lens distortion to a point which is in normalized
+// camera coordinates (i.e. the principal point is at (0, 0)) to get image
+// coordinates in pixels. Templated for use with autodifferentiation.
+template <typename T>
+inline void ApplyRadialDistortionCameraIntrinsics(const T &focal_length_x,
+ const T &focal_length_y,
+ const T &principal_point_x,
+ const T &principal_point_y,
+ const T &k1,
+ const T &k2,
+ const T &k3,
+ const T &p1,
+ const T &p2,
+ const T &normalized_x,
+ const T &normalized_y,
+ T *image_x,
+ T *image_y) {
+ T x = normalized_x;
+ T y = normalized_y;
+
+ // Apply distortion to the normalized points to get (xd, yd).
+ T r2 = x*x + y*y;
+ T r4 = r2 * r2;
+ T r6 = r4 * r2;
+ T r_coeff = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
+ T xd = x * r_coeff + 2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x);
+ T yd = y * r_coeff + 2.0 * p2 * x * y + p1 * (r2 + 2.0 * y * y);
+
+ // Apply focal length and principal point to get the final image coordinates.
+ *image_x = focal_length_x * xd + principal_point_x;
+ *image_y = focal_length_y * yd + principal_point_y;
+}
+
+// Cost functor which computes reprojection error of 3D point X
+// on camera defined by angle-axis rotation and it's translation
+// (which are in the same block due to optimization reasons).
+//
+// This functor uses a radial distortion model.
+struct OpenCVReprojectionError {
+ OpenCVReprojectionError(const double observed_x, const double observed_y)
+ : observed_x(observed_x), observed_y(observed_y) {}
+
+ template <typename T>
+ bool operator()(const T* const intrinsics,
+ const T* const R_t, // Rotation denoted by angle axis
+ // followed with translation
+ const T* const X, // Point coordinates 3x1.
+ T* residuals) const {
+ // Unpack the intrinsics.
+ const T& focal_length = intrinsics[OFFSET_FOCAL_LENGTH];
+ const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X];
+ const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y];
+ const T& k1 = intrinsics[OFFSET_K1];
+ const T& k2 = intrinsics[OFFSET_K2];
+ const T& k3 = intrinsics[OFFSET_K3];
+ const T& p1 = intrinsics[OFFSET_P1];
+ const T& p2 = intrinsics[OFFSET_P2];
+
+ // Compute projective coordinates: x = RX + t.
+ T x[3];
+
+ ceres::AngleAxisRotatePoint(R_t, X, x);
+ x[0] += R_t[3];
+ x[1] += R_t[4];
+ x[2] += R_t[5];
+
+ // Compute normalized coordinates: x /= x[2].
+ T xn = x[0] / x[2];
+ T yn = x[1] / x[2];
+
+ T predicted_x, predicted_y;
+
+ // Apply distortion to the normalized points to get (xd, yd).
+ // TODO(keir): Do early bailouts for zero distortion; these are expensive
+ // jet operations.
+ ApplyRadialDistortionCameraIntrinsics(focal_length,
+ focal_length,
+ principal_point_x,
+ principal_point_y,
+ k1, k2, k3,
+ p1, p2,
+ xn, yn,
+ &predicted_x,
+ &predicted_y);
+
+ // The error is the difference between the predicted and observed position.
+ residuals[0] = predicted_x - observed_x;
+ residuals[1] = predicted_y - observed_y;
+
+ return true;
+ }
+
+ const double observed_x;
+ const double observed_y;
+};
+
+// Print a message to the log which camera intrinsics are gonna to be optimized.
+void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
+ if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
+ LOG(INFO) << "Bundling only camera positions.";
+ } else {
+ std::string bundling_message = "";
+
+#define APPEND_BUNDLING_INTRINSICS(name, flag) \
+ if (bundle_intrinsics & flag) { \
+ if (!bundling_message.empty()) { \
+ bundling_message += ", "; \
+ } \
+ bundling_message += name; \
+ } (void)0
+
+ APPEND_BUNDLING_INTRINSICS("f", BUNDLE_FOCAL_LENGTH);
+ APPEND_BUNDLING_INTRINSICS("px, py", BUNDLE_PRINCIPAL_POINT);
+ APPEND_BUNDLING_INTRINSICS("k1", BUNDLE_RADIAL_K1);
+ APPEND_BUNDLING_INTRINSICS("k2", BUNDLE_RADIAL_K2);
+ APPEND_BUNDLING_INTRINSICS("p1", BUNDLE_TANGENTIAL_P1);
+ APPEND_BUNDLING_INTRINSICS("p2", BUNDLE_TANGENTIAL_P2);
+
+ LOG(INFO) << "Bundling " << bundling_message << ".";
+ }
+}
+
+// Print a message to the log containing all the camera intriniscs values.
+void PrintCameraIntrinsics(const char *text, const double *camera_intrinsics) {
+ std::ostringstream intrinsics_output;
+
+ intrinsics_output << "f=" << camera_intrinsics[OFFSET_FOCAL_LENGTH];
+
+ intrinsics_output <<
+ " cx=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_X] <<
+ " cy=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_Y];
+
+#define APPEND_DISTORTION_COEFFICIENT(name, offset) \
+ { \
+ if (camera_intrinsics[offset] != 0.0) { \
+ intrinsics_output << " " name "=" << camera_intrinsics[offset]; \
+ } \
+ } (void)0
+
+ APPEND_DISTORTION_COEFFICIENT("k1", OFFSET_K1);
+ APPEND_DISTORTION_COEFFICIENT("k2", OFFSET_K2);
+ APPEND_DISTORTION_COEFFICIENT("k3", OFFSET_K3);
+ APPEND_DISTORTION_COEFFICIENT("p1", OFFSET_P1);
+ APPEND_DISTORTION_COEFFICIENT("p2", OFFSET_P2);
+
+#undef APPEND_DISTORTION_COEFFICIENT
+
+ LOG(INFO) << text << intrinsics_output.str();
+}
+
+// Get a vector of camera's rotations denoted by angle axis
+// conjuncted with translations into single block
+//
+// Element with index i matches to a rotation+translation for
+// camera at image i.
+vector<Vec6> PackCamerasRotationAndTranslation(
+ const vector<Marker> &all_markers,
+ const vector<EuclideanCamera> &all_cameras) {
+ vector<Vec6> all_cameras_R_t;
+ int max_image = MaxImage(all_markers);
+
+ all_cameras_R_t.resize(max_image + 1);
+
+ for (int i = 0; i <= max_image; i++) {
+ const EuclideanCamera *camera = CameraForImage(all_cameras, i);
+
+ if (!camera) {
+ continue;
+ }
+
+ ceres::RotationMatrixToAngleAxis(&camera->R(0, 0),
+ &all_cameras_R_t[i](0));
+ all_cameras_R_t[i].tail<3>() = camera->t;
+ }
+
+ return all_cameras_R_t;
+}
+
+// Convert cameras rotations fro mangle axis back to rotation matrix.
+void UnpackCamerasRotationAndTranslation(
+ const vector<Marker> &all_markers,
+ const vector<Vec6> &all_cameras_R_t,
+ vector<EuclideanCamera> *all_cameras) {
+ int max_image = MaxImage(all_markers);
+
+ for (int i = 0; i <= max_image; i++) {
+ EuclideanCamera *camera = CameraForImage(all_cameras, i);
+
+ if (!camera) {
+ continue;
+ }
+
+ ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0),
+ &camera->R(0, 0));
+ camera->t = all_cameras_R_t[i].tail<3>();
+ }
+}
+
+void EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers,
+ const int bundle_intrinsics,
+ const int bundle_constraints,
+ double *camera_intrinsics,
+ vector<EuclideanCamera> *all_cameras,
+ vector<EuclideanPoint> *all_points) {
+ PrintCameraIntrinsics("Original intrinsics: ", camera_intrinsics);
+
+ ceres::Problem::Options problem_options;
+ ceres::Problem problem(problem_options);
+
+ // Convert cameras rotations to angle axis and merge with translation
+ // into single parameter block for maximal minimization speed
+ //
+ // Block for minimization has got the following structure:
+ // <3 elements for angle-axis> <3 elements for translation>
+ vector<Vec6> all_cameras_R_t =
+ PackCamerasRotationAndTranslation(all_markers, *all_cameras);
+
+ // Parameterization used to restrict camera motion for modal solvers.
+ ceres::SubsetParameterization *constant_transform_parameterization = NULL;
+ if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
+ std::vector<int> constant_translation;
+
+ // First three elements are rotation, last three are translation.
+ constant_translation.push_back(3);
+ constant_translation.push_back(4);
+ constant_translation.push_back(5);
+
+ constant_transform_parameterization =
+ new ceres::SubsetParameterization(6, constant_translation);
+ }
+
+ int num_residuals = 0;
+ bool have_locked_camera = false;
+ for (int i = 0; i < all_markers.size(); ++i) {
+ const Marker &marker = all_markers[i];
+ EuclideanCamera *camera = CameraForImage(all_cameras, marker.image);
+ EuclideanPoint *point = PointForTrack(all_points, marker.track);
+ if (camera == NULL || point == NULL) {
+ continue;
+ }
+
+ // Rotation of camera denoted in angle axis followed with
+ // camera translaiton.
+ double *current_camera_R_t = &all_cameras_R_t[camera->image](0);
+
+ problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
+ OpenCVReprojectionError, 2, 8, 6, 3>(
+ new OpenCVReprojectionError(
+ marker.x,
+ marker.y)),
+ NULL,
+ camera_intrinsics,
+ current_camera_R_t,
+ &point->X(0));
+
+ // We lock the first camera to better deal with scene orientation ambiguity.
+ if (!have_locked_camera) {
+ problem.SetParameterBlockConstant(current_camera_R_t);
+ have_locked_camera = true;
+ }
+
+ if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
+ problem.SetParameterization(current_camera_R_t,
+ constant_transform_parameterization);
+ }
+
+ num_residuals++;
+ }
+ LOG(INFO) << "Number of residuals: " << num_residuals;
+
+ if (!num_residuals) {
+ LOG(INFO) << "Skipping running minimizer with zero residuals";
+ return;
+ }
+
+ BundleIntrinsicsLogMessage(bundle_intrinsics);
+
+ if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
+ // No camera intrinsics are being refined,
+ // set the whole parameter block as constant for best performance.
+ problem.SetParameterBlockConstant(camera_intrinsics);
+ } else {
+ // Set the camera intrinsics that are not to be bundled as
+ // constant using some macro trickery.
+
+ std::vector<int> constant_intrinsics;
+#define MAYBE_SET_CONSTANT(bundle_enum, offset) \
+ if (!(bundle_intrinsics & bundle_enum)) { \
+ constant_intrinsics.push_back(offset); \
+ }
+ MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH, OFFSET_FOCAL_LENGTH);
+ MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X);
+ MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y);
+ MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1, OFFSET_K1);
+ MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2, OFFSET_K2);
+ MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1, OFFSET_P1);
+ MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2, OFFSET_P2);
+#undef MAYBE_SET_CONSTANT
+
+ // Always set K3 constant, it's not used at the moment.
+ constant_intrinsics.push_back(OFFSET_K3);
+
+ ceres::SubsetParameterization *subset_parameterization =
+ new ceres::SubsetParameterization(8, constant_intrinsics);
+
+ problem.SetParameterization(camera_intrinsics, subset_parameterization);
+ }
+
+ // Configure the solver.
+ ceres::Solver::Options options;
+ options.use_nonmonotonic_steps = true;
+ options.preconditioner_type = ceres::SCHUR_JACOBI;
+ options.linear_solver_type = ceres::ITERATIVE_SCHUR;
+ options.use_inner_iterations = true;
+ options.max_num_iterations = 100;
+ options.minimizer_progress_to_stdout = true;
+
+ // Solve!
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+
+ std::cout << "Final report:\n" << summary.FullReport();
+
+ // Copy rotations and translations back.
+ UnpackCamerasRotationAndTranslation(all_markers,
+ all_cameras_R_t,
+ all_cameras);
+
+ PrintCameraIntrinsics("Final intrinsics: ", camera_intrinsics);
+}
+} // namespace
+
+int main(int argc, char **argv) {
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+ google::InitGoogleLogging(argv[0]);
+
+ if (FLAGS_input.empty()) {
+ LOG(ERROR) << "Usage: libmv_bundle_adjuster --input=blender_problem";
+ return EXIT_FAILURE;
+ }
+
+ double camera_intrinsics[8];
+ vector<EuclideanCamera> all_cameras;
+ vector<EuclideanPoint> all_points;
+ bool is_image_space;
+ vector<Marker> all_markers;
+
+ if (!ReadProblemFromFile(FLAGS_input,
+ camera_intrinsics,
+ &all_cameras,
+ &all_points,
+ &is_image_space,
+ &all_markers)) {
+ LOG(ERROR) << "Error reading problem file";
+ return EXIT_FAILURE;
+ }
+
+ // If there's no refine_intrinsics passed via command line
+ // (in this case FLAGS_refine_intrinsics will be an empty string)
+ // we use problem's settings to detect whether intrinsics
+ // shall be refined or not.
+ //
+ // Namely, if problem has got markers stored in image (pixel)
+ // space, we do full intrinsics refinement. If markers are
+ // stored in normalized space, and refine_intrinsics is not
+ // set, no refining will happen.
+ //
+ // Using command line argument refine_intrinsics will explicitly
+ // declare which intrinsics need to be refined and in this case
+ // refining flags does not depend on problem at all.
+ int bundle_intrinsics = BUNDLE_NO_INTRINSICS;
+ if (FLAGS_refine_intrinsics.empty()) {
+ if (is_image_space) {
+ bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
+ }
+ } else {
+ if (FLAGS_refine_intrinsics == "radial") {
+ bundle_intrinsics = BUNDLE_FOCAL_LENGTH | BUNDLE_RADIAL;
+ } else if (FLAGS_refine_intrinsics != "none") {
+ LOG(ERROR) << "Unsupported value for refine-intrinsics";
+ return EXIT_FAILURE;
+ }
+ }
+
+ // Run the bundler.
+ EuclideanBundleCommonIntrinsics(all_markers,
+ bundle_intrinsics,
+ BUNDLE_NO_CONSTRAINTS,
+ camera_intrinsics,
+ &all_cameras,
+ &all_points);
+
+ return EXIT_SUCCESS;
+}
diff --git a/examples/libmv_homography.cc b/examples/libmv_homography.cc
new file mode 100644
index 0000000..fe647da
--- /dev/null
+++ b/examples/libmv_homography.cc
@@ -0,0 +1,414 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2014 libmv authors.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+//
+// Author: sergey.vfx@gmail.com (Sergey Sharybin)
+//
+// This file demonstrates solving for a homography between two sets of points.
+// A homography describes a transformation between a sets of points on a plane,
+// perspectively projected into two images. The first step is to solve a
+// homogeneous system of equations via singular value decomposition, giving an
+// algebraic solution for the homography, then solving for a final solution by
+// minimizing the symmetric transfer error in image space with Ceres (called the
+// Gold Standard Solution in "Multiple View Geometry"). The routines are based on
+// the routines from the Libmv library.
+//
+// This example demonstrates custom exit criterion by having a callback check
+// for image-space error.
+
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+typedef Eigen::NumTraits<double> EigenDouble;
+
+typedef Eigen::MatrixXd Mat;
+typedef Eigen::VectorXd Vec;
+typedef Eigen::Matrix<double, 3, 3> Mat3;
+typedef Eigen::Matrix<double, 2, 1> Vec2;
+typedef Eigen::Matrix<double, Eigen::Dynamic, 8> MatX8;
+typedef Eigen::Vector3d Vec3;
+
+namespace {
+
+// This structure contains options that controls how the homography
+// estimation operates.
+//
+// Defaults should be suitable for a wide range of use cases, but
+// better performance and accuracy might require tweaking.
+struct EstimateHomographyOptions {
+ // Default settings for homography estimation which should be suitable
+ // for a wide range of use cases.
+ EstimateHomographyOptions()
+ : max_num_iterations(50),
+ expected_average_symmetric_distance(1e-16) {}
+
+ // Maximal number of iterations for the refinement step.
+ int max_num_iterations;
+
+ // Expected average of symmetric geometric distance between
+ // actual destination points and original ones transformed by
+ // estimated homography matrix.
+ //
+ // Refinement will finish as soon as average of symmetric
+ // geometric distance is less or equal to this value.
+ //
+ // This distance is measured in the same units as input points are.
+ double expected_average_symmetric_distance;
+};
+
+// Calculate symmetric geometric cost terms:
+//
+// forward_error = D(H * x1, x2)
+// backward_error = D(H^-1 * x2, x1)
+//
+// Templated to be used with autodifferentiation.
+template <typename T>
+void SymmetricGeometricDistanceTerms(const Eigen::Matrix<T, 3, 3> &H,
+ const Eigen::Matrix<T, 2, 1> &x1,
+ const Eigen::Matrix<T, 2, 1> &x2,
+ T forward_error[2],
+ T backward_error[2]) {
+ typedef Eigen::Matrix<T, 3, 1> Vec3;
+ Vec3 x(x1(0), x1(1), T(1.0));
+ Vec3 y(x2(0), x2(1), T(1.0));
+
+ Vec3 H_x = H * x;
+ Vec3 Hinv_y = H.inverse() * y;
+
+ H_x /= H_x(2);
+ Hinv_y /= Hinv_y(2);
+
+ forward_error[0] = H_x(0) - y(0);
+ forward_error[1] = H_x(1) - y(1);
+ backward_error[0] = Hinv_y(0) - x(0);
+ backward_error[1] = Hinv_y(1) - x(1);
+}
+
+// Calculate symmetric geometric cost:
+//
+// D(H * x1, x2)^2 + D(H^-1 * x2, x1)^2
+//
+double SymmetricGeometricDistance(const Mat3 &H,
+ const Vec2 &x1,
+ const Vec2 &x2) {
+ Vec2 forward_error, backward_error;
+ SymmetricGeometricDistanceTerms<double>(H,
+ x1,
+ x2,
+ forward_error.data(),
+ backward_error.data());
+ return forward_error.squaredNorm() +
+ backward_error.squaredNorm();
+}
+
+// A parameterization of the 2D homography matrix that uses 8 parameters so
+// that the matrix is normalized (H(2,2) == 1).
+// The homography matrix H is built from a list of 8 parameters (a, b,...g, h)
+// as follows
+//
+// |a b c|
+// H = |d e f|
+// |g h 1|
+//
+template<typename T = double>
+class Homography2DNormalizedParameterization {
+ public:
+ typedef Eigen::Matrix<T, 8, 1> Parameters; // a, b, ... g, h
+ typedef Eigen::Matrix<T, 3, 3> Parameterized; // H
+
+ // Convert from the 8 parameters to a H matrix.
+ static void To(const Parameters &p, Parameterized *h) {
+ *h << p(0), p(1), p(2),
+ p(3), p(4), p(5),
+ p(6), p(7), 1.0;
+ }
+
+ // Convert from a H matrix to the 8 parameters.
+ static void From(const Parameterized &h, Parameters *p) {
+ *p << h(0, 0), h(0, 1), h(0, 2),
+ h(1, 0), h(1, 1), h(1, 2),
+ h(2, 0), h(2, 1);
+ }
+};
+
+// 2D Homography transformation estimation in the case that points are in
+// euclidean coordinates.
+//
+// x = H y
+//
+// x and y vector must have the same direction, we could write
+//
+// crossproduct(|x|, * H * |y| ) = |0|
+//
+// | 0 -1 x2| |a b c| |y1| |0|
+// | 1 0 -x1| * |d e f| * |y2| = |0|
+// |-x2 x1 0| |g h 1| |1 | |0|
+//
+// That gives:
+//
+// (-d+x2*g)*y1 + (-e+x2*h)*y2 + -f+x2 |0|
+// (a-x1*g)*y1 + (b-x1*h)*y2 + c-x1 = |0|
+// (-x2*a+x1*d)*y1 + (-x2*b+x1*e)*y2 + -x2*c+x1*f |0|
+//
+bool Homography2DFromCorrespondencesLinearEuc(
+ const Mat &x1,
+ const Mat &x2,
+ Mat3 *H,
+ double expected_precision) {
+ assert(2 == x1.rows());
+ assert(4 <= x1.cols());
+ assert(x1.rows() == x2.rows());
+ assert(x1.cols() == x2.cols());
+
+ int n = x1.cols();
+ MatX8 L = Mat::Zero(n * 3, 8);
+ Mat b = Mat::Zero(n * 3, 1);
+ for (int i = 0; i < n; ++i) {
+ int j = 3 * i;
+ L(j, 0) = x1(0, i); // a
+ L(j, 1) = x1(1, i); // b
+ L(j, 2) = 1.0; // c
+ L(j, 6) = -x2(0, i) * x1(0, i); // g
+ L(j, 7) = -x2(0, i) * x1(1, i); // h
+ b(j, 0) = x2(0, i); // i
+
+ ++j;
+ L(j, 3) = x1(0, i); // d
+ L(j, 4) = x1(1, i); // e
+ L(j, 5) = 1.0; // f
+ L(j, 6) = -x2(1, i) * x1(0, i); // g
+ L(j, 7) = -x2(1, i) * x1(1, i); // h
+ b(j, 0) = x2(1, i); // i
+
+ // This ensures better stability
+ // TODO(julien) make a lite version without this 3rd set
+ ++j;
+ L(j, 0) = x2(1, i) * x1(0, i); // a
+ L(j, 1) = x2(1, i) * x1(1, i); // b
+ L(j, 2) = x2(1, i); // c
+ L(j, 3) = -x2(0, i) * x1(0, i); // d
+ L(j, 4) = -x2(0, i) * x1(1, i); // e
+ L(j, 5) = -x2(0, i); // f
+ }
+ // Solve Lx=B
+ const Vec h = L.fullPivLu().solve(b);
+ Homography2DNormalizedParameterization<double>::To(h, H);
+ return (L * h).isApprox(b, expected_precision);
+}
+
+// Cost functor which computes symmetric geometric distance
+// used for homography matrix refinement.
+class HomographySymmetricGeometricCostFunctor {
+ public:
+ HomographySymmetricGeometricCostFunctor(const Vec2 &x,
+ const Vec2 &y)
+ : x_(x), y_(y) { }
+
+ template<typename T>
+ bool operator()(const T* homography_parameters, T* residuals) const {
+ typedef Eigen::Matrix<T, 3, 3> Mat3;
+ typedef Eigen::Matrix<T, 2, 1> Vec2;
+
+ Mat3 H(homography_parameters);
+ Vec2 x(T(x_(0)), T(x_(1)));
+ Vec2 y(T(y_(0)), T(y_(1)));
+
+ SymmetricGeometricDistanceTerms<T>(H,
+ x,
+ y,
+ &residuals[0],
+ &residuals[2]);
+ return true;
+ }
+
+ const Vec2 x_;
+ const Vec2 y_;
+};
+
+// Termination checking callback. This is needed to finish the
+// optimization when an absolute error threshold is met, as opposed
+// to Ceres's function_tolerance, which provides for finishing when
+// successful steps reduce the cost function by a fractional amount.
+// In this case, the callback checks for the absolute average reprojection
+// error and terminates when it's below a threshold (for example all
+// points < 0.5px error).
+class TerminationCheckingCallback : public ceres::IterationCallback {
+ public:
+ TerminationCheckingCallback(const Mat &x1, const Mat &x2,
+ const EstimateHomographyOptions &options,
+ Mat3 *H)
+ : options_(options), x1_(x1), x2_(x2), H_(H) {}
+
+ virtual ceres::CallbackReturnType operator()(
+ const ceres::IterationSummary& summary) {
+ // If the step wasn't successful, there's nothing to do.
+ if (!summary.step_is_successful) {
+ return ceres::SOLVER_CONTINUE;
+ }
+
+ // Calculate average of symmetric geometric distance.
+ double average_distance = 0.0;
+ for (int i = 0; i < x1_.cols(); i++) {
+ average_distance += SymmetricGeometricDistance(*H_,
+ x1_.col(i),
+ x2_.col(i));
+ }
+ average_distance /= x1_.cols();
+
+ if (average_distance <= options_.expected_average_symmetric_distance) {
+ return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
+ }
+
+ return ceres::SOLVER_CONTINUE;
+ }
+
+ private:
+ const EstimateHomographyOptions &options_;
+ const Mat &x1_;
+ const Mat &x2_;
+ Mat3 *H_;
+};
+
+bool EstimateHomography2DFromCorrespondences(
+ const Mat &x1,
+ const Mat &x2,
+ const EstimateHomographyOptions &options,
+ Mat3 *H) {
+ assert(2 == x1.rows());
+ assert(4 <= x1.cols());
+ assert(x1.rows() == x2.rows());
+ assert(x1.cols() == x2.cols());
+
+ // Step 1: Algebraic homography estimation.
+ // Assume algebraic estimation always succeeds.
+ Homography2DFromCorrespondencesLinearEuc(x1,
+ x2,
+ H,
+ EigenDouble::dummy_precision());
+
+ LOG(INFO) << "Estimated matrix after algebraic estimation:\n" << *H;
+
+ // Step 2: Refine matrix using Ceres minimizer.
+ ceres::Problem problem;
+ for (int i = 0; i < x1.cols(); i++) {
+ HomographySymmetricGeometricCostFunctor
+ *homography_symmetric_geometric_cost_function =
+ new HomographySymmetricGeometricCostFunctor(x1.col(i),
+ x2.col(i));
+
+ problem.AddResidualBlock(
+ new ceres::AutoDiffCostFunction<
+ HomographySymmetricGeometricCostFunctor,
+ 4, // num_residuals
+ 9>(homography_symmetric_geometric_cost_function),
+ NULL,
+ H->data());
+ }
+
+ // Configure the solve.
+ ceres::Solver::Options solver_options;
+ solver_options.linear_solver_type = ceres::DENSE_QR;
+ solver_options.max_num_iterations = options.max_num_iterations;
+ solver_options.update_state_every_iteration = true;
+
+ // Terminate if the average symmetric distance is good enough.
+ TerminationCheckingCallback callback(x1, x2, options, H);
+ solver_options.callbacks.push_back(&callback);
+
+ // Run the solve.
+ ceres::Solver::Summary summary;
+ ceres::Solve(solver_options, &problem, &summary);
+
+ LOG(INFO) << "Summary:\n" << summary.FullReport();
+ LOG(INFO) << "Final refined matrix:\n" << *H;
+
+ return summary.IsSolutionUsable();
+}
+
+} // namespace libmv
+
+int main(int argc, char **argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ Mat x1(2, 100);
+ for (int i = 0; i < x1.cols(); ++i) {
+ x1(0, i) = rand() % 1024;
+ x1(1, i) = rand() % 1024;
+ }
+
+ Mat3 homography_matrix;
+ // This matrix has been dumped from a Blender test file of plane tracking.
+ homography_matrix << 1.243715, -0.461057, -111.964454,
+ 0.0, 0.617589, -192.379252,
+ 0.0, -0.000983, 1.0;
+
+ Mat x2 = x1;
+ for (int i = 0; i < x2.cols(); ++i) {
+ Vec3 homogenous_x1 = Vec3(x1(0, i), x1(1, i), 1.0);
+ Vec3 homogenous_x2 = homography_matrix * homogenous_x1;
+ x2(0, i) = homogenous_x2(0) / homogenous_x2(2);
+ x2(1, i) = homogenous_x2(1) / homogenous_x2(2);
+
+ // Apply some noise so algebraic estimation is not good enough.
+ x2(0, i) += static_cast<double>(rand() % 1000) / 5000.0;
+ x2(1, i) += static_cast<double>(rand() % 1000) / 5000.0;
+ }
+
+ Mat3 estimated_matrix;
+
+ EstimateHomographyOptions options;
+ options.expected_average_symmetric_distance = 0.02;
+ EstimateHomography2DFromCorrespondences(x1, x2, options, &estimated_matrix);
+
+ // Normalize the matrix for easier comparison.
+ estimated_matrix /= estimated_matrix(2 ,2);
+
+ std::cout << "Original matrix:\n" << homography_matrix << "\n";
+ std::cout << "Estimated matrix:\n" << estimated_matrix << "\n";
+
+ return EXIT_SUCCESS;
+}
diff --git a/examples/more_garbow_hillstrom.cc b/examples/more_garbow_hillstrom.cc
new file mode 100644
index 0000000..02169cd
--- /dev/null
+++ b/examples/more_garbow_hillstrom.cc
@@ -0,0 +1,674 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// Test problems from the paper
+//
+// Testing Unconstrained Optimization Software
+// Jorge J. More, Burton S. Garbow and Kenneth E. Hillstrom
+// ACM Transactions on Mathematical Software, 7(1), pp. 17-41, 1981
+//
+// A subset of these problems were augmented with bounds and used for
+// testing bounds constrained optimization algorithms by
+//
+// A Trust Region Approach to Linearly Constrained Optimization
+// David M. Gay
+// Numerical Analysis (Griffiths, D.F., ed.), pp. 72-105
+// Lecture Notes in Mathematics 1066, Springer Verlag, 1984.
+//
+// The latter paper is behind a paywall. We obtained the bounds on the
+// variables and the function values at the global minimums from
+//
+// http://www.mat.univie.ac.at/~neum/glopt/bounds.html
+//
+// A problem is considered solved if of the log relative error of its
+// objective function is at least 4.
+
+
+#include <cmath>
+#include <iostream> // NOLINT
+#include <sstream> // NOLINT
+#include <string>
+#include "ceres/ceres.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_string(problem, "all", "Which problem to solve");
+DEFINE_bool(use_numeric_diff, false,
+ "Use numeric differentiation instead of automatic "
+ "differentiation.");
+DEFINE_string(numeric_diff_method, "ridders", "When using numeric "
+ "differentiation, selects algorithm. Options are: central, "
+ "forward, ridders.");
+DEFINE_int32(ridders_extrapolations, 3, "Maximal number of extrapolations in "
+ "Ridders' method.");
+
+namespace ceres {
+namespace examples {
+
+const double kDoubleMax = std::numeric_limits<double>::max();
+
+static void SetNumericDiffOptions(ceres::NumericDiffOptions* options) {
+ options->max_num_ridders_extrapolations = FLAGS_ridders_extrapolations;
+}
+
+#define BEGIN_MGH_PROBLEM(name, num_parameters, num_residuals) \
+ struct name { \
+ static const int kNumParameters = num_parameters; \
+ static const double initial_x[kNumParameters]; \
+ static const double lower_bounds[kNumParameters]; \
+ static const double upper_bounds[kNumParameters]; \
+ static const double constrained_optimal_cost; \
+ static const double unconstrained_optimal_cost; \
+ static CostFunction* Create() { \
+ if (FLAGS_use_numeric_diff) { \
+ ceres::NumericDiffOptions options; \
+ SetNumericDiffOptions(&options); \
+ if (FLAGS_numeric_diff_method == "central") { \
+ return new NumericDiffCostFunction<name, \
+ ceres::CENTRAL, \
+ num_residuals, \
+ num_parameters>( \
+ new name, ceres::TAKE_OWNERSHIP, num_residuals, options); \
+ } else if (FLAGS_numeric_diff_method == "forward") { \
+ return new NumericDiffCostFunction<name, \
+ ceres::FORWARD, \
+ num_residuals, \
+ num_parameters>( \
+ new name, ceres::TAKE_OWNERSHIP, num_residuals, options); \
+ } else if (FLAGS_numeric_diff_method == "ridders") { \
+ return new NumericDiffCostFunction<name, \
+ ceres::RIDDERS, \
+ num_residuals, \
+ num_parameters>( \
+ new name, ceres::TAKE_OWNERSHIP, num_residuals, options); \
+ } else { \
+ LOG(ERROR) << "Invalid numeric diff method specified"; \
+ return NULL; \
+ } \
+ } else { \
+ return new AutoDiffCostFunction<name, \
+ num_residuals, \
+ num_parameters>(new name); \
+ } \
+ } \
+ template <typename T> \
+ bool operator()(const T* const x, T* residual) const {
+
+#define END_MGH_PROBLEM return true; } }; // NOLINT
+
+// Rosenbrock function.
+BEGIN_MGH_PROBLEM(TestProblem1, 2, 2)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ residual[0] = 10.0 * (x2 - x1 * x1);
+ residual[1] = 1.0 - x1;
+END_MGH_PROBLEM;
+
+const double TestProblem1::initial_x[] = {-1.2, 1.0};
+const double TestProblem1::lower_bounds[] = {-kDoubleMax, -kDoubleMax};
+const double TestProblem1::upper_bounds[] = {kDoubleMax, kDoubleMax};
+const double TestProblem1::constrained_optimal_cost =
+ std::numeric_limits<double>::quiet_NaN();
+const double TestProblem1::unconstrained_optimal_cost = 0.0;
+
+// Freudenstein and Roth function.
+BEGIN_MGH_PROBLEM(TestProblem2, 2, 2)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ residual[0] = -13.0 + x1 + ((5.0 - x2) * x2 - 2.0) * x2;
+ residual[1] = -29.0 + x1 + ((x2 + 1.0) * x2 - 14.0) * x2;
+END_MGH_PROBLEM;
+
+const double TestProblem2::initial_x[] = {0.5, -2.0};
+const double TestProblem2::lower_bounds[] = {-kDoubleMax, -kDoubleMax};
+const double TestProblem2::upper_bounds[] = {kDoubleMax, kDoubleMax};
+const double TestProblem2::constrained_optimal_cost =
+ std::numeric_limits<double>::quiet_NaN();
+const double TestProblem2::unconstrained_optimal_cost = 0.0;
+
+// Powell badly scaled function.
+BEGIN_MGH_PROBLEM(TestProblem3, 2, 2)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ residual[0] = 10000.0 * x1 * x2 - 1.0;
+ residual[1] = exp(-x1) + exp(-x2) - 1.0001;
+END_MGH_PROBLEM;
+
+const double TestProblem3::initial_x[] = {0.0, 1.0};
+const double TestProblem3::lower_bounds[] = {0.0, 1.0};
+const double TestProblem3::upper_bounds[] = {1.0, 9.0};
+const double TestProblem3::constrained_optimal_cost = 0.15125900e-9;
+const double TestProblem3::unconstrained_optimal_cost = 0.0;
+
+// Brown badly scaled function.
+BEGIN_MGH_PROBLEM(TestProblem4, 2, 3)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ residual[0] = x1 - 1000000.0;
+ residual[1] = x2 - 0.000002;
+ residual[2] = x1 * x2 - 2.0;
+END_MGH_PROBLEM;
+
+const double TestProblem4::initial_x[] = {1.0, 1.0};
+const double TestProblem4::lower_bounds[] = {0.0, 0.00003};
+const double TestProblem4::upper_bounds[] = {1000000.0, 100.0};
+const double TestProblem4::constrained_optimal_cost = 0.78400000e3;
+const double TestProblem4::unconstrained_optimal_cost = 0.0;
+
+// Beale function.
+BEGIN_MGH_PROBLEM(TestProblem5, 2, 3)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ residual[0] = 1.5 - x1 * (1.0 - x2);
+ residual[1] = 2.25 - x1 * (1.0 - x2 * x2);
+ residual[2] = 2.625 - x1 * (1.0 - x2 * x2 * x2);
+END_MGH_PROBLEM;
+
+const double TestProblem5::initial_x[] = {1.0, 1.0};
+const double TestProblem5::lower_bounds[] = {0.6, 0.5};
+const double TestProblem5::upper_bounds[] = {10.0, 100.0};
+const double TestProblem5::constrained_optimal_cost = 0.0;
+const double TestProblem5::unconstrained_optimal_cost = 0.0;
+
+// Jennrich and Sampson function.
+BEGIN_MGH_PROBLEM(TestProblem6, 2, 10)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ for (int i = 1; i <= 10; ++i) {
+ residual[i - 1] = 2.0 + 2.0 * i -
+ (exp(static_cast<double>(i) * x1) +
+ exp(static_cast<double>(i) * x2));
+ }
+END_MGH_PROBLEM;
+
+const double TestProblem6::initial_x[] = {1.0, 1.0};
+const double TestProblem6::lower_bounds[] = {-kDoubleMax, -kDoubleMax};
+const double TestProblem6::upper_bounds[] = {kDoubleMax, kDoubleMax};
+const double TestProblem6::constrained_optimal_cost =
+ std::numeric_limits<double>::quiet_NaN();
+const double TestProblem6::unconstrained_optimal_cost = 124.362;
+
+// Helical valley function.
+BEGIN_MGH_PROBLEM(TestProblem7, 3, 3)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+ const T theta = (0.5 / M_PI) * atan(x2 / x1) + (x1 > 0.0 ? 0.0 : 0.5);
+ residual[0] = 10.0 * (x3 - 10.0 * theta);
+ residual[1] = 10.0 * (sqrt(x1 * x1 + x2 * x2) - 1.0);
+ residual[2] = x3;
+END_MGH_PROBLEM;
+
+const double TestProblem7::initial_x[] = {-1.0, 0.0, 0.0};
+const double TestProblem7::lower_bounds[] = {-100.0, -1.0, -1.0};
+const double TestProblem7::upper_bounds[] = {0.8, 1.0, 1.0};
+const double TestProblem7::constrained_optimal_cost = 0.99042212;
+const double TestProblem7::unconstrained_optimal_cost = 0.0;
+
+// Bard function
+BEGIN_MGH_PROBLEM(TestProblem8, 3, 15)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+
+ double y[] = {0.14, 0.18, 0.22, 0.25,
+ 0.29, 0.32, 0.35, 0.39, 0.37, 0.58,
+ 0.73, 0.96, 1.34, 2.10, 4.39};
+
+ for (int i = 1; i <=15; ++i) {
+ const double u = i;
+ const double v = 16 - i;
+ const double w = std::min(i, 16 - i);
+ residual[i - 1] = y[i - 1] - (x1 + u / (v * x2 + w * x3));
+ }
+END_MGH_PROBLEM;
+
+const double TestProblem8::initial_x[] = {1.0, 1.0, 1.0};
+const double TestProblem8::lower_bounds[] = {
+ -kDoubleMax, -kDoubleMax, -kDoubleMax};
+const double TestProblem8::upper_bounds[] = {
+ kDoubleMax, kDoubleMax, kDoubleMax};
+const double TestProblem8::constrained_optimal_cost =
+ std::numeric_limits<double>::quiet_NaN();
+const double TestProblem8::unconstrained_optimal_cost = 8.21487e-3;
+
+// Gaussian function.
+BEGIN_MGH_PROBLEM(TestProblem9, 3, 15)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+
+ const double y[] = {0.0009, 0.0044, 0.0175, 0.0540, 0.1295, 0.2420, 0.3521,
+ 0.3989,
+ 0.3521, 0.2420, 0.1295, 0.0540, 0.0175, 0.0044, 0.0009};
+ for (int i = 0; i < 15; ++i) {
+ const double t_i = (8.0 - i - 1.0) / 2.0;
+ residual[i] = x1 * exp(-x2 * (t_i - x3) * (t_i - x3) / 2.0) - y[i];
+ }
+END_MGH_PROBLEM;
+
+const double TestProblem9::initial_x[] = {0.4, 1.0, 0.0};
+const double TestProblem9::lower_bounds[] = {0.398, 1.0, -0.5};
+const double TestProblem9::upper_bounds[] = {4.2, 2.0, 0.1};
+const double TestProblem9::constrained_optimal_cost = 0.11279300e-7;
+const double TestProblem9::unconstrained_optimal_cost = 0.112793e-7;
+
+// Meyer function.
+BEGIN_MGH_PROBLEM(TestProblem10, 3, 16)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+
+ const double y[] = {34780, 28610, 23650, 19630, 16370, 13720, 11540, 9744,
+ 8261, 7030, 6005, 5147, 4427, 3820, 3307, 2872};
+
+ for (int i = 0; i < 16; ++i) {
+ const double ti = 45.0 + 5.0 * (i + 1);
+ residual[i] = x1 * exp(x2 / (ti + x3)) - y[i];
+ }
+END_MGH_PROBLEM
+
+const double TestProblem10::initial_x[] = {0.02, 4000, 250};
+const double TestProblem10::lower_bounds[] = {
+ -kDoubleMax, -kDoubleMax, -kDoubleMax};
+const double TestProblem10::upper_bounds[] = {
+ kDoubleMax, kDoubleMax, kDoubleMax};
+const double TestProblem10::constrained_optimal_cost =
+ std::numeric_limits<double>::quiet_NaN();
+const double TestProblem10::unconstrained_optimal_cost = 87.9458;
+
+// Gulf research and development function
+BEGIN_MGH_PROBLEM(TestProblem11, 3, 100)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+ for (int i = 1; i <= 100; ++i) {
+ const double ti = i / 100.0;
+ const double yi = 25.0 + pow(-50.0 * log(ti), 2.0 / 3.0);
+ residual[i - 1] = exp(-pow(abs((yi * 100.0 * i) * x2), x3) / x1) - ti;
+ }
+END_MGH_PROBLEM
+
+const double TestProblem11::initial_x[] = {5.0, 2.5, 0.15};
+const double TestProblem11::lower_bounds[] = {1e-16, 0.0, 0.0};
+const double TestProblem11::upper_bounds[] = {10.0, 10.0, 10.0};
+const double TestProblem11::constrained_optimal_cost = 0.58281431e-4;
+const double TestProblem11::unconstrained_optimal_cost = 0.0;
+
+// Box three-dimensional function.
+BEGIN_MGH_PROBLEM(TestProblem12, 3, 3)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+
+ const double t1 = 0.1;
+ const double t2 = 0.2;
+ const double t3 = 0.3;
+
+ residual[0] = exp(-t1 * x1) - exp(-t1 * x2) - x3 * (exp(-t1) - exp(-10.0 * t1));
+ residual[1] = exp(-t2 * x1) - exp(-t2 * x2) - x3 * (exp(-t2) - exp(-10.0 * t2));
+ residual[2] = exp(-t3 * x1) - exp(-t3 * x2) - x3 * (exp(-t3) - exp(-10.0 * t3));
+END_MGH_PROBLEM
+
+const double TestProblem12::initial_x[] = {0.0, 10.0, 20.0};
+const double TestProblem12::lower_bounds[] = {0.0, 5.0, 0.0};
+const double TestProblem12::upper_bounds[] = {2.0, 9.5, 20.0};
+const double TestProblem12::constrained_optimal_cost = 0.30998153e-5;
+const double TestProblem12::unconstrained_optimal_cost = 0.0;
+
+// Powell Singular function.
+BEGIN_MGH_PROBLEM(TestProblem13, 4, 4)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+ const T x4 = x[3];
+
+ residual[0] = x1 + 10.0 * x2;
+ residual[1] = sqrt(5.0) * (x3 - x4);
+ residual[2] = (x2 - 2.0 * x3) * (x2 - 2.0 * x3);
+ residual[3] = sqrt(10.0) * (x1 - x4) * (x1 - x4);
+END_MGH_PROBLEM
+
+const double TestProblem13::initial_x[] = {3.0, -1.0, 0.0, 1.0};
+const double TestProblem13::lower_bounds[] = {
+ -kDoubleMax, -kDoubleMax, -kDoubleMax};
+const double TestProblem13::upper_bounds[] = {
+ kDoubleMax, kDoubleMax, kDoubleMax};
+const double TestProblem13::constrained_optimal_cost =
+ std::numeric_limits<double>::quiet_NaN();
+const double TestProblem13::unconstrained_optimal_cost = 0.0;
+
+// Wood function.
+BEGIN_MGH_PROBLEM(TestProblem14, 4, 6)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+ const T x4 = x[3];
+
+ residual[0] = 10.0 * (x2 - x1 * x1);
+ residual[1] = 1.0 - x1;
+ residual[2] = sqrt(90.0) * (x4 - x3 * x3);
+ residual[3] = 1.0 - x3;
+ residual[4] = sqrt(10.0) * (x2 + x4 - 2.0);
+ residual[5] = 1.0 / sqrt(10.0) * (x2 - x4);
+END_MGH_PROBLEM;
+
+ const double TestProblem14::initial_x[] = {-3.0, -1.0, -3.0, -1.0};
+ const double TestProblem14::lower_bounds[] = {-100.0, -100.0, -100.0, -100.0};
+ const double TestProblem14::upper_bounds[] = {0.0, 10.0, 100.0, 100.0};
+ const double TestProblem14::constrained_optimal_cost = 0.15567008e1;
+ const double TestProblem14::unconstrained_optimal_cost = 0.0;
+
+ // Kowalik and Osborne function.
+ BEGIN_MGH_PROBLEM(TestProblem15, 4, 11)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+ const T x4 = x[3];
+
+ const double y[] = {0.1957, 0.1947, 0.1735, 0.1600, 0.0844, 0.0627,
+ 0.0456, 0.0342, 0.0323, 0.0235, 0.0246};
+ const double u[] = {4.0, 2.0, 1.0, 0.5, 0.25, 0.167, 0.125, 0.1,
+ 0.0833, 0.0714, 0.0625};
+
+ for (int i = 0; i < 11; ++i) {
+ residual[i] = y[i] - x1 * (u[i] * u[i] + u[i] * x2) /
+ (u[i] * u[i] + u[i] * x3 + x4);
+ }
+END_MGH_PROBLEM;
+
+const double TestProblem15::initial_x[] = {0.25, 0.39, 0.415, 0.39};
+const double TestProblem15::lower_bounds[] = {
+ -kDoubleMax, -kDoubleMax, -kDoubleMax, -kDoubleMax};
+const double TestProblem15::upper_bounds[] = {
+ kDoubleMax, kDoubleMax, kDoubleMax, kDoubleMax};
+const double TestProblem15::constrained_optimal_cost =
+ std::numeric_limits<double>::quiet_NaN();
+const double TestProblem15::unconstrained_optimal_cost = 3.07505e-4;
+
+// Brown and Dennis function.
+BEGIN_MGH_PROBLEM(TestProblem16, 4, 20)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+ const T x4 = x[3];
+
+ for (int i = 0; i < 20; ++i) {
+ const double ti = (i + 1) / 5.0;
+ residual[i] = (x1 + ti * x2 - exp(ti)) * (x1 + ti * x2 - exp(ti)) +
+ (x3 + x4 * sin(ti) - cos(ti)) * (x3 + x4 * sin(ti) - cos(ti));
+ }
+END_MGH_PROBLEM;
+
+const double TestProblem16::initial_x[] = {25.0, 5.0, -5.0, -1.0};
+const double TestProblem16::lower_bounds[] = {-10.0, 0.0, -100.0, -20.0};
+const double TestProblem16::upper_bounds[] = {100.0, 15.0, 0.0, 0.2};
+const double TestProblem16::constrained_optimal_cost = 0.88860479e5;
+const double TestProblem16::unconstrained_optimal_cost = 85822.2;
+
+// Osborne 1 function.
+BEGIN_MGH_PROBLEM(TestProblem17, 5, 33)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+ const T x4 = x[3];
+ const T x5 = x[4];
+
+ const double y[] = {0.844, 0.908, 0.932, 0.936, 0.925, 0.908, 0.881, 0.850, 0.818,
+ 0.784, 0.751, 0.718, 0.685, 0.658, 0.628, 0.603, 0.580, 0.558,
+ 0.538, 0.522, 0.506, 0.490, 0.478, 0.467, 0.457, 0.448, 0.438,
+ 0.431, 0.424, 0.420, 0.414, 0.411, 0.406};
+
+ for (int i = 0; i < 33; ++i) {
+ const double ti = 10.0 * i;
+ residual[i] = y[i] - (x1 + x2 * exp(-ti * x4) + x3 * exp(-ti * x5));
+ }
+END_MGH_PROBLEM;
+
+const double TestProblem17::initial_x[] = {0.5, 1.5, -1.0, 0.01, 0.02};
+const double TestProblem17::lower_bounds[] = {
+ -kDoubleMax, -kDoubleMax, -kDoubleMax, -kDoubleMax};
+const double TestProblem17::upper_bounds[] = {
+ kDoubleMax, kDoubleMax, kDoubleMax, kDoubleMax};
+const double TestProblem17::constrained_optimal_cost =
+ std::numeric_limits<double>::quiet_NaN();
+const double TestProblem17::unconstrained_optimal_cost = 5.46489e-5;
+
+// Biggs EXP6 function.
+BEGIN_MGH_PROBLEM(TestProblem18, 6, 13)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+ const T x4 = x[3];
+ const T x5 = x[4];
+ const T x6 = x[5];
+
+ for (int i = 0; i < 13; ++i) {
+ const double ti = 0.1 * (i + 1.0);
+ const double yi = exp(-ti) - 5.0 * exp(-10.0 * ti) + 3.0 * exp(-4.0 * ti);
+ residual[i] =
+ x3 * exp(-ti * x1) - x4 * exp(-ti * x2) + x6 * exp(-ti * x5) - yi;
+ }
+ END_MGH_PROBLEM
+
+ const double TestProblem18::initial_x[] = {1.0, 2.0, 1.0, 1.0, 1.0, 1.0};
+ const double TestProblem18::lower_bounds[] = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0};
+ const double TestProblem18::upper_bounds[] = {2.0, 8.0, 1.0, 7.0, 5.0, 5.0};
+ const double TestProblem18::constrained_optimal_cost = 0.53209865e-3;
+ const double TestProblem18::unconstrained_optimal_cost = 0.0;
+
+ // Osborne 2 function.
+ BEGIN_MGH_PROBLEM(TestProblem19, 11, 65)
+ const T x1 = x[0];
+ const T x2 = x[1];
+ const T x3 = x[2];
+ const T x4 = x[3];
+ const T x5 = x[4];
+ const T x6 = x[5];
+ const T x7 = x[6];
+ const T x8 = x[7];
+ const T x9 = x[8];
+ const T x10 = x[9];
+ const T x11 = x[10];
+
+ const double y[] = {1.366, 1.191, 1.112, 1.013, 0.991,
+ 0.885, 0.831, 0.847, 0.786, 0.725,
+ 0.746, 0.679, 0.608, 0.655, 0.616,
+ 0.606, 0.602, 0.626, 0.651, 0.724,
+ 0.649, 0.649, 0.694, 0.644, 0.624,
+ 0.661, 0.612, 0.558, 0.533, 0.495,
+ 0.500, 0.423, 0.395, 0.375, 0.372,
+ 0.391, 0.396, 0.405, 0.428, 0.429,
+ 0.523, 0.562, 0.607, 0.653, 0.672,
+ 0.708, 0.633, 0.668, 0.645, 0.632,
+ 0.591, 0.559, 0.597, 0.625, 0.739,
+ 0.710, 0.729, 0.720, 0.636, 0.581,
+ 0.428, 0.292, 0.162, 0.098, 0.054};
+
+ for (int i = 0; i < 65; ++i) {
+ const double ti = i / 10.0;
+ residual[i] = y[i] - (x1 * exp(-(ti * x5)) +
+ x2 * exp(-(ti - x9) * (ti - x9) * x6) +
+ x3 * exp(-(ti - x10) * (ti - x10) * x7) +
+ x4 * exp(-(ti - x11) * (ti - x11) * x8));
+ }
+END_MGH_PROBLEM;
+
+const double TestProblem19::initial_x[] = {1.3, 0.65, 0.65, 0.7, 0.6,
+ 3.0, 5.0, 7.0, 2.0, 4.5, 5.5};
+const double TestProblem19::lower_bounds[] = {
+ -kDoubleMax, -kDoubleMax, -kDoubleMax, -kDoubleMax};
+const double TestProblem19::upper_bounds[] = {
+ kDoubleMax, kDoubleMax, kDoubleMax, kDoubleMax};
+const double TestProblem19::constrained_optimal_cost =
+ std::numeric_limits<double>::quiet_NaN();
+const double TestProblem19::unconstrained_optimal_cost = 4.01377e-2;
+
+
+#undef BEGIN_MGH_PROBLEM
+#undef END_MGH_PROBLEM
+
+template<typename TestProblem> bool Solve(bool is_constrained, int trial) {
+ double x[TestProblem::kNumParameters];
+ for (int i = 0; i < TestProblem::kNumParameters; ++i) {
+ x[i] = pow(10, trial) * TestProblem::initial_x[i];
+ }
+
+ Problem problem;
+ problem.AddResidualBlock(TestProblem::Create(), NULL, x);
+ double optimal_cost = TestProblem::unconstrained_optimal_cost;
+
+ if (is_constrained) {
+ for (int i = 0; i < TestProblem::kNumParameters; ++i) {
+ problem.SetParameterLowerBound(x, i, TestProblem::lower_bounds[i]);
+ problem.SetParameterUpperBound(x, i, TestProblem::upper_bounds[i]);
+ }
+ optimal_cost = TestProblem::constrained_optimal_cost;
+ }
+
+ Solver::Options options;
+ options.parameter_tolerance = 1e-18;
+ options.function_tolerance = 1e-18;
+ options.gradient_tolerance = 1e-18;
+ options.max_num_iterations = 1000;
+ options.linear_solver_type = DENSE_QR;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+
+ const double kMinLogRelativeError = 4.0;
+ const double log_relative_error = -std::log10(
+ std::abs(2.0 * summary.final_cost - optimal_cost) /
+ (optimal_cost > 0.0 ? optimal_cost : 1.0));
+
+ const bool success = log_relative_error >= kMinLogRelativeError;
+ LOG(INFO) << "Expected : " << optimal_cost
+ << " actual: " << 2.0 * summary.final_cost
+ << " " << success
+ << " in " << summary.total_time_in_seconds
+ << " seconds";
+ return success;
+}
+
+} // namespace examples
+} // namespace ceres
+
+int main(int argc, char** argv) {
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+ google::InitGoogleLogging(argv[0]);
+
+ using ceres::examples::Solve;
+
+ int unconstrained_problems = 0;
+ int unconstrained_successes = 0;
+ int constrained_problems = 0;
+ int constrained_successes = 0;
+ std::stringstream ss;
+
+#define UNCONSTRAINED_SOLVE(n) \
+ ss << "Unconstrained Problem " << n << " : "; \
+ if (FLAGS_problem == #n || FLAGS_problem == "all") { \
+ unconstrained_problems += 3; \
+ if (Solve<ceres::examples::TestProblem##n>(false, 0)) { \
+ unconstrained_successes += 1; \
+ ss << "Yes "; \
+ } else { \
+ ss << "No "; \
+ } \
+ if (Solve<ceres::examples::TestProblem##n>(false, 1)) { \
+ unconstrained_successes += 1; \
+ ss << "Yes "; \
+ } else { \
+ ss << "No "; \
+ } \
+ if (Solve<ceres::examples::TestProblem##n>(false, 2)) { \
+ unconstrained_successes += 1; \
+ ss << "Yes "; \
+ } else { \
+ ss << "No "; \
+ } \
+ } \
+ ss << std::endl;
+
+ UNCONSTRAINED_SOLVE(1);
+ UNCONSTRAINED_SOLVE(2);
+ UNCONSTRAINED_SOLVE(3);
+ UNCONSTRAINED_SOLVE(4);
+ UNCONSTRAINED_SOLVE(5);
+ UNCONSTRAINED_SOLVE(6);
+ UNCONSTRAINED_SOLVE(7);
+ UNCONSTRAINED_SOLVE(8);
+ UNCONSTRAINED_SOLVE(9);
+ UNCONSTRAINED_SOLVE(10);
+ UNCONSTRAINED_SOLVE(11);
+ UNCONSTRAINED_SOLVE(12);
+ UNCONSTRAINED_SOLVE(13);
+ UNCONSTRAINED_SOLVE(14);
+ UNCONSTRAINED_SOLVE(15);
+ UNCONSTRAINED_SOLVE(16);
+ UNCONSTRAINED_SOLVE(17);
+ UNCONSTRAINED_SOLVE(18);
+ UNCONSTRAINED_SOLVE(19);
+
+ ss << "Unconstrained : "
+ << unconstrained_successes
+ << "/"
+ << unconstrained_problems << std::endl;
+
+#define CONSTRAINED_SOLVE(n) \
+ ss << "Constrained Problem " << n << " : "; \
+ if (FLAGS_problem == #n || FLAGS_problem == "all") { \
+ constrained_problems += 1; \
+ if (Solve<ceres::examples::TestProblem##n>(true, 0)) { \
+ constrained_successes += 1; \
+ ss << "Yes "; \
+ } else { \
+ ss << "No "; \
+ } \
+ } \
+ ss << std::endl;
+
+ CONSTRAINED_SOLVE(3);
+ CONSTRAINED_SOLVE(4);
+ CONSTRAINED_SOLVE(5);
+ CONSTRAINED_SOLVE(7);
+ CONSTRAINED_SOLVE(9);
+ CONSTRAINED_SOLVE(11);
+ CONSTRAINED_SOLVE(12);
+ CONSTRAINED_SOLVE(14);
+ CONSTRAINED_SOLVE(16);
+ CONSTRAINED_SOLVE(18);
+ ss << "Constrained : "
+ << constrained_successes
+ << "/"
+ << constrained_problems << std::endl;
+
+ std::cout << ss.str();
+ return 0;
+}
diff --git a/examples/nist.cc b/examples/nist.cc
new file mode 100644
index 0000000..8ce7291
--- /dev/null
+++ b/examples/nist.cc
@@ -0,0 +1,689 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2017 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// The National Institute of Standards and Technology has released a
+// set of problems to test non-linear least squares solvers.
+//
+// More information about the background on these problems and
+// suggested evaluation methodology can be found at:
+//
+// http://www.itl.nist.gov/div898/strd/nls/nls_info.shtml
+//
+// The problem data themselves can be found at
+//
+// http://www.itl.nist.gov/div898/strd/nls/nls_main.shtml
+//
+// The problems are divided into three levels of difficulty, Easy,
+// Medium and Hard. For each problem there are two starting guesses,
+// the first one far away from the global minimum and the second
+// closer to it.
+//
+// A problem is considered successfully solved, if every components of
+// the solution matches the globally optimal solution in at least 4
+// digits or more.
+//
+// This dataset was used for an evaluation of Non-linear least squares
+// solvers:
+//
+// P. F. Mondragon & B. Borchers, A Comparison of Nonlinear Regression
+// Codes, Journal of Modern Applied Statistical Methods, 4(1):343-351,
+// 2005.
+//
+// The results from Mondragon & Borchers can be summarized as
+// Excel Gnuplot GaussFit HBN MinPack
+// Average LRE 2.3 4.3 4.0 6.8 4.4
+// Winner 1 5 12 29 12
+//
+// Where the row Winner counts, the number of problems for which the
+// solver had the highest LRE.
+
+// In this file, we implement the same evaluation methodology using
+// Ceres. Currently using Levenberg-Marquardt with DENSE_QR, we get
+//
+// Excel Gnuplot GaussFit HBN MinPack Ceres
+// Average LRE 2.3 4.3 4.0 6.8 4.4 9.4
+// Winner 0 0 5 11 2 41
+
+#include <fstream>
+#include <iostream>
+#include <iterator>
+
+#include "Eigen/Core"
+#include "ceres/ceres.h"
+#include "ceres/tiny_solver.h"
+#include "ceres/tiny_solver_cost_function_adapter.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_bool(use_tiny_solver, false, "Use TinySolver instead of Ceres::Solver");
+DEFINE_string(nist_data_dir, "", "Directory containing the NIST non-linear"
+ "regression examples");
+DEFINE_string(minimizer, "trust_region",
+ "Minimizer type to use, choices are: line_search & trust_region");
+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_string(line_search, "wolfe",
+ "Line search algorithm to use, choices are: armijo and wolfe.");
+DEFINE_string(line_search_direction, "lbfgs",
+ "Line search direction algorithm to use, choices: lbfgs, bfgs");
+DEFINE_int32(max_line_search_iterations, 20,
+ "Maximum number of iterations for each line search.");
+DEFINE_int32(max_line_search_restarts, 10,
+ "Maximum number of restarts of line search direction algorithm.");
+DEFINE_string(line_search_interpolation, "cubic",
+ "Degree of polynomial aproximation in line search, "
+ "choices are: bisection, quadratic & cubic.");
+DEFINE_int32(lbfgs_rank, 20,
+ "Rank of L-BFGS inverse Hessian approximation in line search.");
+DEFINE_bool(approximate_eigenvalue_bfgs_scaling, false,
+ "Use approximate eigenvalue scaling in (L)BFGS line search.");
+DEFINE_double(sufficient_decrease, 1.0e-4,
+ "Line search Armijo sufficient (function) decrease factor.");
+DEFINE_double(sufficient_curvature_decrease, 0.9,
+ "Line search Wolfe sufficient curvature decrease factor.");
+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");
+DEFINE_bool(use_numeric_diff, false,
+ "Use numeric differentiation instead of automatic "
+ "differentiation.");
+DEFINE_string(numeric_diff_method, "ridders", "When using numeric "
+ "differentiation, selects algorithm. Options are: central, "
+ "forward, ridders.");
+DEFINE_double(ridders_step_size, 1e-9, "Initial step size for Ridders "
+ "numeric differentiation.");
+DEFINE_int32(ridders_extrapolations, 3, "Maximal number of Ridders "
+ "extrapolations.");
+
+namespace ceres {
+namespace examples {
+
+using Eigen::Dynamic;
+using Eigen::RowMajor;
+typedef Eigen::Matrix<double, Dynamic, 1> Vector;
+typedef Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> Matrix;
+
+using std::atof;
+using std::atoi;
+using std::cout;
+using std::ifstream;
+using std::string;
+using std::vector;
+
+void SplitStringUsingChar(const string& full,
+ const char delim,
+ vector<string>* result) {
+ std::back_insert_iterator< vector<string> > it(*result);
+
+ const char* p = full.data();
+ const char* end = p + full.size();
+ while (p != end) {
+ if (*p == delim) {
+ ++p;
+ } else {
+ const char* start = p;
+ while (++p != end && *p != delim) {
+ // Skip to the next occurence of the delimiter.
+ }
+ *it++ = string(start, p - start);
+ }
+ }
+}
+
+bool GetAndSplitLine(ifstream& ifs, vector<string>* pieces) {
+ pieces->clear();
+ char buf[256];
+ ifs.getline(buf, 256);
+ SplitStringUsingChar(string(buf), ' ', pieces);
+ return true;
+}
+
+void SkipLines(ifstream& ifs, int num_lines) {
+ char buf[256];
+ for (int i = 0; i < num_lines; ++i) {
+ ifs.getline(buf, 256);
+ }
+}
+
+class NISTProblem {
+ public:
+ explicit NISTProblem(const string& filename) {
+ ifstream ifs(filename.c_str(), ifstream::in);
+ CHECK(ifs) << "Unable to open : " << filename;
+
+ vector<string> pieces;
+ SkipLines(ifs, 24);
+ GetAndSplitLine(ifs, &pieces);
+ const int kNumResponses = atoi(pieces[1].c_str());
+
+ GetAndSplitLine(ifs, &pieces);
+ const int kNumPredictors = atoi(pieces[0].c_str());
+
+ GetAndSplitLine(ifs, &pieces);
+ const int kNumObservations = atoi(pieces[0].c_str());
+
+ SkipLines(ifs, 4);
+ GetAndSplitLine(ifs, &pieces);
+ const int kNumParameters = 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) = atof(pieces[i + 2].c_str());
+ }
+ final_parameters_(0, parameter_id) = 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) = atof(pieces[i + 2].c_str());
+ }
+ final_parameters_(0, parameter_id) = atof(pieces[2 + kNumTries].c_str());
+ }
+
+ // Certfied cost
+ SkipLines(ifs, 1);
+ GetAndSplitLine(ifs, &pieces);
+ certified_cost_ = 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) = atof(pieces[j].c_str());
+ }
+
+ // Predictor variables.
+ for (int j = 0; j < kNumPredictors; ++j) {
+ predictor_(i, j) = atof(pieces[j + kNumResponses].c_str());
+ }
+ }
+ }
+
+ Matrix initial_parameters(int start) const { return initial_parameters_.row(start); } // NOLINT
+ 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, \
+ const int n) \
+ : x_(x), y_(y), n_(n) {} \
+ const double* x_; \
+ const double* y_; \
+ const int n_; \
+ template <typename T> \
+ bool operator()(const T* const b, T* residual) const { \
+ for (int i = 0; i < n_; ++i) { \
+ const T x(x_[i]); \
+ residual[i] = y_[i] - (
+
+#define NIST_END ); } return true; }};
+
+// y = b1 * (b2+x)**(-1/b3) + e
+NIST_BEGIN(Bennet5)
+ b[0] * pow(b[1] + x, -1.0 / b[2])
+NIST_END
+
+// y = b1*(1-exp[-b2*x]) + e
+NIST_BEGIN(BoxBOD)
+ b[0] * (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) /
+ (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) /
+ (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] * (1.0 - exp(-b[1] * x))
+NIST_END
+
+// y = b1 * (1-(1+b2*x/2)**(-2)) + e
+NIST_BEGIN(Misra1b)
+ b[0] * (1.0 - 1.0/ ((1.0 + b[1] * x / 2.0) * (1.0 + b[1] * x / 2.0))) // NOLINT
+NIST_END
+
+// y = b1 * (1-(1+2*b2*x)**(-.5)) + e
+NIST_BEGIN(Misra1c)
+ b[0] * (1.0 - pow(1.0 + 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 / (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])) / kPi
+NIST_END
+
+// y = b1 / (1+exp[b2-b3*x]) + e
+NIST_BEGIN(Rat42)
+ b[0] / (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(1.0 + exp(b[1] - b[2] * x), 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) /
+ (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(2.0 * kPi * x / 12.0) +
+ b[2] * sin(2.0 * kPi * x / 12.0) +
+ b[4] * cos(2.0 * kPi * x / b[3]) +
+ b[5] * sin(2.0 * kPi * x / b[3]) +
+ b[7] * cos(2.0 * kPi * x / b[6]) +
+ b[8] * sin(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(-0.5 * pow((x - b[2])/b[1], 2))
+NIST_END
+
+struct Nelson {
+ public:
+ Nelson(const double* const x, const double* const y, const int n)
+ : x_(x), y_(y), n_(n) {}
+
+ template <typename T>
+ bool operator()(const T* const b, T* residual) const {
+ // log[y] = b1 - b2*x1 * exp[-b3*x2] + e
+ for (int i = 0; i < n_; ++i) {
+ residual[i] = log(y_[i]) - (b[0] - b[1] * x_[2 * i] * exp(-b[2] * x_[2 * i + 1]));
+ }
+ return true;
+ }
+
+ private:
+ const double* x_;
+ const double* y_;
+ const int n_;
+};
+
+static void SetNumericDiffOptions(ceres::NumericDiffOptions* options) {
+ options->max_num_ridders_extrapolations = FLAGS_ridders_extrapolations;
+ options->ridders_relative_initial_step_size = FLAGS_ridders_step_size;
+}
+
+void SetMinimizerOptions(ceres::Solver::Options* options) {
+ CHECK(
+ ceres::StringToMinimizerType(FLAGS_minimizer, &options->minimizer_type));
+ 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));
+ CHECK(ceres::StringToLineSearchDirectionType(
+ FLAGS_line_search_direction, &options->line_search_direction_type));
+ CHECK(ceres::StringToLineSearchType(FLAGS_line_search,
+ &options->line_search_type));
+ CHECK(ceres::StringToLineSearchInterpolationType(
+ FLAGS_line_search_interpolation,
+ &options->line_search_interpolation_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->max_lbfgs_rank = FLAGS_lbfgs_rank;
+ options->line_search_sufficient_function_decrease = FLAGS_sufficient_decrease;
+ options->line_search_sufficient_curvature_decrease =
+ FLAGS_sufficient_curvature_decrease;
+ options->max_num_line_search_step_size_iterations =
+ FLAGS_max_line_search_iterations;
+ options->max_num_line_search_direction_restarts =
+ FLAGS_max_line_search_restarts;
+ options->use_approximate_eigenvalue_bfgs_scaling =
+ FLAGS_approximate_eigenvalue_bfgs_scaling;
+ options->function_tolerance = std::numeric_limits<double>::epsilon();
+ options->gradient_tolerance = std::numeric_limits<double>::epsilon();
+ options->parameter_tolerance = std::numeric_limits<double>::epsilon();
+}
+
+string JoinPath(const string& dirname, const string& basename) {
+#ifdef _WIN32
+ static const char separator = '\\';
+#else
+ static const char separator = '/';
+#endif // _WIN32
+
+ if ((!basename.empty() && basename[0] == separator) || dirname.empty()) {
+ return basename;
+ } else if (dirname[dirname.size() - 1] == separator) {
+ return dirname + basename;
+ } else {
+ return dirname + string(&separator, 1) + basename;
+ }
+}
+
+template <typename Model, int num_parameters>
+CostFunction* CreateCostFunction(const Matrix& predictor,
+ const Matrix& response,
+ const int num_observations) {
+ Model* model =
+ new Model(predictor.data(), response.data(), num_observations);
+ ceres::CostFunction* cost_function = NULL;
+ if (FLAGS_use_numeric_diff) {
+ ceres::NumericDiffOptions options;
+ SetNumericDiffOptions(&options);
+ if (FLAGS_numeric_diff_method == "central") {
+ cost_function = new NumericDiffCostFunction<Model,
+ ceres::CENTRAL,
+ ceres::DYNAMIC,
+ num_parameters>(
+ model,
+ ceres::TAKE_OWNERSHIP,
+ num_observations,
+ options);
+ } else if (FLAGS_numeric_diff_method == "forward") {
+ cost_function = new NumericDiffCostFunction<Model,
+ ceres::FORWARD,
+ ceres::DYNAMIC,
+ num_parameters>(
+ model,
+ ceres::TAKE_OWNERSHIP,
+ num_observations,
+ options);
+ } else if (FLAGS_numeric_diff_method == "ridders") {
+ cost_function = new NumericDiffCostFunction<Model,
+ ceres::RIDDERS,
+ ceres::DYNAMIC,
+ num_parameters>(
+ model,
+ ceres::TAKE_OWNERSHIP,
+ num_observations,
+ options);
+ } else {
+ LOG(ERROR) << "Invalid numeric diff method specified";
+ return 0;
+ }
+ } else {
+ cost_function =
+ new ceres::AutoDiffCostFunction<Model, ceres::DYNAMIC, num_parameters>(
+ model, num_observations);
+ }
+ return cost_function;
+}
+
+double ComputeLRE(const Matrix& expected, const Matrix& actual) {
+ // Compute the LRE by comparing each component of the solution
+ // with the ground truth, and taking the minimum.
+ const double kMaxNumSignificantDigits = 11;
+ double log_relative_error = kMaxNumSignificantDigits + 1;
+ for (int i = 0; i < expected.cols(); ++i) {
+ const double tmp_lre = -std::log10(std::fabs(expected(i) - actual(i)) /
+ std::fabs(expected(i)));
+ // The maximum LRE is capped at 11 - the precision at which the
+ // ground truth is known.
+ //
+ // The minimum LRE is capped at 0 - no digits match between the
+ // computed solution and the ground truth.
+ log_relative_error =
+ std::min(log_relative_error,
+ std::max(0.0, std::min(kMaxNumSignificantDigits, tmp_lre)));
+ }
+ return log_relative_error;
+}
+
+template <typename Model, int num_parameters>
+int RegressionDriver(const string& filename) {
+ NISTProblem nist_problem(JoinPath(FLAGS_nist_data_dir, filename));
+ 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();
+
+ printf("%s\n", filename.c_str());
+
+ // Each NIST problem comes with multiple starting points, so we
+ // construct the problem from scratch for each case and solve it.
+ int num_success = 0;
+ for (int start = 0; start < nist_problem.num_starts(); ++start) {
+ Matrix initial_parameters = nist_problem.initial_parameters(start);
+ ceres::CostFunction* cost_function = CreateCostFunction<Model, num_parameters>(
+ predictor, response, nist_problem.num_observations());
+
+ double initial_cost;
+ double final_cost;
+
+ if (!FLAGS_use_tiny_solver) {
+ ceres::Problem problem;
+ problem.AddResidualBlock(cost_function, NULL, initial_parameters.data());
+ ceres::Solver::Summary summary;
+ ceres::Solver::Options options;
+ SetMinimizerOptions(&options);
+ Solve(options, &problem, &summary);
+ initial_cost = summary.initial_cost;
+ final_cost = summary.final_cost;
+ } else {
+ ceres::TinySolverCostFunctionAdapter<Eigen::Dynamic, num_parameters> cfa(
+ *cost_function);
+ typedef ceres::TinySolver<
+ ceres::TinySolverCostFunctionAdapter<Eigen::Dynamic, num_parameters> >
+ Solver;
+ Solver solver;
+ solver.options.max_num_iterations = FLAGS_num_iterations;
+ solver.options.gradient_tolerance =
+ std::numeric_limits<double>::epsilon();
+ solver.options.parameter_tolerance =
+ std::numeric_limits<double>::epsilon();
+
+ Eigen::Matrix<double, num_parameters, 1> x;
+ x = initial_parameters.transpose();
+ typename Solver::Summary summary = solver.Solve(cfa, &x);
+ initial_parameters = x;
+ initial_cost = summary.initial_cost;
+ final_cost = summary.final_cost;
+ delete cost_function;
+ }
+
+ const double log_relative_error = ComputeLRE(nist_problem.final_parameters(),
+ initial_parameters);
+ const int kMinNumMatchingDigits = 4;
+ if (log_relative_error > kMinNumMatchingDigits) {
+ ++num_success;
+ }
+
+ printf(
+ "start: %d status: %s lre: %4.1f initial cost: %e final cost:%e "
+ "certified cost: %e\n",
+ start + 1,
+ log_relative_error < kMinNumMatchingDigits ? "FAILURE" : "SUCCESS",
+ log_relative_error,
+ initial_cost,
+ final_cost,
+ nist_problem.certified_cost());
+ }
+ return num_success;
+}
+
+
+void SolveNISTProblems() {
+ if (FLAGS_nist_data_dir.empty()) {
+ LOG(FATAL) << "Must specify the directory containing the NIST problems";
+ }
+
+ cout << "Lower Difficulty\n";
+ int easy_success = 0;
+ easy_success += RegressionDriver<Misra1a, 2>("Misra1a.dat");
+ easy_success += RegressionDriver<Chwirut, 3>("Chwirut1.dat");
+ easy_success += RegressionDriver<Chwirut, 3>("Chwirut2.dat");
+ easy_success += RegressionDriver<Lanczos, 6>("Lanczos3.dat");
+ easy_success += RegressionDriver<Gauss, 8>("Gauss1.dat");
+ easy_success += RegressionDriver<Gauss, 8>("Gauss2.dat");
+ easy_success += RegressionDriver<DanWood, 2>("DanWood.dat");
+ easy_success += RegressionDriver<Misra1b, 2>("Misra1b.dat");
+
+ cout << "\nMedium Difficulty\n";
+ int medium_success = 0;
+ medium_success += RegressionDriver<Kirby2, 5>("Kirby2.dat");
+ medium_success += RegressionDriver<Hahn1, 7>("Hahn1.dat");
+ medium_success += RegressionDriver<Nelson, 3>("Nelson.dat");
+ medium_success += RegressionDriver<MGH17, 5>("MGH17.dat");
+ medium_success += RegressionDriver<Lanczos, 6>("Lanczos1.dat");
+ medium_success += RegressionDriver<Lanczos, 6>("Lanczos2.dat");
+ medium_success += RegressionDriver<Gauss, 8>("Gauss3.dat");
+ medium_success += RegressionDriver<Misra1c, 2>("Misra1c.dat");
+ medium_success += RegressionDriver<Misra1d, 2>("Misra1d.dat");
+ medium_success += RegressionDriver<Roszman1, 4>("Roszman1.dat");
+ medium_success += RegressionDriver<ENSO, 9>("ENSO.dat");
+
+ cout << "\nHigher Difficulty\n";
+ int hard_success = 0;
+ hard_success += RegressionDriver<MGH09, 4>("MGH09.dat");
+ hard_success += RegressionDriver<Thurber, 7>("Thurber.dat");
+ hard_success += RegressionDriver<BoxBOD, 2>("BoxBOD.dat");
+ hard_success += RegressionDriver<Rat42, 3>("Rat42.dat");
+ hard_success += RegressionDriver<MGH10, 3>("MGH10.dat");
+ hard_success += RegressionDriver<Eckerle4, 3>("Eckerle4.dat");
+ hard_success += RegressionDriver<Rat43, 4>("Rat43.dat");
+ hard_success += RegressionDriver<Bennet5, 3>("Bennett5.dat");
+
+ cout << "\n";
+ cout << "Easy : " << easy_success << "/16\n";
+ cout << "Medium : " << medium_success << "/22\n";
+ cout << "Hard : " << hard_success << "/16\n";
+ cout << "Total : " << easy_success + medium_success + hard_success
+ << "/54\n";
+}
+
+} // namespace examples
+} // namespace ceres
+
+int main(int argc, char** argv) {
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+ google::InitGoogleLogging(argv[0]);
+ ceres::examples::SolveNISTProblems();
+ return 0;
+}
diff --git a/examples/pgm_image.h b/examples/pgm_image.h
new file mode 100644
index 0000000..1de8d67
--- /dev/null
+++ b/examples/pgm_image.h
@@ -0,0 +1,319 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: 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 bool(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..614c9c6
--- /dev/null
+++ b/examples/powell.cc
@@ -0,0 +1,154 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// An example 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;
+
+struct F1 {
+ template <typename T> bool operator()(const T* const x1,
+ const T* const x2,
+ T* residual) const {
+ // f1 = x1 + 10 * x2;
+ residual[0] = x1[0] + 10.0 * x2[0];
+ return true;
+ }
+};
+
+struct F2 {
+ template <typename T> bool operator()(const T* const x3,
+ const T* const x4,
+ T* residual) const {
+ // f2 = sqrt(5) (x3 - x4)
+ residual[0] = sqrt(5.0) * (x3[0] - x4[0]);
+ return true;
+ }
+};
+
+struct F3 {
+ template <typename T> bool operator()(const T* const x2,
+ const T* const x3,
+ T* residual) const {
+ // f3 = (x2 - 2 x3)^2
+ residual[0] = (x2[0] - 2.0 * x3[0]) * (x2[0] - 2.0 * x3[0]);
+ return true;
+ }
+};
+
+struct F4 {
+ template <typename T> bool operator()(const T* const x1,
+ const T* const x4,
+ T* residual) const {
+ // f4 = sqrt(10) (x1 - x4)^2
+ residual[0] = sqrt(10.0) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
+ return true;
+ }
+};
+
+DEFINE_string(minimizer, "trust_region",
+ "Minimizer type to use, choices are: line_search & trust_region");
+
+int main(int argc, char** argv) {
+ CERES_GFLAGS_NAMESPACE::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);
+
+ Solver::Options options;
+ LOG_IF(FATAL, !ceres::StringToMinimizerType(FLAGS_minimizer,
+ &options.minimizer_type))
+ << "Invalid minimizer: " << FLAGS_minimizer
+ << ", valid options are: trust_region and line_search.";
+
+ options.max_num_iterations = 100;
+ options.linear_solver_type = ceres::DENSE_QR;
+ options.minimizer_progress_to_stdout = true;
+
+ std::cout << "Initial x1 = " << x1
+ << ", x2 = " << x2
+ << ", x3 = " << x3
+ << ", x4 = " << x4
+ << "\n";
+
+ // Run the solver!
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+
+ std::cout << summary.FullReport() << "\n";
+ std::cout << "Final x1 = " << x1
+ << ", x2 = " << x2
+ << ", x3 = " << x3
+ << ", x4 = " << x4
+ << "\n";
+ return 0;
+}
diff --git a/examples/random.h b/examples/random.h
new file mode 100644
index 0000000..7b09b15
--- /dev/null
+++ b/examples/random.h
@@ -0,0 +1,64 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_EXAMPLES_RANDOM_H_
+#define CERES_EXAMPLES_RANDOM_H_
+
+#include <math.h>
+#include <stdlib.h>
+
+namespace ceres {
+namespace examples {
+
+// Return a random number sampled from a uniform distribution in the range
+// [0,1].
+inline double RandDouble() {
+ double r = static_cast<double>(rand());
+ return r / RAND_MAX;
+}
+
+// Marsaglia Polar method for generation standard normal (pseudo)
+// random numbers http://en.wikipedia.org/wiki/Marsaglia_polar_method
+inline double RandNormal() {
+ double x1, x2, w;
+ do {
+ x1 = 2.0 * RandDouble() - 1.0;
+ x2 = 2.0 * RandDouble() - 1.0;
+ w = x1 * x1 + x2 * x2;
+ } while ( w >= 1.0 || w == 0.0 );
+
+ w = sqrt((-2.0 * log(w)) / w);
+ return x1 * w;
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_RANDOM_H_
diff --git a/examples/robot_pose_mle.cc b/examples/robot_pose_mle.cc
new file mode 100644
index 0000000..1f5058a
--- /dev/null
+++ b/examples/robot_pose_mle.cc
@@ -0,0 +1,316 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: joydeepb@ri.cmu.edu (Joydeep Biswas)
+//
+// This example demonstrates how to use the DynamicAutoDiffCostFunction
+// variant of CostFunction. The DynamicAutoDiffCostFunction is meant to
+// be used in cases where the number of parameter blocks or the sizes are not
+// known at compile time.
+//
+// This example simulates a robot traversing down a 1-dimension hallway with
+// noise odometry readings and noisy range readings of the end of the hallway.
+// By fusing the noisy odometry and sensor readings this example demonstrates
+// how to compute the maximum likelihood estimate (MLE) of the robot's pose at
+// each timestep.
+//
+// The robot starts at the origin, and it is travels to the end of a corridor of
+// fixed length specified by the "--corridor_length" flag. It executes a series
+// of motion commands to move forward a fixed length, specified by the
+// "--pose_separation" flag, at which pose it receives relative odometry
+// measurements as well as a range reading of the distance to the end of the
+// hallway. The odometry readings are drawn with Gaussian noise and standard
+// deviation specified by the "--odometry_stddev" flag, and the range readings
+// similarly with standard deviation specified by the "--range-stddev" flag.
+//
+// There are two types of residuals in this problem:
+// 1) The OdometryConstraint residual, that accounts for the odometry readings
+// between successive pose estimatess of the robot.
+// 2) The RangeConstraint residual, that accounts for the errors in the observed
+// range readings from each pose.
+//
+// The OdometryConstraint residual is modeled as an AutoDiffCostFunction with
+// a fixed parameter block size of 1, which is the relative odometry being
+// solved for, between a pair of successive poses of the robot. Differences
+// between observed and computed relative odometry values are penalized weighted
+// by the known standard deviation of the odometry readings.
+//
+// The RangeConstraint residual is modeled as a DynamicAutoDiffCostFunction
+// which sums up the relative odometry estimates to compute the estimated
+// global pose of the robot, and then computes the expected range reading.
+// Differences between the observed and expected range readings are then
+// penalized weighted by the standard deviation of readings of the sensor.
+// Since the number of poses of the robot is not known at compile time, this
+// cost function is implemented as a DynamicAutoDiffCostFunction.
+//
+// The outputs of the example are the initial values of the odometry and range
+// readings, and the range and odometry errors for every pose of the robot.
+// After computing the MLE, the computed poses and corrected odometry values
+// are printed out, along with the corresponding range and odometry errors. Note
+// that as an MLE of a noisy system the errors will not be reduced to zero, but
+// the odometry estimates will be updated to maximize the joint likelihood of
+// all odometry and range readings of the robot.
+//
+// Mathematical Formulation
+// ======================================================
+//
+// Let p_0, .., p_N be (N+1) robot poses, where the robot moves down the
+// corridor starting from p_0 and ending at p_N. We assume that p_0 is the
+// origin of the coordinate system.
+// Odometry u_i is the observed relative odometry between pose p_(i-1) and p_i,
+// and range reading y_i is the range reading of the end of the corridor from
+// pose p_i. Both odometry as well as range readings are noisy, but we wish to
+// compute the maximum likelihood estimate (MLE) of corrected odometry values
+// u*_0 to u*_(N-1), such that the Belief is optimized:
+//
+// Belief(u*_(0:N-1) | u_(0:N-1), y_(0:N-1)) 1.
+// = P(u*_(0:N-1) | u_(0:N-1), y_(0:N-1)) 2.
+// \propto P(y_(0:N-1) | u*_(0:N-1), u_(0:N-1)) P(u*_(0:N-1) | u_(0:N-1)) 3.
+// = \prod_i{ P(y_i | u*_(0:i)) P(u*_i | u_i) } 4.
+//
+// Here, the subscript "(0:i)" is used as shorthand to indicate entries from all
+// timesteps 0 to i for that variable, both inclusive.
+//
+// Bayes' rule is used to derive eq. 3 from 2, and the independence of
+// odometry observations and range readings is expolited to derive 4 from 3.
+//
+// Thus, the Belief, up to scale, is factored as a product of a number of
+// terms, two for each pose, where for each pose term there is one term for the
+// range reading, P(y_i | u*_(0:i) and one term for the odometry reading,
+// P(u*_i | u_i) . Note that the term for the range reading is dependent on all
+// odometry values u*_(0:i), while the odometry term, P(u*_i | u_i) depends only
+// on a single value, u_i. Both the range reading as well as odoemtry
+// probability terms are modeled as the Normal distribution, and have the form:
+//
+// p(x) \propto \exp{-((x - x_mean) / x_stddev)^2}
+//
+// where x refers to either the MLE odometry u* or range reading y, and x_mean
+// is the corresponding mean value, u for the odometry terms, and y_expected,
+// the expected range reading based on all the previous odometry terms.
+// The MLE is thus found by finding those values x* which minimize:
+//
+// x* = \arg\min{((x - x_mean) / x_stddev)^2}
+//
+// which is in the nonlinear least-square form, suited to being solved by Ceres.
+// The non-linear component arise from the computation of x_mean. The residuals
+// ((x - x_mean) / x_stddev) for the residuals that Ceres will optimize. As
+// mentioned earlier, the odometry term for each pose depends only on one
+// variable, and will be computed by an AutoDiffCostFunction, while the term
+// for the range reading will depend on all previous odometry observations, and
+// will be computed by a DynamicAutoDiffCostFunction since the number of
+// odoemtry observations will only be known at run time.
+
+#include <cstdio>
+#include <math.h>
+#include <vector>
+
+#include "ceres/ceres.h"
+#include "ceres/dynamic_autodiff_cost_function.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "random.h"
+
+using ceres::AutoDiffCostFunction;
+using ceres::DynamicAutoDiffCostFunction;
+using ceres::CauchyLoss;
+using ceres::CostFunction;
+using ceres::LossFunction;
+using ceres::Problem;
+using ceres::Solve;
+using ceres::Solver;
+using ceres::examples::RandNormal;
+using std::min;
+using std::vector;
+
+DEFINE_double(corridor_length, 30.0, "Length of the corridor that the robot is "
+ "travelling down.");
+
+DEFINE_double(pose_separation, 0.5, "The distance that the robot traverses "
+ "between successive odometry updates.");
+
+DEFINE_double(odometry_stddev, 0.1, "The standard deviation of "
+ "odometry error of the robot.");
+
+DEFINE_double(range_stddev, 0.01, "The standard deviation of range readings of "
+ "the robot.");
+
+// The stride length of the dynamic_autodiff_cost_function evaluator.
+static const int kStride = 10;
+
+struct OdometryConstraint {
+ typedef AutoDiffCostFunction<OdometryConstraint, 1, 1> OdometryCostFunction;
+
+ OdometryConstraint(double odometry_mean, double odometry_stddev) :
+ odometry_mean(odometry_mean), odometry_stddev(odometry_stddev) {}
+
+ template <typename T>
+ bool operator()(const T* const odometry, T* residual) const {
+ *residual = (*odometry - odometry_mean) / odometry_stddev;
+ return true;
+ }
+
+ static OdometryCostFunction* Create(const double odometry_value) {
+ return new OdometryCostFunction(
+ new OdometryConstraint(odometry_value, FLAGS_odometry_stddev));
+ }
+
+ const double odometry_mean;
+ const double odometry_stddev;
+};
+
+struct RangeConstraint {
+ typedef DynamicAutoDiffCostFunction<RangeConstraint, kStride>
+ RangeCostFunction;
+
+ RangeConstraint(
+ int pose_index,
+ double range_reading,
+ double range_stddev,
+ double corridor_length) :
+ pose_index(pose_index), range_reading(range_reading),
+ range_stddev(range_stddev), corridor_length(corridor_length) {}
+
+ template <typename T>
+ bool operator()(T const* const* relative_poses, T* residuals) const {
+ T global_pose(0);
+ for (int i = 0; i <= pose_index; ++i) {
+ global_pose += relative_poses[i][0];
+ }
+ residuals[0] = (global_pose + range_reading - corridor_length) /
+ range_stddev;
+ return true;
+ }
+
+ // Factory method to create a CostFunction from a RangeConstraint to
+ // conveniently add to a ceres problem.
+ static RangeCostFunction* Create(const int pose_index,
+ const double range_reading,
+ vector<double>* odometry_values,
+ vector<double*>* parameter_blocks) {
+ RangeConstraint* constraint = new RangeConstraint(
+ pose_index, range_reading, FLAGS_range_stddev, FLAGS_corridor_length);
+ RangeCostFunction* cost_function = new RangeCostFunction(constraint);
+ // Add all the parameter blocks that affect this constraint.
+ parameter_blocks->clear();
+ for (int i = 0; i <= pose_index; ++i) {
+ parameter_blocks->push_back(&((*odometry_values)[i]));
+ cost_function->AddParameterBlock(1);
+ }
+ cost_function->SetNumResiduals(1);
+ return (cost_function);
+ }
+
+ const int pose_index;
+ const double range_reading;
+ const double range_stddev;
+ const double corridor_length;
+};
+
+void SimulateRobot(vector<double>* odometry_values,
+ vector<double>* range_readings) {
+ const int num_steps = static_cast<int>(
+ ceil(FLAGS_corridor_length / FLAGS_pose_separation));
+
+ // The robot starts out at the origin.
+ double robot_location = 0.0;
+ for (int i = 0; i < num_steps; ++i) {
+ const double actual_odometry_value = min(
+ FLAGS_pose_separation, FLAGS_corridor_length - robot_location);
+ robot_location += actual_odometry_value;
+ const double actual_range = FLAGS_corridor_length - robot_location;
+ const double observed_odometry =
+ RandNormal() * FLAGS_odometry_stddev + actual_odometry_value;
+ const double observed_range =
+ RandNormal() * FLAGS_range_stddev + actual_range;
+ odometry_values->push_back(observed_odometry);
+ range_readings->push_back(observed_range);
+ }
+}
+
+void PrintState(const vector<double>& odometry_readings,
+ const vector<double>& range_readings) {
+ CHECK_EQ(odometry_readings.size(), range_readings.size());
+ double robot_location = 0.0;
+ printf("pose: location odom range r.error o.error\n");
+ for (int i = 0; i < odometry_readings.size(); ++i) {
+ robot_location += odometry_readings[i];
+ const double range_error =
+ robot_location + range_readings[i] - FLAGS_corridor_length;
+ const double odometry_error =
+ FLAGS_pose_separation - odometry_readings[i];
+ printf("%4d: %8.3f %8.3f %8.3f %8.3f %8.3f\n",
+ static_cast<int>(i), robot_location, odometry_readings[i],
+ range_readings[i], range_error, odometry_error);
+ }
+}
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+ // Make sure that the arguments parsed are all positive.
+ CHECK_GT(FLAGS_corridor_length, 0.0);
+ CHECK_GT(FLAGS_pose_separation, 0.0);
+ CHECK_GT(FLAGS_odometry_stddev, 0.0);
+ CHECK_GT(FLAGS_range_stddev, 0.0);
+
+ vector<double> odometry_values;
+ vector<double> range_readings;
+ SimulateRobot(&odometry_values, &range_readings);
+
+ printf("Initial values:\n");
+ PrintState(odometry_values, range_readings);
+ ceres::Problem problem;
+
+ for (int i = 0; i < odometry_values.size(); ++i) {
+ // Create and add a DynamicAutoDiffCostFunction for the RangeConstraint from
+ // pose i.
+ vector<double*> parameter_blocks;
+ RangeConstraint::RangeCostFunction* range_cost_function =
+ RangeConstraint::Create(
+ i, range_readings[i], &odometry_values, ¶meter_blocks);
+ problem.AddResidualBlock(range_cost_function, NULL, parameter_blocks);
+
+ // Create and add an AutoDiffCostFunction for the OdometryConstraint for
+ // pose i.
+ problem.AddResidualBlock(OdometryConstraint::Create(odometry_values[i]),
+ NULL,
+ &(odometry_values[i]));
+ }
+
+ ceres::Solver::Options solver_options;
+ solver_options.minimizer_progress_to_stdout = true;
+
+ Solver::Summary summary;
+ printf("Solving...\n");
+ Solve(solver_options, &problem, &summary);
+ printf("Done.\n");
+ std::cout << summary.FullReport() << "\n";
+ printf("Final values:\n");
+ PrintState(odometry_values, range_readings);
+ return 0;
+}
diff --git a/examples/robust_curve_fitting.cc b/examples/robust_curve_fitting.cc
new file mode 100644
index 0000000..6186aa1
--- /dev/null
+++ b/examples/robust_curve_fitting.cc
@@ -0,0 +1,165 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+// 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;
+// outlier_noise = rand(size(x)) < 0.05;
+// y_observed = y + noise + outlier_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, 5.543078e+00, // Outlier point
+1.500000e+00, 5.664015e+00, // Outlier point
+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
+};
+
+using ceres::AutoDiffCostFunction;
+using ceres::CostFunction;
+using ceres::CauchyLoss;
+using ceres::Problem;
+using ceres::Solve;
+using ceres::Solver;
+
+struct ExponentialResidual {
+ 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] = y_ - exp(m[0] * x_ + c[0]);
+ return true;
+ }
+
+ private:
+ const double x_;
+ const double y_;
+};
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ double m = 0.0;
+ double c = 0.0;
+
+ Problem problem;
+ for (int i = 0; i < kNumObservations; ++i) {
+ CostFunction* cost_function =
+ new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
+ new ExponentialResidual(data[2 * i], data[2 * i + 1]));
+ problem.AddResidualBlock(cost_function,
+ new CauchyLoss(0.5),
+ &m, &c);
+ }
+
+ Solver::Options options;
+ 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/rosenbrock.cc b/examples/rosenbrock.cc
new file mode 100644
index 0000000..3f64f1c
--- /dev/null
+++ b/examples/rosenbrock.cc
@@ -0,0 +1,74 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+
+// f(x,y) = (1-x)^2 + 100(y - x^2)^2;
+class Rosenbrock : public ceres::FirstOrderFunction {
+ public:
+ virtual ~Rosenbrock() {}
+
+ virtual bool Evaluate(const double* parameters,
+ double* cost,
+ double* gradient) const {
+ const double x = parameters[0];
+ const double y = parameters[1];
+
+ cost[0] = (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x);
+ if (gradient != NULL) {
+ gradient[0] = -2.0 * (1.0 - x) - 200.0 * (y - x * x) * 2.0 * x;
+ gradient[1] = 200.0 * (y - x * x);
+ }
+ return true;
+ }
+
+ virtual int NumParameters() const { return 2; }
+};
+
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ double parameters[2] = {-1.2, 1.0};
+
+ ceres::GradientProblemSolver::Options options;
+ options.minimizer_progress_to_stdout = true;
+
+ ceres::GradientProblemSolver::Summary summary;
+ ceres::GradientProblem problem(new Rosenbrock());
+ ceres::Solve(options, problem, parameters, &summary);
+
+ std::cout << summary.FullReport() << "\n";
+ std::cout << "Initial x: " << -1.2 << " y: " << 1.0 << "\n";
+ std::cout << "Final x: " << parameters[0]
+ << " y: " << parameters[1] << "\n";
+ return 0;
+}
diff --git a/examples/sampled_function/CMakeLists.txt b/examples/sampled_function/CMakeLists.txt
new file mode 100644
index 0000000..57f31d1
--- /dev/null
+++ b/examples/sampled_function/CMakeLists.txt
@@ -0,0 +1,32 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2015 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: vitus@google.com (Michael Vitus)
+
+add_executable(sampled_function sampled_function.cc)
+target_link_libraries(sampled_function ceres)
diff --git a/examples/sampled_function/README.md b/examples/sampled_function/README.md
new file mode 100644
index 0000000..77ce365
--- /dev/null
+++ b/examples/sampled_function/README.md
@@ -0,0 +1,42 @@
+Sampled Functions
+--
+
+It is common to not have an analytical representation of the optimization
+problem but rather a table of values at specific inputs. This commonly occurs
+when working with images or when the functions in the problem are expensive to
+evaluate. To use this data in an optimization problem we can use interpolation
+to evaluate the function and derivatives at intermediate input values.
+
+There are many libraries that implement a variety of interpolation schemes, but
+it is difficult to use them in Ceres' automatic differentiation framework.
+Instead, Ceres provides the ability to interpolate one and two dimensional data.
+
+The one dimensional interpolation is based on the Cubic Hermite Spline. This
+interpolation method requires knowledge of the function derivatives at the
+control points, however we only know the function values. Consequently, we will
+use the data to estimate derivatives at the control points. The choice of how to
+compute the derivatives is not unique and Ceres uses the Catmull–Rom Spline
+variant which uses `0.5 * (p_{k+1} - p_{k-1})` as the derivative for control
+point `p_k.` This produces a first order differentiable interpolating
+function. The two dimensional interpolation scheme is a generalization of the
+one dimensional scheme where the interpolating function is assumed to be
+separable in the two dimensions.
+
+This example shows how to use interpolation schemes within the Ceres automatic
+differentiation framework. This is a one dimensional example and the objective
+function is to minimize `0.5 * f(x)^2` where `f(x) = (x - 4.5)^2`.
+
+It is also possible to use analytical derivatives with the provided
+interpolation schemes by using a `SizedCostFunction` and defining the
+``Evaluate` function. For this example, the evaluate function would be:
+
+```c++
+bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
+ if (jacobians == NULL || jacobians[0] == NULL)
+ interpolator_.Evaluate(parameters[0][0], residuals);
+ else
+ interpolator_.Evaluate(parameters[0][0], residuals, jacobians[0]);
+
+ return true;
+}
+```
diff --git a/examples/sampled_function/sampled_function.cc b/examples/sampled_function/sampled_function.cc
new file mode 100644
index 0000000..093276a
--- /dev/null
+++ b/examples/sampled_function/sampled_function.cc
@@ -0,0 +1,94 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// A simple example of optimizing a sampled function by using cubic
+// interpolation.
+
+#include "ceres/ceres.h"
+#include "ceres/cubic_interpolation.h"
+#include "glog/logging.h"
+
+using ceres::Grid1D;
+using ceres::CubicInterpolator;
+using ceres::AutoDiffCostFunction;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+// A simple cost functor that interfaces an interpolated table of
+// values with automatic differentiation.
+struct InterpolatedCostFunctor {
+ explicit InterpolatedCostFunctor(
+ const CubicInterpolator<Grid1D<double> >& interpolator)
+ : interpolator_(interpolator) {
+ }
+
+ template<typename T> bool operator()(const T* x, T* residuals) const {
+ interpolator_.Evaluate(*x, residuals);
+ return true;
+ }
+
+ static CostFunction* Create(
+ const CubicInterpolator<Grid1D<double> >& interpolator) {
+ return new AutoDiffCostFunction<InterpolatedCostFunctor, 1, 1>(
+ new InterpolatedCostFunctor(interpolator));
+ }
+
+ private:
+ const CubicInterpolator<Grid1D<double> >& interpolator_;
+};
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+
+ // Evaluate the function f(x) = (x - 4.5)^2;
+ const int kNumSamples = 10;
+ double values[kNumSamples];
+ for (int i = 0; i < kNumSamples; ++i) {
+ values[i] = (i - 4.5) * (i - 4.5);
+ }
+
+ Grid1D<double> array(values, 0, kNumSamples);
+ CubicInterpolator<Grid1D<double> > interpolator(array);
+
+ double x = 1.0;
+ Problem problem;
+ CostFunction* cost_function = InterpolatedCostFunctor::Create(interpolator);
+ problem.AddResidualBlock(cost_function, NULL, &x);
+
+ Solver::Options options;
+ options.minimizer_progress_to_stdout = true;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ std::cout << summary.BriefReport() << "\n";
+ std::cout << "Expected x: 4.5. Actual x : " << x << std::endl;
+ return 0;
+}
diff --git a/examples/simple_bundle_adjuster.cc b/examples/simple_bundle_adjuster.cc
new file mode 100644
index 0000000..dec6cd6
--- /dev/null
+++ b/examples/simple_bundle_adjuster.cc
@@ -0,0 +1,218 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: 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 = 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 - observed_x;
+ residuals[1] = predicted_y - observed_y;
+
+ return true;
+ }
+
+ // Factory to hide the construction of the CostFunction object from
+ // the client code.
+ static ceres::CostFunction* Create(const double observed_x,
+ const double observed_y) {
+ return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
+ new SnavelyReprojectionError(observed_x, observed_y)));
+ }
+
+ 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;
+ }
+
+ const double* observations = bal_problem.observations();
+
+ // 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 =
+ SnavelyReprojectionError::Create(observations[2 * i + 0],
+ 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/slam/CMakeLists.txt b/examples/slam/CMakeLists.txt
new file mode 100644
index 0000000..c72aa16
--- /dev/null
+++ b/examples/slam/CMakeLists.txt
@@ -0,0 +1,33 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2016 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: vitus@google.com (Michael Vitus)
+include_directories(./)
+
+add_subdirectory(pose_graph_2d)
+add_subdirectory(pose_graph_3d)
diff --git a/examples/slam/common/read_g2o.h b/examples/slam/common/read_g2o.h
new file mode 100644
index 0000000..71b071c
--- /dev/null
+++ b/examples/slam/common/read_g2o.h
@@ -0,0 +1,141 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Reads a file in the g2o filename format that describes a pose graph problem.
+
+#ifndef EXAMPLES_CERES_READ_G2O_H_
+#define EXAMPLES_CERES_READ_G2O_H_
+
+#include <fstream>
+#include <string>
+
+#include "glog/logging.h"
+
+namespace ceres {
+namespace examples {
+
+// Reads a single pose from the input and inserts it into the map. Returns false
+// if there is a duplicate entry.
+template <typename Pose, typename Allocator>
+bool ReadVertex(std::ifstream* infile,
+ std::map<int, Pose, std::less<int>, Allocator>* poses) {
+ int id;
+ Pose pose;
+ *infile >> id >> pose;
+
+ // Ensure we don't have duplicate poses.
+ if (poses->find(id) != poses->end()) {
+ LOG(ERROR) << "Duplicate vertex with ID: " << id;
+ return false;
+ }
+ (*poses)[id] = pose;
+
+ return true;
+}
+
+// Reads the contraints between two vertices in the pose graph
+template <typename Constraint, typename Allocator>
+void ReadConstraint(std::ifstream* infile,
+ std::vector<Constraint, Allocator>* constraints) {
+ Constraint constraint;
+ *infile >> constraint;
+
+ constraints->push_back(constraint);
+}
+
+// Reads a file in the g2o filename format that describes a pose graph
+// problem. The g2o format consists of two entries, vertices and constraints.
+//
+// In 2D, a vertex is defined as follows:
+//
+// VERTEX_SE2 ID x_meters y_meters yaw_radians
+//
+// A constraint is defined as follows:
+//
+// EDGE_SE2 ID_A ID_B A_x_B A_y_B A_yaw_B I_11 I_12 I_13 I_22 I_23 I_33
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement.
+//
+//
+// In 3D, a vertex is defined as follows:
+//
+// VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w
+//
+// where the quaternion is in Hamilton form.
+// A constraint is defined as follows:
+//
+// EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT
+//
+// where I_ij is the (i, j)-th entry of the information matrix for the
+// measurement. Only the upper-triangular part is stored. The measurement order
+// is the delta position followed by the delta orientation.
+template <typename Pose, typename Constraint, typename MapAllocator,
+ typename VectorAllocator>
+bool ReadG2oFile(const std::string& filename,
+ std::map<int, Pose, std::less<int>, MapAllocator>* poses,
+ std::vector<Constraint, VectorAllocator>* constraints) {
+ CHECK(poses != NULL);
+ CHECK(constraints != NULL);
+
+ poses->clear();
+ constraints->clear();
+
+ std::ifstream infile(filename.c_str());
+ if (!infile) {
+ return false;
+ }
+
+ std::string data_type;
+ while (infile.good()) {
+ // Read whether the type is a node or a constraint.
+ infile >> data_type;
+ if (data_type == Pose::name()) {
+ if (!ReadVertex(&infile, poses)) {
+ return false;
+ }
+ } else if (data_type == Constraint::name()) {
+ ReadConstraint(&infile, constraints);
+ } else {
+ LOG(ERROR) << "Unknown data type: " << data_type;
+ return false;
+ }
+
+ // Clear any trailing whitespace from the line.
+ infile >> std::ws;
+ }
+
+ return true;
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_READ_G2O_H_
diff --git a/examples/slam/pose_graph_2d/CMakeLists.txt b/examples/slam/pose_graph_2d/CMakeLists.txt
new file mode 100644
index 0000000..d654e8c
--- /dev/null
+++ b/examples/slam/pose_graph_2d/CMakeLists.txt
@@ -0,0 +1,39 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2016 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: vitus@google.com (Michael Vitus)
+
+if (GFLAGS)
+ add_executable(pose_graph_2d
+ angle_local_parameterization.h
+ normalize_angle.h
+ pose_graph_2d.cc
+ pose_graph_2d_error_term.h
+ types.h)
+ target_link_libraries(pose_graph_2d ceres ${GFLAGS_LIBRARIES})
+endif (GFLAGS)
\ No newline at end of file
diff --git a/examples/slam/pose_graph_2d/README.md b/examples/slam/pose_graph_2d/README.md
new file mode 100644
index 0000000..b5ff987
--- /dev/null
+++ b/examples/slam/pose_graph_2d/README.md
@@ -0,0 +1,46 @@
+Pose Graph 2D
+----------------
+
+The Simultaneous Localization and Mapping (SLAM) problem consists of building a
+map of an unknown environment while simultaneously localizing against this
+map. The main difficulty of this problem stems from not having any additional
+external aiding information such as GPS. SLAM has been considered one of the
+fundamental challenges of robotics. A pose graph optimization problem is one
+example of a SLAM problem.
+
+This package defines the necessary Ceres cost functions needed to model the
+2-dimensional pose graph optimization problem as well as a binary to build and
+solve the problem. The cost functions are shown for instruction purposes and can
+be speed up by using analytical derivatives which take longer to implement.
+
+Running
+-----------
+This package includes an executable `pose_graph_2d` that will read a problem
+definition file. This executable can work with any 2D problem definition that
+uses the g2o format. It would be relatively straightforward to implement a new
+reader for a different format such as TORO or others. `pose_graph_2d` will print
+the Ceres solver full summary and then output to disk the original and optimized
+poses (`poses_original.txt` and `poses_optimized.txt`, respectively) of the
+robot in the following format:
+
+```
+pose_id x y yaw_radians
+pose_id x y yaw_radians
+pose_id x y yaw_radians
+...
+```
+
+where `pose_id` is the corresponding integer ID from the file definition. Note,
+the file will be sorted in ascending order for the `pose_id`.
+
+The executable `pose_graph_2d` has one flag `--input` which is the path to the
+problem definition. To run the executable,
+
+```
+/path/to/bin/pose_graph_2d --input /path/to/dataset/dataset.g2o
+```
+
+A python script is provided to visualize the resulting output files.
+```
+/path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
+```
diff --git a/examples/slam/pose_graph_2d/angle_local_parameterization.h b/examples/slam/pose_graph_2d/angle_local_parameterization.h
new file mode 100644
index 0000000..428cccc
--- /dev/null
+++ b/examples/slam/pose_graph_2d/angle_local_parameterization.h
@@ -0,0 +1,63 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
+
+#include "ceres/local_parameterization.h"
+#include "normalize_angle.h"
+
+namespace ceres {
+namespace examples {
+
+// Defines a local parameterization for updating the angle to be constrained in
+// [-pi to pi).
+class AngleLocalParameterization {
+ public:
+
+ template <typename T>
+ bool operator()(const T* theta_radians, const T* delta_theta_radians,
+ T* theta_radians_plus_delta) const {
+ *theta_radians_plus_delta =
+ NormalizeAngle(*theta_radians + *delta_theta_radians);
+
+ return true;
+ }
+
+ static ceres::LocalParameterization* Create() {
+ return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
+ 1, 1>);
+ }
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
diff --git a/examples/slam/pose_graph_2d/normalize_angle.h b/examples/slam/pose_graph_2d/normalize_angle.h
new file mode 100644
index 0000000..afd5df4
--- /dev/null
+++ b/examples/slam/pose_graph_2d/normalize_angle.h
@@ -0,0 +1,53 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
+
+#include <cmath>
+
+#include "ceres/ceres.h"
+
+namespace ceres {
+namespace examples {
+
+// Normalizes the angle in radians between [-pi and pi).
+template <typename T>
+inline T NormalizeAngle(const T& angle_radians) {
+ // Use ceres::floor because it is specialized for double and Jet types.
+ T two_pi(2.0 * M_PI);
+ return angle_radians -
+ two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
diff --git a/examples/slam/pose_graph_2d/plot_results.py b/examples/slam/pose_graph_2d/plot_results.py
new file mode 100755
index 0000000..2c4d16e
--- /dev/null
+++ b/examples/slam/pose_graph_2d/plot_results.py
@@ -0,0 +1,45 @@
+#!/usr/bin/python
+#
+# Plots the results from the 2D pose graph optimization. It will draw a line
+# between consecutive vertices. The commandline expects two optional filenames:
+#
+# ./plot_results.py --initial_poses optional --optimized_poses optional
+#
+# The files have the following format:
+# ID x y yaw_radians
+
+import matplotlib.pyplot as plot
+import numpy
+import sys
+from optparse import OptionParser
+
+parser = OptionParser()
+parser.add_option("--initial_poses", dest="initial_poses",
+ default="", help="The filename that contains the original poses.")
+parser.add_option("--optimized_poses", dest="optimized_poses",
+ default="", help="The filename that contains the optimized poses.")
+(options, args) = parser.parse_args()
+
+# Read the original and optimized poses files.
+poses_original = None
+if options.initial_poses != '':
+ poses_original = numpy.genfromtxt(options.initial_poses, usecols = (1, 2))
+
+poses_optimized = None
+if options.optimized_poses != '':
+ poses_optimized = numpy.genfromtxt(options.optimized_poses, usecols = (1, 2))
+
+# Plots the results for the specified poses.
+plot.figure()
+if poses_original is not None:
+ plot.plot(poses_original[:, 0], poses_original[:, 1], '-', label="Original",
+ alpha=0.5, color="green")
+
+if poses_optimized is not None:
+ plot.plot(poses_optimized[:, 0], poses_optimized[:, 1], '-', label="Optimized",
+ alpha=0.5, color="blue")
+
+plot.axis('equal')
+plot.legend()
+# Show the plot and wait for the user to close.
+plot.show()
diff --git a/examples/slam/pose_graph_2d/pose_graph_2d.cc b/examples/slam/pose_graph_2d/pose_graph_2d.cc
new file mode 100644
index 0000000..b9374db
--- /dev/null
+++ b/examples/slam/pose_graph_2d/pose_graph_2d.cc
@@ -0,0 +1,182 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// An example of solving a graph-based formulation of Simultaneous Localization
+// and Mapping (SLAM). It reads a 2D pose graph problem definition file in the
+// g2o format, formulates and solves the Ceres optimization problem, and outputs
+// the original and optimized poses to file for plotting.
+
+#include <fstream>
+#include <iostream>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "angle_local_parameterization.h"
+#include "ceres/ceres.h"
+#include "common/read_g2o.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "pose_graph_2d_error_term.h"
+#include "types.h"
+
+DEFINE_string(input, "", "The pose graph definition filename in g2o format.");
+
+namespace ceres {
+namespace examples {
+
+// Constructs the nonlinear least squares optimization problem from the pose
+// graph constraints.
+void BuildOptimizationProblem(const std::vector<Constraint2d>& constraints,
+ std::map<int, Pose2d>* poses,
+ ceres::Problem* problem) {
+ CHECK(poses != NULL);
+ CHECK(problem != NULL);
+ if (constraints.empty()) {
+ LOG(INFO) << "No constraints, no problem to optimize.";
+ return;
+ }
+
+ ceres::LossFunction* loss_function = NULL;
+ ceres::LocalParameterization* angle_local_parameterization =
+ AngleLocalParameterization::Create();
+
+ for (std::vector<Constraint2d>::const_iterator constraints_iter =
+ constraints.begin();
+ constraints_iter != constraints.end(); ++constraints_iter) {
+ const Constraint2d& constraint = *constraints_iter;
+
+ std::map<int, Pose2d>::iterator pose_begin_iter =
+ poses->find(constraint.id_begin);
+ CHECK(pose_begin_iter != poses->end())
+ << "Pose with ID: " << constraint.id_begin << " not found.";
+ std::map<int, Pose2d>::iterator pose_end_iter =
+ poses->find(constraint.id_end);
+ CHECK(pose_end_iter != poses->end())
+ << "Pose with ID: " << constraint.id_end << " not found.";
+
+ const Eigen::Matrix3d sqrt_information =
+ constraint.information.llt().matrixL();
+ // Ceres will take ownership of the pointer.
+ ceres::CostFunction* cost_function = PoseGraph2dErrorTerm::Create(
+ constraint.x, constraint.y, constraint.yaw_radians, sqrt_information);
+ problem->AddResidualBlock(
+ cost_function, loss_function, &pose_begin_iter->second.x,
+ &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians,
+ &pose_end_iter->second.x, &pose_end_iter->second.y,
+ &pose_end_iter->second.yaw_radians);
+
+ problem->SetParameterization(&pose_begin_iter->second.yaw_radians,
+ angle_local_parameterization);
+ problem->SetParameterization(&pose_end_iter->second.yaw_radians,
+ angle_local_parameterization);
+ }
+
+ // The pose graph optimization problem has three DOFs that are not fully
+ // constrained. This is typically referred to as gauge freedom. You can apply
+ // a rigid body transformation to all the nodes and the optimization problem
+ // will still have the exact same cost. The Levenberg-Marquardt algorithm has
+ // internal damping which mitigate this issue, but it is better to properly
+ // constrain the gauge freedom. This can be done by setting one of the poses
+ // as constant so the optimizer cannot change it.
+ std::map<int, Pose2d>::iterator pose_start_iter =
+ poses->begin();
+ CHECK(pose_start_iter != poses->end()) << "There are no poses.";
+ problem->SetParameterBlockConstant(&pose_start_iter->second.x);
+ problem->SetParameterBlockConstant(&pose_start_iter->second.y);
+ problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
+}
+
+// Returns true if the solve was successful.
+bool SolveOptimizationProblem(ceres::Problem* problem) {
+ CHECK(problem != NULL);
+
+ ceres::Solver::Options options;
+ options.max_num_iterations = 100;
+ options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, problem, &summary);
+
+ std::cout << summary.FullReport() << '\n';
+
+ return summary.IsSolutionUsable();
+}
+
+// Output the poses to the file with format: ID x y yaw_radians.
+bool OutputPoses(const std::string& filename,
+ const std::map<int, Pose2d>& poses) {
+ std::fstream outfile;
+ outfile.open(filename.c_str(), std::istream::out);
+ if (!outfile) {
+ std::cerr << "Error opening the file: " << filename << '\n';
+ return false;
+ }
+ for (std::map<int, Pose2d>::const_iterator poses_iter = poses.begin();
+ poses_iter != poses.end(); ++poses_iter) {
+ const std::map<int, Pose2d>::value_type& pair = *poses_iter;
+ outfile << pair.first << " " << pair.second.x << " " << pair.second.y
+ << ' ' << pair.second.yaw_radians << '\n';
+ }
+ return true;
+}
+
+} // namespace examples
+} // namespace ceres
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+
+ CHECK(FLAGS_input != "") << "Need to specify the filename to read.";
+
+ std::map<int, ceres::examples::Pose2d> poses;
+ std::vector<ceres::examples::Constraint2d> constraints;
+
+ CHECK(ceres::examples::ReadG2oFile(FLAGS_input, &poses, &constraints))
+ << "Error reading the file: " << FLAGS_input;
+
+ std::cout << "Number of poses: " << poses.size() << '\n';
+ std::cout << "Number of constraints: " << constraints.size() << '\n';
+
+ CHECK(ceres::examples::OutputPoses("poses_original.txt", poses))
+ << "Error outputting to poses_original.txt";
+
+ ceres::Problem problem;
+ ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);
+
+ CHECK(ceres::examples::SolveOptimizationProblem(&problem))
+ << "The solve was not successful, exiting.";
+
+ CHECK(ceres::examples::OutputPoses("poses_optimized.txt", poses))
+ << "Error outputting to poses_original.txt";
+
+ return 0;
+}
diff --git a/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h b/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h
new file mode 100644
index 0000000..20656d2
--- /dev/null
+++ b/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h
@@ -0,0 +1,112 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Cost function for a 2D pose graph formulation.
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
+
+#include "Eigen/Core"
+
+namespace ceres {
+namespace examples {
+
+template <typename T>
+Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
+ const T cos_yaw = ceres::cos(yaw_radians);
+ const T sin_yaw = ceres::sin(yaw_radians);
+
+ Eigen::Matrix<T, 2, 2> rotation;
+ rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
+ return rotation;
+}
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement.
+//
+// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
+// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
+//
+// where r_a is the rotation matrix that rotates a vector represented in frame A
+// into the global frame, and Normalize(*) ensures the angles are in the range
+// [-pi, pi).
+class PoseGraph2dErrorTerm {
+ public:
+ PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians,
+ const Eigen::Matrix3d& sqrt_information)
+ : p_ab_(x_ab, y_ab),
+ yaw_ab_radians_(yaw_ab_radians),
+ sqrt_information_(sqrt_information) {}
+
+ template <typename T>
+ bool operator()(const T* const x_a, const T* const y_a, const T* const yaw_a,
+ const T* const x_b, const T* const y_b, const T* const yaw_b,
+ T* residuals_ptr) const {
+ const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
+ const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
+
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals_map(residuals_ptr);
+
+ residuals_map.template head<2>() =
+ RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) -
+ p_ab_.cast<T>();
+ residuals_map(2) = ceres::examples::NormalizeAngle(
+ (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
+
+ // Scale the residuals by the square root information matrix to account for
+ // the measurement uncertainty.
+ residuals_map = sqrt_information_.template cast<T>() * residuals_map;
+
+ return true;
+ }
+
+ static ceres::CostFunction* Create(double x_ab, double y_ab,
+ double yaw_ab_radians,
+ const Eigen::Matrix3d& sqrt_information) {
+ return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1,
+ 1, 1>(new PoseGraph2dErrorTerm(
+ x_ab, y_ab, yaw_ab_radians, sqrt_information)));
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+ // The position of B relative to A in the A frame.
+ const Eigen::Vector2d p_ab_;
+ // The orientation of frame B relative to frame A.
+ const double yaw_ab_radians_;
+ // The inverse square root of the measurement covariance matrix.
+ const Eigen::Matrix3d sqrt_information_;
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
diff --git a/examples/slam/pose_graph_2d/types.h b/examples/slam/pose_graph_2d/types.h
new file mode 100644
index 0000000..a54d9bf
--- /dev/null
+++ b/examples/slam/pose_graph_2d/types.h
@@ -0,0 +1,105 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+//
+// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of
+// the graph has a unique integer ID with a position and orientation. There are
+// delta transformation constraints between two vertices.
+
+#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+#define CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+
+#include <fstream>
+
+#include "Eigen/Core"
+#include "normalize_angle.h"
+
+namespace ceres {
+namespace examples {
+
+// The state for each vertex in the pose graph.
+struct Pose2d {
+ double x;
+ double y;
+ double yaw_radians;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() {
+ return "VERTEX_SE2";
+ }
+};
+
+std::istream& operator>>(std::istream& input, Pose2d& pose) {
+ input >> pose.x >> pose.y >> pose.yaw_radians;
+ // Normalize the angle between -pi to pi.
+ pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
+ return input;
+}
+
+// The constraint between two vertices in the pose graph. The constraint is the
+// transformation from vertex id_begin to vertex id_end.
+struct Constraint2d {
+ int id_begin;
+ int id_end;
+
+ double x;
+ double y;
+ double yaw_radians;
+
+ // The inverse of the covariance matrix for the measurement. The order of the
+ // entries are x, y, and yaw.
+ Eigen::Matrix3d information;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() {
+ return "EDGE_SE2";
+ }
+};
+
+std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
+ input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
+ constraint.y >> constraint.yaw_radians >>
+ constraint.information(0, 0) >> constraint.information(0, 1) >>
+ constraint.information(0, 2) >> constraint.information(1, 1) >>
+ constraint.information(1, 2) >> constraint.information(2, 2);
+
+ // Set the lower triangular part of the information matrix.
+ constraint.information(1, 0) = constraint.information(0, 1);
+ constraint.information(2, 0) = constraint.information(0, 2);
+ constraint.information(2, 1) = constraint.information(1, 2);
+
+ // Normalize the angle between -pi to pi.
+ constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
+ return input;
+}
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
diff --git a/examples/slam/pose_graph_3d/CMakeLists.txt b/examples/slam/pose_graph_3d/CMakeLists.txt
new file mode 100644
index 0000000..2c5fdc3
--- /dev/null
+++ b/examples/slam/pose_graph_3d/CMakeLists.txt
@@ -0,0 +1,34 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2016 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: vitus@google.com (Michael Vitus)
+
+if (GFLAGS)
+ add_executable(pose_graph_3d pose_graph_3d.cc)
+ target_link_libraries(pose_graph_3d ceres ${GFLAGS_LIBRARIES})
+endif (GFLAGS)
\ No newline at end of file
diff --git a/examples/slam/pose_graph_3d/README.md b/examples/slam/pose_graph_3d/README.md
new file mode 100644
index 0000000..badc1ec
--- /dev/null
+++ b/examples/slam/pose_graph_3d/README.md
@@ -0,0 +1,54 @@
+Pose Graph 3D
+----------------
+
+The Simultaneous Localization and Mapping (SLAM) problem consists of building a
+map of an unknown environment while simultaneously localizing against this
+map. The main difficulty of this problem stems from not having any additional
+external aiding information such as GPS. SLAM has been considered one of the
+fundamental challenges of robotics. A pose graph optimization problem is one
+example of a SLAM problem.
+
+The example also illustrates how to use Eigen's geometry module with Ceres'
+automatic differentiation functionality. To represent the orientation, we will
+use Eigen's quaternion which uses the Hamiltonian convention but has different
+element ordering as compared with Ceres's rotation representation. Specifically
+they differ by whether the scalar component q_w is first or last; the element
+order for Ceres's quaternion is [q_w, q_x, q_y, q_z] where as Eigen's quaternion
+is [q_x, q_y, q_z, q_w].
+
+This package defines the necessary Ceres cost functions needed to model the
+3-dimensional pose graph optimization problem as well as a binary to build and
+solve the problem. The cost functions are shown for instruction purposes and can
+be speed up by using analytical derivatives which take longer to implement.
+
+
+Running
+-----------
+This package includes an executable `pose_graph_3d` that will read a problem
+definition file. This executable can work with any 3D problem definition that
+uses the g2o format with quaternions used for the orientation representation. It
+would be relatively straightforward to implement a new reader for a different
+format such as TORO or others. `pose_graph_3d` will print the Ceres solver full
+summary and then output to disk the original and optimized poses
+(`poses_original.txt` and `poses_optimized.txt`, respectively) of the robot in
+the following format:
+```
+pose_id x y z q_x q_y q_z q_w
+pose_id x y z q_x q_y q_z q_w
+pose_id x y z q_x q_y q_z q_w
+...
+```
+where `pose_id` is the corresponding integer ID from the file definition. Note,
+the file will be sorted in ascending order for the `pose_id`.
+
+The executable `pose_graph_3d` has one flag `--input` which is the path to the
+problem definition. To run the executable,
+```
+/path/to/bin/pose_graph_3d --input /path/to/dataset/dataset.g2o
+```
+
+A script is provided to visualize the resulting output files. There is also an
+option to enable equal axes using ```--axes_equal```.
+```
+/path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
+```
diff --git a/examples/slam/pose_graph_3d/plot_results.py b/examples/slam/pose_graph_3d/plot_results.py
new file mode 100755
index 0000000..defb9e6
--- /dev/null
+++ b/examples/slam/pose_graph_3d/plot_results.py
@@ -0,0 +1,80 @@
+#!/usr/bin/python
+#
+# Plots the results from the 3D pose graph optimization. It will draw a line
+# between consecutive vertices. The commandline expects two optional filenames:
+#
+# ./plot_results.py --initial_poses filename --optimized_poses filename
+#
+# The files have the following format:
+# ID x y z q_x q_y q_z q_w
+
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plot
+import numpy
+import sys
+from optparse import OptionParser
+
+def set_axes_equal(axes):
+ ''' Sets the axes of a 3D plot to have equal scale. '''
+ x_limits = axes.get_xlim3d()
+ y_limits = axes.get_ylim3d()
+ z_limits = axes.get_zlim3d()
+
+ x_range = abs(x_limits[1] - x_limits[0])
+ x_middle = numpy.mean(x_limits)
+ y_range = abs(y_limits[1] - y_limits[0])
+ y_middle = numpy.mean(y_limits)
+ z_range = abs(z_limits[1] - z_limits[0])
+ z_middle = numpy.mean(z_limits)
+
+ length = 0.5 * max([x_range, y_range, z_range])
+
+ axes.set_xlim3d([x_middle - length, x_middle + length])
+ axes.set_ylim3d([y_middle - length, y_middle + length])
+ axes.set_zlim3d([z_middle - length, z_middle + length])
+
+parser = OptionParser()
+parser.add_option("--initial_poses", dest="initial_poses",
+ default="", help="The filename that contains the original poses.")
+parser.add_option("--optimized_poses", dest="optimized_poses",
+ default="", help="The filename that contains the optimized poses.")
+parser.add_option("-e", "--axes_equal", action="store_true", dest="axes_equal",
+ default="", help="Make the plot axes equal.")
+(options, args) = parser.parse_args()
+
+# Read the original and optimized poses files.
+poses_original = None
+if options.initial_poses != '':
+ poses_original = numpy.genfromtxt(options.initial_poses,
+ usecols = (1, 2, 3))
+
+poses_optimized = None
+if options.optimized_poses != '':
+ poses_optimized = numpy.genfromtxt(options.optimized_poses,
+ usecols = (1, 2, 3))
+
+# Plots the results for the specified poses.
+figure = plot.figure()
+
+if poses_original is not None:
+ axes = plot.subplot(1, 2, 1, projection='3d')
+ plot.plot(poses_original[:, 0], poses_original[:, 1], poses_original[:, 2],
+ '-', alpha=0.5, color="green")
+ plot.title('Original')
+ if options.axes_equal:
+ axes.set_aspect('equal')
+ set_axes_equal(axes)
+
+
+if poses_optimized is not None:
+ axes = plot.subplot(1, 2, 2, projection='3d')
+ plot.plot(poses_optimized[:, 0], poses_optimized[:, 1], poses_optimized[:, 2],
+ '-', alpha=0.5, color="blue")
+ plot.title('Optimized')
+ if options.axes_equal:
+ axes.set_aspect('equal')
+ set_axes_equal(plot.gca())
+
+
+# Show the plot and wait for the user to close.
+plot.show()
diff --git a/examples/slam/pose_graph_3d/pose_graph_3d.cc b/examples/slam/pose_graph_3d/pose_graph_3d.cc
new file mode 100644
index 0000000..dcc85af
--- /dev/null
+++ b/examples/slam/pose_graph_3d/pose_graph_3d.cc
@@ -0,0 +1,174 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#include <iostream>
+#include <fstream>
+#include <string>
+
+#include "ceres/ceres.h"
+#include "common/read_g2o.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "pose_graph_3d_error_term.h"
+#include "types.h"
+
+DEFINE_string(input, "", "The pose graph definition filename in g2o format.");
+
+namespace ceres {
+namespace examples {
+
+// Constructs the nonlinear least squares optimization problem from the pose
+// graph constraints.
+void BuildOptimizationProblem(const VectorOfConstraints& constraints,
+ MapOfPoses* poses, ceres::Problem* problem) {
+ CHECK(poses != NULL);
+ CHECK(problem != NULL);
+ if (constraints.empty()) {
+ LOG(INFO) << "No constraints, no problem to optimize.";
+ return;
+ }
+
+ ceres::LossFunction* loss_function = NULL;
+ ceres::LocalParameterization* quaternion_local_parameterization =
+ new EigenQuaternionParameterization;
+
+ for (VectorOfConstraints::const_iterator constraints_iter =
+ constraints.begin();
+ constraints_iter != constraints.end(); ++constraints_iter) {
+ const Constraint3d& constraint = *constraints_iter;
+
+ MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
+ CHECK(pose_begin_iter != poses->end())
+ << "Pose with ID: " << constraint.id_begin << " not found.";
+ MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
+ CHECK(pose_end_iter != poses->end())
+ << "Pose with ID: " << constraint.id_end << " not found.";
+
+ const Eigen::Matrix<double, 6, 6> sqrt_information =
+ constraint.information.llt().matrixL();
+ // Ceres will take ownership of the pointer.
+ ceres::CostFunction* cost_function =
+ PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);
+
+ problem->AddResidualBlock(cost_function, loss_function,
+ pose_begin_iter->second.p.data(),
+ pose_begin_iter->second.q.coeffs().data(),
+ pose_end_iter->second.p.data(),
+ pose_end_iter->second.q.coeffs().data());
+
+ problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
+ problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
+ }
+
+ // The pose graph optimization problem has six DOFs that are not fully
+ // constrained. This is typically referred to as gauge freedom. You can apply
+ // a rigid body transformation to all the nodes and the optimization problem
+ // will still have the exact same cost. The Levenberg-Marquardt algorithm has
+ // internal damping which mitigates this issue, but it is better to properly
+ // constrain the gauge freedom. This can be done by setting one of the poses
+ // as constant so the optimizer cannot change it.
+ MapOfPoses::iterator pose_start_iter = poses->begin();
+ CHECK(pose_start_iter != poses->end()) << "There are no poses.";
+ problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
+ problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
+}
+
+// Returns true if the solve was successful.
+bool SolveOptimizationProblem(ceres::Problem* problem) {
+ CHECK(problem != NULL);
+
+ ceres::Solver::Options options;
+ options.max_num_iterations = 200;
+ options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, problem, &summary);
+
+ std::cout << summary.FullReport() << '\n';
+
+ return summary.IsSolutionUsable();
+}
+
+// Output the poses to the file with format: id x y z q_x q_y q_z q_w.
+bool OutputPoses(const std::string& filename, const MapOfPoses& poses) {
+ std::fstream outfile;
+ outfile.open(filename.c_str(), std::istream::out);
+ if (!outfile) {
+ LOG(ERROR) << "Error opening the file: " << filename;
+ return false;
+ }
+ for (std::map<int, Pose3d, std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
+ const_iterator poses_iter = poses.begin();
+ poses_iter != poses.end(); ++poses_iter) {
+ const std::map<int, Pose3d, std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Pose3d> > >::
+ value_type& pair = *poses_iter;
+ outfile << pair.first << " " << pair.second.p.transpose() << " "
+ << pair.second.q.x() << " " << pair.second.q.y() << " "
+ << pair.second.q.z() << " " << pair.second.q.w() << '\n';
+ }
+ return true;
+}
+
+} // namespace examples
+} // namespace ceres
+
+int main(int argc, char** argv) {
+ google::InitGoogleLogging(argv[0]);
+ CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
+
+ CHECK(FLAGS_input != "") << "Need to specify the filename to read.";
+
+ ceres::examples::MapOfPoses poses;
+ ceres::examples::VectorOfConstraints constraints;
+
+ CHECK(ceres::examples::ReadG2oFile(FLAGS_input, &poses, &constraints))
+ << "Error reading the file: " << FLAGS_input;
+
+ std::cout << "Number of poses: " << poses.size() << '\n';
+ std::cout << "Number of constraints: " << constraints.size() << '\n';
+
+ CHECK(ceres::examples::OutputPoses("poses_original.txt", poses))
+ << "Error outputting to poses_original.txt";
+
+ ceres::Problem problem;
+ ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);
+
+ CHECK(ceres::examples::SolveOptimizationProblem(&problem))
+ << "The solve was not successful, exiting.";
+
+ CHECK(ceres::examples::OutputPoses("poses_optimized.txt", poses))
+ << "Error outputting to poses_original.txt";
+
+ return 0;
+}
diff --git a/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h b/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h
new file mode 100644
index 0000000..aca819e
--- /dev/null
+++ b/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h
@@ -0,0 +1,131 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+#define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+
+#include "Eigen/Core"
+#include "ceres/autodiff_cost_function.h"
+
+#include "types.h"
+
+namespace ceres {
+namespace examples {
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement. We have two poses x_a
+// and x_b. Through sensor measurements we can measure the transformation of
+// frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
+// between the current estimate of the poses and the measurement.
+//
+// In this formulation, we have chosen to represent the rigid transformation as
+// a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
+// [x, y, z, w].
+
+// The estimated measurement is:
+// t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]
+// [ q_ab ] [ q_a^{-1] * q_b ]
+//
+// where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
+// quaternion. Now we can compute an error metric between the estimated and
+// measurement transformation. For the orientation error, we will use the
+// standard multiplicative error resulting in:
+//
+// error = [ p_ab - \hat{p}_ab ]
+// [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
+//
+// where Vec(*) returns the vector (imaginary) part of the quaternion. Since
+// the measurement has an uncertainty associated with how accurate it is, we
+// will weight the errors by the square root of the measurement information
+// matrix:
+//
+// residuals = I^{1/2) * error
+// where I is the information matrix which is the inverse of the covariance.
+class PoseGraph3dErrorTerm {
+ public:
+ PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
+ const Eigen::Matrix<double, 6, 6>& sqrt_information)
+ : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
+
+ template <typename T>
+ bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
+ const T* const p_b_ptr, const T* const q_b_ptr,
+ T* residuals_ptr) const {
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_a(p_a_ptr);
+ Eigen::Map<const Eigen::Quaternion<T> > q_a(q_a_ptr);
+
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_b(p_b_ptr);
+ Eigen::Map<const Eigen::Quaternion<T> > q_b(q_b_ptr);
+
+ // Compute the relative transformation between the two frames.
+ Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
+ Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
+
+ // Represent the displacement between the two frames in the A frame.
+ Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
+
+ // Compute the error between the two orientation estimates.
+ Eigen::Quaternion<T> delta_q =
+ t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
+
+ // Compute the residuals.
+ // [ position ] [ delta_p ]
+ // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
+ Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
+ residuals.template block<3, 1>(0, 0) =
+ p_ab_estimated - t_ab_measured_.p.template cast<T>();
+ residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
+
+ // Scale the residuals by the measurement uncertainty.
+ residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
+
+ return true;
+ }
+
+ static ceres::CostFunction* Create(
+ const Pose3d& t_ab_measured,
+ const Eigen::Matrix<double, 6, 6>& sqrt_information) {
+ return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
+ new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+ // The measurement for the position of B relative to A in the A frame.
+ const Pose3d t_ab_measured_;
+ // The square root of the measurement information matrix.
+ const Eigen::Matrix<double, 6, 6> sqrt_information_;
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
diff --git a/examples/slam/pose_graph_3d/types.h b/examples/slam/pose_graph_3d/types.h
new file mode 100644
index 0000000..2f12501
--- /dev/null
+++ b/examples/slam/pose_graph_3d/types.h
@@ -0,0 +1,114 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef EXAMPLES_CERES_TYPES_H_
+#define EXAMPLES_CERES_TYPES_H_
+
+#include <istream>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+
+namespace ceres {
+namespace examples {
+
+struct Pose3d {
+ Eigen::Vector3d p;
+ Eigen::Quaterniond q;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() {
+ return "VERTEX_SE3:QUAT";
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+std::istream& operator>>(std::istream& input, Pose3d& pose) {
+ input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >>
+ pose.q.y() >> pose.q.z() >> pose.q.w();
+ // Normalize the quaternion to account for precision loss due to
+ // serialization.
+ pose.q.normalize();
+ return input;
+}
+
+typedef std::map<int, Pose3d, std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Pose3d> > >
+ MapOfPoses;
+
+// The constraint between two vertices in the pose graph. The constraint is the
+// transformation from vertex id_begin to vertex id_end.
+struct Constraint3d {
+ int id_begin;
+ int id_end;
+
+ // The transformation that represents the pose of the end frame E w.r.t. the
+ // begin frame B. In other words, it transforms a vector in the E frame to
+ // the B frame.
+ Pose3d t_be;
+
+ // The inverse of the covariance matrix for the measurement. The order of the
+ // entries are x, y, z, delta orientation.
+ Eigen::Matrix<double, 6, 6> information;
+
+ // The name of the data type in the g2o file format.
+ static std::string name() {
+ return "EDGE_SE3:QUAT";
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
+ Pose3d& t_be = constraint.t_be;
+ input >> constraint.id_begin >> constraint.id_end >> t_be;
+
+ for (int i = 0; i < 6 && input.good(); ++i) {
+ for (int j = i; j < 6 && input.good(); ++j) {
+ input >> constraint.information(i, j);
+ if (i != j) {
+ constraint.information(j, i) = constraint.information(i, j);
+ }
+ }
+ }
+ return input;
+}
+
+typedef std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d> >
+ VectorOfConstraints;
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_TYPES_H_
diff --git a/examples/snavely_reprojection_error.h b/examples/snavely_reprojection_error.h
new file mode 100644
index 0000000..5e51eb4
--- /dev/null
+++ b/examples/snavely_reprojection_error.h
@@ -0,0 +1,177 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//
+// 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];
+ 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 xp = - p[0] / p[2];
+ const T yp = - p[1] / p[2];
+
+ // Apply second and fourth order radial distortion.
+ const T& l1 = camera[7];
+ const T& l2 = camera[8];
+ const T r2 = xp*xp + yp*yp;
+ const T distortion = 1.0 + r2 * (l1 + l2 * r2);
+
+
+ // Compute final projected point position.
+ const T& focal = camera[6];
+ const T predicted_x = focal * distortion * xp;
+ const T predicted_y = focal * distortion * yp;
+
+ // The error is the difference between the predicted and observed position.
+ residuals[0] = predicted_x - observed_x;
+ residuals[1] = predicted_y - observed_y;
+
+ return true;
+ }
+
+ // Factory to hide the construction of the CostFunction object from
+ // the client code.
+ static ceres::CostFunction* Create(const double observed_x,
+ const double observed_y) {
+ return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
+ new SnavelyReprojectionError(observed_x, observed_y)));
+ }
+
+ 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,
+ const T* const point,
+ T* residuals) const {
+ // camera[0,1,2,3] is are the rotation of the camera as a quaternion.
+ //
+ // We use QuaternionRotatePoint as it does not assume that the
+ // quaternion is normalized, since one of the ways to run the
+ // bundle adjuster is to let Ceres optimize all 4 quaternion
+ // parameters without a local parameterization.
+ T p[3];
+ QuaternionRotatePoint(camera, point, p);
+
+ p[0] += camera[4];
+ p[1] += camera[5];
+ p[2] += camera[6];
+
+ // 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 xp = - p[0] / p[2];
+ const T yp = - p[1] / p[2];
+
+ // Apply second and fourth order radial distortion.
+ const T& l1 = camera[8];
+ const T& l2 = camera[9];
+
+ const T r2 = xp*xp + yp*yp;
+ const T distortion = 1.0 + r2 * (l1 + l2 * r2);
+
+ // Compute final projected point position.
+ const T& focal = camera[7];
+ const T predicted_x = focal * distortion * xp;
+ const T predicted_y = focal * distortion * yp;
+
+ // The error is the difference between the predicted and observed position.
+ residuals[0] = predicted_x - observed_x;
+ residuals[1] = predicted_y - observed_y;
+
+ return true;
+ }
+
+ // Factory to hide the construction of the CostFunction object from
+ // the client code.
+ static ceres::CostFunction* Create(const double observed_x,
+ const double observed_y) {
+ return (new ceres::AutoDiffCostFunction<
+ SnavelyReprojectionErrorWithQuaternions, 2, 10, 3>(
+ new SnavelyReprojectionErrorWithQuaternions(observed_x,
+ observed_y)));
+ }
+
+ double observed_x;
+ double observed_y;
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_