Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h
new file mode 100644
index 0000000..db3f6af
--- /dev/null
+++ b/include/ceres/autodiff_cost_function.h
@@ -0,0 +1,209 @@
+// 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)
+//
+// Create CostFunctions as needed by the least squares framework, with
+// Jacobians computed via automatic differentiation. For more
+// information on automatic differentiation, see the wikipedia article
+// at http://en.wikipedia.org/wiki/Automatic_differentiation
+//
+// To get an auto differentiated cost function, you must define a class with a
+// templated operator() (a functor) that computes the cost function in terms of
+// the template parameter T. The autodiff framework substitutes appropriate
+// "jet" objects for T in order to compute the derivative when necessary, but
+// this is hidden, and you should write the function as if T were a scalar type
+// (e.g. a double-precision floating point number).
+//
+// The function must write the computed value in the last argument
+// (the only non-const one) and return true to indicate
+// success. Please see cost_function.h for details on how the return
+// value maybe used to impose simple constraints on the parameter
+// block.
+//
+// For example, consider a scalar error e = k - x'y, where both x and y are
+// two-dimensional column vector parameters, the prime sign indicates
+// transposition, and k is a constant. The form of this error, which is the
+// difference between a constant and an expression, is a common pattern in least
+// squares problems. For example, the value x'y might be the model expectation
+// for a series of measurements, where there is an instance of the cost function
+// for each measurement k.
+//
+// The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
+// the squaring is implicitly done by the optimization framework.
+//
+// To write an auto-differentiable cost function for the above model, first
+// define the object
+//
+// class MyScalarCostFunctor {
+// MyScalarCostFunctor(double k): k_(k) {}
+//
+// template <typename T>
+// bool operator()(const T* const x , const T* const y, T* e) const {
+// e[0] = T(k_) - x[0] * y[0] + x[1] * y[1];
+// return true;
+// }
+//
+// private:
+// double k_;
+// };
+//
+// Note that in the declaration of operator() the input parameters x and y come
+// first, and are passed as const pointers to arrays of T. If there were three
+// input parameters, then the third input parameter would come after y. The
+// output is always the last parameter, and is also a pointer to an array. In
+// the example above, e is a scalar, so only e[0] is set.
+//
+// Then given this class definition, the auto differentiated cost function for
+// it can be constructed as follows.
+//
+// CostFunction* cost_function
+// = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
+// new MyScalarCostFunctor(1.0)); ^ ^ ^
+// | | |
+// Dimension of residual -----+ | |
+// Dimension of x ---------------+ |
+// Dimension of y ------------------+
+//
+// In this example, there is usually an instance for each measurement of k.
+//
+// In the instantiation above, the template parameters following
+// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a
+// 1-dimensional output from two arguments, both 2-dimensional.
+//
+// AutoDiffCostFunction also supports cost functions with a
+// runtime-determined number of residuals. For example:
+//
+// CostFunction* cost_function
+// = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>(
+// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^
+// runtime_number_of_residuals); <----+ | | |
+// | | | |
+// | | | |
+// Actual number of residuals ------+ | | |
+// Indicate dynamic number of residuals --------+ | |
+// Dimension of x ------------------------------------+ |
+// Dimension of y ---------------------------------------+
+//
+// WARNING #1: Since the functor will get instantiated with different types for
+// T, you must convert from other numeric types to T before mixing
+// computations with other variables of type T. In the example above, this is
+// seen where instead of using k_ directly, k_ is wrapped with T(k_).
+//
+// WARNING #2: A common beginner's error when first using autodiff cost
+// functions is to get the sizing wrong. In particular, there is a tendency to
+// set the template parameters to (dimension of residual, number of parameters)
+// instead of passing a dimension parameter for *every parameter*. In the
+// example above, that would be <MyScalarCostFunctor, 1, 2>, which is missing
+// the last '2' argument. Please be careful when setting the size parameters.
+
+#ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
+#define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
+
+#include <memory>
+#include "ceres/internal/autodiff.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// A cost function which computes the derivative of the cost with respect to
+// the parameters (a.k.a. the jacobian) using an auto differentiation framework.
+// The first template argument is the functor object, described in the header
+// comment. The second argument is the dimension of the residual (or
+// ceres::DYNAMIC to indicate it will be set at runtime), and subsequent
+// arguments describe the size of the Nth parameter, one per parameter.
+//
+// The constructors take ownership of the cost functor.
+//
+// If the number of residuals (argument kNumResiduals below) is
+// ceres::DYNAMIC, then the two-argument constructor must be used. The
+// second constructor takes a number of residuals (in addition to the
+// templated number of residuals). This allows for varying the number
+// of residuals for a single autodiff cost function at runtime.
+template <typename CostFunctor,
+ int kNumResiduals, // Number of residuals, or ceres::DYNAMIC.
+ int... Ns> // Number of parameters in each parameter block.
+class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> {
+ public:
+ // Takes ownership of functor. Uses the template-provided value for the
+ // number of residuals ("kNumResiduals").
+ explicit AutoDiffCostFunction(CostFunctor* functor)
+ : functor_(functor) {
+ static_assert(kNumResiduals != DYNAMIC,
+ "Can't run the fixed-size constructor if the number of "
+ "residuals is set to ceres::DYNAMIC.");
+ }
+
+ // Takes ownership of functor. Ignores the template-provided
+ // kNumResiduals in favor of the "num_residuals" argument provided.
+ //
+ // This allows for having autodiff cost functions which return varying
+ // numbers of residuals at runtime.
+ AutoDiffCostFunction(CostFunctor* functor, int num_residuals)
+ : functor_(functor) {
+ static_assert(kNumResiduals == DYNAMIC,
+ "Can't run the dynamic-size constructor if the number of "
+ "residuals is not ceres::DYNAMIC.");
+ SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals);
+ }
+
+ virtual ~AutoDiffCostFunction() {}
+
+ // Implementation details follow; clients of the autodiff cost function should
+ // not have to examine below here.
+ //
+ // To handle variadic cost functions, some template magic is needed. It's
+ // mostly hidden inside autodiff.h.
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ using ParameterDims =
+ typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims;
+
+ if (!jacobians) {
+ return internal::VariadicEvaluate<ParameterDims>(*functor_,
+ parameters,
+ residuals);
+ }
+ return internal::AutoDifferentiate<ParameterDims>(
+ *functor_,
+ parameters,
+ SizedCostFunction<kNumResiduals, Ns...>::num_residuals(),
+ residuals,
+ jacobians);
+ }
+
+ private:
+ std::unique_ptr<CostFunctor> functor_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
diff --git a/include/ceres/autodiff_local_parameterization.h b/include/ceres/autodiff_local_parameterization.h
new file mode 100644
index 0000000..649e05d
--- /dev/null
+++ b/include/ceres/autodiff_local_parameterization.h
@@ -0,0 +1,151 @@
+// 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: sergey.vfx@gmail.com (Sergey Sharybin)
+// mierle@gmail.com (Keir Mierle)
+// sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
+#define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
+
+#include <memory>
+#include "ceres/local_parameterization.h"
+#include "ceres/internal/autodiff.h"
+
+namespace ceres {
+
+// Create local parameterization with Jacobians computed via automatic
+// differentiation. For more information on local parameterizations,
+// see include/ceres/local_parameterization.h
+//
+// To get an auto differentiated local parameterization, you must define
+// a class with a templated operator() (a functor) that computes
+//
+// x_plus_delta = Plus(x, delta);
+//
+// the template parameter T. The autodiff framework substitutes appropriate
+// "Jet" objects for T in order to compute the derivative when necessary, but
+// this is hidden, and you should write the function as if T were a scalar type
+// (e.g. a double-precision floating point number).
+//
+// The function must write the computed value in the last argument (the only
+// non-const one) and return true to indicate success.
+//
+// For example, Quaternions have a three dimensional local
+// parameterization. It's plus operation can be implemented as (taken
+// from internal/ceres/auto_diff_local_parameterization_test.cc)
+//
+// struct QuaternionPlus {
+// template<typename T>
+// bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
+// const T squared_norm_delta =
+// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
+//
+// T q_delta[4];
+// if (squared_norm_delta > T(0.0)) {
+// T norm_delta = sqrt(squared_norm_delta);
+// const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
+// q_delta[0] = cos(norm_delta);
+// q_delta[1] = sin_delta_by_delta * delta[0];
+// q_delta[2] = sin_delta_by_delta * delta[1];
+// q_delta[3] = sin_delta_by_delta * delta[2];
+// } else {
+// // We do not just use q_delta = [1,0,0,0] here because that is a
+// // constant and when used for automatic differentiation will
+// // lead to a zero derivative. Instead we take a first order
+// // approximation and evaluate it at zero.
+// q_delta[0] = T(1.0);
+// q_delta[1] = delta[0];
+// q_delta[2] = delta[1];
+// q_delta[3] = delta[2];
+// }
+//
+// QuaternionProduct(q_delta, x, x_plus_delta);
+// return true;
+// }
+// };
+//
+// Then given this struct, the auto differentiated local
+// parameterization can now be constructed as
+//
+// LocalParameterization* local_parameterization =
+// new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>;
+// | |
+// Global Size ---------------+ |
+// Local Size -------------------+
+//
+// WARNING: Since the functor will get instantiated with different types for
+// T, you must to convert from other numeric types to T before mixing
+// computations with other variables of type T. In the example above, this is
+// seen where instead of using k_ directly, k_ is wrapped with T(k_).
+
+template <typename Functor, int kGlobalSize, int kLocalSize>
+class AutoDiffLocalParameterization : public LocalParameterization {
+ public:
+ AutoDiffLocalParameterization() :
+ functor_(new Functor()) {}
+
+ // Takes ownership of functor.
+ explicit AutoDiffLocalParameterization(Functor* functor) :
+ functor_(functor) {}
+
+ virtual ~AutoDiffLocalParameterization() {}
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const {
+ return (*functor_)(x, delta, x_plus_delta);
+ }
+
+ virtual bool ComputeJacobian(const double* x, double* jacobian) const {
+ double zero_delta[kLocalSize];
+ for (int i = 0; i < kLocalSize; ++i) {
+ zero_delta[i] = 0.0;
+ }
+
+ double x_plus_delta[kGlobalSize];
+ for (int i = 0; i < kGlobalSize; ++i) {
+ x_plus_delta[i] = 0.0;
+ }
+
+ const double* parameter_ptrs[2] = {x, zero_delta};
+ double* jacobian_ptrs[2] = { NULL, jacobian };
+ return internal::AutoDifferentiate<
+ internal::StaticParameterDims<kGlobalSize, kLocalSize>>(
+ *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs);
+ }
+
+ virtual int GlobalSize() const { return kGlobalSize; }
+ virtual int LocalSize() const { return kLocalSize; }
+
+ private:
+ std::unique_ptr<Functor> functor_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
diff --git a/include/ceres/c_api.h b/include/ceres/c_api.h
new file mode 100644
index 0000000..df7c9b6
--- /dev/null
+++ b/include/ceres/c_api.h
@@ -0,0 +1,146 @@
+/* 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)
+ *
+ * A minimal C API for Ceres. Not all functionality is included. This API is
+ * not intended for clients of Ceres, but is instead intended for easing the
+ * process of binding Ceres to other languages.
+ *
+ * Currently this is a work in progress.
+ */
+
+#ifndef CERES_PUBLIC_C_API_H_
+#define CERES_PUBLIC_C_API_H_
+
+#include "ceres/internal/port.h"
+#include "ceres/internal/disable_warnings.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Init the Ceres private data. Must be called before anything else. */
+CERES_EXPORT void ceres_init();
+
+/* Equivalent to CostFunction::Evaluate() in the C++ API.
+ *
+ * The user may keep private information inside the opaque user_data object.
+ * The pointer here is the same one passed in the ceres_add_residual_block().
+ */
+typedef int (*ceres_cost_function_t)(void* user_data,
+ double** parameters,
+ double* residuals,
+ double** jacobians);
+
+/* Equivalent to LossFunction::Evaluate() from the C++ API. */
+typedef void (*ceres_loss_function_t)(void* user_data,
+ double squared_norm,
+ double out[3]);
+
+/* Create callback data for Ceres' stock loss functions.
+ *
+ * Ceres has several loss functions available by default, and these functions
+ * expose those to the C API. To use the stock loss functions, call
+ * ceres_create_*_loss_data(), which internally creates an instance of one of
+ * the stock loss functions (for example ceres::CauchyLoss), and pass the
+ * returned "loss_function_data" along with the ceres_stock_loss_function to
+ * ceres_add_residual_block().
+ *
+ * For example:
+ *
+ * void* cauchy_loss_function_data =
+ * ceres_create_cauchy_loss_function_data(1.2, 0.0);
+ * ceres_problem_add_residual_block(
+ * problem,
+ * my_cost_function,
+ * my_cost_function_data,
+ * ceres_stock_loss_function,
+ * cauchy_loss_function_data,
+ * 1,
+ * 2,
+ * parameter_sizes,
+ * parameter_pointers);
+ * ...
+ * ceres_free_stock_loss_function_data(cauchy_loss_function_data);
+ *
+ * See loss_function.h for the details of each loss function.
+ */
+CERES_EXPORT void* ceres_create_huber_loss_function_data(double a);
+CERES_EXPORT void* ceres_create_softl1_loss_function_data(double a);
+CERES_EXPORT void* ceres_create_cauchy_loss_function_data(double a);
+CERES_EXPORT void* ceres_create_arctan_loss_function_data(double a);
+CERES_EXPORT void* ceres_create_tolerant_loss_function_data(double a, double b);
+
+/* Free the given stock loss function data. */
+CERES_EXPORT void ceres_free_stock_loss_function_data(void* loss_function_data);
+
+/* This is an implementation of ceres_loss_function_t contained within Ceres
+ * itself, intended as a way to access the various stock Ceres loss functions
+ * from the C API. This should be passed to ceres_add_residual() below, in
+ * combination with a user_data pointer generated by
+ * ceres_create_stock_loss_function() above. */
+CERES_EXPORT void ceres_stock_loss_function(void* user_data,
+ double squared_norm,
+ double out[3]);
+
+/* Equivalent to Problem from the C++ API. */
+struct ceres_problem_s;
+typedef struct ceres_problem_s ceres_problem_t;
+
+struct ceres_residual_block_id_s;
+typedef struct ceres_residual_block_id_s ceres_residual_block_id_t;
+
+/* Create and destroy a problem */
+/* TODO(keir): Add options for the problem. */
+CERES_EXPORT ceres_problem_t* ceres_create_problem();
+CERES_EXPORT void ceres_free_problem(ceres_problem_t* problem);
+
+/* Add a residual block. */
+CERES_EXPORT ceres_residual_block_id_t* ceres_problem_add_residual_block(
+ ceres_problem_t* problem,
+ ceres_cost_function_t cost_function,
+ void* cost_function_data,
+ ceres_loss_function_t loss_function,
+ void* loss_function_data,
+ int num_residuals,
+ int num_parameter_blocks,
+ int* parameter_block_sizes,
+ double** parameters);
+
+CERES_EXPORT void ceres_solve(ceres_problem_t* problem);
+
+/* TODO(keir): Figure out a way to pass a config in. */
+
+#ifdef __cplusplus
+}
+#endif
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif /* CERES_PUBLIC_C_API_H_ */
diff --git a/include/ceres/ceres.h b/include/ceres/ceres.h
new file mode 100644
index 0000000..6e077cc
--- /dev/null
+++ b/include/ceres/ceres.h
@@ -0,0 +1,65 @@
+// 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 a forwarding header containing the public symbols exported from
+// Ceres. Anything in the "ceres" namespace is available for use.
+
+#ifndef CERES_PUBLIC_CERES_H_
+#define CERES_PUBLIC_CERES_H_
+
+#include "ceres/autodiff_cost_function.h"
+#include "ceres/autodiff_local_parameterization.h"
+#include "ceres/conditioned_cost_function.h"
+#include "ceres/context.h"
+#include "ceres/cost_function.h"
+#include "ceres/cost_function_to_functor.h"
+#include "ceres/covariance.h"
+#include "ceres/crs_matrix.h"
+#include "ceres/dynamic_autodiff_cost_function.h"
+#include "ceres/dynamic_cost_function.h"
+#include "ceres/dynamic_cost_function_to_functor.h"
+#include "ceres/dynamic_numeric_diff_cost_function.h"
+#include "ceres/gradient_checker.h"
+#include "ceres/gradient_problem.h"
+#include "ceres/gradient_problem_solver.h"
+#include "ceres/iteration_callback.h"
+#include "ceres/jet.h"
+#include "ceres/local_parameterization.h"
+#include "ceres/loss_function.h"
+#include "ceres/numeric_diff_cost_function.h"
+#include "ceres/numeric_diff_options.h"
+#include "ceres/ordered_groups.h"
+#include "ceres/problem.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/solver.h"
+#include "ceres/types.h"
+#include "ceres/version.h"
+
+#endif // CERES_PUBLIC_CERES_H_
diff --git a/include/ceres/conditioned_cost_function.h b/include/ceres/conditioned_cost_function.h
new file mode 100644
index 0000000..f92787e
--- /dev/null
+++ b/include/ceres/conditioned_cost_function.h
@@ -0,0 +1,101 @@
+// 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: wjr@google.com (William Rucklidge)
+//
+// This file contains a cost function that can apply a transformation to
+// each residual value before they are square-summed.
+
+#ifndef CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_
+#define CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_
+
+#include <vector>
+
+#include <memory>
+#include "ceres/cost_function.h"
+#include "ceres/types.h"
+#include "ceres/internal/disable_warnings.h"
+
+namespace ceres {
+
+// This class allows you to apply different conditioning to the residual
+// values of a wrapped cost function. An example where this is useful is
+// where you have an existing cost function that produces N values, but you
+// want the total cost to be something other than just the sum of these
+// squared values - maybe you want to apply a different scaling to some
+// values, to change their contribution to the cost.
+//
+// Usage:
+//
+// // my_cost_function produces N residuals
+// CostFunction* my_cost_function = ...
+// CHECK_EQ(N, my_cost_function->num_residuals());
+// vector<CostFunction*> conditioners;
+//
+// // Make N 1x1 cost functions (1 parameter, 1 residual)
+// CostFunction* f_1 = ...
+// conditioners.push_back(f_1);
+// ...
+// CostFunction* f_N = ...
+// conditioners.push_back(f_N);
+// ConditionedCostFunction* ccf =
+// new ConditionedCostFunction(my_cost_function, conditioners);
+//
+// Now ccf's residual i (i=0..N-1) will be passed though the i'th conditioner.
+//
+// ccf_residual[i] = f_i(my_cost_function_residual[i])
+//
+// and the Jacobian will be affected appropriately.
+class CERES_EXPORT ConditionedCostFunction : public CostFunction {
+ public:
+ // Builds a cost function based on a wrapped cost function, and a
+ // per-residual conditioner. Takes ownership of all of the wrapped cost
+ // functions, or not, depending on the ownership parameter. Conditioners
+ // may be NULL, in which case the corresponding residual is not modified.
+ //
+ // The conditioners can repeat.
+ ConditionedCostFunction(CostFunction* wrapped_cost_function,
+ const std::vector<CostFunction*>& conditioners,
+ Ownership ownership);
+ virtual ~ConditionedCostFunction();
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const;
+
+ private:
+ std::unique_ptr<CostFunction> wrapped_cost_function_;
+ std::vector<CostFunction*> conditioners_;
+ Ownership ownership_;
+};
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_
diff --git a/include/ceres/context.h b/include/ceres/context.h
new file mode 100644
index 0000000..cf7c436
--- /dev/null
+++ b/include/ceres/context.h
@@ -0,0 +1,56 @@
+// 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: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_PUBLIC_CONTEXT_H_
+#define CERES_PUBLIC_CONTEXT_H_
+
+namespace ceres {
+
+// A global context for processing data in Ceres. This provides a mechanism to
+// allow Ceres to reuse items that are expensive to create between multiple
+// calls; for example, thread pools. The same Context can be used on multiple
+// Problems, either serially or in parallel. When using it with multiple
+// Problems at the same time, they may end up contending for resources
+// (e.g. threads) managed by the Context.
+class Context {
+ public:
+ Context() {}
+ Context(const Context&) = delete;
+ void operator=(const Context&) = delete;
+
+ virtual ~Context() {}
+
+ // Creates a context object and the caller takes ownership.
+ static Context* Create();
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_CONTEXT_H_
diff --git a/include/ceres/cost_function.h b/include/ceres/cost_function.h
new file mode 100644
index 0000000..39425e8
--- /dev/null
+++ b/include/ceres/cost_function.h
@@ -0,0 +1,147 @@
+// 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)
+// keir@google.m (Keir Mierle)
+//
+// This is the interface through which the least squares solver accesses the
+// residual and Jacobian of the least squares problem. Users are expected to
+// subclass CostFunction to define their own terms in the least squares problem.
+//
+// It is recommended that users define templated residual functors for use as
+// arguments for AutoDiffCostFunction (see autodiff_cost_function.h), instead of
+// directly implementing the CostFunction interface. This often results in both
+// shorter code and faster execution than hand-coded derivatives. However,
+// specialized cases may demand direct implementation of the lower-level
+// CostFunction interface; for example, this is true when calling legacy code
+// which is not templated on numeric types.
+
+#ifndef CERES_PUBLIC_COST_FUNCTION_H_
+#define CERES_PUBLIC_COST_FUNCTION_H_
+
+#include <cstdint>
+#include <vector>
+#include "ceres/internal/port.h"
+#include "ceres/internal/disable_warnings.h"
+
+namespace ceres {
+
+// This class implements the computation of the cost (a.k.a. residual) terms as
+// a function of the input (control) variables, and is the interface for users
+// to describe their least squares problem to Ceres. In other words, this is the
+// modeling layer between users and the Ceres optimizer. The signature of the
+// function (number and sizes of input parameter blocks and number of outputs)
+// is stored in parameter_block_sizes_ and num_residuals_ respectively. User
+// code inheriting from this class is expected to set these two members with the
+// corresponding accessors. This information will be verified by the Problem
+// when added with AddResidualBlock().
+class CERES_EXPORT CostFunction {
+ public:
+ CostFunction() : num_residuals_(0) {}
+ CostFunction(const CostFunction&) = delete;
+ void operator=(const CostFunction&) = delete;
+
+ virtual ~CostFunction() {}
+
+ // Inputs:
+ //
+ // parameters is an array of pointers to arrays containing the
+ // various parameter blocks. parameters has the same number of
+ // elements as parameter_block_sizes_. Parameter blocks are in the
+ // same order as parameter_block_sizes_.i.e.,
+ //
+ // parameters_[i] = double[parameter_block_sizes_[i]]
+ //
+ // Outputs:
+ //
+ // residuals is an array of size num_residuals_.
+ //
+ // jacobians is an array of size parameter_block_sizes_ containing
+ // pointers to storage for jacobian blocks corresponding to each
+ // parameter block. Jacobian blocks are in the same order as
+ // parameter_block_sizes, i.e. jacobians[i], is an
+ // array that contains num_residuals_* parameter_block_sizes_[i]
+ // elements. Each jacobian block is stored in row-major order, i.e.,
+ //
+ // jacobians[i][r*parameter_block_size_[i] + c] =
+ // d residual[r] / d parameters[i][c]
+ //
+ // If jacobians is NULL, then no derivatives are returned; this is
+ // the case when computing cost only. If jacobians[i] is NULL, then
+ // the jacobian block corresponding to the i'th parameter block must
+ // not to be returned.
+ //
+ // The return value indicates whether the computation of the
+ // residuals and/or jacobians was successful or not.
+ //
+ // This can be used to communicate numerical failures in jacobian
+ // computations for instance.
+ //
+ // A more interesting and common use is to impose constraints on the
+ // parameters. If the initial values of the parameter blocks satisfy
+ // the constraints, then returning false whenever the constraints
+ // are not satisfied will prevent the solver from moving into the
+ // infeasible region. This is not a very sophisticated mechanism for
+ // enforcing constraints, but is often good enough.
+ //
+ // Note that it is important that the initial values of the
+ // parameter block must be feasible, otherwise the solver will
+ // declare a numerical problem at iteration 0.
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const = 0;
+
+ const std::vector<int32_t>& parameter_block_sizes() const {
+ return parameter_block_sizes_;
+ }
+
+ int num_residuals() const {
+ return num_residuals_;
+ }
+
+ protected:
+ std::vector<int32_t>* mutable_parameter_block_sizes() {
+ return ¶meter_block_sizes_;
+ }
+
+ void set_num_residuals(int num_residuals) {
+ num_residuals_ = num_residuals;
+ }
+
+ private:
+ // Cost function signature metadata: number of inputs & their sizes,
+ // number of outputs (residuals).
+ std::vector<int32_t> parameter_block_sizes_;
+ int num_residuals_;
+};
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_COST_FUNCTION_H_
diff --git a/include/ceres/cost_function_to_functor.h b/include/ceres/cost_function_to_functor.h
new file mode 100644
index 0000000..678c80c
--- /dev/null
+++ b/include/ceres/cost_function_to_functor.h
@@ -0,0 +1,171 @@
+// 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)
+//
+// CostFunctionToFunctor is an adapter class that allows users to use
+// SizedCostFunction objects in templated functors which are to be used for
+// automatic differentiation. This allows the user to seamlessly mix
+// analytic, numeric and automatic differentiation.
+//
+// For example, let us assume that
+//
+// class IntrinsicProjection : public SizedCostFunction<2, 5, 3> {
+// public:
+// IntrinsicProjection(const double* observation);
+// virtual bool Evaluate(double const* const* parameters,
+// double* residuals,
+// double** jacobians) const;
+// };
+//
+// is a cost function that implements the projection of a point in its
+// local coordinate system onto its image plane and subtracts it from
+// the observed point projection. It can compute its residual and
+// jacobians either via analytic or numerical differentiation.
+//
+// Now we would like to compose the action of this CostFunction with
+// the action of camera extrinsics, i.e., rotation and
+// translation. Say we have a templated function
+//
+// template<typename T>
+// void RotateAndTranslatePoint(const T* rotation,
+// const T* translation,
+// const T* point,
+// T* result);
+//
+// Then we can now do the following,
+//
+// struct CameraProjection {
+// CameraProjection(const double* observation)
+// : intrinsic_projection_(new IntrinsicProjection(observation)) {
+// }
+// template <typename T>
+// bool operator()(const T* rotation,
+// const T* translation,
+// const T* intrinsics,
+// const T* point,
+// T* residual) const {
+// T transformed_point[3];
+// RotateAndTranslatePoint(rotation, translation, point, transformed_point);
+//
+// // Note that we call intrinsic_projection_, just like it was
+// // any other templated functor.
+//
+// return intrinsic_projection_(intrinsics, transformed_point, residual);
+// }
+//
+// private:
+// CostFunctionToFunctor<2,5,3> intrinsic_projection_;
+// };
+
+#ifndef CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_
+#define CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_
+
+#include <cstdint>
+#include <numeric>
+#include <tuple>
+#include <vector>
+
+#include "ceres/cost_function.h"
+#include "ceres/dynamic_cost_function_to_functor.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/integer_sequence.h"
+#include "ceres/internal/parameter_dims.h"
+#include "ceres/internal/port.h"
+#include "ceres/types.h"
+
+namespace ceres {
+
+template <int kNumResiduals, int... Ns>
+class CostFunctionToFunctor {
+ public:
+ // Takes ownership of cost_function.
+ explicit CostFunctionToFunctor(CostFunction* cost_function)
+ : cost_functor_(cost_function) {
+ CHECK(cost_function != nullptr);
+ CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC);
+
+ const std::vector<int32_t>& parameter_block_sizes =
+ cost_function->parameter_block_sizes();
+ const int num_parameter_blocks = ParameterDims::kNumParameterBlocks;
+ CHECK_EQ(static_cast<int>(parameter_block_sizes.size()),
+ num_parameter_blocks);
+
+ if (parameter_block_sizes.size() == num_parameter_blocks) {
+ for (int block = 0; block < num_parameter_blocks; ++block) {
+ CHECK_EQ(ParameterDims::GetDim(block), parameter_block_sizes[block])
+ << "Parameter block size missmatch. The specified static parameter "
+ "block dimension does not match the one from the cost function.";
+ }
+ }
+
+ CHECK_EQ(accumulate(parameter_block_sizes.begin(),
+ parameter_block_sizes.end(), 0),
+ ParameterDims::kNumParameters);
+ }
+
+ template <typename T, typename... Ts>
+ bool operator()(const T* p1, Ts*... ps) const {
+ // Add one because of residual block.
+ static_assert(sizeof...(Ts) + 1 == ParameterDims::kNumParameterBlocks + 1,
+ "Invalid number of parameter blocks specified.");
+
+ auto params = std::make_tuple(p1, ps...);
+
+ // Extract residual pointer from params. The residual pointer is the
+ // last pointer.
+ constexpr int kResidualIndex = ParameterDims::kNumParameterBlocks;
+ T* residuals = std::get<kResidualIndex>(params);
+
+ // Extract parameter block pointers from params.
+ using Indices =
+ internal::make_integer_sequence<int,
+ ParameterDims::kNumParameterBlocks>;
+ std::array<const T*, ParameterDims::kNumParameterBlocks> parameter_blocks =
+ GetParameterPointers<T>(params, Indices());
+
+ return cost_functor_(parameter_blocks.data(), residuals);
+ }
+
+ private:
+ using ParameterDims = internal::StaticParameterDims<Ns...>;
+
+ template <typename T, typename Tuple, int... Indices>
+ static std::array<const T*, ParameterDims::kNumParameterBlocks>
+ GetParameterPointers(const Tuple& paramPointers,
+ internal::integer_sequence<int, Indices...>) {
+ return std::array<const T*, ParameterDims::kNumParameterBlocks>{
+ {std::get<Indices>(paramPointers)...}};
+ }
+
+ DynamicCostFunctionToFunctor cost_functor_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_
diff --git a/include/ceres/covariance.h b/include/ceres/covariance.h
new file mode 100644
index 0000000..da9f525
--- /dev/null
+++ b/include/ceres/covariance.h
@@ -0,0 +1,458 @@
+// 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_PUBLIC_COVARIANCE_H_
+#define CERES_PUBLIC_COVARIANCE_H_
+
+#include <memory>
+#include <utility>
+#include <vector>
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/port.h"
+#include "ceres/types.h"
+
+namespace ceres {
+
+class Problem;
+
+namespace internal {
+class CovarianceImpl;
+} // namespace internal
+
+// WARNING
+// =======
+// It is very easy to use this class incorrectly without understanding
+// the underlying mathematics. Please read and understand the
+// documentation completely before attempting to use this class.
+//
+//
+// This class allows the user to evaluate the covariance for a
+// non-linear least squares problem and provides random access to its
+// blocks
+//
+// Background
+// ==========
+// One way to assess the quality of the solution returned by a
+// non-linear least squares solver is to analyze the covariance of the
+// solution.
+//
+// Let us consider the non-linear regression problem
+//
+// y = f(x) + N(0, I)
+//
+// i.e., the observation y is a random non-linear function of the
+// independent variable x with mean f(x) and identity covariance. Then
+// the maximum likelihood estimate of x given observations y is the
+// solution to the non-linear least squares problem:
+//
+// x* = arg min_x |f(x)|^2
+//
+// And the covariance of x* is given by
+//
+// C(x*) = inverse[J'(x*)J(x*)]
+//
+// Here J(x*) is the Jacobian of f at x*. The above formula assumes
+// that J(x*) has full column rank.
+//
+// If J(x*) is rank deficient, then the covariance matrix C(x*) is
+// also rank deficient and is given by
+//
+// C(x*) = pseudoinverse[J'(x*)J(x*)]
+//
+// Note that in the above, we assumed that the covariance
+// matrix for y was identity. This is an important assumption. If this
+// is not the case and we have
+//
+// y = f(x) + N(0, S)
+//
+// Where S is a positive semi-definite matrix denoting the covariance
+// of y, then the maximum likelihood problem to be solved is
+//
+// x* = arg min_x f'(x) inverse[S] f(x)
+//
+// and the corresponding covariance estimate of x* is given by
+//
+// C(x*) = inverse[J'(x*) inverse[S] J(x*)]
+//
+// So, if it is the case that the observations being fitted to have a
+// covariance matrix not equal to identity, then it is the user's
+// responsibility that the corresponding cost functions are correctly
+// scaled, e.g. in the above case the cost function for this problem
+// should evaluate S^{-1/2} f(x) instead of just f(x), where S^{-1/2}
+// is the inverse square root of the covariance matrix S.
+//
+// This class allows the user to evaluate the covariance for a
+// non-linear least squares problem and provides random access to its
+// blocks. The computation assumes that the CostFunctions compute
+// residuals such that their covariance is identity.
+//
+// Since the computation of the covariance matrix requires computing
+// the inverse of a potentially large matrix, this can involve a
+// rather large amount of time and memory. However, it is usually the
+// case that the user is only interested in a small part of the
+// covariance matrix. Quite often just the block diagonal. This class
+// allows the user to specify the parts of the covariance matrix that
+// she is interested in and then uses this information to only compute
+// and store those parts of the covariance matrix.
+//
+// Rank of the Jacobian
+// --------------------
+// As we noted above, if the jacobian is rank deficient, then the
+// inverse of J'J is not defined and instead a pseudo inverse needs to
+// be computed.
+//
+// The rank deficiency in J can be structural -- columns which are
+// always known to be zero or numerical -- depending on the exact
+// values in the Jacobian.
+//
+// Structural rank deficiency occurs when the problem contains
+// parameter blocks that are constant. This class correctly handles
+// structural rank deficiency like that.
+//
+// Numerical rank deficiency, where the rank of the matrix cannot be
+// predicted by its sparsity structure and requires looking at its
+// numerical values is more complicated. Here again there are two
+// cases.
+//
+// a. The rank deficiency arises from overparameterization. e.g., a
+// four dimensional quaternion used to parameterize SO(3), which is
+// a three dimensional manifold. In cases like this, the user should
+// use an appropriate LocalParameterization. Not only will this lead
+// to better numerical behaviour of the Solver, it will also expose
+// the rank deficiency to the Covariance object so that it can
+// handle it correctly.
+//
+// b. More general numerical rank deficiency in the Jacobian
+// requires the computation of the so called Singular Value
+// Decomposition (SVD) of J'J. We do not know how to do this for
+// large sparse matrices efficiently. For small and moderate sized
+// problems this is done using dense linear algebra.
+//
+// Gauge Invariance
+// ----------------
+// In structure from motion (3D reconstruction) problems, the
+// reconstruction is ambiguous up to a similarity transform. This is
+// known as a Gauge Ambiguity. Handling Gauges correctly requires the
+// use of SVD or custom inversion algorithms. For small problems the
+// user can use the dense algorithm. For more details see
+//
+// Ken-ichi Kanatani, Daniel D. Morris: Gauges and gauge
+// transformations for uncertainty description of geometric structure
+// with indeterminacy. IEEE Transactions on Information Theory 47(5):
+// 2017-2028 (2001)
+//
+// Example Usage
+// =============
+//
+// double x[3];
+// double y[2];
+//
+// Problem problem;
+// problem.AddParameterBlock(x, 3);
+// problem.AddParameterBlock(y, 2);
+// <Build Problem>
+// <Solve Problem>
+//
+// Covariance::Options options;
+// Covariance covariance(options);
+//
+// std::vector<std::pair<const double*, const double*>> covariance_blocks;
+// covariance_blocks.push_back(make_pair(x, x));
+// covariance_blocks.push_back(make_pair(y, y));
+// covariance_blocks.push_back(make_pair(x, y));
+//
+// CHECK(covariance.Compute(covariance_blocks, &problem));
+//
+// double covariance_xx[3 * 3];
+// double covariance_yy[2 * 2];
+// double covariance_xy[3 * 2];
+// covariance.GetCovarianceBlock(x, x, covariance_xx)
+// covariance.GetCovarianceBlock(y, y, covariance_yy)
+// covariance.GetCovarianceBlock(x, y, covariance_xy)
+//
+class CERES_EXPORT Covariance {
+ public:
+ struct CERES_EXPORT Options {
+ // Sparse linear algebra library to use when a sparse matrix
+ // factorization is being used to compute the covariance matrix.
+ //
+ // Currently this only applies to SPARSE_QR.
+ SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type =
+#if !defined(CERES_NO_SUITESPARSE)
+ SUITE_SPARSE;
+#else
+ // Eigen's QR factorization is always available.
+ EIGEN_SPARSE;
+#endif
+
+ // Ceres supports two different algorithms for covariance
+ // estimation, which represent different tradeoffs in speed,
+ // accuracy and reliability.
+ //
+ // 1. DENSE_SVD uses Eigen's JacobiSVD to perform the
+ // computations. It computes the singular value decomposition
+ //
+ // U * S * V' = J
+ //
+ // and then uses it to compute the pseudo inverse of J'J as
+ //
+ // pseudoinverse[J'J]^ = V * pseudoinverse[S] * V'
+ //
+ // It is an accurate but slow method and should only be used
+ // for small to moderate sized problems. It can handle
+ // full-rank as well as rank deficient Jacobians.
+ //
+ // 2. SPARSE_QR uses the sparse QR factorization algorithm
+ // to compute the decomposition
+ //
+ // Q * R = J
+ //
+ // [J'J]^-1 = [R*R']^-1
+ //
+ // SPARSE_QR is not capable of computing the covariance if the
+ // Jacobian is rank deficient. Depending on the value of
+ // Covariance::Options::sparse_linear_algebra_library_type, either
+ // Eigen's Sparse QR factorization algorithm will be used or
+ // SuiteSparse's high performance SuiteSparseQR algorithm will be
+ // used.
+ CovarianceAlgorithmType algorithm_type = SPARSE_QR;
+
+ // If the Jacobian matrix is near singular, then inverting J'J
+ // will result in unreliable results, e.g, if
+ //
+ // J = [1.0 1.0 ]
+ // [1.0 1.0000001 ]
+ //
+ // which is essentially a rank deficient matrix, we have
+ //
+ // inv(J'J) = [ 2.0471e+14 -2.0471e+14]
+ // [-2.0471e+14 2.0471e+14]
+ //
+ // This is not a useful result. Therefore, by default
+ // Covariance::Compute will return false if a rank deficient
+ // Jacobian is encountered. How rank deficiency is detected
+ // depends on the algorithm being used.
+ //
+ // 1. DENSE_SVD
+ //
+ // min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)
+ //
+ // where min_sigma and max_sigma are the minimum and maxiumum
+ // singular values of J respectively.
+ //
+ // 2. SPARSE_QR
+ //
+ // rank(J) < num_col(J)
+ //
+ // Here rank(J) is the estimate of the rank of J returned by the
+ // sparse QR factorization algorithm. It is a fairly reliable
+ // indication of rank deficiency.
+ //
+ double min_reciprocal_condition_number = 1e-14;
+
+ // When using DENSE_SVD, the user has more control in dealing with
+ // singular and near singular covariance matrices.
+ //
+ // As mentioned above, when the covariance matrix is near
+ // singular, instead of computing the inverse of J'J, the
+ // Moore-Penrose pseudoinverse of J'J should be computed.
+ //
+ // If J'J has the eigen decomposition (lambda_i, e_i), where
+ // lambda_i is the i^th eigenvalue and e_i is the corresponding
+ // eigenvector, then the inverse of J'J is
+ //
+ // inverse[J'J] = sum_i e_i e_i' / lambda_i
+ //
+ // and computing the pseudo inverse involves dropping terms from
+ // this sum that correspond to small eigenvalues.
+ //
+ // How terms are dropped is controlled by
+ // min_reciprocal_condition_number and null_space_rank.
+ //
+ // If null_space_rank is non-negative, then the smallest
+ // null_space_rank eigenvalue/eigenvectors are dropped
+ // irrespective of the magnitude of lambda_i. If the ratio of the
+ // smallest non-zero eigenvalue to the largest eigenvalue in the
+ // truncated matrix is still below
+ // min_reciprocal_condition_number, then the Covariance::Compute()
+ // will fail and return false.
+ //
+ // Setting null_space_rank = -1 drops all terms for which
+ //
+ // lambda_i / lambda_max < min_reciprocal_condition_number.
+ //
+ // This option has no effect on the SUITE_SPARSE_QR and
+ // EIGEN_SPARSE_QR algorithms.
+ int null_space_rank = 0;
+
+ int num_threads = 1;
+
+ // Even though the residual blocks in the problem may contain loss
+ // functions, setting apply_loss_function to false will turn off
+ // the application of the loss function to the output of the cost
+ // function and in turn its effect on the covariance.
+ //
+ // TODO(sameergaarwal): Expand this based on Jim's experiments.
+ bool apply_loss_function = true;
+ };
+
+ explicit Covariance(const Options& options);
+ ~Covariance();
+
+ // Compute a part of the covariance matrix.
+ //
+ // The vector covariance_blocks, indexes into the covariance matrix
+ // block-wise using pairs of parameter blocks. This allows the
+ // covariance estimation algorithm to only compute and store these
+ // blocks.
+ //
+ // Since the covariance matrix is symmetric, if the user passes
+ // (block1, block2), then GetCovarianceBlock can be called with
+ // block1, block2 as well as block2, block1.
+ //
+ // covariance_blocks cannot contain duplicates. Bad things will
+ // happen if they do.
+ //
+ // Note that the list of covariance_blocks is only used to determine
+ // what parts of the covariance matrix are computed. The full
+ // Jacobian is used to do the computation, i.e. they do not have an
+ // impact on what part of the Jacobian is used for computation.
+ //
+ // The return value indicates the success or failure of the
+ // covariance computation. Please see the documentation for
+ // Covariance::Options for more on the conditions under which this
+ // function returns false.
+ bool Compute(
+ const std::vector<std::pair<const double*,
+ const double*>>& covariance_blocks,
+ Problem* problem);
+
+ // Compute a part of the covariance matrix.
+ //
+ // The vector parameter_blocks contains the parameter blocks that
+ // are used for computing the covariance matrix. From this vector
+ // all covariance pairs are generated. This allows the covariance
+ // estimation algorithm to only compute and store these blocks.
+ //
+ // parameter_blocks cannot contain duplicates. Bad things will
+ // happen if they do.
+ //
+ // Note that the list of covariance_blocks is only used to determine
+ // what parts of the covariance matrix are computed. The full
+ // Jacobian is used to do the computation, i.e. they do not have an
+ // impact on what part of the Jacobian is used for computation.
+ //
+ // The return value indicates the success or failure of the
+ // covariance computation. Please see the documentation for
+ // Covariance::Options for more on the conditions under which this
+ // function returns false.
+ bool Compute(const std::vector<const double*>& parameter_blocks,
+ Problem* problem);
+
+ // Return the block of the cross-covariance matrix corresponding to
+ // parameter_block1 and parameter_block2.
+ //
+ // Compute must be called before the first call to
+ // GetCovarianceBlock and the pair <parameter_block1,
+ // parameter_block2> OR the pair <parameter_block2,
+ // parameter_block1> must have been present in the vector
+ // covariance_blocks when Compute was called. Otherwise
+ // GetCovarianceBlock will return false.
+ //
+ // covariance_block must point to a memory location that can store a
+ // parameter_block1_size x parameter_block2_size matrix. The
+ // returned covariance will be a row-major matrix.
+ bool GetCovarianceBlock(const double* parameter_block1,
+ const double* parameter_block2,
+ double* covariance_block) const;
+
+ // Return the block of the cross-covariance matrix corresponding to
+ // parameter_block1 and parameter_block2.
+ // Returns cross-covariance in the tangent space if a local
+ // parameterization is associated with either parameter block;
+ // else returns cross-covariance in the ambient space.
+ //
+ // Compute must be called before the first call to
+ // GetCovarianceBlock and the pair <parameter_block1,
+ // parameter_block2> OR the pair <parameter_block2,
+ // parameter_block1> must have been present in the vector
+ // covariance_blocks when Compute was called. Otherwise
+ // GetCovarianceBlock will return false.
+ //
+ // covariance_block must point to a memory location that can store a
+ // parameter_block1_local_size x parameter_block2_local_size matrix. The
+ // returned covariance will be a row-major matrix.
+ bool GetCovarianceBlockInTangentSpace(const double* parameter_block1,
+ const double* parameter_block2,
+ double* covariance_block) const;
+
+ // Return the covariance matrix corresponding to all parameter_blocks.
+ //
+ // Compute must be called before calling GetCovarianceMatrix and all
+ // parameter_blocks must have been present in the vector
+ // parameter_blocks when Compute was called. Otherwise
+ // GetCovarianceMatrix returns false.
+ //
+ // covariance_matrix must point to a memory location that can store
+ // the size of the covariance matrix. The covariance matrix will be
+ // a square matrix whose row and column count is equal to the sum of
+ // the sizes of the individual parameter blocks. The covariance
+ // matrix will be a row-major matrix.
+ bool GetCovarianceMatrix(const std::vector<const double *> ¶meter_blocks,
+ double *covariance_matrix);
+
+ // Return the covariance matrix corresponding to parameter_blocks
+ // in the tangent space if a local parameterization is associated
+ // with one of the parameter blocks else returns the covariance
+ // matrix in the ambient space.
+ //
+ // Compute must be called before calling GetCovarianceMatrix and all
+ // parameter_blocks must have been present in the vector
+ // parameters_blocks when Compute was called. Otherwise
+ // GetCovarianceMatrix returns false.
+ //
+ // covariance_matrix must point to a memory location that can store
+ // the size of the covariance matrix. The covariance matrix will be
+ // a square matrix whose row and column count is equal to the sum of
+ // the sizes of the tangent spaces of the individual parameter
+ // blocks. The covariance matrix will be a row-major matrix.
+ bool GetCovarianceMatrixInTangentSpace(
+ const std::vector<const double*>& parameter_blocks,
+ double* covariance_matrix);
+
+ private:
+ std::unique_ptr<internal::CovarianceImpl> impl_;
+};
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_COVARIANCE_H_
diff --git a/include/ceres/crs_matrix.h b/include/ceres/crs_matrix.h
new file mode 100644
index 0000000..23687c4
--- /dev/null
+++ b/include/ceres/crs_matrix.h
@@ -0,0 +1,86 @@
+// 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_PUBLIC_CRS_MATRIX_H_
+#define CERES_PUBLIC_CRS_MATRIX_H_
+
+#include <vector>
+#include "ceres/internal/port.h"
+#include "ceres/internal/disable_warnings.h"
+
+namespace ceres {
+
+// A compressed row sparse matrix used primarily for communicating the
+// Jacobian matrix to the user.
+struct CERES_EXPORT CRSMatrix {
+ CRSMatrix() : num_rows(0), num_cols(0) {}
+
+ int num_rows;
+ int num_cols;
+
+ // A compressed row matrix stores its contents in three arrays,
+ // rows, cols and values.
+ //
+ // rows is a num_rows + 1 sized array that points into the cols and
+ // values array. For each row i:
+ //
+ // cols[rows[i]] ... cols[rows[i + 1] - 1] are the indices of the
+ // non-zero columns of row i.
+ //
+ // values[rows[i]] .. values[rows[i + 1] - 1] are the values of the
+ // corresponding entries.
+ //
+ // cols and values contain as many entries as there are non-zeros in
+ // the matrix.
+ //
+ // e.g, consider the 3x4 sparse matrix
+ //
+ // [ 0 10 0 4 ]
+ // [ 0 2 -3 2 ]
+ // [ 1 2 0 0 ]
+ //
+ // The three arrays will be:
+ //
+ //
+ // -row0- ---row1--- -row2-
+ // rows = [ 0, 2, 5, 7]
+ // cols = [ 1, 3, 1, 2, 3, 0, 1]
+ // values = [10, 4, 2, -3, 2, 1, 2]
+
+ std::vector<int> cols;
+ std::vector<int> rows;
+ std::vector<double> values;
+};
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_CRS_MATRIX_H_
diff --git a/include/ceres/cubic_interpolation.h b/include/ceres/cubic_interpolation.h
new file mode 100644
index 0000000..88a3ebf
--- /dev/null
+++ b/include/ceres/cubic_interpolation.h
@@ -0,0 +1,439 @@
+// 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_PUBLIC_CUBIC_INTERPOLATION_H_
+#define CERES_PUBLIC_CUBIC_INTERPOLATION_H_
+
+#include "ceres/internal/port.h"
+#include "Eigen/Core"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// Given samples from a function sampled at four equally spaced points,
+//
+// p0 = f(-1)
+// p1 = f(0)
+// p2 = f(1)
+// p3 = f(2)
+//
+// Evaluate the cubic Hermite spline (also known as the Catmull-Rom
+// spline) at a point x that lies in the interval [0, 1].
+//
+// This is also the interpolation kernel (for the case of a = 0.5) as
+// proposed by R. Keys, in:
+//
+// "Cubic convolution interpolation for digital image processing".
+// IEEE Transactions on Acoustics, Speech, and Signal Processing
+// 29 (6): 1153–1160.
+//
+// For more details see
+//
+// http://en.wikipedia.org/wiki/Cubic_Hermite_spline
+// http://en.wikipedia.org/wiki/Bicubic_interpolation
+//
+// f if not NULL will contain the interpolated function values.
+// dfdx if not NULL will contain the interpolated derivative values.
+template <int kDataDimension>
+void CubicHermiteSpline(const Eigen::Matrix<double, kDataDimension, 1>& p0,
+ const Eigen::Matrix<double, kDataDimension, 1>& p1,
+ const Eigen::Matrix<double, kDataDimension, 1>& p2,
+ const Eigen::Matrix<double, kDataDimension, 1>& p3,
+ const double x,
+ double* f,
+ double* dfdx) {
+ typedef Eigen::Matrix<double, kDataDimension, 1> VType;
+ const VType a = 0.5 * (-p0 + 3.0 * p1 - 3.0 * p2 + p3);
+ const VType b = 0.5 * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3);
+ const VType c = 0.5 * (-p0 + p2);
+ const VType d = p1;
+
+ // Use Horner's rule to evaluate the function value and its
+ // derivative.
+
+ // f = ax^3 + bx^2 + cx + d
+ if (f != NULL) {
+ Eigen::Map<VType>(f, kDataDimension) = d + x * (c + x * (b + x * a));
+ }
+
+ // dfdx = 3ax^2 + 2bx + c
+ if (dfdx != NULL) {
+ Eigen::Map<VType>(dfdx, kDataDimension) = c + x * (2.0 * b + 3.0 * a * x);
+ }
+}
+
+// Given as input an infinite one dimensional grid, which provides the
+// following interface.
+//
+// class Grid {
+// public:
+// enum { DATA_DIMENSION = 2; };
+// void GetValue(int n, double* f) const;
+// };
+//
+// Here, GetValue gives the value of a function f (possibly vector
+// valued) for any integer n.
+//
+// The enum DATA_DIMENSION indicates the dimensionality of the
+// function being interpolated. For example if you are interpolating
+// rotations in axis-angle format over time, then DATA_DIMENSION = 3.
+//
+// CubicInterpolator uses cubic Hermite splines to produce a smooth
+// approximation to it that can be used to evaluate the f(x) and f'(x)
+// at any point on the real number line.
+//
+// For more details on cubic interpolation see
+//
+// http://en.wikipedia.org/wiki/Cubic_Hermite_spline
+//
+// Example usage:
+//
+// const double data[] = {1.0, 2.0, 5.0, 6.0};
+// Grid1D<double, 1> grid(x, 0, 4);
+// CubicInterpolator<Grid1D<double, 1>> interpolator(grid);
+// double f, dfdx;
+// interpolator.Evaluator(1.5, &f, &dfdx);
+template<typename Grid>
+class CubicInterpolator {
+ public:
+ explicit CubicInterpolator(const Grid& grid)
+ : grid_(grid) {
+ // The + casts the enum into an int before doing the
+ // comparison. It is needed to prevent
+ // "-Wunnamed-type-template-args" related errors.
+ CHECK_GE(+Grid::DATA_DIMENSION, 1);
+ }
+
+ void Evaluate(double x, double* f, double* dfdx) const {
+ const int n = std::floor(x);
+ Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;
+ grid_.GetValue(n - 1, p0.data());
+ grid_.GetValue(n, p1.data());
+ grid_.GetValue(n + 1, p2.data());
+ grid_.GetValue(n + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, x - n, f, dfdx);
+ }
+
+ // The following two Evaluate overloads are needed for interfacing
+ // with automatic differentiation. The first is for when a scalar
+ // evaluation is done, and the second one is for when Jets are used.
+ void Evaluate(const double& x, double* f) const {
+ Evaluate(x, f, NULL);
+ }
+
+ template<typename JetT> void Evaluate(const JetT& x, JetT* f) const {
+ double fx[Grid::DATA_DIMENSION], dfdx[Grid::DATA_DIMENSION];
+ Evaluate(x.a, fx, dfdx);
+ for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {
+ f[i].a = fx[i];
+ f[i].v = dfdx[i] * x.v;
+ }
+ }
+
+ private:
+ const Grid& grid_;
+};
+
+// An object that implements an infinite one dimensional grid needed
+// by the CubicInterpolator where the source of the function values is
+// an array of type T on the interval
+//
+// [begin, ..., end - 1]
+//
+// Since the input array is finite and the grid is infinite, values
+// outside this interval needs to be computed. Grid1D uses the value
+// from the nearest edge.
+//
+// The function being provided can be vector valued, in which case
+// kDataDimension > 1. The dimensional slices of the function maybe
+// interleaved, or they maybe stacked, i.e, if the function has
+// kDataDimension = 2, if kInterleaved = true, then it is stored as
+//
+// f01, f02, f11, f12 ....
+//
+// and if kInterleaved = false, then it is stored as
+//
+// f01, f11, .. fn1, f02, f12, .. , fn2
+//
+template <typename T,
+ int kDataDimension = 1,
+ bool kInterleaved = true>
+struct Grid1D {
+ public:
+ enum { DATA_DIMENSION = kDataDimension };
+
+ Grid1D(const T* data, const int begin, const int end)
+ : data_(data), begin_(begin), end_(end), num_values_(end - begin) {
+ CHECK_LT(begin, end);
+ }
+
+ EIGEN_STRONG_INLINE void GetValue(const int n, double* f) const {
+ const int idx = std::min(std::max(begin_, n), end_ - 1) - begin_;
+ if (kInterleaved) {
+ for (int i = 0; i < kDataDimension; ++i) {
+ f[i] = static_cast<double>(data_[kDataDimension * idx + i]);
+ }
+ } else {
+ for (int i = 0; i < kDataDimension; ++i) {
+ f[i] = static_cast<double>(data_[i * num_values_ + idx]);
+ }
+ }
+ }
+
+ private:
+ const T* data_;
+ const int begin_;
+ const int end_;
+ const int num_values_;
+};
+
+// Given as input an infinite two dimensional grid like object, which
+// provides the following interface:
+//
+// struct Grid {
+// enum { DATA_DIMENSION = 1 };
+// void GetValue(int row, int col, double* f) const;
+// };
+//
+// Where, GetValue gives us the value of a function f (possibly vector
+// valued) for any pairs of integers (row, col), and the enum
+// DATA_DIMENSION indicates the dimensionality of the function being
+// interpolated. For example if you are interpolating a color image
+// with three channels (Red, Green & Blue), then DATA_DIMENSION = 3.
+//
+// BiCubicInterpolator uses the cubic convolution interpolation
+// algorithm of R. Keys, to produce a smooth approximation to it that
+// can be used to evaluate the f(r,c), df(r, c)/dr and df(r,c)/dc at
+// any point in the real plane.
+//
+// For more details on the algorithm used here see:
+//
+// "Cubic convolution interpolation for digital image processing".
+// Robert G. Keys, IEEE Trans. on Acoustics, Speech, and Signal
+// Processing 29 (6): 1153–1160, 1981.
+//
+// http://en.wikipedia.org/wiki/Cubic_Hermite_spline
+// http://en.wikipedia.org/wiki/Bicubic_interpolation
+//
+// Example usage:
+//
+// const double data[] = {1.0, 3.0, -1.0, 4.0,
+// 3.6, 2.1, 4.2, 2.0,
+// 2.0, 1.0, 3.1, 5.2};
+// Grid2D<double, 1> grid(data, 3, 4);
+// BiCubicInterpolator<Grid2D<double, 1>> interpolator(grid);
+// double f, dfdr, dfdc;
+// interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc);
+
+template<typename Grid>
+class BiCubicInterpolator {
+ public:
+ explicit BiCubicInterpolator(const Grid& grid)
+ : grid_(grid) {
+ // The + casts the enum into an int before doing the
+ // comparison. It is needed to prevent
+ // "-Wunnamed-type-template-args" related errors.
+ CHECK_GE(+Grid::DATA_DIMENSION, 1);
+ }
+
+ // Evaluate the interpolated function value and/or its
+ // derivative. Returns false if r or c is out of bounds.
+ void Evaluate(double r, double c,
+ double* f, double* dfdr, double* dfdc) const {
+ // BiCubic interpolation requires 16 values around the point being
+ // evaluated. We will use pij, to indicate the elements of the
+ // 4x4 grid of values.
+ //
+ // col
+ // p00 p01 p02 p03
+ // row p10 p11 p12 p13
+ // p20 p21 p22 p23
+ // p30 p31 p32 p33
+ //
+ // The point (r,c) being evaluated is assumed to lie in the square
+ // defined by p11, p12, p22 and p21.
+
+ const int row = std::floor(r);
+ const int col = std::floor(c);
+
+ Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;
+
+ // Interpolate along each of the four rows, evaluating the function
+ // value and the horizontal derivative in each row.
+ Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> f0, f1, f2, f3;
+ Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> df0dc, df1dc, df2dc, df3dc;
+
+ grid_.GetValue(row - 1, col - 1, p0.data());
+ grid_.GetValue(row - 1, col , p1.data());
+ grid_.GetValue(row - 1, col + 1, p2.data());
+ grid_.GetValue(row - 1, col + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
+ f0.data(), df0dc.data());
+
+ grid_.GetValue(row, col - 1, p0.data());
+ grid_.GetValue(row, col , p1.data());
+ grid_.GetValue(row, col + 1, p2.data());
+ grid_.GetValue(row, col + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
+ f1.data(), df1dc.data());
+
+ grid_.GetValue(row + 1, col - 1, p0.data());
+ grid_.GetValue(row + 1, col , p1.data());
+ grid_.GetValue(row + 1, col + 1, p2.data());
+ grid_.GetValue(row + 1, col + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
+ f2.data(), df2dc.data());
+
+ grid_.GetValue(row + 2, col - 1, p0.data());
+ grid_.GetValue(row + 2, col , p1.data());
+ grid_.GetValue(row + 2, col + 1, p2.data());
+ grid_.GetValue(row + 2, col + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
+ f3.data(), df3dc.data());
+
+ // Interpolate vertically the interpolated value from each row and
+ // compute the derivative along the columns.
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(f0, f1, f2, f3, r - row, f, dfdr);
+ if (dfdc != NULL) {
+ // Interpolate vertically the derivative along the columns.
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(df0dc, df1dc, df2dc, df3dc,
+ r - row, dfdc, NULL);
+ }
+ }
+
+ // The following two Evaluate overloads are needed for interfacing
+ // with automatic differentiation. The first is for when a scalar
+ // evaluation is done, and the second one is for when Jets are used.
+ void Evaluate(const double& r, const double& c, double* f) const {
+ Evaluate(r, c, f, NULL, NULL);
+ }
+
+ template<typename JetT> void Evaluate(const JetT& r,
+ const JetT& c,
+ JetT* f) const {
+ double frc[Grid::DATA_DIMENSION];
+ double dfdr[Grid::DATA_DIMENSION];
+ double dfdc[Grid::DATA_DIMENSION];
+ Evaluate(r.a, c.a, frc, dfdr, dfdc);
+ for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {
+ f[i].a = frc[i];
+ f[i].v = dfdr[i] * r.v + dfdc[i] * c.v;
+ }
+ }
+
+ private:
+ const Grid& grid_;
+};
+
+// An object that implements an infinite two dimensional grid needed
+// by the BiCubicInterpolator where the source of the function values
+// is an grid of type T on the grid
+//
+// [(row_start, col_start), ..., (row_start, col_end - 1)]
+// [ ... ]
+// [(row_end - 1, col_start), ..., (row_end - 1, col_end - 1)]
+//
+// Since the input grid is finite and the grid is infinite, values
+// outside this interval needs to be computed. Grid2D uses the value
+// from the nearest edge.
+//
+// The function being provided can be vector valued, in which case
+// kDataDimension > 1. The data maybe stored in row or column major
+// format and the various dimensional slices of the function maybe
+// interleaved, or they maybe stacked, i.e, if the function has
+// kDataDimension = 2, is stored in row-major format and if
+// kInterleaved = true, then it is stored as
+//
+// f001, f002, f011, f012, ...
+//
+// A commonly occuring example are color images (RGB) where the three
+// channels are stored interleaved.
+//
+// If kInterleaved = false, then it is stored as
+//
+// f001, f011, ..., fnm1, f002, f012, ...
+template <typename T,
+ int kDataDimension = 1,
+ bool kRowMajor = true,
+ bool kInterleaved = true>
+struct Grid2D {
+ public:
+ enum { DATA_DIMENSION = kDataDimension };
+
+ Grid2D(const T* data,
+ const int row_begin, const int row_end,
+ const int col_begin, const int col_end)
+ : data_(data),
+ row_begin_(row_begin), row_end_(row_end),
+ col_begin_(col_begin), col_end_(col_end),
+ num_rows_(row_end - row_begin), num_cols_(col_end - col_begin),
+ num_values_(num_rows_ * num_cols_) {
+ CHECK_GE(kDataDimension, 1);
+ CHECK_LT(row_begin, row_end);
+ CHECK_LT(col_begin, col_end);
+ }
+
+ EIGEN_STRONG_INLINE void GetValue(const int r, const int c, double* f) const {
+ const int row_idx =
+ std::min(std::max(row_begin_, r), row_end_ - 1) - row_begin_;
+ const int col_idx =
+ std::min(std::max(col_begin_, c), col_end_ - 1) - col_begin_;
+
+ const int n =
+ (kRowMajor)
+ ? num_cols_ * row_idx + col_idx
+ : num_rows_ * col_idx + row_idx;
+
+
+ if (kInterleaved) {
+ for (int i = 0; i < kDataDimension; ++i) {
+ f[i] = static_cast<double>(data_[kDataDimension * n + i]);
+ }
+ } else {
+ for (int i = 0; i < kDataDimension; ++i) {
+ f[i] = static_cast<double>(data_[i * num_values_ + n]);
+ }
+ }
+ }
+
+ private:
+ const T* data_;
+ const int row_begin_;
+ const int row_end_;
+ const int col_begin_;
+ const int col_end_;
+ const int num_rows_;
+ const int num_cols_;
+ const int num_values_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_CUBIC_INTERPOLATOR_H_
diff --git a/include/ceres/dynamic_autodiff_cost_function.h b/include/ceres/dynamic_autodiff_cost_function.h
new file mode 100644
index 0000000..1bfb7a5
--- /dev/null
+++ b/include/ceres/dynamic_autodiff_cost_function.h
@@ -0,0 +1,252 @@
+// 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)
+// mierle@gmail.com (Keir Mierle)
+
+#ifndef CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
+#define CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
+
+#include <cmath>
+#include <numeric>
+#include <vector>
+
+#include <memory>
+#include "ceres/dynamic_cost_function.h"
+#include "ceres/jet.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// This autodiff implementation differs from the one found in
+// autodiff_cost_function.h by supporting autodiff on cost functions
+// with variable numbers of parameters with variable sizes. With the
+// other implementation, all the sizes (both the number of parameter
+// blocks and the size of each block) must be fixed at compile time.
+//
+// The functor API differs slightly from the API for fixed size
+// autodiff; the expected interface for the cost functors is:
+//
+// struct MyCostFunctor {
+// template<typename T>
+// bool operator()(T const* const* parameters, T* residuals) const {
+// // Use parameters[i] to access the i'th parameter block.
+// }
+// };
+//
+// Since the sizing of the parameters is done at runtime, you must
+// also specify the sizes after creating the dynamic autodiff cost
+// function. For example:
+//
+// DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function(
+// new MyCostFunctor());
+// cost_function.AddParameterBlock(5);
+// cost_function.AddParameterBlock(10);
+// cost_function.SetNumResiduals(21);
+//
+// Under the hood, the implementation evaluates the cost function
+// multiple times, computing a small set of the derivatives (four by
+// default, controlled by the Stride template parameter) with each
+// pass. There is a tradeoff with the size of the passes; you may want
+// to experiment with the stride.
+template <typename CostFunctor, int Stride = 4>
+class DynamicAutoDiffCostFunction : public DynamicCostFunction {
+ public:
+ explicit DynamicAutoDiffCostFunction(CostFunctor* functor)
+ : functor_(functor) {}
+
+ virtual ~DynamicAutoDiffCostFunction() {}
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ CHECK_GT(num_residuals(), 0)
+ << "You must call DynamicAutoDiffCostFunction::SetNumResiduals() "
+ << "before DynamicAutoDiffCostFunction::Evaluate().";
+
+ if (jacobians == NULL) {
+ return (*functor_)(parameters, residuals);
+ }
+
+ // The difficulty with Jets, as implemented in Ceres, is that they were
+ // originally designed for strictly compile-sized use. At this point, there
+ // is a large body of code that assumes inside a cost functor it is
+ // acceptable to do e.g. T(1.5) and get an appropriately sized jet back.
+ //
+ // Unfortunately, it is impossible to communicate the expected size of a
+ // dynamically sized jet to the static instantiations that existing code
+ // depends on.
+ //
+ // To work around this issue, the solution here is to evaluate the
+ // jacobians in a series of passes, each one computing Stride *
+ // num_residuals() derivatives. This is done with small, fixed-size jets.
+ const int num_parameter_blocks =
+ static_cast<int>(parameter_block_sizes().size());
+ const int num_parameters = std::accumulate(parameter_block_sizes().begin(),
+ parameter_block_sizes().end(),
+ 0);
+
+ // Allocate scratch space for the strided evaluation.
+ std::vector<Jet<double, Stride>> input_jets(num_parameters);
+ std::vector<Jet<double, Stride>> output_jets(num_residuals());
+
+ // Make the parameter pack that is sent to the functor (reused).
+ std::vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks,
+ static_cast<Jet<double, Stride>* >(NULL));
+ int num_active_parameters = 0;
+
+ // To handle constant parameters between non-constant parameter blocks, the
+ // start position --- a raw parameter index --- of each contiguous block of
+ // non-constant parameters is recorded in start_derivative_section.
+ std::vector<int> start_derivative_section;
+ bool in_derivative_section = false;
+ int parameter_cursor = 0;
+
+ // Discover the derivative sections and set the parameter values.
+ for (int i = 0; i < num_parameter_blocks; ++i) {
+ jet_parameters[i] = &input_jets[parameter_cursor];
+
+ const int parameter_block_size = parameter_block_sizes()[i];
+ if (jacobians[i] != NULL) {
+ if (!in_derivative_section) {
+ start_derivative_section.push_back(parameter_cursor);
+ in_derivative_section = true;
+ }
+
+ num_active_parameters += parameter_block_size;
+ } else {
+ in_derivative_section = false;
+ }
+
+ for (int j = 0; j < parameter_block_size; ++j, parameter_cursor++) {
+ input_jets[parameter_cursor].a = parameters[i][j];
+ }
+ }
+
+ // When `num_active_parameters % Stride != 0` then it can be the case
+ // that `active_parameter_count < Stride` while parameter_cursor is less
+ // than the total number of parameters and with no remaining non-constant
+ // parameter blocks. Pushing parameter_cursor (the total number of
+ // parameters) as a final entry to start_derivative_section is required
+ // because if a constant parameter block is encountered after the
+ // last non-constant block then current_derivative_section is incremented
+ // and would otherwise index an invalid position in
+ // start_derivative_section. Setting the final element to the total number
+ // of parameters means that this can only happen at most once in the loop
+ // below.
+ start_derivative_section.push_back(parameter_cursor);
+
+ // Evaluate all of the strides. Each stride is a chunk of the derivative to
+ // evaluate, typically some size proportional to the size of the SIMD
+ // registers of the CPU.
+ int num_strides = static_cast<int>(ceil(num_active_parameters /
+ static_cast<float>(Stride)));
+
+ int current_derivative_section = 0;
+ int current_derivative_section_cursor = 0;
+
+ for (int pass = 0; pass < num_strides; ++pass) {
+ // Set most of the jet components to zero, except for
+ // non-constant #Stride parameters.
+ const int initial_derivative_section = current_derivative_section;
+ const int initial_derivative_section_cursor =
+ current_derivative_section_cursor;
+
+ int active_parameter_count = 0;
+ parameter_cursor = 0;
+
+ for (int i = 0; i < num_parameter_blocks; ++i) {
+ for (int j = 0; j < parameter_block_sizes()[i];
+ ++j, parameter_cursor++) {
+ input_jets[parameter_cursor].v.setZero();
+ if (active_parameter_count < Stride &&
+ parameter_cursor >= (
+ start_derivative_section[current_derivative_section] +
+ current_derivative_section_cursor)) {
+ if (jacobians[i] != NULL) {
+ input_jets[parameter_cursor].v[active_parameter_count] = 1.0;
+ ++active_parameter_count;
+ ++current_derivative_section_cursor;
+ } else {
+ ++current_derivative_section;
+ current_derivative_section_cursor = 0;
+ }
+ }
+ }
+ }
+
+ if (!(*functor_)(&jet_parameters[0], &output_jets[0])) {
+ return false;
+ }
+
+ // Copy the pieces of the jacobians into their final place.
+ active_parameter_count = 0;
+
+ current_derivative_section = initial_derivative_section;
+ current_derivative_section_cursor = initial_derivative_section_cursor;
+
+ for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) {
+ for (int j = 0; j < parameter_block_sizes()[i];
+ ++j, parameter_cursor++) {
+ if (active_parameter_count < Stride &&
+ parameter_cursor >= (
+ start_derivative_section[current_derivative_section] +
+ current_derivative_section_cursor)) {
+ if (jacobians[i] != NULL) {
+ for (int k = 0; k < num_residuals(); ++k) {
+ jacobians[i][k * parameter_block_sizes()[i] + j] =
+ output_jets[k].v[active_parameter_count];
+ }
+ ++active_parameter_count;
+ ++current_derivative_section_cursor;
+ } else {
+ ++current_derivative_section;
+ current_derivative_section_cursor = 0;
+ }
+ }
+ }
+ }
+
+ // Only copy the residuals over once (even though we compute them on
+ // every loop).
+ if (pass == num_strides - 1) {
+ for (int k = 0; k < num_residuals(); ++k) {
+ residuals[k] = output_jets[k].a;
+ }
+ }
+ }
+ return true;
+ }
+
+ private:
+ std::unique_ptr<CostFunctor> functor_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
diff --git a/include/ceres/dynamic_cost_function.h b/include/ceres/dynamic_cost_function.h
new file mode 100644
index 0000000..6c0aa31
--- /dev/null
+++ b/include/ceres/dynamic_cost_function.h
@@ -0,0 +1,56 @@
+// 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: sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_
+#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_
+
+#include "ceres/cost_function.h"
+
+namespace ceres {
+
+// A common base class for DynamicAutoDiffCostFunction and
+// DynamicNumericDiffCostFunction which depend on methods that can add
+// parameter blocks and set the number of residuals at run time.
+class CERES_EXPORT DynamicCostFunction : public CostFunction {
+ public:
+ ~DynamicCostFunction() {}
+
+ virtual void AddParameterBlock(int size) {
+ mutable_parameter_block_sizes()->push_back(size);
+ }
+
+ virtual void SetNumResiduals(int num_residuals) {
+ set_num_residuals(num_residuals);
+ }
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_
diff --git a/include/ceres/dynamic_cost_function_to_functor.h b/include/ceres/dynamic_cost_function_to_functor.h
new file mode 100644
index 0000000..7ae5291
--- /dev/null
+++ b/include/ceres/dynamic_cost_function_to_functor.h
@@ -0,0 +1,190 @@
+// 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)
+// dgossow@google.com (David Gossow)
+
+#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
+#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
+
+#include <memory>
+#include <numeric>
+#include <vector>
+
+#include "ceres/dynamic_cost_function.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/port.h"
+
+namespace ceres {
+
+// DynamicCostFunctionToFunctor allows users to use CostFunction
+// objects in templated functors which are to be used for automatic
+// differentiation. It works similar to CostFunctionToFunctor, with the
+// difference that it allows you to wrap a cost function with dynamic numbers
+// of parameters and residuals.
+//
+// For example, let us assume that
+//
+// class IntrinsicProjection : public CostFunction {
+// public:
+// IntrinsicProjection(const double* observation);
+// virtual bool Evaluate(double const* const* parameters,
+// double* residuals,
+// double** jacobians) const;
+// };
+//
+// is a cost function that implements the projection of a point in its
+// local coordinate system onto its image plane and subtracts it from
+// the observed point projection. It can compute its residual and
+// either via analytic or numerical differentiation can compute its
+// jacobians. The intrinsics are passed in as parameters[0] and the point as
+// parameters[1].
+//
+// Now we would like to compose the action of this CostFunction with
+// the action of camera extrinsics, i.e., rotation and
+// translation. Say we have a templated function
+//
+// template<typename T>
+// void RotateAndTranslatePoint(double const* const* parameters,
+// double* residuals);
+//
+// Then we can now do the following,
+//
+// struct CameraProjection {
+// CameraProjection(const double* observation)
+// : intrinsic_projection_.(new IntrinsicProjection(observation)) {
+// }
+// template <typename T>
+// bool operator()(T const* const* parameters,
+// T* residual) const {
+// const T* rotation = parameters[0];
+// const T* translation = parameters[1];
+// const T* intrinsics = parameters[2];
+// const T* point = parameters[3];
+// T transformed_point[3];
+// RotateAndTranslatePoint(rotation, translation, point, transformed_point);
+//
+// // Note that we call intrinsic_projection_, just like it was
+// // any other templated functor.
+// const T* projection_parameters[2];
+// projection_parameters[0] = intrinsics;
+// projection_parameters[1] = transformed_point;
+// return intrinsic_projection_(projection_parameters, residual);
+// }
+//
+// private:
+// DynamicCostFunctionToFunctor intrinsic_projection_;
+// };
+class DynamicCostFunctionToFunctor {
+ public:
+ // Takes ownership of cost_function.
+ explicit DynamicCostFunctionToFunctor(CostFunction* cost_function)
+ : cost_function_(cost_function) {
+ CHECK(cost_function != nullptr);
+ }
+
+ bool operator()(double const* const* parameters, double* residuals) const {
+ return cost_function_->Evaluate(parameters, residuals, NULL);
+ }
+
+ template <typename JetT>
+ bool operator()(JetT const* const* inputs, JetT* output) const {
+ const std::vector<int32_t>& parameter_block_sizes =
+ cost_function_->parameter_block_sizes();
+ const int num_parameter_blocks =
+ static_cast<int>(parameter_block_sizes.size());
+ const int num_residuals = cost_function_->num_residuals();
+ const int num_parameters = std::accumulate(parameter_block_sizes.begin(),
+ parameter_block_sizes.end(), 0);
+
+ internal::FixedArray<double> parameters(num_parameters);
+ internal::FixedArray<double*> parameter_blocks(num_parameter_blocks);
+ internal::FixedArray<double> jacobians(num_residuals * num_parameters);
+ internal::FixedArray<double*> jacobian_blocks(num_parameter_blocks);
+ internal::FixedArray<double> residuals(num_residuals);
+
+ // Build a set of arrays to get the residuals and jacobians from
+ // the CostFunction wrapped by this functor.
+ double* parameter_ptr = parameters.get();
+ double* jacobian_ptr = jacobians.get();
+ for (int i = 0; i < num_parameter_blocks; ++i) {
+ parameter_blocks[i] = parameter_ptr;
+ jacobian_blocks[i] = jacobian_ptr;
+ for (int j = 0; j < parameter_block_sizes[i]; ++j) {
+ *parameter_ptr++ = inputs[i][j].a;
+ }
+ jacobian_ptr += num_residuals * parameter_block_sizes[i];
+ }
+
+ if (!cost_function_->Evaluate(parameter_blocks.get(),
+ residuals.get(),
+ jacobian_blocks.get())) {
+ return false;
+ }
+
+ // Now that we have the incoming Jets, which are carrying the
+ // partial derivatives of each of the inputs w.r.t to some other
+ // underlying parameters. The derivative of the outputs of the
+ // cost function w.r.t to the same underlying parameters can now
+ // be computed by applying the chain rule.
+ //
+ // d output[i] d output[i] d input[j]
+ // -------------- = sum_j ----------- * ------------
+ // d parameter[k] d input[j] d parameter[k]
+ //
+ // d input[j]
+ // -------------- = inputs[j], so
+ // d parameter[k]
+ //
+ // outputJet[i] = sum_k jacobian[i][k] * inputJet[k]
+ //
+ // The following loop, iterates over the residuals, computing one
+ // output jet at a time.
+ for (int i = 0; i < num_residuals; ++i) {
+ output[i].a = residuals[i];
+ output[i].v.setZero();
+
+ for (int j = 0; j < num_parameter_blocks; ++j) {
+ const int32_t block_size = parameter_block_sizes[j];
+ for (int k = 0; k < parameter_block_sizes[j]; ++k) {
+ output[i].v +=
+ jacobian_blocks[j][i * block_size + k] * inputs[j][k].v;
+ }
+ }
+ }
+
+ return true;
+ }
+
+ private:
+ std::unique_ptr<CostFunction> cost_function_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h
new file mode 100644
index 0000000..33ac5e1
--- /dev/null
+++ b/include/ceres/dynamic_numeric_diff_cost_function.h
@@ -0,0 +1,161 @@
+// 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)
+// sameeragarwal@google.com (Sameer Agarwal)
+// thadh@gmail.com (Thad Hughes)
+// tbennun@gmail.com (Tal Ben-Nun)
+
+#ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_
+#define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_
+
+#include <cmath>
+#include <memory>
+#include <numeric>
+#include <vector>
+
+#include "ceres/dynamic_cost_function.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/numeric_diff.h"
+#include "ceres/internal/parameter_dims.h"
+#include "ceres/numeric_diff_options.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// This numeric diff implementation differs from the one found in
+// numeric_diff_cost_function.h by supporting numericdiff on cost
+// functions with variable numbers of parameters with variable
+// sizes. With the other implementation, all the sizes (both the
+// number of parameter blocks and the size of each block) must be
+// fixed at compile time.
+//
+// The functor API differs slightly from the API for fixed size
+// numeric diff; the expected interface for the cost functors is:
+//
+// struct MyCostFunctor {
+// bool operator()(double const* const* parameters, double* residuals) const {
+// // Use parameters[i] to access the i'th parameter block.
+// }
+// }
+//
+// Since the sizing of the parameters is done at runtime, you must
+// also specify the sizes after creating the
+// DynamicNumericDiffCostFunction. For example:
+//
+// DynamicAutoDiffCostFunction<MyCostFunctor, CENTRAL> cost_function(
+// new MyCostFunctor());
+// cost_function.AddParameterBlock(5);
+// cost_function.AddParameterBlock(10);
+// cost_function.SetNumResiduals(21);
+template <typename CostFunctor, NumericDiffMethodType method = CENTRAL>
+class DynamicNumericDiffCostFunction : public DynamicCostFunction {
+ public:
+ explicit DynamicNumericDiffCostFunction(
+ const CostFunctor* functor,
+ Ownership ownership = TAKE_OWNERSHIP,
+ const NumericDiffOptions& options = NumericDiffOptions())
+ : functor_(functor),
+ ownership_(ownership),
+ options_(options) {
+ }
+
+ virtual ~DynamicNumericDiffCostFunction() {
+ if (ownership_ != TAKE_OWNERSHIP) {
+ functor_.release();
+ }
+ }
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ using internal::NumericDiff;
+ CHECK_GT(num_residuals(), 0)
+ << "You must call DynamicNumericDiffCostFunction::SetNumResiduals() "
+ << "before DynamicNumericDiffCostFunction::Evaluate().";
+
+ const std::vector<int32_t>& block_sizes = parameter_block_sizes();
+ CHECK(!block_sizes.empty())
+ << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() "
+ << "before DynamicNumericDiffCostFunction::Evaluate().";
+
+ const bool status =
+ internal::VariadicEvaluate<internal::DynamicParameterDims>(
+ *functor_.get(),
+ parameters,
+ residuals);
+ if (jacobians == NULL || !status) {
+ return status;
+ }
+
+ // Create local space for a copy of the parameters which will get mutated.
+ int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0);
+ std::vector<double> parameters_copy(parameters_size);
+ std::vector<double*> parameters_references_copy(block_sizes.size());
+ parameters_references_copy[0] = ¶meters_copy[0];
+ for (size_t block = 1; block < block_sizes.size(); ++block) {
+ parameters_references_copy[block] = parameters_references_copy[block - 1]
+ + block_sizes[block - 1];
+ }
+
+ // Copy the parameters into the local temp space.
+ for (size_t block = 0; block < block_sizes.size(); ++block) {
+ memcpy(parameters_references_copy[block],
+ parameters[block],
+ block_sizes[block] * sizeof(*parameters[block]));
+ }
+
+ for (size_t block = 0; block < block_sizes.size(); ++block) {
+ if (jacobians[block] != NULL &&
+ !NumericDiff<CostFunctor, method, ceres::DYNAMIC,
+ internal::DynamicParameterDims, ceres::DYNAMIC,
+ ceres::DYNAMIC>::
+ EvaluateJacobianForParameterBlock(
+ functor_.get(),
+ residuals,
+ options_,
+ this->num_residuals(),
+ block,
+ block_sizes[block],
+ ¶meters_references_copy[0],
+ jacobians[block])) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ private:
+ std::unique_ptr<const CostFunctor> functor_;
+ Ownership ownership_;
+ NumericDiffOptions options_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
diff --git a/include/ceres/evaluation_callback.h b/include/ceres/evaluation_callback.h
new file mode 100644
index 0000000..a0a8601
--- /dev/null
+++ b/include/ceres/evaluation_callback.h
@@ -0,0 +1,77 @@
+// 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)
+
+#ifndef CERES_PUBLIC_EVALUATION_CALLBACK_H_
+#define CERES_PUBLIC_EVALUATION_CALLBACK_H_
+
+#include "ceres/internal/port.h"
+
+namespace ceres {
+
+// Using this callback interface, Ceres can notify you when it is about to
+// evaluate the residuals or jacobians. With the callback, you can share
+// computation between residual blocks by doing the shared computation in
+// PrepareForEvaluation() before Ceres calls CostFunction::Evaluate() on all
+// the residuals. It also enables caching results between a pure residual
+// evaluation and a residual & jacobian evaluation, via the
+// new_evaluation_point argument.
+//
+// One use case for this callback is if the cost function compute is moved to
+// the GPU. In that case, the prepare call does the actual cost function
+// evaluation, and subsequent calls from Ceres to the actual cost functions
+// merely copy the results from the GPU onto the corresponding blocks for Ceres
+// to plug into the solver.
+//
+// NOTE: Ceres provides no mechanism to share data other than the notification
+// from the callback. Users must provide access to pre-computed shared data to
+// their cost functions behind the scenes; this all happens without Ceres
+// knowing. One approach is to put a pointer to the shared data in each cost
+// function (recommended) or to use a global shared variable (discouraged;
+// bug-prone). As far as Ceres is concerned, it is evaluating cost functions
+// like any other; it just so happens that behind the scenes the cost functions
+// reuse pre-computed data to execute faster.
+class CERES_EXPORT EvaluationCallback {
+ public:
+ virtual ~EvaluationCallback() {}
+
+ // Called before Ceres requests residuals or jacobians for a given setting of
+ // the parameters. User parameters (the double* values provided to the cost
+ // functions) are fixed until the next call to PrepareForEvaluation(). If
+ // new_evaluation_point == true, then this is a new point that is different
+ // from the last evaluated point. Otherwise, it is the same point that was
+ // evaluated previously (either jacobian or residual) and the user can use
+ // cached results from previous evaluations.
+ virtual void PrepareForEvaluation(bool evaluate_jacobians,
+ bool new_evaluation_point) = 0;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_EVALUATION_CALLBACK_H_
diff --git a/include/ceres/gradient_checker.h b/include/ceres/gradient_checker.h
new file mode 100644
index 0000000..e23df76
--- /dev/null
+++ b/include/ceres/gradient_checker.h
@@ -0,0 +1,150 @@
+// 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 2007 Google Inc. All Rights Reserved.
+//
+// Authors: wjr@google.com (William Rucklidge),
+// keir@google.com (Keir Mierle),
+// dgossow@google.com (David Gossow)
+
+#ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_
+#define CERES_PUBLIC_GRADIENT_CHECKER_H_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "ceres/cost_function.h"
+#include "ceres/dynamic_numeric_diff_cost_function.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/local_parameterization.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// GradientChecker compares the Jacobians returned by a cost function against
+// derivatives estimated using finite differencing.
+//
+// The condition enforced is that
+//
+// (J_actual(i, j) - J_numeric(i, j))
+// ------------------------------------ < relative_precision
+// max(J_actual(i, j), J_numeric(i, j))
+//
+// where J_actual(i, j) is the jacobian as computed by the supplied cost
+// function (by the user) multiplied by the local parameterization Jacobian
+// and J_numeric is the jacobian as computed by finite differences, multiplied
+// by the local parameterization Jacobian as well.
+//
+// How to use: Fill in an array of pointers to parameter blocks for your
+// CostFunction, and then call Probe(). Check that the return value is 'true'.
+class CERES_EXPORT GradientChecker {
+ public:
+ // This will not take ownership of the cost function or local
+ // parameterizations.
+ //
+ // function: The cost function to probe.
+ // local_parameterizations: A vector of local parameterizations for each
+ // parameter. May be NULL or contain NULL pointers to indicate that the
+ // respective parameter does not have a local parameterization.
+ // options: Options to use for numerical differentiation.
+ GradientChecker(
+ const CostFunction* function,
+ const std::vector<const LocalParameterization*>* local_parameterizations,
+ const NumericDiffOptions& options);
+
+ // Contains results from a call to Probe for later inspection.
+ struct CERES_EXPORT ProbeResults {
+ // The return value of the cost function.
+ bool return_value;
+
+ // Computed residual vector.
+ Vector residuals;
+
+ // The sizes of the Jacobians below are dictated by the cost function's
+ // parameter block size and residual block sizes. If a parameter block
+ // has a local parameterization associated with it, the size of the "local"
+ // Jacobian will be determined by the local parameterization dimension and
+ // residual block size, otherwise it will be identical to the regular
+ // Jacobian.
+
+ // Derivatives as computed by the cost function.
+ std::vector<Matrix> jacobians;
+
+ // Derivatives as computed by the cost function in local space.
+ std::vector<Matrix> local_jacobians;
+
+ // Derivatives as computed by numerical differentiation in local space.
+ std::vector<Matrix> numeric_jacobians;
+
+ // Derivatives as computed by numerical differentiation in local space.
+ std::vector<Matrix> local_numeric_jacobians;
+
+ // Contains the maximum relative error found in the local Jacobians.
+ double maximum_relative_error;
+
+ // If an error was detected, this will contain a detailed description of
+ // that error.
+ std::string error_log;
+ };
+
+ // Call the cost function, compute alternative Jacobians using finite
+ // differencing and compare results. If local parameterizations are given,
+ // the Jacobians will be multiplied by the local parameterization Jacobians
+ // before performing the check, which effectively means that all errors along
+ // the null space of the local parameterization will be ignored.
+ // Returns false if the Jacobians don't match, the cost function return false,
+ // or if the cost function returns different residual when called with a
+ // Jacobian output argument vs. calling it without. Otherwise returns true.
+ //
+ // parameters: The parameter values at which to probe.
+ // relative_precision: A threshold for the relative difference between the
+ // Jacobians. If the Jacobians differ by more than this amount, then the
+ // probe fails.
+ // results: On return, the Jacobians (and other information) will be stored
+ // here. May be NULL.
+ //
+ // Returns true if no problems are detected and the difference between the
+ // Jacobians is less than error_tolerance.
+ bool Probe(double const* const* parameters,
+ double relative_precision,
+ ProbeResults* results) const;
+
+ private:
+ GradientChecker() = delete;
+ GradientChecker(const GradientChecker&) = delete;
+ void operator=(const GradientChecker&) = delete;
+
+ std::vector<const LocalParameterization*> local_parameterizations_;
+ const CostFunction* function_;
+ std::unique_ptr<CostFunction> finite_diff_cost_function_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_GRADIENT_CHECKER_H_
diff --git a/include/ceres/gradient_problem.h b/include/ceres/gradient_problem.h
new file mode 100644
index 0000000..6adcfd0
--- /dev/null
+++ b/include/ceres/gradient_problem.h
@@ -0,0 +1,126 @@
+// 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_PUBLIC_GRADIENT_PROBLEM_H_
+#define CERES_PUBLIC_GRADIENT_PROBLEM_H_
+
+#include <memory>
+#include "ceres/internal/port.h"
+#include "ceres/local_parameterization.h"
+
+namespace ceres {
+
+class FirstOrderFunction;
+
+// Instances of GradientProblem represent general non-linear
+// optimization problems that must be solved using just the value of
+// the objective function and its gradient. Unlike the Problem class,
+// which can only be used to model non-linear least squares problems,
+// instances of GradientProblem not restricted in the form of the
+// objective function.
+//
+// Structurally GradientProblem is a composition of a
+// FirstOrderFunction and optionally a LocalParameterization.
+//
+// The FirstOrderFunction is responsible for evaluating the cost and
+// gradient of the objective function.
+//
+// The LocalParameterization is responsible for going back and forth
+// between the ambient space and the local tangent space. (See
+// local_parameterization.h for more details). When a
+// LocalParameterization is not provided, then the tangent space is
+// assumed to coincide with the ambient Euclidean space that the
+// gradient vector lives in.
+//
+// Example usage:
+//
+// The following demonstrate the problem construction for Rosenbrock's function
+//
+// 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; };
+// };
+//
+// ceres::GradientProblem problem(new Rosenbrock());
+class CERES_EXPORT GradientProblem {
+ public:
+ // Takes ownership of the function.
+ explicit GradientProblem(FirstOrderFunction* function);
+
+ // Takes ownership of the function and the parameterization.
+ GradientProblem(FirstOrderFunction* function,
+ LocalParameterization* parameterization);
+
+ int NumParameters() const;
+ int NumLocalParameters() const;
+
+ // This call is not thread safe.
+ bool Evaluate(const double* parameters, double* cost, double* gradient) const;
+ bool Plus(const double* x, const double* delta, double* x_plus_delta) const;
+
+ private:
+ std::unique_ptr<FirstOrderFunction> function_;
+ std::unique_ptr<LocalParameterization> parameterization_;
+ std::unique_ptr<double[]> scratch_;
+};
+
+// A FirstOrderFunction object implements the evaluation of a function
+// and its gradient.
+class CERES_EXPORT FirstOrderFunction {
+ public:
+ virtual ~FirstOrderFunction() {}
+ // cost is never NULL. gradient may be null.
+ virtual bool Evaluate(const double* const parameters,
+ double* cost,
+ double* gradient) const = 0;
+ virtual int NumParameters() const = 0;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_GRADIENT_PROBLEM_H_
diff --git a/include/ceres/gradient_problem_solver.h b/include/ceres/gradient_problem_solver.h
new file mode 100644
index 0000000..1831d8d
--- /dev/null
+++ b/include/ceres/gradient_problem_solver.h
@@ -0,0 +1,350 @@
+// 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_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_
+#define CERES_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_
+
+#include <cmath>
+#include <string>
+#include <vector>
+#include "ceres/internal/port.h"
+#include "ceres/iteration_callback.h"
+#include "ceres/types.h"
+#include "ceres/internal/disable_warnings.h"
+
+namespace ceres {
+
+class GradientProblem;
+
+class CERES_EXPORT GradientProblemSolver {
+ public:
+ virtual ~GradientProblemSolver();
+
+ // The options structure contains, not surprisingly, options that control how
+ // the solver operates. The defaults should be suitable for a wide range of
+ // problems; however, better performance is often obtainable with tweaking.
+ //
+ // The constants are defined inside types.h
+ struct CERES_EXPORT Options {
+ // Returns true if the options struct has a valid
+ // configuration. Returns false otherwise, and fills in *error
+ // with a message describing the problem.
+ bool IsValid(std::string* error) const;
+
+ // Minimizer options ----------------------------------------
+ LineSearchDirectionType line_search_direction_type = LBFGS;
+ LineSearchType line_search_type = WOLFE;
+ NonlinearConjugateGradientType nonlinear_conjugate_gradient_type =
+ FLETCHER_REEVES;
+
+ // The LBFGS hessian approximation is a low rank approximation to
+ // the inverse of the Hessian matrix. The rank of the
+ // approximation determines (linearly) the space and time
+ // complexity of using the approximation. Higher the rank, the
+ // better is the quality of the approximation. The increase in
+ // quality is however is bounded for a number of reasons.
+ //
+ // 1. The method only uses secant information and not actual
+ // derivatives.
+ //
+ // 2. The Hessian approximation is constrained to be positive
+ // definite.
+ //
+ // So increasing this rank to a large number will cost time and
+ // space complexity without the corresponding increase in solution
+ // quality. There are no hard and fast rules for choosing the
+ // maximum rank. The best choice usually requires some problem
+ // specific experimentation.
+ //
+ // For more theoretical and implementation details of the LBFGS
+ // method, please see:
+ //
+ // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with
+ // Limited Storage". Mathematics of Computation 35 (151): 773–782.
+ int max_lbfgs_rank = 20;
+
+ // As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS),
+ // the initial inverse Hessian approximation is taken to be the Identity.
+ // However, Oren showed that using instead I * \gamma, where \gamma is
+ // chosen to approximate an eigenvalue of the true inverse Hessian can
+ // result in improved convergence in a wide variety of cases. Setting
+ // use_approximate_eigenvalue_bfgs_scaling to true enables this scaling.
+ //
+ // It is important to note that approximate eigenvalue scaling does not
+ // always improve convergence, and that it can in fact significantly degrade
+ // performance for certain classes of problem, which is why it is disabled
+ // by default. In particular it can degrade performance when the
+ // sensitivity of the problem to different parameters varies significantly,
+ // as in this case a single scalar factor fails to capture this variation
+ // and detrimentally downscales parts of the jacobian approximation which
+ // correspond to low-sensitivity parameters. It can also reduce the
+ // robustness of the solution to errors in the jacobians.
+ //
+ // Oren S.S., Self-scaling variable metric (SSVM) algorithms
+ // Part II: Implementation and experiments, Management Science,
+ // 20(5), 863-874, 1974.
+ bool use_approximate_eigenvalue_bfgs_scaling = false;
+
+ // Degree of the polynomial used to approximate the objective
+ // function. Valid values are BISECTION, QUADRATIC and CUBIC.
+ //
+ // BISECTION corresponds to pure backtracking search with no
+ // interpolation.
+ LineSearchInterpolationType line_search_interpolation_type = CUBIC;
+
+ // If during the line search, the step_size falls below this
+ // value, it is truncated to zero.
+ double min_line_search_step_size = 1e-9;
+
+ // Line search parameters.
+
+ // Solving the line search problem exactly is computationally
+ // prohibitive. Fortunately, line search based optimization
+ // algorithms can still guarantee convergence if instead of an
+ // exact solution, the line search algorithm returns a solution
+ // which decreases the value of the objective function
+ // sufficiently. More precisely, we are looking for a step_size
+ // s.t.
+ //
+ // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size
+ //
+ double line_search_sufficient_function_decrease = 1e-4;
+
+ // In each iteration of the line search,
+ //
+ // new_step_size >= max_line_search_step_contraction * step_size
+ //
+ // Note that by definition, for contraction:
+ //
+ // 0 < max_step_contraction < min_step_contraction < 1
+ //
+ double max_line_search_step_contraction = 1e-3;
+
+ // In each iteration of the line search,
+ //
+ // new_step_size <= min_line_search_step_contraction * step_size
+ //
+ // Note that by definition, for contraction:
+ //
+ // 0 < max_step_contraction < min_step_contraction < 1
+ //
+ double min_line_search_step_contraction = 0.6;
+
+ // Maximum number of trial step size iterations during each line search,
+ // if a step size satisfying the search conditions cannot be found within
+ // this number of trials, the line search will terminate.
+ int max_num_line_search_step_size_iterations = 20;
+
+ // Maximum number of restarts of the line search direction algorithm before
+ // terminating the optimization. Restarts of the line search direction
+ // algorithm occur when the current algorithm fails to produce a new descent
+ // direction. This typically indicates a numerical failure, or a breakdown
+ // in the validity of the approximations used.
+ int max_num_line_search_direction_restarts = 5;
+
+ // The strong Wolfe conditions consist of the Armijo sufficient
+ // decrease condition, and an additional requirement that the
+ // step-size be chosen s.t. the _magnitude_ ('strong' Wolfe
+ // conditions) of the gradient along the search direction
+ // decreases sufficiently. Precisely, this second condition
+ // is that we seek a step_size s.t.
+ //
+ // |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)|
+ //
+ // Where f() is the line search objective and f'() is the derivative
+ // of f w.r.t step_size (d f / d step_size).
+ double line_search_sufficient_curvature_decrease = 0.9;
+
+ // During the bracketing phase of the Wolfe search, the step size is
+ // increased until either a point satisfying the Wolfe conditions is
+ // found, or an upper bound for a bracket containing a point satisfying
+ // the conditions is found. Precisely, at each iteration of the
+ // expansion:
+ //
+ // new_step_size <= max_step_expansion * step_size.
+ //
+ // By definition for expansion, max_step_expansion > 1.0.
+ double max_line_search_step_expansion = 10.0;
+
+ // Maximum number of iterations for the minimizer to run for.
+ int max_num_iterations = 50;
+
+ // Maximum time for which the minimizer should run for.
+ double max_solver_time_in_seconds = 1e9;
+
+ // Minimizer terminates when
+ //
+ // (new_cost - old_cost) < function_tolerance * old_cost;
+ //
+ double function_tolerance = 1e-6;
+
+ // Minimizer terminates when
+ //
+ // max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance
+ //
+ // This value should typically be 1e-4 * function_tolerance.
+ double gradient_tolerance = 1e-10;
+
+ // Minimizer terminates when
+ //
+ // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance)
+ //
+ double parameter_tolerance = 1e-8;
+
+ // Logging options ---------------------------------------------------------
+
+ LoggingType logging_type = PER_MINIMIZER_ITERATION;
+
+ // By default the Minimizer progress is logged to VLOG(1), which
+ // is sent to STDERR depending on the vlog level. If this flag is
+ // set to true, and logging_type is not SILENT, the logging output
+ // is sent to STDOUT.
+ bool minimizer_progress_to_stdout = false;
+
+ // If true, the user's parameter blocks are updated at the end of
+ // every Minimizer iteration, otherwise they are updated when the
+ // Minimizer terminates. This is useful if, for example, the user
+ // wishes to visualize the state of the optimization every
+ // iteration.
+ bool update_state_every_iteration = false;
+
+ // Callbacks that are executed at the end of each iteration of the
+ // Minimizer. An iteration may terminate midway, either due to
+ // numerical failures or because one of the convergence tests has
+ // been satisfied. In this case none of the callbacks are
+ // executed.
+
+ // Callbacks are executed in the order that they are specified in
+ // this vector. By default, parameter blocks are updated only at
+ // the end of the optimization, i.e when the Minimizer
+ // terminates. This behaviour is controlled by
+ // update_state_every_variable. If the user wishes to have access
+ // to the update parameter blocks when his/her callbacks are
+ // executed, then set update_state_every_iteration to true.
+ //
+ // The solver does NOT take ownership of these pointers.
+ std::vector<IterationCallback*> callbacks;
+ };
+
+ struct CERES_EXPORT Summary {
+ // A brief one line description of the state of the solver after
+ // termination.
+ std::string BriefReport() const;
+
+ // A full multiline description of the state of the solver after
+ // termination.
+ std::string FullReport() const;
+
+ bool IsSolutionUsable() const;
+
+ // Minimizer summary -------------------------------------------------
+ TerminationType termination_type = FAILURE;
+
+ // Reason why the solver terminated.
+ std::string message = "ceres::GradientProblemSolve was not called.";
+
+ // Cost of the problem (value of the objective function) before
+ // the optimization.
+ double initial_cost = -1.0;
+
+ // Cost of the problem (value of the objective function) after the
+ // optimization.
+ double final_cost = -1.0;
+
+ // IterationSummary for each minimizer iteration in order.
+ std::vector<IterationSummary> iterations;
+
+ // Number of times the cost (and not the gradient) was evaluated.
+ int num_cost_evaluations = -1;
+
+ // Number of times the gradient (and the cost) were evaluated.
+ int num_gradient_evaluations = -1;
+
+ // Sum total of all time spent inside Ceres when Solve is called.
+ double total_time_in_seconds = -1.0;
+
+ // Time (in seconds) spent evaluating the cost.
+ double cost_evaluation_time_in_seconds = -1.0;
+
+ // Time (in seconds) spent evaluating the gradient.
+ double gradient_evaluation_time_in_seconds = -1.0;
+
+ // Time (in seconds) spent minimizing the interpolating polynomial
+ // to compute the next candidate step size as part of a line search.
+ double line_search_polynomial_minimization_time_in_seconds = -1.0;
+
+ // Number of parameters in the problem.
+ int num_parameters = -1;
+
+ // Dimension of the tangent space of the problem.
+ int num_local_parameters = -1;
+
+ // Type of line search direction used.
+ LineSearchDirectionType line_search_direction_type = LBFGS;
+
+ // Type of the line search algorithm used.
+ LineSearchType line_search_type = WOLFE;
+
+ // When performing line search, the degree of the polynomial used
+ // to approximate the objective function.
+ LineSearchInterpolationType line_search_interpolation_type = CUBIC;
+
+ // If the line search direction is NONLINEAR_CONJUGATE_GRADIENT,
+ // then this indicates the particular variant of non-linear
+ // conjugate gradient used.
+ NonlinearConjugateGradientType nonlinear_conjugate_gradient_type = FLETCHER_REEVES;
+
+ // If the type of the line search direction is LBFGS, then this
+ // indicates the rank of the Hessian approximation.
+ int max_lbfgs_rank = -1;
+ };
+
+ // Once a least squares problem has been built, this function takes
+ // the problem and optimizes it based on the values of the options
+ // parameters. Upon return, a detailed summary of the work performed
+ // by the preprocessor, the non-linear minimizer and the linear
+ // solver are reported in the summary object.
+ virtual void Solve(const GradientProblemSolver::Options& options,
+ const GradientProblem& problem,
+ double* parameters,
+ GradientProblemSolver::Summary* summary);
+};
+
+// Helper function which avoids going through the interface.
+CERES_EXPORT void Solve(const GradientProblemSolver::Options& options,
+ const GradientProblem& problem,
+ double* parameters,
+ GradientProblemSolver::Summary* summary);
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_
diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h
new file mode 100644
index 0000000..ff47fbf
--- /dev/null
+++ b/include/ceres/internal/autodiff.h
@@ -0,0 +1,318 @@
+// 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)
+//
+// Computation of the Jacobian matrix for vector-valued functions of multiple
+// variables, using automatic differentiation based on the implementation of
+// dual numbers in jet.h. Before reading the rest of this file, it is advisable
+// to read jet.h's header comment in detail.
+//
+// The helper wrapper AutoDifferentiate() computes the jacobian of
+// functors with templated operator() taking this form:
+//
+// struct F {
+// template<typename T>
+// bool operator()(const T *x, const T *y, ..., T *z) {
+// // Compute z[] based on x[], y[], ...
+// // return true if computation succeeded, false otherwise.
+// }
+// };
+//
+// All inputs and outputs may be vector-valued.
+//
+// To understand how jets are used to compute the jacobian, a
+// picture may help. Consider a vector-valued function, F, returning 3
+// dimensions and taking a vector-valued parameter of 4 dimensions:
+//
+// y x
+// [ * ] F [ * ]
+// [ * ] <--- [ * ]
+// [ * ] [ * ]
+// [ * ]
+//
+// Similar to the 2-parameter example for f described in jet.h, computing the
+// jacobian dy/dx is done by substituting a suitable jet object for x and all
+// intermediate steps of the computation of F. Since x is has 4 dimensions, use
+// a Jet<double, 4>.
+//
+// Before substituting a jet object for x, the dual components are set
+// appropriately for each dimension of x:
+//
+// y x
+// [ * | * * * * ] f [ * | 1 0 0 0 ] x0
+// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1
+// [ * | * * * * ] [ * | 0 0 1 0 ] x2
+// ---+--- [ * | 0 0 0 1 ] x3
+// | ^ ^ ^ ^
+// dy/dx | | | +----- infinitesimal for x3
+// | | +------- infinitesimal for x2
+// | +--------- infinitesimal for x1
+// +----------- infinitesimal for x0
+//
+// The reason to set the internal 4x4 submatrix to the identity is that we wish
+// to take the derivative of y separately with respect to each dimension of x.
+// Each column of the 4x4 identity is therefore for a single component of the
+// independent variable x.
+//
+// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the
+// extended y vector, indicated in the above diagram.
+//
+// Functors with multiple parameters
+// ---------------------------------
+// In practice, it is often convenient to use a function f of two or more
+// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet
+// framework is designed for a single-parameter vector-valued input. The wrapper
+// in this file addresses this issue adding support for functions with one or
+// more parameter vectors.
+//
+// To support multiple parameters, all the parameter vectors are concatenated
+// into one and treated as a single parameter vector, except that since the
+// functor expects different inputs, we need to construct the jets as if they
+// were part of a single parameter vector. The extended jets are passed
+// separately for each parameter.
+//
+// For example, consider a functor F taking two vector parameters, p[2] and
+// q[3], and producing an output y[4]:
+//
+// struct F {
+// template<typename T>
+// bool operator()(const T *p, const T *q, T *z) {
+// // ...
+// }
+// };
+//
+// In this case, the necessary jet type is Jet<double, 5>. Here is a
+// visualization of the jet objects in this case:
+//
+// Dual components for p ----+
+// |
+// -+-
+// y [ * | 1 0 | 0 0 0 ] --- p[0]
+// [ * | 0 1 | 0 0 0 ] --- p[1]
+// [ * | . . | + + + ] |
+// [ * | . . | + + + ] v
+// [ * | . . | + + + ] <--- F(p, q)
+// [ * | . . | + + + ] ^
+// ^^^ ^^^^^ |
+// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0]
+// [ * | 0 0 | 0 1 0 ] --- q[1]
+// [ * | 0 0 | 0 0 1 ] --- q[2]
+// --+--
+// |
+// Dual components for q --------------+
+//
+// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+"
+// of y in the above diagram are the derivatives of y with respect to p and q
+// respectively. This is how autodiff works for functors taking multiple vector
+// valued arguments (up to 6).
+//
+// Jacobian NULL pointers
+// ----------------------
+// In general, the functions below will accept NULL pointers for all or some of
+// the Jacobian parameters, meaning that those Jacobians will not be computed.
+
+#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_
+#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_
+
+#include <stddef.h>
+
+#include <array>
+
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/parameter_dims.h"
+#include "ceres/internal/variadic_evaluate.h"
+#include "ceres/jet.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+// Extends src by a 1st order perturbation for every dimension and puts it in
+// dst. The size of src is N. Since this is also used for perturbations in
+// blocked arrays, offset is used to shift which part of the jet the
+// perturbation occurs. This is used to set up the extended x augmented by an
+// identity matrix. The JetT type should be a Jet type, and T should be a
+// numeric type (e.g. double). For example,
+//
+// 0 1 2 3 4 5 6 7 8
+// dst[0] [ * | . . | 1 0 0 | . . . ]
+// dst[1] [ * | . . | 0 1 0 | . . . ]
+// dst[2] [ * | . . | 0 0 1 | . . . ]
+//
+// is what would get put in dst if N was 3, offset was 3, and the jet type JetT
+// was 8-dimensional.
+template <int Offset, int N, typename T, typename JetT>
+inline void Make1stOrderPerturbation(const T* src, JetT* dst) {
+ DCHECK(src);
+ DCHECK(dst);
+ for (int j = 0; j < N; ++j) {
+ dst[j].a = src[j];
+ dst[j].v.setZero();
+ dst[j].v[Offset + j] = T(1.0);
+ }
+}
+
+// Calls Make1stOrderPerturbation for every parameter block.
+//
+// Example:
+// If one having three parameter blocks with dimensions (3, 2, 4), the call
+// Make1stOrderPerturbations<integer_sequence<3, 2, 4>::Apply(params, x);
+// will result in the following calls to Make1stOrderPerturbation:
+// Make1stOrderPerturbation<0, 3>(params[0], x + 0);
+// Make1stOrderPerturbation<3, 2>(params[1], x + 3);
+// Make1stOrderPerturbation<5, 4>(params[2], x + 5);
+template <typename Seq, int ParameterIdx = 0, int Offset = 0>
+struct Make1stOrderPerturbations;
+
+template <int N, int... Ns, int ParameterIdx, int Offset>
+struct Make1stOrderPerturbations<integer_sequence<int, N, Ns...>, ParameterIdx,
+ Offset> {
+ template <typename T, typename JetT>
+ static void Apply(T const* const* parameters, JetT* x) {
+ Make1stOrderPerturbation<Offset, N>(parameters[ParameterIdx], x + Offset);
+ Make1stOrderPerturbations<integer_sequence<int, Ns...>, ParameterIdx + 1,
+ Offset + N>::Apply(parameters, x);
+ }
+};
+
+// End of 'recursion'. Nothing more to do.
+template <int ParameterIdx, int Total>
+struct Make1stOrderPerturbations<integer_sequence<int>, ParameterIdx, Total> {
+ template <typename T, typename JetT>
+ static void Apply(T const* const* /* NOT USED */, JetT* /* NOT USED */) {}
+};
+
+// Takes the 0th order part of src, assumed to be a Jet type, and puts it in
+// dst. This is used to pick out the "vector" part of the extended y.
+template <typename JetT, typename T>
+inline void Take0thOrderPart(int M, const JetT* src, T dst) {
+ DCHECK(src);
+ for (int i = 0; i < M; ++i) {
+ dst[i] = src[i].a;
+ }
+}
+
+// Takes N 1st order parts, starting at index N0, and puts them in the M x N
+// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
+template <int N0, int N, typename JetT, typename T>
+inline void Take1stOrderPart(const int M, const JetT* src, T* dst) {
+ DCHECK(src);
+ DCHECK(dst);
+ for (int i = 0; i < M; ++i) {
+ Eigen::Map<Eigen::Matrix<T, N, 1>>(dst + N * i, N) =
+ src[i].v.template segment<N>(N0);
+ }
+}
+
+// Calls Take1stOrderPart for every parameter block.
+//
+// Example:
+// If one having three parameter blocks with dimensions (3, 2, 4), the call
+// Take1stOrderParts<integer_sequence<3, 2, 4>::Apply(num_outputs,
+// output,
+// jacobians);
+// will result in the following calls to Take1stOrderPart:
+// if (jacobians[0]) {
+// Take1stOrderPart<0, 3>(num_outputs, output, jacobians[0]);
+// }
+// if (jacobians[1]) {
+// Take1stOrderPart<3, 2>(num_outputs, output, jacobians[1]);
+// }
+// if (jacobians[2]) {
+// Take1stOrderPart<5, 4>(num_outputs, output, jacobians[2]);
+// }
+template <typename Seq, int ParameterIdx = 0, int Offset = 0>
+struct Take1stOrderParts;
+
+template <int N, int... Ns, int ParameterIdx, int Offset>
+struct Take1stOrderParts<integer_sequence<int, N, Ns...>, ParameterIdx,
+ Offset> {
+ template <typename JetT, typename T>
+ static void Apply(int num_outputs, JetT* output, T** jacobians) {
+ if (jacobians[ParameterIdx]) {
+ Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]);
+ }
+ Take1stOrderParts<integer_sequence<int, Ns...>, ParameterIdx + 1,
+ Offset + N>::Apply(num_outputs, output, jacobians);
+ }
+};
+
+// End of 'recursion'. Nothing more to do.
+template <int ParameterIdx, int Offset>
+struct Take1stOrderParts<integer_sequence<int>, ParameterIdx, Offset> {
+ template <typename T, typename JetT>
+ static void Apply(int /* NOT USED*/, JetT* /* NOT USED*/,
+ T** /* NOT USED */) {}
+};
+
+template <typename ParameterDims, typename Functor, typename T>
+inline bool AutoDifferentiate(const Functor& functor,
+ T const *const *parameters,
+ int num_outputs,
+ T* function_value,
+ T** jacobians) {
+ DCHECK_GT(num_outputs, 0);
+
+ typedef Jet<T, ParameterDims::kNumParameters> JetT;
+ FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(ParameterDims::kNumParameters +
+ num_outputs);
+
+ using Parameters = typename ParameterDims::Parameters;
+
+ // These are the positions of the respective jets in the fixed array x.
+ std::array<JetT*, ParameterDims::kNumParameterBlocks> unpacked_parameters =
+ ParameterDims::GetUnpackedParameters(x.get());
+ JetT* output = x.get() + ParameterDims::kNumParameters;
+
+ // Invalidate the output Jets, so that we can detect if the user
+ // did not assign values to all of them.
+ for (int i = 0; i < num_outputs; ++i) {
+ output[i].a = kImpossibleValue;
+ output[i].v.setConstant(kImpossibleValue);
+ }
+
+ Make1stOrderPerturbations<Parameters>::Apply(parameters, x.get());
+
+ if (!VariadicEvaluate<ParameterDims>(functor, unpacked_parameters.data(),
+ output)) {
+ return false;
+ }
+
+ Take0thOrderPart(num_outputs, output, function_value);
+ Take1stOrderParts<Parameters>::Apply(num_outputs, output, jacobians);
+
+ return true;
+}
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_
diff --git a/include/ceres/internal/disable_warnings.h b/include/ceres/internal/disable_warnings.h
new file mode 100644
index 0000000..fd848fe
--- /dev/null
+++ b/include/ceres/internal/disable_warnings.h
@@ -0,0 +1,44 @@
+// 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.
+//
+// This file has the sole purpose to silence warnings when including Ceres.
+
+// This is not your usual header guard. The macro CERES_WARNINGS_DISABLED
+// shows up again in reenable_warnings.h.
+#ifndef CERES_WARNINGS_DISABLED
+#define CERES_WARNINGS_DISABLED
+
+#ifdef _MSC_VER
+#pragma warning( push )
+// Disable the warning C4251 which is triggered by stl classes in
+// Ceres' public interface. To quote MSDN: "C4251 can be ignored "
+// "if you are deriving from a type in the Standard C++ Library"
+#pragma warning( disable : 4251 )
+#endif
+
+#endif // CERES_WARNINGS_DISABLED
diff --git a/include/ceres/internal/eigen.h b/include/ceres/internal/eigen.h
new file mode 100644
index 0000000..59545df
--- /dev/null
+++ b/include/ceres/internal/eigen.h
@@ -0,0 +1,80 @@
+// 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_INTERNAL_EIGEN_H_
+#define CERES_INTERNAL_EIGEN_H_
+
+#include "Eigen/Core"
+
+namespace ceres {
+
+typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
+typedef Eigen::Matrix<double,
+ Eigen::Dynamic,
+ Eigen::Dynamic,
+ Eigen::RowMajor> Matrix;
+typedef Eigen::Map<Vector> VectorRef;
+typedef Eigen::Map<Matrix> MatrixRef;
+typedef Eigen::Map<const Vector> ConstVectorRef;
+typedef Eigen::Map<const Matrix> ConstMatrixRef;
+
+// Column major matrices for DenseSparseMatrix/DenseQRSolver
+typedef Eigen::Matrix<double,
+ Eigen::Dynamic,
+ Eigen::Dynamic,
+ Eigen::ColMajor> ColMajorMatrix;
+
+typedef Eigen::Map<ColMajorMatrix, 0,
+ Eigen::Stride<Eigen::Dynamic, 1>> ColMajorMatrixRef;
+
+typedef Eigen::Map<const ColMajorMatrix,
+ 0,
+ Eigen::Stride<Eigen::Dynamic, 1>> ConstColMajorMatrixRef;
+
+// C++ does not support templated typdefs, thus the need for this
+// struct so that we can support statically sized Matrix and Maps.
+ template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic>
+struct EigenTypes {
+ typedef Eigen::Matrix<double,
+ num_rows,
+ num_cols,
+ num_cols == 1 ? Eigen::ColMajor : Eigen::RowMajor>
+ Matrix;
+
+ typedef Eigen::Map<Matrix> MatrixRef;
+ typedef Eigen::Map<const Matrix> ConstMatrixRef;
+ typedef Eigen::Matrix<double, num_rows, 1> Vector;
+ typedef Eigen::Map<Eigen::Matrix<double, num_rows, 1>> VectorRef;
+ typedef Eigen::Map<const Eigen::Matrix<double, num_rows, 1>> ConstVectorRef;
+};
+
+} // namespace ceres
+
+#endif // CERES_INTERNAL_EIGEN_H_
diff --git a/include/ceres/internal/fixed_array.h b/include/ceres/internal/fixed_array.h
new file mode 100644
index 0000000..dcb2ace
--- /dev/null
+++ b/include/ceres/internal/fixed_array.h
@@ -0,0 +1,190 @@
+// 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: rennie@google.com (Jeffrey Rennie)
+// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray
+
+#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
+#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
+
+#include <cstddef>
+#include "Eigen/Core"
+#include "ceres/internal/manual_constructor.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+// A FixedArray<T> represents a non-resizable array of T where the
+// length of the array does not need to be a compile time constant.
+//
+// FixedArray allocates small arrays inline, and large arrays on
+// the heap. It is a good replacement for non-standard and deprecated
+// uses of alloca() and variable length arrays (a GCC extension).
+//
+// FixedArray keeps performance fast for small arrays, because it
+// avoids heap operations. It also helps reduce the chances of
+// accidentally overflowing your stack if large input is passed to
+// your function.
+//
+// Also, FixedArray is useful for writing portable code. Not all
+// compilers support arrays of dynamic size.
+
+// Most users should not specify an inline_elements argument and let
+// FixedArray<> automatically determine the number of elements
+// to store inline based on sizeof(T).
+//
+// If inline_elements is specified, the FixedArray<> implementation
+// will store arrays of length <= inline_elements inline.
+//
+// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize
+// simple types like int, double, bool, etc.
+//
+// Non-POD types will be default-initialized just like regular vectors or
+// arrays.
+
+#if defined(_WIN64)
+ typedef __int64 ssize_t;
+#elif defined(_WIN32)
+ typedef __int32 ssize_t;
+#endif
+
+template <typename T, ssize_t inline_elements = -1>
+class FixedArray {
+ public:
+ // For playing nicely with stl:
+ typedef T value_type;
+ typedef T* iterator;
+ typedef T const* const_iterator;
+ typedef T& reference;
+ typedef T const& const_reference;
+ typedef T* pointer;
+ typedef std::ptrdiff_t difference_type;
+ typedef size_t size_type;
+
+ // REQUIRES: n >= 0
+ // Creates an array object that can store "n" elements.
+ //
+ // FixedArray<T> will not zero-initialize POD (simple) types like int,
+ // double, bool, etc.
+ // Non-POD types will be default-initialized just like regular vectors or
+ // arrays.
+ explicit FixedArray(size_type n);
+
+ // Releases any resources.
+ ~FixedArray();
+
+ // Returns the length of the array.
+ inline size_type size() const { return size_; }
+
+ // Returns the memory size of the array in bytes.
+ inline size_t memsize() const { return size_ * sizeof(T); }
+
+ // Returns a pointer to the underlying element array.
+ inline const T* get() const { return &array_[0].element; }
+ inline T* get() { return &array_[0].element; }
+
+ // REQUIRES: 0 <= i < size()
+ // Returns a reference to the "i"th element.
+ inline T& operator[](size_type i) {
+ DCHECK_LT(i, size_);
+ return array_[i].element;
+ }
+
+ // REQUIRES: 0 <= i < size()
+ // Returns a reference to the "i"th element.
+ inline const T& operator[](size_type i) const {
+ DCHECK_LT(i, size_);
+ return array_[i].element;
+ }
+
+ inline iterator begin() { return &array_[0].element; }
+ inline iterator end() { return &array_[size_].element; }
+
+ inline const_iterator begin() const { return &array_[0].element; }
+ inline const_iterator end() const { return &array_[size_].element; }
+
+ private:
+ // Container to hold elements of type T. This is necessary to handle
+ // the case where T is a (C-style) array. The size of InnerContainer
+ // and T must be the same, otherwise callers' assumptions about use
+ // of this code will be broken.
+ struct InnerContainer {
+ T element;
+ };
+
+ // How many elements should we store inline?
+ // a. If not specified, use a default of 256 bytes (256 bytes
+ // seems small enough to not cause stack overflow or unnecessary
+ // stack pollution, while still allowing stack allocation for
+ // reasonably long character arrays.
+ // b. Never use 0 length arrays (not ISO C++)
+ static const size_type S1 = ((inline_elements < 0)
+ ? (256/sizeof(T)) : inline_elements);
+ static const size_type S2 = (S1 <= 0) ? 1 : S1;
+ static const size_type kInlineElements = S2;
+
+ size_type const size_;
+ InnerContainer* const array_;
+
+ // Allocate some space, not an array of elements of type T, so that we can
+ // skip calling the T constructors and destructors for space we never use.
+ ManualConstructor<InnerContainer> inline_space_[kInlineElements];
+};
+
+// Implementation details follow
+
+template <class T, ssize_t S>
+inline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n)
+ : size_(n),
+ array_((n <= kInlineElements
+ ? reinterpret_cast<InnerContainer*>(inline_space_)
+ : new InnerContainer[n])) {
+ // Construct only the elements actually used.
+ if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) {
+ for (size_t i = 0; i != size_; ++i) {
+ inline_space_[i].Init();
+ }
+ }
+}
+
+template <class T, ssize_t S>
+inline FixedArray<T, S>::~FixedArray() {
+ if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) {
+ delete[] array_;
+ } else {
+ for (size_t i = 0; i != size_; ++i) {
+ inline_space_[i].Destroy();
+ }
+ }
+}
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
diff --git a/include/ceres/internal/integer_sequence.h b/include/ceres/internal/integer_sequence.h
new file mode 100644
index 0000000..3936b92
--- /dev/null
+++ b/include/ceres/internal/integer_sequence.h
@@ -0,0 +1,106 @@
+// 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: jodebo_beck@gmx.de (Johannes Beck)
+//
+// This class mimics std::integer_sequence. That is the reason to follow the
+// naming convention of the stl and not the google one. Once Ceres switches
+// to c++ 14 this class can be removed.
+
+#ifndef CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_H_
+#define CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_H_
+
+#if __cplusplus >= 201402L
+// We have at least c++ 14 support. Use integer_sequence from the standard.
+// Sometimes the STL implementation uses a compiler intrinsic to generate
+// the sequences which will speed up compilation.
+#include <utility>
+
+namespace ceres {
+namespace internal {
+template <typename T, T... Ns>
+using integer_sequence = std::integer_sequence<T, Ns...>;
+
+template <typename T, T N>
+using make_integer_sequence = std::make_integer_sequence<T, N>;
+
+} // namespace internal
+} // namespace ceres
+#else
+
+namespace ceres {
+namespace internal {
+
+template <typename T, T... Ns>
+struct integer_sequence {
+ using value_type = T;
+};
+
+// Implementation of make_integer_sequence.
+//
+// Recursively instantiate make_integer_sequence_impl until Ns
+// contains the sequence 0, 1, ..., Total-1.
+//
+// Example for Total = 4:
+// T CurIdx, Total, Ns...
+// make_integer_sequence_impl<int, 0, 4 >
+// make_integer_sequence_impl<int, 1, 4, 0 >
+// make_integer_sequence_impl<int, 2, 4, 0, 1 >
+// make_integer_sequence_impl<int, 3, 4, 0, 1, 2 >
+// make_integer_sequence_impl<int, 4, 4, 0, 1, 2, 3>
+// ^^^^^^^^^^
+// resulting sequence.
+//
+// The implemented algorithm has linear complexity for simplicity. A O(log(N))
+// implementation can be found e.g. here:
+// https://stackoverflow.com/questions/17424477/implementation-c14-make-integer-sequence
+template <typename T, T CurIdx, T Total, T... Ns>
+struct make_integer_sequence_impl {
+ using type = typename make_integer_sequence_impl<T, CurIdx + 1, Total, Ns...,
+ CurIdx>::type;
+};
+
+// End of 'recursion' when CurIdx reaches Total. All indices 0, 1, ..., N-1 are
+// contained in Ns. The final integer_sequence is created here.
+template <typename T, T Total, T... Ns>
+struct make_integer_sequence_impl<T, Total, Total, Ns...> {
+ using type = integer_sequence<T, Ns...>;
+};
+
+// A helper alias template to simplify creation of integer_sequence with 0, 1,
+// ..., N-1 as Ns:
+template <typename T, T N>
+using make_integer_sequence =
+ typename make_integer_sequence_impl<T, 0, N>::type;
+
+} // namespace internal
+} // namespace ceres
+
+#endif
+
+#endif // CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_H_
diff --git a/include/ceres/internal/integer_sequence_algorithm.h b/include/ceres/internal/integer_sequence_algorithm.h
new file mode 100644
index 0000000..bdeab93
--- /dev/null
+++ b/include/ceres/internal/integer_sequence_algorithm.h
@@ -0,0 +1,165 @@
+// 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: jodebo_beck@gmx.de (Johannes Beck)
+//
+// Algorithms to be used together with integer_sequence, like computing the sum
+// or the exclusive scan (sometimes called exclusive prefix sum) at compile
+// time.
+
+#ifndef CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_
+#define CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_
+
+#include "integer_sequence.h"
+
+namespace ceres {
+namespace internal {
+
+// Implementation of calculating the sum of an integer sequence.
+// Recursively instantiate SumImpl and calculate the sum of the N first
+// numbers. This reduces the number of instantiations and speeds up
+// compilation.
+//
+// Examples:
+// 1) integer_sequence<int, 5>:
+// Value = 5
+//
+// 2) integer_sequence<int, 4, 2>:
+// Value = 4 + 2 + SumImpl<integer_sequence<int>>::Value
+// Value = 4 + 2 + 0
+//
+// 3) integer_sequence<int, 2, 1, 4>:
+// Value = 2 + 1 + SumImpl<integer_sequence<int, 4>>::Value
+// Value = 2 + 1 + 4
+template <typename Seq>
+struct SumImpl;
+
+// Strip of and sum the first number.
+template <typename T, T N, T... Ns>
+struct SumImpl<integer_sequence<T, N, Ns...>> {
+ static constexpr T Value = N + SumImpl<integer_sequence<T, Ns...>>::Value;
+};
+
+// Strip of and sum the first two numbers.
+template <typename T, T N1, T N2, T... Ns>
+struct SumImpl<integer_sequence<T, N1, N2, Ns...>> {
+ static constexpr T Value =
+ N1 + N2 + SumImpl<integer_sequence<T, Ns...>>::Value;
+};
+
+// Strip of and sum the first four numbers.
+template <typename T, T N1, T N2, T N3, T N4, T... Ns>
+struct SumImpl<integer_sequence<T, N1, N2, N3, N4, Ns...>> {
+ static constexpr T Value =
+ N1 + N2 + N3 + N4 + SumImpl<integer_sequence<T, Ns...>>::Value;
+};
+
+// Only one number is left. 'Value' is just that number ('recursion' ends).
+template <typename T, T N>
+struct SumImpl<integer_sequence<T, N>> {
+ static constexpr T Value = N;
+};
+
+// No number is left. 'Value' is the identity element (for sum this is zero).
+template <typename T>
+struct SumImpl<integer_sequence<T>> {
+ static constexpr T Value = T(0);
+};
+
+// Calculate the sum of an integer sequence. The resulting sum will be stored in
+// 'Value'.
+template <typename Seq>
+class Sum {
+ using T = typename Seq::value_type;
+
+ public:
+ static constexpr T Value = SumImpl<Seq>::Value;
+};
+
+// Implementation of calculating an exclusive scan (exclusive prefix sum) of an
+// integer sequence. Exclusive means that the i-th input element is not included
+// in the i-th sum. Calculating the exclusive scan for an input array I results
+// in the following output R:
+//
+// R[0] = 0
+// R[1] = I[0];
+// R[2] = I[0] + I[1];
+// R[3] = I[0] + I[1] + I[2];
+// ...
+//
+// In C++17 std::exclusive_scan does the same operation at runtime (but
+// cannot be used to calculate the prefix sum at compile time). See
+// https://en.cppreference.com/w/cpp/algorithm/exclusive_scan for a more
+// detailed description.
+//
+// Example for integer_sequence<int, 1, 4, 3> (seq := integer_sequence):
+// T , Sum, Ns... , Rs...
+// ExclusiveScanImpl<int, 0, seq<int, 1, 4, 3>, seq<int >>
+// ExclusiveScanImpl<int, 1, seq<int, 4, 3>, seq<int, 0 >>
+// ExclusiveScanImpl<int, 5, seq<int, 3>, seq<int, 0, 1 >>
+// ExclusiveScanImpl<int, 8, seq<int >, seq<int, 0, 1, 5>>
+// ^^^^^^^^^^^^^^^^^
+// resulting sequence
+template <typename T, T Sum, typename SeqIn, typename SeqOut>
+struct ExclusiveScanImpl;
+
+template <typename T, T Sum, T N, T... Ns, T... Rs>
+struct ExclusiveScanImpl<T, Sum, integer_sequence<T, N, Ns...>,
+ integer_sequence<T, Rs...>> {
+ using Type =
+ typename ExclusiveScanImpl<T, Sum + N, integer_sequence<T, Ns...>,
+ integer_sequence<T, Rs..., Sum>>::Type;
+};
+
+// End of 'recursion'. The resulting type is SeqOut.
+template <typename T, T Sum, typename SeqOut>
+struct ExclusiveScanImpl<T, Sum, integer_sequence<T>, SeqOut> {
+ using Type = SeqOut;
+};
+
+// Calculates the exclusive scan of the specified integer sequence. The last
+// element (the total) is not included in the resulting sequence so they have
+// same length. This means the exclusive scan of integer_sequence<int, 1, 2, 3>
+// will be integer_sequence<int, 0, 1, 3>.
+template <typename Seq>
+class ExclusiveScanT {
+ using T = typename Seq::value_type;
+
+ public:
+ using Type =
+ typename ExclusiveScanImpl<T, T(0), Seq, integer_sequence<T>>::Type;
+};
+
+// Helper to use exclusive scan without typename.
+template <typename Seq>
+using ExclusiveScan = typename ExclusiveScanT<Seq>::Type;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_
diff --git a/include/ceres/internal/manual_constructor.h b/include/ceres/internal/manual_constructor.h
new file mode 100644
index 0000000..60f147a
--- /dev/null
+++ b/include/ceres/internal/manual_constructor.h
@@ -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: kenton@google.com (Kenton Varda)
+//
+// ManualConstructor statically-allocates space in which to store some
+// object, but does not initialize it. You can then call the constructor
+// and destructor for the object yourself as you see fit. This is useful
+// for memory management optimizations, where you want to initialize and
+// destroy an object multiple times but only allocate it once.
+//
+// (When I say ManualConstructor statically allocates space, I mean that
+// the ManualConstructor object itself is forced to be the right size.)
+
+#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
+#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
+
+#include <new>
+#include <utility>
+
+namespace ceres {
+namespace internal {
+
+// ------- Define CERES_ALIGNED_CHAR_ARRAY --------------------------------
+
+// Platform independent macros to get aligned memory allocations.
+// For example
+//
+// MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16);
+//
+// Gives us an instance of MyFoo which is aligned at a 16 byte
+// boundary.
+#if defined(_MSC_VER)
+#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n))
+#define CERES_ALIGN_OF(T) __alignof(T)
+#elif defined(__GNUC__)
+#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n)))
+#define CERES_ALIGN_OF(T) __alignof(T)
+#endif
+
+#ifndef CERES_ALIGNED_CHAR_ARRAY
+
+// Because MSVC and older GCCs require that the argument to their alignment
+// construct to be a literal constant integer, we use a template instantiated
+// at all the possible powers of two.
+template<int alignment, int size> struct AlignType { };
+template<int size> struct AlignType<0, size> { typedef char result[size]; };
+
+#if !defined(CERES_ALIGN_ATTRIBUTE)
+#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler
+#else // !defined(CERES_ALIGN_ATTRIBUTE)
+
+#define CERES_ALIGN_TYPE_TEMPLATE(X) \
+ template<int size> struct AlignType<X, size> { \
+ typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \
+ }
+
+CERES_ALIGN_TYPE_TEMPLATE(1);
+CERES_ALIGN_TYPE_TEMPLATE(2);
+CERES_ALIGN_TYPE_TEMPLATE(4);
+CERES_ALIGN_TYPE_TEMPLATE(8);
+CERES_ALIGN_TYPE_TEMPLATE(16);
+CERES_ALIGN_TYPE_TEMPLATE(32);
+CERES_ALIGN_TYPE_TEMPLATE(64);
+CERES_ALIGN_TYPE_TEMPLATE(128);
+CERES_ALIGN_TYPE_TEMPLATE(256);
+CERES_ALIGN_TYPE_TEMPLATE(512);
+CERES_ALIGN_TYPE_TEMPLATE(1024);
+CERES_ALIGN_TYPE_TEMPLATE(2048);
+CERES_ALIGN_TYPE_TEMPLATE(4096);
+CERES_ALIGN_TYPE_TEMPLATE(8192);
+// Any larger and MSVC++ will complain.
+
+#undef CERES_ALIGN_TYPE_TEMPLATE
+
+#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \
+ typename AlignType<CERES_ALIGN_OF(T), sizeof(T) * Size>::result
+
+#endif // !defined(CERES_ALIGN_ATTRIBUTE)
+
+#endif // CERES_ALIGNED_CHAR_ARRAY
+
+template <typename Type>
+class ManualConstructor {
+ public:
+ // No constructor or destructor because one of the most useful uses of
+ // this class is as part of a union, and members of a union cannot have
+ // constructors or destructors. And, anyway, the whole point of this
+ // class is to bypass these.
+
+ inline Type* get() {
+ return reinterpret_cast<Type*>(space_);
+ }
+ inline const Type* get() const {
+ return reinterpret_cast<const Type*>(space_);
+ }
+
+ inline Type* operator->() { return get(); }
+ inline const Type* operator->() const { return get(); }
+
+ inline Type& operator*() { return *get(); }
+ inline const Type& operator*() const { return *get(); }
+
+ // This is needed to get around the strict aliasing warning GCC generates.
+ inline void* space() {
+ return reinterpret_cast<void*>(space_);
+ }
+
+ template <typename... Ts>
+ inline void Init(Ts&&... ps) {
+ new(space()) Type(std::forward<Ts>(ps)...);
+ }
+
+ inline void Destroy() {
+ get()->~Type();
+ }
+
+ private:
+ CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_;
+};
+
+#undef CERES_ALIGNED_CHAR_ARRAY
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h
new file mode 100644
index 0000000..8851bc7
--- /dev/null
+++ b/include/ceres/internal/numeric_diff.h
@@ -0,0 +1,499 @@
+// 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)
+// mierle@gmail.com (Keir Mierle)
+// tbennun@gmail.com (Tal Ben-Nun)
+//
+// Finite differencing routines used by NumericDiffCostFunction.
+
+#ifndef CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_
+#define CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_
+
+#include <cstring>
+
+#include "Eigen/Dense"
+#include "Eigen/StdVector"
+#include "ceres/cost_function.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/variadic_evaluate.h"
+#include "ceres/numeric_diff_options.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+
+namespace ceres {
+namespace internal {
+
+// This is split from the main class because C++ doesn't allow partial template
+// specializations for member functions. The alternative is to repeat the main
+// class for differing numbers of parameters, which is also unfortunate.
+template <typename CostFunctor, NumericDiffMethodType kMethod,
+ int kNumResiduals, typename ParameterDims, int kParameterBlock,
+ int kParameterBlockSize>
+struct NumericDiff {
+ // Mutates parameters but must restore them before return.
+ static bool EvaluateJacobianForParameterBlock(
+ const CostFunctor* functor,
+ const double* residuals_at_eval_point,
+ const NumericDiffOptions& options,
+ int num_residuals,
+ int parameter_block_index,
+ int parameter_block_size,
+ double **parameters,
+ double *jacobian) {
+ using Eigen::Map;
+ using Eigen::Matrix;
+ using Eigen::RowMajor;
+ using Eigen::ColMajor;
+
+ DCHECK(jacobian);
+
+ const int num_residuals_internal =
+ (kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals);
+ const int parameter_block_index_internal =
+ (kParameterBlock != ceres::DYNAMIC ? kParameterBlock :
+ parameter_block_index);
+ const int parameter_block_size_internal =
+ (kParameterBlockSize != ceres::DYNAMIC ? kParameterBlockSize :
+ parameter_block_size);
+
+ typedef Matrix<double, kNumResiduals, 1> ResidualVector;
+ typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
+
+ // The convoluted reasoning for choosing the Row/Column major
+ // ordering of the matrix is an artifact of the restrictions in
+ // Eigen that prevent it from creating RowMajor matrices with a
+ // single column. In these cases, we ask for a ColMajor matrix.
+ typedef Matrix<double,
+ kNumResiduals,
+ kParameterBlockSize,
+ (kParameterBlockSize == 1) ? ColMajor : RowMajor>
+ JacobianMatrix;
+
+ Map<JacobianMatrix> parameter_jacobian(jacobian,
+ num_residuals_internal,
+ parameter_block_size_internal);
+
+ Map<ParameterVector> x_plus_delta(
+ parameters[parameter_block_index_internal],
+ parameter_block_size_internal);
+ ParameterVector x(x_plus_delta);
+ ParameterVector step_size = x.array().abs() *
+ ((kMethod == RIDDERS) ? options.ridders_relative_initial_step_size :
+ options.relative_step_size);
+
+ // It is not a good idea to make the step size arbitrarily
+ // small. This will lead to problems with round off and numerical
+ // instability when dividing by the step size. The general
+ // recommendation is to not go down below sqrt(epsilon).
+ double min_step_size = std::sqrt(std::numeric_limits<double>::epsilon());
+
+ // For Ridders' method, the initial step size is required to be large,
+ // thus ridders_relative_initial_step_size is used.
+ if (kMethod == RIDDERS) {
+ min_step_size = std::max(min_step_size,
+ options.ridders_relative_initial_step_size);
+ }
+
+ // For each parameter in the parameter block, use finite differences to
+ // compute the derivative for that parameter.
+ FixedArray<double> temp_residual_array(num_residuals_internal);
+ FixedArray<double> residual_array(num_residuals_internal);
+ Map<ResidualVector> residuals(residual_array.get(),
+ num_residuals_internal);
+
+ for (int j = 0; j < parameter_block_size_internal; ++j) {
+ const double delta = std::max(min_step_size, step_size(j));
+
+ if (kMethod == RIDDERS) {
+ if (!EvaluateRiddersJacobianColumn(functor, j, delta,
+ options,
+ num_residuals_internal,
+ parameter_block_size_internal,
+ x.data(),
+ residuals_at_eval_point,
+ parameters,
+ x_plus_delta.data(),
+ temp_residual_array.get(),
+ residual_array.get())) {
+ return false;
+ }
+ } else {
+ if (!EvaluateJacobianColumn(functor, j, delta,
+ num_residuals_internal,
+ parameter_block_size_internal,
+ x.data(),
+ residuals_at_eval_point,
+ parameters,
+ x_plus_delta.data(),
+ temp_residual_array.get(),
+ residual_array.get())) {
+ return false;
+ }
+ }
+
+ parameter_jacobian.col(j).matrix() = residuals;
+ }
+ return true;
+ }
+
+ static bool EvaluateJacobianColumn(const CostFunctor* functor,
+ int parameter_index,
+ double delta,
+ int num_residuals,
+ int parameter_block_size,
+ const double* x_ptr,
+ const double* residuals_at_eval_point,
+ double** parameters,
+ double* x_plus_delta_ptr,
+ double* temp_residuals_ptr,
+ double* residuals_ptr) {
+ using Eigen::Map;
+ using Eigen::Matrix;
+
+ typedef Matrix<double, kNumResiduals, 1> ResidualVector;
+ typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
+
+ Map<const ParameterVector> x(x_ptr, parameter_block_size);
+ Map<ParameterVector> x_plus_delta(x_plus_delta_ptr,
+ parameter_block_size);
+
+ Map<ResidualVector> residuals(residuals_ptr, num_residuals);
+ Map<ResidualVector> temp_residuals(temp_residuals_ptr, num_residuals);
+
+ // Mutate 1 element at a time and then restore.
+ x_plus_delta(parameter_index) = x(parameter_index) + delta;
+
+ if (!VariadicEvaluate<ParameterDims>(*functor,
+ parameters,
+ residuals.data())) {
+ return false;
+ }
+
+ // Compute this column of the jacobian in 3 steps:
+ // 1. Store residuals for the forward part.
+ // 2. Subtract residuals for the backward (or 0) part.
+ // 3. Divide out the run.
+ double one_over_delta = 1.0 / delta;
+ if (kMethod == CENTRAL || kMethod == RIDDERS) {
+ // Compute the function on the other side of x(parameter_index).
+ x_plus_delta(parameter_index) = x(parameter_index) - delta;
+
+ if (!VariadicEvaluate<ParameterDims>(*functor,
+ parameters,
+ temp_residuals.data())) {
+ return false;
+ }
+
+ residuals -= temp_residuals;
+ one_over_delta /= 2;
+ } else {
+ // Forward difference only; reuse existing residuals evaluation.
+ residuals -=
+ Map<const ResidualVector>(residuals_at_eval_point,
+ num_residuals);
+ }
+
+ // Restore x_plus_delta.
+ x_plus_delta(parameter_index) = x(parameter_index);
+
+ // Divide out the run to get slope.
+ residuals *= one_over_delta;
+
+ return true;
+ }
+
+ // This numeric difference implementation uses adaptive differentiation
+ // on the parameters to obtain the Jacobian matrix. The adaptive algorithm
+ // is based on Ridders' method for adaptive differentiation, which creates
+ // a Romberg tableau from varying step sizes and extrapolates the
+ // intermediate results to obtain the current computational error.
+ //
+ // References:
+ // C.J.F. Ridders, Accurate computation of F'(x) and F'(x) F"(x), Advances
+ // in Engineering Software (1978), Volume 4, Issue 2, April 1982,
+ // Pages 75-76, ISSN 0141-1195,
+ // http://dx.doi.org/10.1016/S0141-1195(82)80057-0.
+ static bool EvaluateRiddersJacobianColumn(
+ const CostFunctor* functor,
+ int parameter_index,
+ double delta,
+ const NumericDiffOptions& options,
+ int num_residuals,
+ int parameter_block_size,
+ const double* x_ptr,
+ const double* residuals_at_eval_point,
+ double** parameters,
+ double* x_plus_delta_ptr,
+ double* temp_residuals_ptr,
+ double* residuals_ptr) {
+ using Eigen::Map;
+ using Eigen::Matrix;
+ using Eigen::aligned_allocator;
+
+ typedef Matrix<double, kNumResiduals, 1> ResidualVector;
+ typedef Matrix<double, kNumResiduals, Eigen::Dynamic> ResidualCandidateMatrix;
+ typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
+
+ Map<const ParameterVector> x(x_ptr, parameter_block_size);
+ Map<ParameterVector> x_plus_delta(x_plus_delta_ptr,
+ parameter_block_size);
+
+ Map<ResidualVector> residuals(residuals_ptr, num_residuals);
+ Map<ResidualVector> temp_residuals(temp_residuals_ptr, num_residuals);
+
+ // In order for the algorithm to converge, the step size should be
+ // initialized to a value that is large enough to produce a significant
+ // change in the function.
+ // As the derivative is estimated, the step size decreases.
+ // By default, the step sizes are chosen so that the middle column
+ // of the Romberg tableau uses the input delta.
+ double current_step_size = delta *
+ pow(options.ridders_step_shrink_factor,
+ options.max_num_ridders_extrapolations / 2);
+
+ // Double-buffering temporary differential candidate vectors
+ // from previous step size.
+ ResidualCandidateMatrix stepsize_candidates_a(
+ num_residuals,
+ options.max_num_ridders_extrapolations);
+ ResidualCandidateMatrix stepsize_candidates_b(
+ num_residuals,
+ options.max_num_ridders_extrapolations);
+ ResidualCandidateMatrix* current_candidates = &stepsize_candidates_a;
+ ResidualCandidateMatrix* previous_candidates = &stepsize_candidates_b;
+
+ // Represents the computational error of the derivative. This variable is
+ // initially set to a large value, and is set to the difference between
+ // current and previous finite difference extrapolations.
+ // norm_error is supposed to decrease as the finite difference tableau
+ // generation progresses, serving both as an estimate for differentiation
+ // error and as a measure of differentiation numerical stability.
+ double norm_error = std::numeric_limits<double>::max();
+
+ // Loop over decreasing step sizes until:
+ // 1. Error is smaller than a given value (ridders_epsilon),
+ // 2. Maximal order of extrapolation reached, or
+ // 3. Extrapolation becomes numerically unstable.
+ for (int i = 0; i < options.max_num_ridders_extrapolations; ++i) {
+ // Compute the numerical derivative at this step size.
+ if (!EvaluateJacobianColumn(functor, parameter_index, current_step_size,
+ num_residuals,
+ parameter_block_size,
+ x.data(),
+ residuals_at_eval_point,
+ parameters,
+ x_plus_delta.data(),
+ temp_residuals.data(),
+ current_candidates->col(0).data())) {
+ // Something went wrong; bail.
+ return false;
+ }
+
+ // Store initial results.
+ if (i == 0) {
+ residuals = current_candidates->col(0);
+ }
+
+ // Shrink differentiation step size.
+ current_step_size /= options.ridders_step_shrink_factor;
+
+ // Extrapolation factor for Richardson acceleration method (see below).
+ double richardson_factor = options.ridders_step_shrink_factor *
+ options.ridders_step_shrink_factor;
+ for (int k = 1; k <= i; ++k) {
+ // Extrapolate the various orders of finite differences using
+ // the Richardson acceleration method.
+ current_candidates->col(k) =
+ (richardson_factor * current_candidates->col(k - 1) -
+ previous_candidates->col(k - 1)) / (richardson_factor - 1.0);
+
+ richardson_factor *= options.ridders_step_shrink_factor *
+ options.ridders_step_shrink_factor;
+
+ // Compute the difference between the previous value and the current.
+ double candidate_error = std::max(
+ (current_candidates->col(k) -
+ current_candidates->col(k - 1)).norm(),
+ (current_candidates->col(k) -
+ previous_candidates->col(k - 1)).norm());
+
+ // If the error has decreased, update results.
+ if (candidate_error <= norm_error) {
+ norm_error = candidate_error;
+ residuals = current_candidates->col(k);
+
+ // If the error is small enough, stop.
+ if (norm_error < options.ridders_epsilon) {
+ break;
+ }
+ }
+ }
+
+ // After breaking out of the inner loop, declare convergence.
+ if (norm_error < options.ridders_epsilon) {
+ break;
+ }
+
+ // Check to see if the current gradient estimate is numerically unstable.
+ // If so, bail out and return the last stable result.
+ if (i > 0) {
+ double tableau_error = (current_candidates->col(i) -
+ previous_candidates->col(i - 1)).norm();
+
+ // Compare current error to the chosen candidate's error.
+ if (tableau_error >= 2 * norm_error) {
+ break;
+ }
+ }
+
+ std::swap(current_candidates, previous_candidates);
+ }
+ return true;
+ }
+};
+
+// This function calls NumericDiff<...>::EvaluateJacobianForParameterBlock for
+// each parameter block.
+//
+// Example:
+// A call to
+// EvaluateJacobianForParameterBlocks<StaticParameterDims<2, 3>>(
+// functor,
+// residuals_at_eval_point,
+// options,
+// num_residuals,
+// parameters,
+// jacobians);
+// will result in the following calls to
+// NumericDiff<...>::EvaluateJacobianForParameterBlock:
+//
+// if (jacobians[0] != nullptr) {
+// if (!NumericDiff<
+// CostFunctor,
+// method,
+// kNumResiduals,
+// StaticParameterDims<2, 3>,
+// 0,
+// 2>::EvaluateJacobianForParameterBlock(functor,
+// residuals_at_eval_point,
+// options,
+// num_residuals,
+// 0,
+// 2,
+// parameters,
+// jacobians[0])) {
+// return false;
+// }
+// }
+// if (jacobians[1] != nullptr) {
+// if (!NumericDiff<
+// CostFunctor,
+// method,
+// kNumResiduals,
+// StaticParameterDims<2, 3>,
+// 1,
+// 3>::EvaluateJacobianForParameterBlock(functor,
+// residuals_at_eval_point,
+// options,
+// num_residuals,
+// 1,
+// 3,
+// parameters,
+// jacobians[1])) {
+// return false;
+// }
+// }
+template <typename ParameterDims,
+ typename Parameters = typename ParameterDims::Parameters,
+ int ParameterIdx = 0>
+struct EvaluateJacobianForParameterBlocks;
+
+template <typename ParameterDims, int N, int... Ns, int ParameterIdx>
+struct EvaluateJacobianForParameterBlocks<ParameterDims,
+ integer_sequence<int, N, Ns...>,
+ ParameterIdx> {
+ template <NumericDiffMethodType method,
+ int kNumResiduals,
+ typename CostFunctor>
+ static bool Apply(const CostFunctor* functor,
+ const double* residuals_at_eval_point,
+ const NumericDiffOptions& options,
+ int num_residuals,
+ double** parameters,
+ double** jacobians) {
+ if (jacobians[ParameterIdx] != nullptr) {
+ if (!NumericDiff<
+ CostFunctor,
+ method,
+ kNumResiduals,
+ ParameterDims,
+ ParameterIdx,
+ N>::EvaluateJacobianForParameterBlock(functor,
+ residuals_at_eval_point,
+ options,
+ num_residuals,
+ ParameterIdx,
+ N,
+ parameters,
+ jacobians[ParameterIdx])) {
+ return false;
+ }
+ }
+
+ return EvaluateJacobianForParameterBlocks<ParameterDims,
+ integer_sequence<int, Ns...>,
+ ParameterIdx + 1>::
+ template Apply<method, kNumResiduals>(functor,
+ residuals_at_eval_point,
+ options,
+ num_residuals,
+ parameters,
+ jacobians);
+ }
+};
+
+// End of 'recursion'. Nothing more to do.
+template <typename ParameterDims, int ParameterIdx>
+struct EvaluateJacobianForParameterBlocks<ParameterDims, integer_sequence<int>,
+ ParameterIdx> {
+ template <NumericDiffMethodType method, int kNumResiduals,
+ typename CostFunctor>
+ static bool Apply(const CostFunctor* /* NOT USED*/,
+ const double* /* NOT USED*/,
+ const NumericDiffOptions& /* NOT USED*/, int /* NOT USED*/,
+ double** /* NOT USED*/, double** /* NOT USED*/) {
+ return true;
+ }
+};
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_
diff --git a/include/ceres/internal/parameter_dims.h b/include/ceres/internal/parameter_dims.h
new file mode 100644
index 0000000..2272e8d
--- /dev/null
+++ b/include/ceres/internal/parameter_dims.h
@@ -0,0 +1,124 @@
+// 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: jodebo_beck@gmx.de (Johannes Beck)
+
+#ifndef CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_
+#define CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_
+
+#include <array>
+
+#include "ceres/internal/integer_sequence.h"
+#include "ceres/internal/integer_sequence_algorithm.h"
+
+namespace ceres {
+namespace internal {
+
+// Checks, whether the given parameter block sizes are valid. Valid means every
+// dimension is bigger than zero.
+constexpr bool IsValidParameterDimensionSequence(integer_sequence<int>) {
+ return true;
+}
+
+template <int N, int... Ts>
+constexpr bool IsValidParameterDimensionSequence(
+ integer_sequence<int, N, Ts...>) {
+ return (N <= 0) ? false
+ : IsValidParameterDimensionSequence(
+ integer_sequence<int, Ts...>());
+}
+
+// Helper class that represents the parameter dimensions. The parameter
+// dimensions are either dynamic or the sizes are known at compile time. It is
+// used to pass parameter block dimensions around (e.g. between functions or
+// classes).
+//
+// As an example if one have three parameter blocks with dimensions (2, 4, 1),
+// one would use 'StaticParameterDims<2, 4, 1>' which is a synonym for
+// 'ParameterDims<false, 2, 4, 1>'.
+// For dynamic parameter dims, one would just use 'DynamicParameterDims', which
+// is a synonym for 'ParameterDims<true>'.
+template <bool IsDynamic, int... Ns>
+class ParameterDims {
+ public:
+ using Parameters = integer_sequence<int, Ns...>;
+
+ // The parameter dimensions are only valid if all parameter block dimensions
+ // are greater than zero.
+ static constexpr bool kIsValid =
+ IsValidParameterDimensionSequence(Parameters());
+ static_assert(kIsValid,
+ "Invalid parameter block dimension detected. Each parameter "
+ "block dimension must be bigger than zero.");
+
+ static constexpr bool kIsDynamic = IsDynamic;
+ static constexpr int kNumParameterBlocks = sizeof...(Ns);
+ static_assert(kIsDynamic || kNumParameterBlocks > 0,
+ "At least one parameter block must be specified.");
+
+ static constexpr int kNumParameters =
+ Sum<integer_sequence<int, Ns...>>::Value;
+
+ static constexpr int GetDim(int dim) { return params_[dim]; }
+
+ // If one has all parameters packed into a single array this function unpacks
+ // the parameters.
+ template <typename T>
+ static inline std::array<T*, kNumParameterBlocks> GetUnpackedParameters(
+ T* ptr) {
+ using Offsets = ExclusiveScan<Parameters>;
+ return GetUnpackedParameters(ptr, Offsets());
+ }
+
+ private:
+ template <typename T, int... Indices>
+ static inline std::array<T*, kNumParameterBlocks> GetUnpackedParameters(
+ T* ptr, integer_sequence<int, Indices...>) {
+ return std::array<T*, kNumParameterBlocks>{{ptr + Indices...}};
+ }
+
+ static constexpr std::array<int, kNumParameterBlocks> params_{Ns...};
+};
+
+// Even static constexpr member variables needs to be defined (not only
+// declared). As the ParameterDims class is tempalted this definition must
+// be in the header file.
+template <bool IsDynamic, int... Ns>
+constexpr std::array<int, ParameterDims<IsDynamic, Ns...>::kNumParameterBlocks>
+ ParameterDims<IsDynamic, Ns...>::params_;
+
+// Using declarations for static and dynamic parameter dims. This makes client
+// code easier to read.
+template <int... Ns>
+using StaticParameterDims = ParameterDims<false, Ns...>;
+using DynamicParameterDims = ParameterDims<true>;
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_
diff --git a/include/ceres/internal/port.h b/include/ceres/internal/port.h
new file mode 100644
index 0000000..07bb505
--- /dev/null
+++ b/include/ceres/internal/port.h
@@ -0,0 +1,90 @@
+// 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)
+
+#ifndef CERES_PUBLIC_INTERNAL_PORT_H_
+#define CERES_PUBLIC_INTERNAL_PORT_H_
+
+// This file needs to compile as c code.
+#include "ceres/internal/config.h"
+
+#if defined(CERES_USE_OPENMP)
+# if defined(CERES_USE_CXX11_THREADS) || defined(CERES_NO_THREADS)
+# error CERES_USE_OPENMP is mutually exclusive to CERES_USE_CXX11_THREADS and CERES_NO_THREADS
+# endif
+#elif defined(CERES_USE_CXX11_THREADS)
+# if defined(CERES_USE_OPENMP) || defined(CERES_NO_THREADS)
+# error CERES_USE_CXX11_THREADS is mutually exclusive to CERES_USE_OPENMP, CERES_USE_CXX11_THREADS and CERES_NO_THREADS
+# endif
+#elif defined(CERES_NO_THREADS)
+# if defined(CERES_USE_OPENMP) || defined(CERES_USE_CXX11_THREADS)
+# error CERES_NO_THREADS is mutually exclusive to CERES_USE_OPENMP and CERES_USE_CXX11_THREADS
+# endif
+#else
+# error One of CERES_USE_OPENMP, CERES_USE_CXX11_THREADS or CERES_NO_THREADS must be defined.
+#endif
+
+// CERES_NO_SPARSE should be automatically defined by config.h if Ceres was
+// compiled without any sparse back-end. Verify that it has not subsequently
+// been inconsistently redefined.
+#if defined(CERES_NO_SPARSE)
+# if !defined(CERES_NO_SUITESPARSE)
+# error CERES_NO_SPARSE requires CERES_NO_SUITESPARSE.
+# endif
+# if !defined(CERES_NO_CXSPARSE)
+# error CERES_NO_SPARSE requires CERES_NO_CXSPARSE
+# endif
+# if !defined(CERES_NO_ACCELERATE_SPARSE)
+# error CERES_NO_SPARSE requires CERES_NO_ACCELERATE_SPARSE
+# endif
+# if defined(CERES_USE_EIGEN_SPARSE)
+# error CERES_NO_SPARSE requires !CERES_USE_EIGEN_SPARSE
+# endif
+#endif
+
+// A macro to signal which functions and classes are exported when
+// building a DLL with MSVC.
+//
+// Note that the ordering here is important, CERES_BUILDING_SHARED_LIBRARY
+// is only defined locally when Ceres is compiled, it is never exported to
+// users. However, in order that we do not have to configure config.h
+// separately for building vs installing, if we are using MSVC and building
+// a shared library, then both CERES_BUILDING_SHARED_LIBRARY and
+// CERES_USING_SHARED_LIBRARY will be defined when Ceres is compiled.
+// Hence it is important that the check for CERES_BUILDING_SHARED_LIBRARY
+// happens first.
+#if defined(_MSC_VER) && defined(CERES_BUILDING_SHARED_LIBRARY)
+# define CERES_EXPORT __declspec(dllexport)
+#elif defined(_MSC_VER) && defined(CERES_USING_SHARED_LIBRARY)
+# define CERES_EXPORT __declspec(dllimport)
+#else
+# define CERES_EXPORT
+#endif
+
+#endif // CERES_PUBLIC_INTERNAL_PORT_H_
diff --git a/include/ceres/internal/reenable_warnings.h b/include/ceres/internal/reenable_warnings.h
new file mode 100644
index 0000000..7e41025
--- /dev/null
+++ b/include/ceres/internal/reenable_warnings.h
@@ -0,0 +1,38 @@
+// 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.
+//
+
+// This is not your usual header guard. See disable_warnings.h
+#ifdef CERES_WARNINGS_DISABLED
+#undef CERES_WARNINGS_DISABLED
+
+#ifdef _MSC_VER
+#pragma warning( pop )
+#endif
+
+#endif // CERES_WARNINGS_DISABLED
diff --git a/include/ceres/internal/variadic_evaluate.h b/include/ceres/internal/variadic_evaluate.h
new file mode 100644
index 0000000..26428d0
--- /dev/null
+++ b/include/ceres/internal/variadic_evaluate.h
@@ -0,0 +1,105 @@
+// 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)
+// mierle@gmail.com (Keir Mierle)
+// jodebo_beck@gmx.de (Johannes Beck)
+
+#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
+#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
+
+#include <stddef.h>
+
+#include <type_traits>
+
+#include "ceres/cost_function.h"
+#include "ceres/internal/parameter_dims.h"
+
+namespace ceres {
+namespace internal {
+
+// For fixed size cost functors
+template <typename Functor, typename T, int... Indices>
+inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
+ T* output, std::false_type /*is_dynamic*/,
+ integer_sequence<int, Indices...>) {
+ static_assert(sizeof...(Indices),
+ "Invalid number of parameter blocks. At least one parameter "
+ "block must be specified.");
+ return functor(input[Indices]..., output);
+}
+
+// For dynamic sized cost functors
+template <typename Functor, typename T>
+inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
+ T* output, std::true_type /*is_dynamic*/,
+ integer_sequence<int>) {
+ return functor(input, output);
+}
+
+// For ceres cost functors (not ceres::CostFunction)
+template <typename ParameterDims, typename Functor, typename T>
+inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
+ T* output, const void* /* NOT USED */) {
+ using ParameterBlockIndices =
+ make_integer_sequence<int, ParameterDims::kNumParameterBlocks>;
+ using IsDynamic = std::integral_constant<bool, ParameterDims::kIsDynamic>;
+ return VariadicEvaluateImpl(functor, input, output, IsDynamic(),
+ ParameterBlockIndices());
+}
+
+// For ceres::CostFunction
+template <typename ParameterDims, typename Functor, typename T>
+inline bool VariadicEvaluateImpl(const Functor& functor, T const* const* input,
+ T* output,
+ const CostFunction* /* NOT USED */) {
+ return functor.Evaluate(input, output, nullptr);
+}
+
+// Variadic evaluate is a helper function to evaluate ceres cost function or
+// functors using an input, output and the parameter dimensions. There are
+// several ways different possibilities:
+// 1) If the passed functor is a 'ceres::CostFunction' its evaluate method is
+// called.
+// 2) If the functor is not a 'ceres::CostFunction' and the specified parameter
+// dims is dynamic, the functor must have the following signature
+// 'bool(T const* const* input, T* output)'.
+// 3) If the functor is not a 'ceres::CostFunction' and the specified parameter
+// dims is not dynamic, the input is expanded by using the number of parameter
+// blocks. The signature of the functor must have the following signature
+// 'bool()(const T* i_1, const T* i_2, ... const T* i_n, T* output)'.
+template <typename ParameterDims, typename Functor, typename T>
+inline bool VariadicEvaluate(const Functor& functor, T const* const* input,
+ T* output) {
+ return VariadicEvaluateImpl<ParameterDims>(functor, input, output, &functor);
+}
+
+} // namespace internal
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
diff --git a/include/ceres/iteration_callback.h b/include/ceres/iteration_callback.h
new file mode 100644
index 0000000..bd1e782
--- /dev/null
+++ b/include/ceres/iteration_callback.h
@@ -0,0 +1,225 @@
+// 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)
+//
+// When an iteration callback is specified, Ceres calls the callback
+// after each minimizer step (if the minimizer has not converged) and
+// passes it an IterationSummary object, defined below.
+
+#ifndef CERES_PUBLIC_ITERATION_CALLBACK_H_
+#define CERES_PUBLIC_ITERATION_CALLBACK_H_
+
+#include "ceres/types.h"
+#include "ceres/internal/disable_warnings.h"
+
+namespace ceres {
+
+// This struct describes the state of the optimizer after each
+// iteration of the minimization.
+struct CERES_EXPORT IterationSummary {
+ IterationSummary()
+ : iteration(0),
+ step_is_valid(false),
+ step_is_nonmonotonic(false),
+ step_is_successful(false),
+ cost(0.0),
+ cost_change(0.0),
+ gradient_max_norm(0.0),
+ gradient_norm(0.0),
+ step_norm(0.0),
+ eta(0.0),
+ step_size(0.0),
+ line_search_function_evaluations(0),
+ line_search_gradient_evaluations(0),
+ line_search_iterations(0),
+ linear_solver_iterations(0),
+ iteration_time_in_seconds(0.0),
+ step_solver_time_in_seconds(0.0),
+ cumulative_time_in_seconds(0.0) {}
+
+ // Current iteration number.
+ int iteration;
+
+ // Step was numerically valid, i.e., all values are finite and the
+ // step reduces the value of the linearized model.
+ //
+ // Note: step_is_valid is always true when iteration = 0.
+ bool step_is_valid;
+
+ // Step did not reduce the value of the objective function
+ // sufficiently, but it was accepted because of the relaxed
+ // acceptance criterion used by the non-monotonic trust region
+ // algorithm.
+ //
+ // Note: step_is_nonmonotonic is always false when iteration = 0;
+ bool step_is_nonmonotonic;
+
+ // Whether or not the minimizer accepted this step or not. If the
+ // ordinary trust region algorithm is used, this means that the
+ // relative reduction in the objective function value was greater
+ // than Solver::Options::min_relative_decrease. However, if the
+ // non-monotonic trust region algorithm is used
+ // (Solver::Options:use_nonmonotonic_steps = true), then even if the
+ // relative decrease is not sufficient, the algorithm may accept the
+ // step and the step is declared successful.
+ //
+ // Note: step_is_successful is always true when iteration = 0.
+ bool step_is_successful;
+
+ // Value of the objective function.
+ double cost;
+
+ // Change in the value of the objective function in this
+ // iteration. This can be positive or negative.
+ double cost_change;
+
+ // Infinity norm of the gradient vector.
+ double gradient_max_norm;
+
+ // 2-norm of the gradient vector.
+ double gradient_norm;
+
+ // 2-norm of the size of the step computed by the optimization
+ // algorithm.
+ double step_norm;
+
+ // For trust region algorithms, the ratio of the actual change in
+ // cost and the change in the cost of the linearized approximation.
+ double relative_decrease;
+
+ // Size of the trust region at the end of the current iteration. For
+ // the Levenberg-Marquardt algorithm, the regularization parameter
+ // mu = 1.0 / trust_region_radius.
+ double trust_region_radius;
+
+ // For the inexact step Levenberg-Marquardt algorithm, this is the
+ // relative accuracy with which the Newton(LM) step is solved. This
+ // number affects only the iterative solvers capable of solving
+ // linear systems inexactly. Factorization-based exact solvers
+ // ignore it.
+ double eta;
+
+ // Step sized computed by the line search algorithm.
+ double step_size;
+
+ // Number of function value evaluations used by the line search algorithm.
+ int line_search_function_evaluations;
+
+ // Number of function gradient evaluations used by the line search algorithm.
+ int line_search_gradient_evaluations;
+
+ // Number of iterations taken by the line search algorithm.
+ int line_search_iterations;
+
+ // Number of iterations taken by the linear solver to solve for the
+ // Newton step.
+ int linear_solver_iterations;
+
+ // All times reported below are wall times.
+
+ // Time (in seconds) spent inside the minimizer loop in the current
+ // iteration.
+ double iteration_time_in_seconds;
+
+ // Time (in seconds) spent inside the trust region step solver.
+ double step_solver_time_in_seconds;
+
+ // Time (in seconds) since the user called Solve().
+ double cumulative_time_in_seconds;
+};
+
+// Interface for specifying callbacks that are executed at the end of
+// each iteration of the Minimizer. The solver uses the return value
+// of operator() to decide whether to continue solving or to
+// terminate. The user can return three values.
+//
+// SOLVER_ABORT indicates that the callback detected an abnormal
+// situation. The solver returns without updating the parameter blocks
+// (unless Solver::Options::update_state_every_iteration is set
+// true). Solver returns with Solver::Summary::termination_type set to
+// USER_ABORT.
+//
+// SOLVER_TERMINATE_SUCCESSFULLY indicates that there is no need to
+// optimize anymore (some user specified termination criterion has
+// been met). Solver returns with Solver::Summary::termination_type
+// set to USER_SUCCESS.
+//
+// SOLVER_CONTINUE indicates that the solver should continue
+// optimizing.
+//
+// For example, the following Callback is used internally by Ceres to
+// log the progress of the optimization.
+//
+// Callback for logging the state of the minimizer to STDERR or STDOUT
+// depending on the user's preferences and logging level.
+//
+// class LoggingCallback : public IterationCallback {
+// public:
+// explicit LoggingCallback(bool log_to_stdout)
+// : log_to_stdout_(log_to_stdout) {}
+//
+// ~LoggingCallback() {}
+//
+// CallbackReturnType operator()(const IterationSummary& summary) {
+// const char* kReportRowFormat =
+// "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e "
+// "rho:% 3.2e mu:% 3.2e eta:% 3.2e li:% 3d";
+// string output = StringPrintf(kReportRowFormat,
+// summary.iteration,
+// summary.cost,
+// summary.cost_change,
+// summary.gradient_max_norm,
+// summary.step_norm,
+// summary.relative_decrease,
+// summary.trust_region_radius,
+// summary.eta,
+// summary.linear_solver_iterations);
+// if (log_to_stdout_) {
+// cout << output << endl;
+// } else {
+// VLOG(1) << output;
+// }
+// return SOLVER_CONTINUE;
+// }
+//
+// private:
+// const bool log_to_stdout_;
+// };
+//
+class CERES_EXPORT IterationCallback {
+ public:
+ virtual ~IterationCallback() {}
+ virtual CallbackReturnType operator()(const IterationSummary& summary) = 0;
+};
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_ITERATION_CALLBACK_H_
diff --git a/include/ceres/jet.h b/include/ceres/jet.h
new file mode 100644
index 0000000..2b54064
--- /dev/null
+++ b/include/ceres/jet.h
@@ -0,0 +1,990 @@
+// 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 implementation of N-dimensional dual numbers, for automatically
+// computing exact derivatives of functions.
+//
+// While a complete treatment of the mechanics of automatic differentiation is
+// beyond the scope of this header (see
+// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the
+// basic idea is to extend normal arithmetic with an extra element, "e," often
+// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual
+// numbers are extensions of the real numbers analogous to complex numbers:
+// whereas complex numbers augment the reals by introducing an imaginary unit i
+// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such
+// that e^2 = 0. Dual numbers have two components: the "real" component and the
+// "infinitesimal" component, generally written as x + y*e. Surprisingly, this
+// leads to a convenient method for computing exact derivatives without needing
+// to manipulate complicated symbolic expressions.
+//
+// For example, consider the function
+//
+// f(x) = x^2 ,
+//
+// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20.
+// Next, argument 10 with an infinitesimal to get:
+//
+// f(10 + e) = (10 + e)^2
+// = 100 + 2 * 10 * e + e^2
+// = 100 + 20 * e -+-
+// -- |
+// | +--- This is zero, since e^2 = 0
+// |
+// +----------------- This is df/dx!
+//
+// Note that the derivative of f with respect to x is simply the infinitesimal
+// component of the value of f(x + e). So, in order to take the derivative of
+// any function, it is only necessary to replace the numeric "object" used in
+// the function with one extended with infinitesimals. The class Jet, defined in
+// this header, is one such example of this, where substitution is done with
+// templates.
+//
+// To handle derivatives of functions taking multiple arguments, different
+// infinitesimals are used, one for each variable to take the derivative of. For
+// example, consider a scalar function of two scalar parameters x and y:
+//
+// f(x, y) = x^2 + x * y
+//
+// Following the technique above, to compute the derivatives df/dx and df/dy for
+// f(1, 3) involves doing two evaluations of f, the first time replacing x with
+// x + e, the second time replacing y with y + e.
+//
+// For df/dx:
+//
+// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3
+// = 1 + 2 * e + 3 + 3 * e
+// = 4 + 5 * e
+//
+// --> df/dx = 5
+//
+// For df/dy:
+//
+// f(1, 3 + e) = 1^2 + 1 * (3 + e)
+// = 1 + 3 + e
+// = 4 + e
+//
+// --> df/dy = 1
+//
+// To take the gradient of f with the implementation of dual numbers ("jets") in
+// this file, it is necessary to create a single jet type which has components
+// for the derivative in x and y, and passing them to a templated version of f:
+//
+// template<typename T>
+// T f(const T &x, const T &y) {
+// return x * x + x * y;
+// }
+//
+// // The "2" means there should be 2 dual number components.
+// // It computes the partial derivative at x=10, y=20.
+// Jet<double, 2> x(10, 0); // Pick the 0th dual number for x.
+// Jet<double, 2> y(20, 1); // Pick the 1st dual number for y.
+// Jet<double, 2> z = f(x, y);
+//
+// LOG(INFO) << "df/dx = " << z.v[0]
+// << "df/dy = " << z.v[1];
+//
+// Most users should not use Jet objects directly; a wrapper around Jet objects,
+// which makes computing the derivative, gradient, or jacobian of templated
+// functors simple, is in autodiff.h. Even autodiff.h should not be used
+// directly; instead autodiff_cost_function.h is typically the file of interest.
+//
+// For the more mathematically inclined, this file implements first-order
+// "jets". A 1st order jet is an element of the ring
+//
+// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2
+//
+// which essentially means that each jet consists of a "scalar" value 'a' from T
+// and a 1st order perturbation vector 'v' of length N:
+//
+// x = a + \sum_i v[i] t_i
+//
+// A shorthand is to write an element as x = a + u, where u is the perturbation.
+// Then, the main point about the arithmetic of jets is that the product of
+// perturbations is zero:
+//
+// (a + u) * (b + v) = ab + av + bu + uv
+// = ab + (av + bu) + 0
+//
+// which is what operator* implements below. Addition is simpler:
+//
+// (a + u) + (b + v) = (a + b) + (u + v).
+//
+// The only remaining question is how to evaluate the function of a jet, for
+// which we use the chain rule:
+//
+// f(a + u) = f(a) + f'(a) u
+//
+// where f'(a) is the (scalar) derivative of f at a.
+//
+// By pushing these things through sufficiently and suitably templated
+// functions, we can do automatic differentiation. Just be sure to turn on
+// function inlining and common-subexpression elimination, or it will be very
+// slow!
+//
+// WARNING: Most Ceres users should not directly include this file or know the
+// details of how jets work. Instead the suggested method for automatic
+// derivatives is to use autodiff_cost_function.h, which is a wrapper around
+// both jets.h and autodiff.h to make taking derivatives of cost functions for
+// use in Ceres easier.
+
+#ifndef CERES_PUBLIC_JET_H_
+#define CERES_PUBLIC_JET_H_
+
+#include <cmath>
+#include <iosfwd>
+#include <iostream> // NOLINT
+#include <limits>
+#include <string>
+
+#include "Eigen/Core"
+#include "ceres/internal/port.h"
+
+namespace ceres {
+
+template <typename T, int N>
+struct Jet {
+ enum { DIMENSION = N };
+ typedef T Scalar;
+
+ // Default-construct "a" because otherwise this can lead to false errors about
+ // uninitialized uses when other classes relying on default constructed T
+ // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that
+ // the C++ standard mandates that e.g. default constructed doubles are
+ // initialized to 0.0; see sections 8.5 of the C++03 standard.
+ Jet() : a() {
+ v.setZero();
+ }
+
+ // Constructor from scalar: a + 0.
+ explicit Jet(const T& value) {
+ a = value;
+ v.setZero();
+ }
+
+ // Constructor from scalar plus variable: a + t_i.
+ Jet(const T& value, int k) {
+ a = value;
+ v.setZero();
+ v[k] = T(1.0);
+ }
+
+ // Constructor from scalar and vector part
+ // The use of Eigen::DenseBase allows Eigen expressions
+ // to be passed in without being fully evaluated until
+ // they are assigned to v
+ template<typename Derived>
+ EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived> &v)
+ : a(a), v(v) {
+ }
+
+ // Compound operators
+ Jet<T, N>& operator+=(const Jet<T, N> &y) {
+ *this = *this + y;
+ return *this;
+ }
+
+ Jet<T, N>& operator-=(const Jet<T, N> &y) {
+ *this = *this - y;
+ return *this;
+ }
+
+ Jet<T, N>& operator*=(const Jet<T, N> &y) {
+ *this = *this * y;
+ return *this;
+ }
+
+ Jet<T, N>& operator/=(const Jet<T, N> &y) {
+ *this = *this / y;
+ return *this;
+ }
+
+ // Compound with scalar operators.
+ Jet<T, N>& operator+=(const T& s) {
+ *this = *this + s;
+ return *this;
+ }
+
+ Jet<T, N>& operator-=(const T& s) {
+ *this = *this - s;
+ return *this;
+ }
+
+ Jet<T, N>& operator*=(const T& s) {
+ *this = *this * s;
+ return *this;
+ }
+
+ Jet<T, N>& operator/=(const T& s) {
+ *this = *this / s;
+ return *this;
+ }
+
+ // The scalar part.
+ T a;
+
+ // The infinitesimal part.
+ //
+ // We allocate Jets on the stack and other places they might not be aligned
+ // to X(=16 [SSE], 32 [AVX] etc)-byte boundaries, which would prevent the safe
+ // use of vectorisation. If we have C++11, we can specify the alignment.
+ // However, the standard gives wide latitude as to what alignments are valid,
+ // and it might be that the maximum supported alignment *guaranteed* to be
+ // supported is < 16, in which case we do not specify an alignment, as this
+ // implies the host is not a modern x86 machine. If using < C++11, we cannot
+ // specify alignment.
+
+#if defined(EIGEN_DONT_VECTORIZE)
+ Eigen::Matrix<T, N, 1, Eigen::DontAlign> v;
+#else
+ // Enable vectorisation iff the maximum supported scalar alignment is >=
+ // 16 bytes, as this is the minimum required by Eigen for any vectorisation.
+ //
+ // NOTE: It might be the case that we could get >= 16-byte alignment even if
+ // max_align_t < 16. However we can't guarantee that this
+ // would happen (and it should not for any modern x86 machine) and if it
+ // didn't, we could get misaligned Jets.
+ static constexpr int kAlignOrNot =
+ // Work around a GCC 4.8 bug
+ // (https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56019) where
+ // std::max_align_t is misplaced.
+#if defined (__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ == 8
+ alignof(::max_align_t) >= 16
+#else
+ alignof(std::max_align_t) >= 16
+#endif
+ ? Eigen::AutoAlign : Eigen::DontAlign;
+
+#if defined(EIGEN_MAX_ALIGN_BYTES)
+ // Eigen >= 3.3 supports AVX & FMA instructions that require 32-byte alignment
+ // (greater for AVX512). Rather than duplicating the detection logic, use
+ // Eigen's macro for the alignment size.
+ //
+ // NOTE: EIGEN_MAX_ALIGN_BYTES can be > 16 (e.g. 32 for AVX), even though
+ // kMaxAlignBytes will max out at 16. We are therefore relying on
+ // Eigen's detection logic to ensure that this does not result in
+ // misaligned Jets.
+#define CERES_JET_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
+#else
+ // Eigen < 3.3 only supported 16-byte alignment.
+#define CERES_JET_ALIGN_BYTES 16
+#endif
+
+ // Default to the native alignment if 16-byte alignment is not guaranteed to
+ // be supported. We cannot use alignof(T) as if we do, GCC 4.8 complains that
+ // the alignment 'is not an integer constant', although Clang accepts it.
+ static constexpr size_t kAlignment = kAlignOrNot == Eigen::AutoAlign
+ ? CERES_JET_ALIGN_BYTES : alignof(double);
+
+#undef CERES_JET_ALIGN_BYTES
+ alignas(kAlignment) Eigen::Matrix<T, N, 1, kAlignOrNot> v;
+#endif
+};
+
+// Unary +
+template<typename T, int N> inline
+Jet<T, N> const& operator+(const Jet<T, N>& f) {
+ return f;
+}
+
+// TODO(keir): Try adding __attribute__((always_inline)) to these functions to
+// see if it causes a performance increase.
+
+// Unary -
+template<typename T, int N> inline
+Jet<T, N> operator-(const Jet<T, N>&f) {
+ return Jet<T, N>(-f.a, -f.v);
+}
+
+// Binary +
+template<typename T, int N> inline
+Jet<T, N> operator+(const Jet<T, N>& f,
+ const Jet<T, N>& g) {
+ return Jet<T, N>(f.a + g.a, f.v + g.v);
+}
+
+// Binary + with a scalar: x + s
+template<typename T, int N> inline
+Jet<T, N> operator+(const Jet<T, N>& f, T s) {
+ return Jet<T, N>(f.a + s, f.v);
+}
+
+// Binary + with a scalar: s + x
+template<typename T, int N> inline
+Jet<T, N> operator+(T s, const Jet<T, N>& f) {
+ return Jet<T, N>(f.a + s, f.v);
+}
+
+// Binary -
+template<typename T, int N> inline
+Jet<T, N> operator-(const Jet<T, N>& f,
+ const Jet<T, N>& g) {
+ return Jet<T, N>(f.a - g.a, f.v - g.v);
+}
+
+// Binary - with a scalar: x - s
+template<typename T, int N> inline
+Jet<T, N> operator-(const Jet<T, N>& f, T s) {
+ return Jet<T, N>(f.a - s, f.v);
+}
+
+// Binary - with a scalar: s - x
+template<typename T, int N> inline
+Jet<T, N> operator-(T s, const Jet<T, N>& f) {
+ return Jet<T, N>(s - f.a, -f.v);
+}
+
+// Binary *
+template<typename T, int N> inline
+Jet<T, N> operator*(const Jet<T, N>& f,
+ const Jet<T, N>& g) {
+ return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a);
+}
+
+// Binary * with a scalar: x * s
+template<typename T, int N> inline
+Jet<T, N> operator*(const Jet<T, N>& f, T s) {
+ return Jet<T, N>(f.a * s, f.v * s);
+}
+
+// Binary * with a scalar: s * x
+template<typename T, int N> inline
+Jet<T, N> operator*(T s, const Jet<T, N>& f) {
+ return Jet<T, N>(f.a * s, f.v * s);
+}
+
+// Binary /
+template<typename T, int N> inline
+Jet<T, N> operator/(const Jet<T, N>& f,
+ const Jet<T, N>& g) {
+ // This uses:
+ //
+ // a + u (a + u)(b - v) (a + u)(b - v)
+ // ----- = -------------- = --------------
+ // b + v (b + v)(b - v) b^2
+ //
+ // which holds because v*v = 0.
+ const T g_a_inverse = T(1.0) / g.a;
+ const T f_a_by_g_a = f.a * g_a_inverse;
+ return Jet<T, N>(f_a_by_g_a, (f.v - f_a_by_g_a * g.v) * g_a_inverse);
+}
+
+// Binary / with a scalar: s / x
+template<typename T, int N> inline
+Jet<T, N> operator/(T s, const Jet<T, N>& g) {
+ const T minus_s_g_a_inverse2 = -s / (g.a * g.a);
+ return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2);
+}
+
+// Binary / with a scalar: x / s
+template<typename T, int N> inline
+Jet<T, N> operator/(const Jet<T, N>& f, T s) {
+ const T s_inverse = T(1.0) / s;
+ return Jet<T, N>(f.a * s_inverse, f.v * s_inverse);
+}
+
+// Binary comparison operators for both scalars and jets.
+#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \
+template<typename T, int N> inline \
+bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \
+ return f.a op g.a; \
+} \
+template<typename T, int N> inline \
+bool operator op(const T& s, const Jet<T, N>& g) { \
+ return s op g.a; \
+} \
+template<typename T, int N> inline \
+bool operator op(const Jet<T, N>& f, const T& s) { \
+ return f.a op s; \
+}
+CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT
+#undef CERES_DEFINE_JET_COMPARISON_OPERATOR
+
+// Pull some functions from namespace std.
+//
+// This is necessary because we want to use the same name (e.g. 'sqrt') for
+// double-valued and Jet-valued functions, but we are not allowed to put
+// Jet-valued functions inside namespace std.
+using std::abs;
+using std::acos;
+using std::asin;
+using std::atan;
+using std::atan2;
+using std::cbrt;
+using std::ceil;
+using std::cos;
+using std::cosh;
+using std::exp;
+using std::exp2;
+using std::floor;
+using std::fmax;
+using std::fmin;
+using std::hypot;
+using std::isfinite;
+using std::isinf;
+using std::isnan;
+using std::isnormal;
+using std::log;
+using std::log2;
+using std::pow;
+using std::sin;
+using std::sinh;
+using std::sqrt;
+using std::tan;
+using std::tanh;
+
+// Legacy names from pre-C++11 days.
+inline bool IsFinite (double x) { return std::isfinite(x); }
+inline bool IsInfinite(double x) { return std::isinf(x); }
+inline bool IsNaN (double x) { return std::isnan(x); }
+inline bool IsNormal (double x) { return std::isnormal(x); }
+
+// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
+
+// abs(x + h) ~= x + h or -(x + h)
+template <typename T, int N> inline
+Jet<T, N> abs(const Jet<T, N>& f) {
+ return f.a < T(0.0) ? -f : f;
+}
+
+// log(a + h) ~= log(a) + h / a
+template <typename T, int N> inline
+Jet<T, N> log(const Jet<T, N>& f) {
+ const T a_inverse = T(1.0) / f.a;
+ return Jet<T, N>(log(f.a), f.v * a_inverse);
+}
+
+// exp(a + h) ~= exp(a) + exp(a) h
+template <typename T, int N> inline
+Jet<T, N> exp(const Jet<T, N>& f) {
+ const T tmp = exp(f.a);
+ return Jet<T, N>(tmp, tmp * f.v);
+}
+
+// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
+template <typename T, int N> inline
+Jet<T, N> sqrt(const Jet<T, N>& f) {
+ const T tmp = sqrt(f.a);
+ const T two_a_inverse = T(1.0) / (T(2.0) * tmp);
+ return Jet<T, N>(tmp, f.v * two_a_inverse);
+}
+
+// cos(a + h) ~= cos(a) - sin(a) h
+template <typename T, int N> inline
+Jet<T, N> cos(const Jet<T, N>& f) {
+ return Jet<T, N>(cos(f.a), - sin(f.a) * f.v);
+}
+
+// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h
+template <typename T, int N> inline
+Jet<T, N> acos(const Jet<T, N>& f) {
+ const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a);
+ return Jet<T, N>(acos(f.a), tmp * f.v);
+}
+
+// sin(a + h) ~= sin(a) + cos(a) h
+template <typename T, int N> inline
+Jet<T, N> sin(const Jet<T, N>& f) {
+ return Jet<T, N>(sin(f.a), cos(f.a) * f.v);
+}
+
+// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h
+template <typename T, int N> inline
+Jet<T, N> asin(const Jet<T, N>& f) {
+ const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a);
+ return Jet<T, N>(asin(f.a), tmp * f.v);
+}
+
+// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h
+template <typename T, int N> inline
+Jet<T, N> tan(const Jet<T, N>& f) {
+ const T tan_a = tan(f.a);
+ const T tmp = T(1.0) + tan_a * tan_a;
+ return Jet<T, N>(tan_a, tmp * f.v);
+}
+
+// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h
+template <typename T, int N> inline
+Jet<T, N> atan(const Jet<T, N>& f) {
+ const T tmp = T(1.0) / (T(1.0) + f.a * f.a);
+ return Jet<T, N>(atan(f.a), tmp * f.v);
+}
+
+// sinh(a + h) ~= sinh(a) + cosh(a) h
+template <typename T, int N> inline
+Jet<T, N> sinh(const Jet<T, N>& f) {
+ return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v);
+}
+
+// cosh(a + h) ~= cosh(a) + sinh(a) h
+template <typename T, int N> inline
+Jet<T, N> cosh(const Jet<T, N>& f) {
+ return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v);
+}
+
+// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h
+template <typename T, int N> inline
+Jet<T, N> tanh(const Jet<T, N>& f) {
+ const T tanh_a = tanh(f.a);
+ const T tmp = T(1.0) - tanh_a * tanh_a;
+ return Jet<T, N>(tanh_a, tmp * f.v);
+}
+
+// The floor function should be used with extreme care as this operation will
+// result in a zero derivative which provides no information to the solver.
+//
+// floor(a + h) ~= floor(a) + 0
+template <typename T, int N> inline
+Jet<T, N> floor(const Jet<T, N>& f) {
+ return Jet<T, N>(floor(f.a));
+}
+
+// The ceil function should be used with extreme care as this operation will
+// result in a zero derivative which provides no information to the solver.
+//
+// ceil(a + h) ~= ceil(a) + 0
+template <typename T, int N> inline
+Jet<T, N> ceil(const Jet<T, N>& f) {
+ return Jet<T, N>(ceil(f.a));
+}
+
+// Some new additions to C++11:
+
+// cbrt(a + h) ~= cbrt(a) + h / (3 a ^ (2/3))
+template <typename T, int N> inline
+Jet<T, N> cbrt(const Jet<T, N>& f) {
+ const T derivative = T(1.0) / (T(3.0) * cbrt(f.a * f.a));
+ return Jet<T, N>(cbrt(f.a), f.v * derivative);
+}
+
+// exp2(x + h) = 2^(x+h) ~= 2^x + h*2^x*log(2)
+template <typename T, int N> inline
+Jet<T, N> exp2(const Jet<T, N>& f) {
+ const T tmp = exp2(f.a);
+ const T derivative = tmp * log(T(2));
+ return Jet<T, N>(tmp, f.v * derivative);
+}
+
+// log2(x + h) ~= log2(x) + h / (x * log(2))
+template <typename T, int N> inline
+Jet<T, N> log2(const Jet<T, N>& f) {
+ const T derivative = T(1.0) / (f.a * log(T(2)));
+ return Jet<T, N>(log2(f.a), f.v * derivative);
+}
+
+// Like sqrt(x^2 + y^2),
+// but acts to prevent underflow/overflow for small/large x/y.
+// Note that the function is non-smooth at x=y=0,
+// so the derivative is undefined there.
+template <typename T, int N> inline
+Jet<T, N> hypot(const Jet<T, N>& x, const Jet<T, N>& y) {
+ // d/da sqrt(a) = 0.5 / sqrt(a)
+ // d/dx x^2 + y^2 = 2x
+ // So by the chain rule:
+ // d/dx sqrt(x^2 + y^2) = 0.5 / sqrt(x^2 + y^2) * 2x = x / sqrt(x^2 + y^2)
+ // d/dy sqrt(x^2 + y^2) = y / sqrt(x^2 + y^2)
+ const T tmp = hypot(x.a, y.a);
+ return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v);
+}
+
+template <typename T, int N> inline
+const Jet<T, N>& fmax(const Jet<T, N>& x, const Jet<T, N>& y) {
+ return x < y ? y : x;
+}
+
+template <typename T, int N> inline
+const Jet<T, N>& fmin(const Jet<T, N>& x, const Jet<T, N>& y) {
+ return y < x ? y : x;
+}
+
+// Bessel functions of the first kind with integer order equal to 0, 1, n.
+//
+// Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of
+// _j[0,1,n](). Where available on MSVC, use _j[0,1,n]() to avoid deprecated
+// function errors in client code (the specific warning is suppressed when
+// Ceres itself is built).
+inline double BesselJ0(double x) {
+#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS)
+ return _j0(x);
+#else
+ return j0(x);
+#endif
+}
+inline double BesselJ1(double x) {
+#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS)
+ return _j1(x);
+#else
+ return j1(x);
+#endif
+}
+inline double BesselJn(int n, double x) {
+#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS)
+ return _jn(n, x);
+#else
+ return jn(n, x);
+#endif
+}
+
+// For the formulae of the derivatives of the Bessel functions see the book:
+// Olver, Lozier, Boisvert, Clark, NIST Handbook of Mathematical Functions,
+// Cambridge University Press 2010.
+//
+// Formulae are also available at http://dlmf.nist.gov
+
+// See formula http://dlmf.nist.gov/10.6#E3
+// j0(a + h) ~= j0(a) - j1(a) h
+template <typename T, int N> inline
+Jet<T, N> BesselJ0(const Jet<T, N>& f) {
+ return Jet<T, N>(BesselJ0(f.a),
+ -BesselJ1(f.a) * f.v);
+}
+
+// See formula http://dlmf.nist.gov/10.6#E1
+// j1(a + h) ~= j1(a) + 0.5 ( j0(a) - j2(a) ) h
+template <typename T, int N> inline
+Jet<T, N> BesselJ1(const Jet<T, N>& f) {
+ return Jet<T, N>(BesselJ1(f.a),
+ T(0.5) * (BesselJ0(f.a) - BesselJn(2, f.a)) * f.v);
+}
+
+// See formula http://dlmf.nist.gov/10.6#E1
+// j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h
+template <typename T, int N> inline
+Jet<T, N> BesselJn(int n, const Jet<T, N>& f) {
+ return Jet<T, N>(BesselJn(n, f.a),
+ T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v);
+}
+
+// Jet Classification. It is not clear what the appropriate semantics are for
+// these classifications. This picks that std::isfinite and std::isnormal are "all"
+// operations, i.e. all elements of the jet must be finite for the jet itself
+// to be finite (or normal). For IsNaN and IsInfinite, the answer is less
+// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any
+// part of a jet is nan or inf, then the entire jet is nan or inf. This leads
+// to strange situations like a jet can be both IsInfinite and IsNaN, but in
+// practice the "any" semantics are the most useful for e.g. checking that
+// derivatives are sane.
+
+// The jet is finite if all parts of the jet are finite.
+template <typename T, int N> inline
+bool isfinite(const Jet<T, N>& f) {
+ if (!std::isfinite(f.a)) {
+ return false;
+ }
+ for (int i = 0; i < N; ++i) {
+ if (!std::isfinite(f.v[i])) {
+ return false;
+ }
+ }
+ return true;
+}
+
+// The jet is infinite if any part of the Jet is infinite.
+template <typename T, int N> inline
+bool isinf(const Jet<T, N>& f) {
+ if (std::isinf(f.a)) {
+ return true;
+ }
+ for (int i = 0; i < N; ++i) {
+ if (std::isinf(f.v[i])) {
+ return true;
+ }
+ }
+ return false;
+}
+
+
+// The jet is NaN if any part of the jet is NaN.
+template <typename T, int N> inline
+bool isnan(const Jet<T, N>& f) {
+ if (std::isnan(f.a)) {
+ return true;
+ }
+ for (int i = 0; i < N; ++i) {
+ if (std::isnan(f.v[i])) {
+ return true;
+ }
+ }
+ return false;
+}
+
+// The jet is normal if all parts of the jet are normal.
+template <typename T, int N> inline
+bool isnormal(const Jet<T, N>& f) {
+ if (!std::isnormal(f.a)) {
+ return false;
+ }
+ for (int i = 0; i < N; ++i) {
+ if (!std::isnormal(f.v[i])) {
+ return false;
+ }
+ }
+ return true;
+}
+
+// Legacy functions from the pre-C++11 days.
+template <typename T, int N>
+inline bool IsFinite(const Jet<T, N>& f) {
+ return isfinite(f);
+}
+
+template <typename T, int N>
+inline bool IsNaN(const Jet<T, N>& f) {
+ return isnan(f);
+}
+
+template <typename T, int N>
+inline bool IsNormal(const Jet<T, N>& f) {
+ return isnormal(f);
+}
+
+// The jet is infinite if any part of the jet is infinite.
+template <typename T, int N> inline
+bool IsInfinite(const Jet<T, N>& f) {
+ return isinf(f);
+}
+
+// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
+//
+// In words: the rate of change of theta is 1/r times the rate of
+// change of (x, y) in the positive angular direction.
+template <typename T, int N> inline
+Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) {
+ // Note order of arguments:
+ //
+ // f = a + da
+ // g = b + db
+
+ T const tmp = T(1.0) / (f.a * f.a + g.a * g.a);
+ return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v));
+}
+
+
+// pow -- base is a differentiable function, exponent is a constant.
+// (a+da)^p ~= a^p + p*a^(p-1) da
+template <typename T, int N> inline
+Jet<T, N> pow(const Jet<T, N>& f, double g) {
+ T const tmp = g * pow(f.a, g - T(1.0));
+ return Jet<T, N>(pow(f.a, g), tmp * f.v);
+}
+
+// pow -- base is a constant, exponent is a differentiable function.
+// We have various special cases, see the comment for pow(Jet, Jet) for
+// analysis:
+//
+// 1. For f > 0 we have: (f)^(g + dg) ~= f^g + f^g log(f) dg
+//
+// 2. For f == 0 and g > 0 we have: (f)^(g + dg) ~= f^g
+//
+// 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg
+// != 0, the derivatives are not defined and we return NaN.
+
+template <typename T, int N> inline
+Jet<T, N> pow(double f, const Jet<T, N>& g) {
+ if (f == 0 && g.a > 0) {
+ // Handle case 2.
+ return Jet<T, N>(T(0.0));
+ }
+ if (f < 0 && g.a == floor(g.a)) {
+ // Handle case 3.
+ Jet<T, N> ret(pow(f, g.a));
+ for (int i = 0; i < N; i++) {
+ if (g.v[i] != T(0.0)) {
+ // Return a NaN when g.v != 0.
+ ret.v[i] = std::numeric_limits<T>::quiet_NaN();
+ }
+ }
+ return ret;
+ }
+ // Handle case 1.
+ T const tmp = pow(f, g.a);
+ return Jet<T, N>(tmp, log(f) * tmp * g.v);
+}
+
+// pow -- both base and exponent are differentiable functions. This has a
+// variety of special cases that require careful handling.
+//
+// 1. For f > 0:
+// (f + df)^(g + dg) ~= f^g + f^(g - 1) * (g * df + f * log(f) * dg)
+// The numerical evaluation of f * log(f) for f > 0 is well behaved, even for
+// extremely small values (e.g. 1e-99).
+//
+// 2. For f == 0 and g > 1: (f + df)^(g + dg) ~= 0
+// This cases is needed because log(0) can not be evaluated in the f > 0
+// expression. However the function f*log(f) is well behaved around f == 0
+// and its limit as f-->0 is zero.
+//
+// 3. For f == 0 and g == 1: (f + df)^(g + dg) ~= 0 + df
+//
+// 4. For f == 0 and 0 < g < 1: The value is finite but the derivatives are not.
+//
+// 5. For f == 0 and g < 0: The value and derivatives of f^g are not finite.
+//
+// 6. For f == 0 and g == 0: The C standard incorrectly defines 0^0 to be 1
+// "because there are applications that can exploit this definition". We
+// (arbitrarily) decree that derivatives here will be nonfinite, since that
+// is consistent with the behavior for f == 0, g < 0 and 0 < g < 1.
+// Practically any definition could have been justified because mathematical
+// consistency has been lost at this point.
+//
+// 7. For f < 0, g integer, dg == 0: (f + df)^(g + dg) ~= f^g + g * f^(g - 1) df
+// This is equivalent to the case where f is a differentiable function and g
+// is a constant (to first order).
+//
+// 8. For f < 0, g integer, dg != 0: The value is finite but the derivatives are
+// not, because any change in the value of g moves us away from the point
+// with a real-valued answer into the region with complex-valued answers.
+//
+// 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite.
+
+template <typename T, int N> inline
+Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
+ if (f.a == 0 && g.a >= 1) {
+ // Handle cases 2 and 3.
+ if (g.a > 1) {
+ return Jet<T, N>(T(0.0));
+ }
+ return f;
+ }
+ if (f.a < 0 && g.a == floor(g.a)) {
+ // Handle cases 7 and 8.
+ T const tmp = g.a * pow(f.a, g.a - T(1.0));
+ Jet<T, N> ret(pow(f.a, g.a), tmp * f.v);
+ for (int i = 0; i < N; i++) {
+ if (g.v[i] != T(0.0)) {
+ // Return a NaN when g.v != 0.
+ ret.v[i] = std::numeric_limits<T>::quiet_NaN();
+ }
+ }
+ return ret;
+ }
+ // Handle the remaining cases. For cases 4,5,6,9 we allow the log() function
+ // to generate -HUGE_VAL or NaN, since those cases result in a nonfinite
+ // derivative.
+ T const tmp1 = pow(f.a, g.a);
+ T const tmp2 = g.a * pow(f.a, g.a - T(1.0));
+ T const tmp3 = tmp1 * log(f.a);
+ return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
+}
+
+// Note: This has to be in the ceres namespace for argument dependent lookup to
+// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with
+// strange compile errors.
+template <typename T, int N>
+inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) {
+ s << "[" << z.a << " ; ";
+ for (int i = 0; i < N; ++i) {
+ s << z.v[i];
+ if (i != N - 1) {
+ s << ", ";
+ }
+ }
+ s << "]";
+ return s;
+}
+
+} // namespace ceres
+
+namespace Eigen {
+
+// Creating a specialization of NumTraits enables placing Jet objects inside
+// Eigen arrays, getting all the goodness of Eigen combined with autodiff.
+template<typename T, int N>
+struct NumTraits<ceres::Jet<T, N>> {
+ typedef ceres::Jet<T, N> Real;
+ typedef ceres::Jet<T, N> NonInteger;
+ typedef ceres::Jet<T, N> Nested;
+ typedef ceres::Jet<T, N> Literal;
+
+ static typename ceres::Jet<T, N> dummy_precision() {
+ return ceres::Jet<T, N>(1e-12);
+ }
+
+ static inline Real epsilon() {
+ return Real(std::numeric_limits<T>::epsilon());
+ }
+
+ static inline int digits10() { return NumTraits<T>::digits10(); }
+
+ enum {
+ IsComplex = 0,
+ IsInteger = 0,
+ IsSigned,
+ ReadCost = 1,
+ AddCost = 1,
+ // For Jet types, multiplication is more expensive than addition.
+ MulCost = 3,
+ HasFloatingPoint = 1,
+ RequireInitialization = 1
+ };
+
+ template<bool Vectorized>
+ struct Div {
+ enum {
+#if defined(EIGEN_VECTORIZE_AVX)
+ AVX = true,
+#else
+ AVX = false,
+#endif
+
+ // Assuming that for Jets, division is as expensive as
+ // multiplication.
+ Cost = 3
+ };
+ };
+
+ static inline Real highest() { return Real(std::numeric_limits<T>::max()); }
+ static inline Real lowest() { return Real(-std::numeric_limits<T>::max()); }
+};
+
+#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
+// Specifying the return type of binary operations between Jets and scalar types
+// allows you to perform matrix/array operations with Eigen matrices and arrays
+// such as addition, subtraction, multiplication, and division where one Eigen
+// matrix/array is of type Jet and the other is a scalar type. This improves
+// performance by using the optimized scalar-to-Jet binary operations but
+// is only available on Eigen versions >= 3.3
+template <typename BinaryOp, typename T, int N>
+struct ScalarBinaryOpTraits<ceres::Jet<T, N>, T, BinaryOp> {
+ typedef ceres::Jet<T, N> ReturnType;
+};
+template <typename BinaryOp, typename T, int N>
+struct ScalarBinaryOpTraits<T, ceres::Jet<T, N>, BinaryOp> {
+ typedef ceres::Jet<T, N> ReturnType;
+};
+#endif // EIGEN_VERSION_AT_LEAST(3, 3, 0)
+
+} // namespace Eigen
+
+#endif // CERES_PUBLIC_JET_H_
diff --git a/include/ceres/local_parameterization.h b/include/ceres/local_parameterization.h
new file mode 100644
index 0000000..5eed035
--- /dev/null
+++ b/include/ceres/local_parameterization.h
@@ -0,0 +1,322 @@
+// 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)
+// sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
+#define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
+
+#include <vector>
+#include "ceres/internal/port.h"
+#include "ceres/internal/disable_warnings.h"
+
+namespace ceres {
+
+// Purpose: Sometimes parameter blocks x can overparameterize a problem
+//
+// min f(x)
+// x
+//
+// In that case it is desirable to choose a parameterization for the
+// block itself to remove the null directions of the cost. More
+// generally, if x lies on a manifold of a smaller dimension than the
+// ambient space that it is embedded in, then it is numerically and
+// computationally more effective to optimize it using a
+// parameterization that lives in the tangent space of that manifold
+// at each point.
+//
+// For example, a sphere in three dimensions is a 2 dimensional
+// manifold, embedded in a three dimensional space. At each point on
+// the sphere, the plane tangent to it defines a two dimensional
+// tangent space. For a cost function defined on this sphere, given a
+// point x, moving in the direction normal to the sphere at that point
+// is not useful. Thus a better way to do a local optimization is to
+// optimize over two dimensional vector delta in the tangent space at
+// that point and then "move" to the point x + delta, where the move
+// operation involves projecting back onto the sphere. Doing so
+// removes a redundant dimension from the optimization, making it
+// numerically more robust and efficient.
+//
+// More generally we can define a function
+//
+// x_plus_delta = Plus(x, delta),
+//
+// where x_plus_delta has the same size as x, and delta is of size
+// less than or equal to x. The function Plus, generalizes the
+// definition of vector addition. Thus it satisfies the identify
+//
+// Plus(x, 0) = x, for all x.
+//
+// A trivial version of Plus is when delta is of the same size as x
+// and
+//
+// Plus(x, delta) = x + delta
+//
+// A more interesting case if x is two dimensional vector, and the
+// user wishes to hold the first coordinate constant. Then, delta is a
+// scalar and Plus is defined as
+//
+// Plus(x, delta) = x + [0] * delta
+// [1]
+//
+// An example that occurs commonly in Structure from Motion problems
+// is when camera rotations are parameterized using Quaternion. There,
+// it is useful only make updates orthogonal to that 4-vector defining
+// the quaternion. One way to do this is to let delta be a 3
+// dimensional vector and define Plus to be
+//
+// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x
+//
+// The multiplication between the two 4-vectors on the RHS is the
+// standard quaternion product.
+//
+// Given g and a point x, optimizing f can now be restated as
+//
+// min f(Plus(x, delta))
+// delta
+//
+// Given a solution delta to this problem, the optimal value is then
+// given by
+//
+// x* = Plus(x, delta)
+//
+// The class LocalParameterization defines the function Plus and its
+// Jacobian which is needed to compute the Jacobian of f w.r.t delta.
+class CERES_EXPORT LocalParameterization {
+ public:
+ virtual ~LocalParameterization();
+
+ // Generalization of the addition operation,
+ //
+ // x_plus_delta = Plus(x, delta)
+ //
+ // with the condition that Plus(x, 0) = x.
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const = 0;
+
+ // The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
+ //
+ // jacobian is a row-major GlobalSize() x LocalSize() matrix.
+ virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;
+
+ // local_matrix = global_matrix * jacobian
+ //
+ // global_matrix is a num_rows x GlobalSize row major matrix.
+ // local_matrix is a num_rows x LocalSize row major matrix.
+ // jacobian(x) is the matrix returned by ComputeJacobian at x.
+ //
+ // This is only used by GradientProblem. For most normal uses, it is
+ // okay to use the default implementation.
+ virtual bool MultiplyByJacobian(const double* x,
+ const int num_rows,
+ const double* global_matrix,
+ double* local_matrix) const;
+
+ // Size of x.
+ virtual int GlobalSize() const = 0;
+
+ // Size of delta.
+ virtual int LocalSize() const = 0;
+};
+
+// Some basic parameterizations
+
+// Identity Parameterization: Plus(x, delta) = x + delta
+class CERES_EXPORT IdentityParameterization : public LocalParameterization {
+ public:
+ explicit IdentityParameterization(int size);
+ virtual ~IdentityParameterization() {}
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const;
+ virtual bool ComputeJacobian(const double* x,
+ double* jacobian) const;
+ virtual bool MultiplyByJacobian(const double* x,
+ const int num_cols,
+ const double* global_matrix,
+ double* local_matrix) const;
+ virtual int GlobalSize() const { return size_; }
+ virtual int LocalSize() const { return size_; }
+
+ private:
+ const int size_;
+};
+
+// Hold a subset of the parameters inside a parameter block constant.
+class CERES_EXPORT SubsetParameterization : public LocalParameterization {
+ public:
+ explicit SubsetParameterization(int size,
+ const std::vector<int>& constant_parameters);
+ virtual ~SubsetParameterization() {}
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const;
+ virtual bool ComputeJacobian(const double* x,
+ double* jacobian) const;
+ virtual bool MultiplyByJacobian(const double* x,
+ const int num_cols,
+ const double* global_matrix,
+ double* local_matrix) const;
+ virtual int GlobalSize() const {
+ return static_cast<int>(constancy_mask_.size());
+ }
+ virtual int LocalSize() const { return local_size_; }
+
+ private:
+ const int local_size_;
+ std::vector<char> constancy_mask_;
+};
+
+// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x
+// with * being the quaternion multiplication operator. Here we assume
+// that the first element of the quaternion vector is the real (cos
+// theta) part.
+class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
+ public:
+ virtual ~QuaternionParameterization() {}
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const;
+ virtual bool ComputeJacobian(const double* x,
+ double* jacobian) const;
+ virtual int GlobalSize() const { return 4; }
+ virtual int LocalSize() const { return 3; }
+};
+
+// Implements the quaternion local parameterization for Eigen's representation
+// of the quaternion. Eigen uses a different internal memory layout for the
+// elements of the quaternion than what is commonly used. Specifically, Eigen
+// stores the elements in memory as [x, y, z, w] where the real part is last
+// whereas it is typically stored first. Note, when creating an Eigen quaternion
+// through the constructor the elements are accepted in w, x, y, z order. Since
+// Ceres operates on parameter blocks which are raw double pointers this
+// difference is important and requires a different parameterization.
+//
+// Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x
+// with * being the quaternion multiplication operator.
+class CERES_EXPORT EigenQuaternionParameterization
+ : public ceres::LocalParameterization {
+ public:
+ virtual ~EigenQuaternionParameterization() {}
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const;
+ virtual bool ComputeJacobian(const double* x, double* jacobian) const;
+ virtual int GlobalSize() const { return 4; }
+ virtual int LocalSize() const { return 3; }
+};
+
+// This provides a parameterization for homogeneous vectors which are commonly
+// used in Structure for Motion problems. One example where they are used is
+// in representing points whose triangulation is ill-conditioned. Here
+// it is advantageous to use an over-parameterization since homogeneous vectors
+// can represent points at infinity.
+//
+// The plus operator is defined as
+// Plus(x, delta) =
+// [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x
+// with * defined as an operator which applies the update orthogonal to x to
+// remain on the sphere. We assume that the last element of x is the scalar
+// component. The size of the homogeneous vector is required to be greater than
+// 1.
+class CERES_EXPORT HomogeneousVectorParameterization :
+ public LocalParameterization {
+ public:
+ explicit HomogeneousVectorParameterization(int size);
+ virtual ~HomogeneousVectorParameterization() {}
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const;
+ virtual bool ComputeJacobian(const double* x,
+ double* jacobian) const;
+ virtual int GlobalSize() const { return size_; }
+ virtual int LocalSize() const { return size_ - 1; }
+
+ private:
+ const int size_;
+};
+
+// Construct a local parameterization by taking the Cartesian product
+// of a number of other local parameterizations. This is useful, when
+// a parameter block is the cartesian product of two or more
+// manifolds. For example the parameters of a camera consist of a
+// rotation and a translation, i.e., SO(3) x R^3.
+//
+// Currently this class supports taking the cartesian product of up to
+// four local parameterizations.
+//
+// Example usage:
+//
+// ProductParameterization product_param(new QuaterionionParameterization(),
+// new IdentityParameterization(3));
+//
+// is the local parameterization for a rigid transformation, where the
+// rotation is represented using a quaternion.
+class CERES_EXPORT ProductParameterization : public LocalParameterization {
+ public:
+ //
+ // NOTE: All the constructors take ownership of the input local
+ // parameterizations.
+ //
+ ProductParameterization(LocalParameterization* local_param1,
+ LocalParameterization* local_param2);
+
+ ProductParameterization(LocalParameterization* local_param1,
+ LocalParameterization* local_param2,
+ LocalParameterization* local_param3);
+
+ ProductParameterization(LocalParameterization* local_param1,
+ LocalParameterization* local_param2,
+ LocalParameterization* local_param3,
+ LocalParameterization* local_param4);
+
+ virtual ~ProductParameterization();
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const;
+ virtual bool ComputeJacobian(const double* x,
+ double* jacobian) const;
+ virtual int GlobalSize() const { return global_size_; }
+ virtual int LocalSize() const { return local_size_; }
+
+ private:
+ void Init();
+
+ std::vector<LocalParameterization*> local_params_;
+ int local_size_;
+ int global_size_;
+ int buffer_size_;
+};
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
diff --git a/include/ceres/loss_function.h b/include/ceres/loss_function.h
new file mode 100644
index 0000000..97a70b6
--- /dev/null
+++ b/include/ceres/loss_function.h
@@ -0,0 +1,430 @@
+// 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)
+//
+// The LossFunction interface is the way users describe how residuals
+// are converted to cost terms for the overall problem cost function.
+// For the exact manner in which loss functions are converted to the
+// overall cost for a problem, see problem.h.
+//
+// For least squares problem where there are no outliers and standard
+// squared loss is expected, it is not necessary to create a loss
+// function; instead passing a NULL to the problem when adding
+// residuals implies a standard squared loss.
+//
+// For least squares problems where the minimization may encounter
+// input terms that contain outliers, that is, completely bogus
+// measurements, it is important to use a loss function that reduces
+// their associated penalty.
+//
+// Consider a structure from motion problem. The unknowns are 3D
+// points and camera parameters, and the measurements are image
+// coordinates describing the expected reprojected position for a
+// point in a camera. For example, we want to model the geometry of a
+// street scene with fire hydrants and cars, observed by a moving
+// camera with unknown parameters, and the only 3D points we care
+// about are the pointy tippy-tops of the fire hydrants. Our magic
+// image processing algorithm, which is responsible for producing the
+// measurements that are input to Ceres, has found and matched all
+// such tippy-tops in all image frames, except that in one of the
+// frame it mistook a car's headlight for a hydrant. If we didn't do
+// anything special (i.e. if we used a basic quadratic loss), the
+// residual for the erroneous measurement will result in extreme error
+// due to the quadratic nature of squared loss. This results in the
+// entire solution getting pulled away from the optimum to reduce
+// the large error that would otherwise be attributed to the wrong
+// measurement.
+//
+// Using a robust loss function, the cost for large residuals is
+// reduced. In the example above, this leads to outlier terms getting
+// downweighted so they do not overly influence the final solution.
+//
+// What cost function is best?
+//
+// In general, there isn't a principled way to select a robust loss
+// function. The authors suggest starting with a non-robust cost, then
+// only experimenting with robust loss functions if standard squared
+// loss doesn't work.
+
+#ifndef CERES_PUBLIC_LOSS_FUNCTION_H_
+#define CERES_PUBLIC_LOSS_FUNCTION_H_
+
+#include <memory>
+#include "glog/logging.h"
+#include "ceres/types.h"
+#include "ceres/internal/disable_warnings.h"
+
+namespace ceres {
+
+class CERES_EXPORT LossFunction {
+ public:
+ virtual ~LossFunction() {}
+
+ // For a residual vector with squared 2-norm 'sq_norm', this method
+ // is required to fill in the value and derivatives of the loss
+ // function (rho in this example):
+ //
+ // out[0] = rho(sq_norm),
+ // out[1] = rho'(sq_norm),
+ // out[2] = rho''(sq_norm),
+ //
+ // Here the convention is that the contribution of a term to the
+ // cost function is given by 1/2 rho(s), where
+ //
+ // s = ||residuals||^2.
+ //
+ // Calling the method with a negative value of 's' is an error and
+ // the implementations are not required to handle that case.
+ //
+ // Most sane choices of rho() satisfy:
+ //
+ // rho(0) = 0,
+ // rho'(0) = 1,
+ // rho'(s) < 1 in outlier region,
+ // rho''(s) < 0 in outlier region,
+ //
+ // so that they mimic the least squares cost for small residuals.
+ virtual void Evaluate(double sq_norm, double out[3]) const = 0;
+};
+
+// Some common implementations follow below.
+//
+// Note: in the region of interest (i.e. s < 3) we have:
+// TrivialLoss >= HuberLoss >= SoftLOneLoss >= CauchyLoss
+
+
+// This corresponds to no robustification.
+//
+// rho(s) = s
+//
+// At s = 0: rho = [0, 1, 0].
+//
+// It is not normally necessary to use this, as passing NULL for the
+// loss function when building the problem accomplishes the same
+// thing.
+class CERES_EXPORT TrivialLoss : public LossFunction {
+ public:
+ virtual void Evaluate(double, double*) const;
+};
+
+// Scaling
+// -------
+// Given one robustifier
+// s -> rho(s)
+// one can change the length scale at which robustification takes
+// place, by adding a scale factor 'a' as follows:
+//
+// s -> a^2 rho(s / a^2).
+//
+// The first and second derivatives are:
+//
+// s -> rho'(s / a^2),
+// s -> (1 / a^2) rho''(s / a^2),
+//
+// but the behaviour near s = 0 is the same as the original function,
+// i.e.
+//
+// rho(s) = s + higher order terms,
+// a^2 rho(s / a^2) = s + higher order terms.
+//
+// The scalar 'a' should be positive.
+//
+// The reason for the appearance of squaring is that 'a' is in the
+// units of the residual vector norm whereas 's' is a squared
+// norm. For applications it is more convenient to specify 'a' than
+// its square. The commonly used robustifiers below are described in
+// un-scaled format (a = 1) but their implementations work for any
+// non-zero value of 'a'.
+
+// Huber.
+//
+// rho(s) = s for s <= 1,
+// rho(s) = 2 sqrt(s) - 1 for s >= 1.
+//
+// At s = 0: rho = [0, 1, 0].
+//
+// The scaling parameter 'a' corresponds to 'delta' on this page:
+// http://en.wikipedia.org/wiki/Huber_Loss_Function
+class CERES_EXPORT HuberLoss : public LossFunction {
+ public:
+ explicit HuberLoss(double a) : a_(a), b_(a * a) { }
+ virtual void Evaluate(double, double*) const;
+
+ private:
+ const double a_;
+ // b = a^2.
+ const double b_;
+};
+
+// Soft L1, similar to Huber but smooth.
+//
+// rho(s) = 2 (sqrt(1 + s) - 1).
+//
+// At s = 0: rho = [0, 1, -1/2].
+class CERES_EXPORT SoftLOneLoss : public LossFunction {
+ public:
+ explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) { }
+ virtual void Evaluate(double, double*) const;
+
+ private:
+ // b = a^2.
+ const double b_;
+ // c = 1 / a^2.
+ const double c_;
+};
+
+// Inspired by the Cauchy distribution
+//
+// rho(s) = log(1 + s).
+//
+// At s = 0: rho = [0, 1, -1].
+class CERES_EXPORT CauchyLoss : public LossFunction {
+ public:
+ explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) { }
+ virtual void Evaluate(double, double*) const;
+
+ private:
+ // b = a^2.
+ const double b_;
+ // c = 1 / a^2.
+ const double c_;
+};
+
+// Loss that is capped beyond a certain level using the arc-tangent function.
+// The scaling parameter 'a' determines the level where falloff occurs.
+// For costs much smaller than 'a', the loss function is linear and behaves like
+// TrivialLoss, and for values much larger than 'a' the value asymptotically
+// approaches the constant value of a * PI / 2.
+//
+// rho(s) = a atan(s / a).
+//
+// At s = 0: rho = [0, 1, 0].
+class CERES_EXPORT ArctanLoss : public LossFunction {
+ public:
+ explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) { }
+ virtual void Evaluate(double, double*) const;
+
+ private:
+ const double a_;
+ // b = 1 / a^2.
+ const double b_;
+};
+
+// Loss function that maps to approximately zero cost in a range around the
+// origin, and reverts to linear in error (quadratic in cost) beyond this range.
+// The tolerance parameter 'a' sets the nominal point at which the
+// transition occurs, and the transition size parameter 'b' sets the nominal
+// distance over which most of the transition occurs. Both a and b must be
+// greater than zero, and typically b will be set to a fraction of a.
+// The slope rho'[s] varies smoothly from about 0 at s <= a - b to
+// about 1 at s >= a + b.
+//
+// The term is computed as:
+//
+// rho(s) = b log(1 + exp((s - a) / b)) - c0.
+//
+// where c0 is chosen so that rho(0) == 0
+//
+// c0 = b log(1 + exp(-a / b)
+//
+// This has the following useful properties:
+//
+// rho(s) == 0 for s = 0
+// rho'(s) ~= 0 for s << a - b
+// rho'(s) ~= 1 for s >> a + b
+// rho''(s) > 0 for all s
+//
+// In addition, all derivatives are continuous, and the curvature is
+// concentrated in the range a - b to a + b.
+//
+// At s = 0: rho = [0, ~0, ~0].
+class CERES_EXPORT TolerantLoss : public LossFunction {
+ public:
+ explicit TolerantLoss(double a, double b);
+ virtual void Evaluate(double, double*) const;
+
+ private:
+ const double a_, b_, c_;
+};
+
+// This is the Tukey biweight loss function which aggressively
+// attempts to suppress large errors.
+//
+// The term is computed as:
+//
+// rho(s) = a^2 / 6 * (1 - (1 - s / a^2)^3 ) for s <= a^2,
+// rho(s) = a^2 / 6 for s > a^2.
+//
+// At s = 0: rho = [0, 0.5, -1 / a^2]
+class CERES_EXPORT TukeyLoss : public ceres::LossFunction {
+ public:
+ explicit TukeyLoss(double a) : a_squared_(a * a) { }
+ virtual void Evaluate(double, double*) const;
+
+ private:
+ const double a_squared_;
+};
+
+// Composition of two loss functions. The error is the result of first
+// evaluating g followed by f to yield the composition f(g(s)).
+// The loss functions must not be NULL.
+class CERES_EXPORT ComposedLoss : public LossFunction {
+ public:
+ explicit ComposedLoss(const LossFunction* f, Ownership ownership_f,
+ const LossFunction* g, Ownership ownership_g);
+ virtual ~ComposedLoss();
+ virtual void Evaluate(double, double*) const;
+
+ private:
+ std::unique_ptr<const LossFunction> f_, g_;
+ const Ownership ownership_f_, ownership_g_;
+};
+
+// The discussion above has to do with length scaling: it affects the space
+// in which s is measured. Sometimes you want to simply scale the output
+// value of the robustifier. For example, you might want to weight
+// different error terms differently (e.g., weight pixel reprojection
+// errors differently from terrain errors).
+//
+// If rho is the wrapped robustifier, then this simply outputs
+// s -> a * rho(s)
+//
+// The first and second derivatives are, not surprisingly
+// s -> a * rho'(s)
+// s -> a * rho''(s)
+//
+// Since we treat the a NULL Loss function as the Identity loss
+// function, rho = NULL is a valid input and will result in the input
+// being scaled by a. This provides a simple way of implementing a
+// scaled ResidualBlock.
+class CERES_EXPORT ScaledLoss : public LossFunction {
+ public:
+ // Constructs a ScaledLoss wrapping another loss function. Takes
+ // ownership of the wrapped loss function or not depending on the
+ // ownership parameter.
+ ScaledLoss(const LossFunction* rho, double a, Ownership ownership) :
+ rho_(rho), a_(a), ownership_(ownership) { }
+ ScaledLoss(const ScaledLoss&) = delete;
+ void operator=(const ScaledLoss&) = delete;
+
+ virtual ~ScaledLoss() {
+ if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
+ rho_.release();
+ }
+ }
+ virtual void Evaluate(double, double*) const;
+
+ private:
+ std::unique_ptr<const LossFunction> rho_;
+ const double a_;
+ const Ownership ownership_;
+};
+
+// Sometimes after the optimization problem has been constructed, we
+// wish to mutate the scale of the loss function. For example, when
+// performing estimation from data which has substantial outliers,
+// convergence can be improved by starting out with a large scale,
+// optimizing the problem and then reducing the scale. This can have
+// better convergence behaviour than just using a loss function with a
+// small scale.
+//
+// This templated class allows the user to implement a loss function
+// whose scale can be mutated after an optimization problem has been
+// constructed.
+//
+// Since we treat the a NULL Loss function as the Identity loss
+// function, rho = NULL is a valid input.
+//
+// Example usage
+//
+// Problem problem;
+//
+// // Add parameter blocks
+//
+// CostFunction* cost_function =
+// new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>(
+// new UW_Camera_Mapper(feature_x, feature_y));
+//
+// LossFunctionWrapper* loss_function(new HuberLoss(1.0), TAKE_OWNERSHIP);
+//
+// problem.AddResidualBlock(cost_function, loss_function, parameters);
+//
+// Solver::Options options;
+// Solger::Summary summary;
+//
+// Solve(options, &problem, &summary)
+//
+// loss_function->Reset(new HuberLoss(1.0), TAKE_OWNERSHIP);
+//
+// Solve(options, &problem, &summary)
+//
+class CERES_EXPORT LossFunctionWrapper : public LossFunction {
+ public:
+ LossFunctionWrapper(LossFunction* rho, Ownership ownership)
+ : rho_(rho), ownership_(ownership) {
+ }
+
+ LossFunctionWrapper(const LossFunctionWrapper&) = delete;
+ void operator=(const LossFunctionWrapper&) = delete;
+
+ virtual ~LossFunctionWrapper() {
+ if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
+ rho_.release();
+ }
+ }
+
+ virtual void Evaluate(double sq_norm, double out[3]) const {
+ if (rho_.get() == NULL) {
+ out[0] = sq_norm;
+ out[1] = 1.0;
+ out[2] = 0.0;
+ }
+ else {
+ rho_->Evaluate(sq_norm, out);
+ }
+ }
+
+ void Reset(LossFunction* rho, Ownership ownership) {
+ if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
+ rho_.release();
+ }
+ rho_.reset(rho);
+ ownership_ = ownership;
+ }
+
+ private:
+ std::unique_ptr<const LossFunction> rho_;
+ Ownership ownership_;
+};
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_LOSS_FUNCTION_H_
diff --git a/include/ceres/normal_prior.h b/include/ceres/normal_prior.h
new file mode 100644
index 0000000..cd98b4c
--- /dev/null
+++ b/include/ceres/normal_prior.h
@@ -0,0 +1,78 @@
+// 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)
+//
+// Cost term that implements a prior on a parameter block using a
+// normal distribution.
+
+#ifndef CERES_PUBLIC_NORMAL_PRIOR_H_
+#define CERES_PUBLIC_NORMAL_PRIOR_H_
+
+#include "ceres/cost_function.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/disable_warnings.h"
+
+namespace ceres {
+
+// Implements a cost function of the form
+//
+// cost(x) = ||A(x - b)||^2
+//
+// where, the matrix A and the vector b are fixed and x is the
+// variable. In case the user is interested in implementing a cost
+// function of the form
+//
+// cost(x) = (x - mu)^T S^{-1} (x - mu)
+//
+// where, mu is a vector and S is a covariance matrix, then, A =
+// S^{-1/2}, i.e the matrix A is the square root of the inverse of the
+// covariance, also known as the stiffness matrix. There are however
+// no restrictions on the shape of A. It is free to be rectangular,
+// which would be the case if the covariance matrix S is rank
+// deficient.
+
+class CERES_EXPORT NormalPrior: public CostFunction {
+ public:
+ // Check that the number of rows in the vector b are the same as the
+ // number of columns in the matrix A, crash otherwise.
+ NormalPrior(const Matrix& A, const Vector& b);
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const;
+ private:
+ Matrix A_;
+ Vector b_;
+};
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_NORMAL_PRIOR_H_
diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h
new file mode 100644
index 0000000..f6cd32a
--- /dev/null
+++ b/include/ceres/numeric_diff_cost_function.h
@@ -0,0 +1,253 @@
+// 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)
+// sameeragarwal@google.com (Sameer Agarwal)
+//
+// Create CostFunctions as needed by the least squares framework with jacobians
+// computed via numeric (a.k.a. finite) differentiation. For more details see
+// http://en.wikipedia.org/wiki/Numerical_differentiation.
+//
+// To get an numerically differentiated cost function, you must define
+// a class with a operator() (a functor) that computes the residuals.
+//
+// The function must write the computed value in the last argument
+// (the only non-const one) and return true to indicate success.
+// Please see cost_function.h for details on how the return value
+// maybe used to impose simple constraints on the parameter block.
+//
+// For example, consider a scalar error e = k - x'y, where both x and y are
+// two-dimensional column vector parameters, the prime sign indicates
+// transposition, and k is a constant. The form of this error, which is the
+// difference between a constant and an expression, is a common pattern in least
+// squares problems. For example, the value x'y might be the model expectation
+// for a series of measurements, where there is an instance of the cost function
+// for each measurement k.
+//
+// The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
+// the squaring is implicitly done by the optimization framework.
+//
+// To write an numerically-differentiable cost function for the above model,
+// first define the object
+//
+// class MyScalarCostFunctor {
+// explicit MyScalarCostFunctor(double k): k_(k) {}
+//
+// bool operator()(const double* const x,
+// const double* const y,
+// double* residuals) const {
+// residuals[0] = k_ - x[0] * y[0] - x[1] * y[1];
+// return true;
+// }
+//
+// private:
+// double k_;
+// };
+//
+// Note that in the declaration of operator() the input parameters x
+// and y come first, and are passed as const pointers to arrays of
+// doubles. If there were three input parameters, then the third input
+// parameter would come after y. The output is always the last
+// parameter, and is also a pointer to an array. In the example above,
+// the residual is a scalar, so only residuals[0] is set.
+//
+// Then given this class definition, the numerically differentiated
+// cost function with central differences used for computing the
+// derivative can be constructed as follows.
+//
+// CostFunction* cost_function
+// = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, 1, 2, 2>(
+// new MyScalarCostFunctor(1.0)); ^ ^ ^ ^
+// | | | |
+// Finite Differencing Scheme -+ | | |
+// Dimension of residual ------------+ | |
+// Dimension of x ----------------------+ |
+// Dimension of y -------------------------+
+//
+// In this example, there is usually an instance for each measurement of k.
+//
+// In the instantiation above, the template parameters following
+// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing
+// a 1-dimensional output from two arguments, both 2-dimensional.
+//
+// NumericDiffCostFunction also supports cost functions with a
+// runtime-determined number of residuals. For example:
+//
+// CostFunction* cost_function
+// = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>(
+// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^
+// TAKE_OWNERSHIP, | | |
+// runtime_number_of_residuals); <----+ | | |
+// | | | |
+// | | | |
+// Actual number of residuals ------+ | | |
+// Indicate dynamic number of residuals --------------------+ | |
+// Dimension of x ------------------------------------------------+ |
+// Dimension of y ---------------------------------------------------+
+//
+// The central difference method is considerably more accurate at the cost of
+// twice as many function evaluations than forward difference. Consider using
+// central differences begin with, and only after that works, trying forward
+// difference to improve performance.
+//
+// WARNING #1: A common beginner's error when first using
+// NumericDiffCostFunction is to get the sizing wrong. In particular,
+// there is a tendency to set the template parameters to (dimension of
+// residual, number of parameters) instead of passing a dimension
+// parameter for *every parameter*. In the example above, that would
+// be <MyScalarCostFunctor, 1, 2>, which is missing the last '2'
+// argument. Please be careful when setting the size parameters.
+//
+////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////
+//
+// ALTERNATE INTERFACE
+//
+// For a variety of reasons, including compatibility with legacy code,
+// NumericDiffCostFunction can also take CostFunction objects as
+// input. The following describes how.
+//
+// To get a numerically differentiated cost function, define a
+// subclass of CostFunction such that the Evaluate() function ignores
+// the jacobian parameter. The numeric differentiation wrapper will
+// fill in the jacobian parameter if necessary by repeatedly calling
+// the Evaluate() function with small changes to the appropriate
+// parameters, and computing the slope. For performance, the numeric
+// differentiation wrapper class is templated on the concrete cost
+// function, even though it could be implemented only in terms of the
+// virtual CostFunction interface.
+//
+// The numerically differentiated version of a cost function for a cost function
+// can be constructed as follows:
+//
+// CostFunction* cost_function
+// = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>(
+// new MyCostFunction(...), TAKE_OWNERSHIP);
+//
+// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8
+// respectively. Look at the tests for a more detailed example.
+//
+// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives.
+
+#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
+#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
+
+#include <array>
+#include <memory>
+
+#include "Eigen/Dense"
+#include "ceres/cost_function.h"
+#include "ceres/internal/numeric_diff.h"
+#include "ceres/internal/parameter_dims.h"
+#include "ceres/numeric_diff_options.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+template <typename CostFunctor,
+ NumericDiffMethodType method = CENTRAL,
+ int kNumResiduals = 0, // Number of residuals, or ceres::DYNAMIC
+ int... Ns> // Parameters dimensions for each block.
+class NumericDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> {
+ public:
+ NumericDiffCostFunction(
+ CostFunctor* functor,
+ Ownership ownership = TAKE_OWNERSHIP,
+ int num_residuals = kNumResiduals,
+ const NumericDiffOptions& options = NumericDiffOptions())
+ : functor_(functor),
+ ownership_(ownership),
+ options_(options) {
+ if (kNumResiduals == DYNAMIC) {
+ SizedCostFunction<kNumResiduals, Ns...>::set_num_residuals(num_residuals);
+ }
+ }
+
+ ~NumericDiffCostFunction() {
+ if (ownership_ != TAKE_OWNERSHIP) {
+ functor_.release();
+ }
+ }
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ using internal::FixedArray;
+ using internal::NumericDiff;
+
+ using ParameterDims =
+ typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims;
+ using Parameters = typename ParameterDims::Parameters;
+
+ constexpr int kNumParameters = ParameterDims::kNumParameters;
+ constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks;
+
+ // Get the function value (residuals) at the the point to evaluate.
+ if (!internal::VariadicEvaluate<ParameterDims>(*functor_,
+ parameters,
+ residuals)) {
+ return false;
+ }
+
+ if (jacobians == NULL) {
+ return true;
+ }
+
+ // Create a copy of the parameters which will get mutated.
+ FixedArray<double> parameters_copy(kNumParameters);
+ std::array<double*, kNumParameterBlocks> parameters_reference_copy =
+ ParameterDims::GetUnpackedParameters(parameters_copy.get());
+
+ for (int block = 0; block < kNumParameterBlocks; ++block) {
+ memcpy(parameters_reference_copy[block], parameters[block],
+ sizeof(double) * ParameterDims::GetDim(block));
+ }
+
+ internal::EvaluateJacobianForParameterBlocks<ParameterDims>::template Apply<
+ method, kNumResiduals>(
+ functor_.get(),
+ residuals,
+ options_,
+ SizedCostFunction<kNumResiduals, Ns...>::num_residuals(),
+ parameters_reference_copy.data(),
+ jacobians);
+
+ return true;
+ }
+
+ private:
+ std::unique_ptr<CostFunctor> functor_;
+ Ownership ownership_;
+ NumericDiffOptions options_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_
diff --git a/include/ceres/numeric_diff_options.h b/include/ceres/numeric_diff_options.h
new file mode 100644
index 0000000..a77be44
--- /dev/null
+++ b/include/ceres/numeric_diff_options.h
@@ -0,0 +1,73 @@
+// 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: tbennun@gmail.com (Tal Ben-Nun)
+//
+
+#ifndef CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_
+#define CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_
+
+#include "ceres/internal/port.h"
+
+namespace ceres {
+
+// Options pertaining to numeric differentiation (e.g., convergence criteria,
+// step sizes).
+struct CERES_EXPORT NumericDiffOptions {
+ // Numeric differentiation step size (multiplied by parameter block's
+ // order of magnitude). If parameters are close to zero, the step size
+ // is set to sqrt(machine_epsilon).
+ double relative_step_size = 1e-6;
+
+ // Initial step size for Ridders adaptive numeric differentiation (multiplied
+ // by parameter block's order of magnitude).
+ // If parameters are close to zero, Ridders' method sets the step size
+ // directly to this value. This parameter is separate from
+ // "relative_step_size" in order to set a different default value.
+ //
+ // Note: For Ridders' method to converge, the step size should be initialized
+ // to a value that is large enough to produce a significant change in the
+ // function. As the derivative is estimated, the step size decreases.
+ double ridders_relative_initial_step_size = 1e-2;
+
+ // Maximal number of adaptive extrapolations (sampling) in Ridders' method.
+ int max_num_ridders_extrapolations = 10;
+
+ // Convergence criterion on extrapolation error for Ridders adaptive
+ // differentiation. The available error estimation methods are defined in
+ // NumericDiffErrorType and set in the "ridders_error_method" field.
+ double ridders_epsilon = 1e-12;
+
+ // The factor in which to shrink the step size with each extrapolation in
+ // Ridders' method.
+ double ridders_step_shrink_factor = 2.0;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_
diff --git a/include/ceres/ordered_groups.h b/include/ceres/ordered_groups.h
new file mode 100644
index 0000000..0bff41f
--- /dev/null
+++ b/include/ceres/ordered_groups.h
@@ -0,0 +1,202 @@
+// 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_PUBLIC_ORDERED_GROUPS_H_
+#define CERES_PUBLIC_ORDERED_GROUPS_H_
+
+#include <map>
+#include <set>
+#include <unordered_map>
+#include <vector>
+#include "ceres/internal/port.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// A class for storing and manipulating an ordered collection of
+// groups/sets with the following semantics:
+//
+// Group ids are non-negative integer values. Elements are any type
+// that can serve as a key in a map or an element of a set.
+//
+// An element can only belong to one group at a time. A group may
+// contain an arbitrary number of elements.
+//
+// Groups are ordered by their group id.
+template <typename T>
+class OrderedGroups {
+ public:
+ // Add an element to a group. If a group with this id does not
+ // exist, one is created. This method can be called any number of
+ // times for the same element. Group ids should be non-negative
+ // numbers.
+ //
+ // Return value indicates if adding the element was a success.
+ bool AddElementToGroup(const T element, const int group) {
+ if (group < 0) {
+ return false;
+ }
+
+ auto it = element_to_group_.find(element);
+ if (it != element_to_group_.end()) {
+ if (it->second == group) {
+ // Element is already in the right group, nothing to do.
+ return true;
+ }
+
+ group_to_elements_[it->second].erase(element);
+ if (group_to_elements_[it->second].size() == 0) {
+ group_to_elements_.erase(it->second);
+ }
+ }
+
+ element_to_group_[element] = group;
+ group_to_elements_[group].insert(element);
+ return true;
+ }
+
+ void Clear() {
+ group_to_elements_.clear();
+ element_to_group_.clear();
+ }
+
+ // Remove the element, no matter what group it is in. Return value
+ // indicates if the element was actually removed.
+ bool Remove(const T element) {
+ const int current_group = GroupId(element);
+ if (current_group < 0) {
+ return false;
+ }
+
+ group_to_elements_[current_group].erase(element);
+
+ if (group_to_elements_[current_group].size() == 0) {
+ // If the group is empty, then get rid of it.
+ group_to_elements_.erase(current_group);
+ }
+
+ element_to_group_.erase(element);
+ return true;
+ }
+
+ // Bulk remove elements. The return value indicates the number of
+ // elements successfully removed.
+ int Remove(const std::vector<T>& elements) {
+ if (NumElements() == 0 || elements.size() == 0) {
+ return 0;
+ }
+
+ int num_removed = 0;
+ for (int i = 0; i < elements.size(); ++i) {
+ num_removed += Remove(elements[i]);
+ }
+ return num_removed;
+ }
+
+ // Reverse the order of the groups in place.
+ void Reverse() {
+ if (NumGroups() == 0) {
+ return;
+ }
+
+ auto it = group_to_elements_.rbegin();
+ std::map<int, std::set<T>> new_group_to_elements;
+ new_group_to_elements[it->first] = it->second;
+
+ int new_group_id = it->first + 1;
+ for (++it; it != group_to_elements_.rend(); ++it) {
+ for (const auto& element : it->second) {
+ element_to_group_[element] = new_group_id;
+ }
+ new_group_to_elements[new_group_id] = it->second;
+ new_group_id++;
+ }
+
+ group_to_elements_.swap(new_group_to_elements);
+ }
+
+ // Return the group id for the element. If the element is not a
+ // member of any group, return -1.
+ int GroupId(const T element) const {
+ auto it = element_to_group_.find(element);
+ if (it == element_to_group_.end()) {
+ return -1;
+ }
+ return it->second;
+ }
+
+ bool IsMember(const T element) const {
+ auto it = element_to_group_.find(element);
+ return (it != element_to_group_.end());
+ }
+
+ // This function always succeeds, i.e., implicitly there exists a
+ // group for every integer.
+ int GroupSize(const int group) const {
+ auto it = group_to_elements_.find(group);
+ return (it == group_to_elements_.end()) ? 0 : it->second.size();
+ }
+
+ int NumElements() const {
+ return element_to_group_.size();
+ }
+
+ // Number of groups with one or more elements.
+ int NumGroups() const {
+ return group_to_elements_.size();
+ }
+
+ // The first group with one or more elements. Calling this when
+ // there are no groups with non-zero elements will result in a
+ // crash.
+ int MinNonZeroGroup() const {
+ CHECK_NE(NumGroups(), 0);
+ return group_to_elements_.begin()->first;
+ }
+
+ const std::map<int, std::set<T>>& group_to_elements() const {
+ return group_to_elements_;
+ }
+
+ const std::map<T, int>& element_to_group() const {
+ return element_to_group_;
+ }
+
+ private:
+ std::map<int, std::set<T>> group_to_elements_;
+ std::unordered_map<T, int> element_to_group_;
+};
+
+// Typedef for the most commonly used version of OrderedGroups.
+typedef OrderedGroups<double*> ParameterBlockOrdering;
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_ORDERED_GROUP_H_
diff --git a/include/ceres/problem.h b/include/ceres/problem.h
new file mode 100644
index 0000000..503e0fe
--- /dev/null
+++ b/include/ceres/problem.h
@@ -0,0 +1,467 @@
+// 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)
+// keir@google.com (Keir Mierle)
+//
+// The Problem object is used to build and hold least squares problems.
+
+#ifndef CERES_PUBLIC_PROBLEM_H_
+#define CERES_PUBLIC_PROBLEM_H_
+
+#include <array>
+#include <cstddef>
+#include <map>
+#include <memory>
+#include <set>
+#include <vector>
+
+#include "ceres/context.h"
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/port.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+class CostFunction;
+class LossFunction;
+class LocalParameterization;
+class Solver;
+struct CRSMatrix;
+
+namespace internal {
+class Preprocessor;
+class ProblemImpl;
+class ParameterBlock;
+class ResidualBlock;
+} // namespace internal
+
+// A ResidualBlockId is an opaque handle clients can use to remove residual
+// blocks from a Problem after adding them.
+typedef internal::ResidualBlock* ResidualBlockId;
+
+// A class to represent non-linear least squares problems. Such
+// problems have a cost function that is a sum of error terms (known
+// as "residuals"), where each residual is a function of some subset
+// of the parameters. The cost function takes the form
+//
+// N 1
+// SUM --- loss( || r_i1, r_i2,..., r_ik ||^2 ),
+// i=1 2
+//
+// where
+//
+// r_ij is residual number i, component j; the residual is a
+// function of some subset of the parameters x1...xk. For
+// example, in a structure from motion problem a residual
+// might be the difference between a measured point in an
+// image and the reprojected position for the matching
+// camera, point pair. The residual would have two
+// components, error in x and error in y.
+//
+// loss(y) is the loss function; for example, squared error or
+// Huber L1 loss. If loss(y) = y, then the cost function is
+// non-robustified least squares.
+//
+// This class is specifically designed to address the important subset
+// of "sparse" least squares problems, where each component of the
+// residual depends only on a small number number of parameters, even
+// though the total number of residuals and parameters may be very
+// large. This property affords tremendous gains in scale, allowing
+// efficient solving of large problems that are otherwise
+// inaccessible.
+//
+// The canonical example of a sparse least squares problem is
+// "structure-from-motion" (SFM), where the parameters are points and
+// cameras, and residuals are reprojection errors. Typically a single
+// residual will depend only on 9 parameters (3 for the point, 6 for
+// the camera).
+//
+// To create a least squares problem, use the AddResidualBlock() and
+// AddParameterBlock() methods, documented below. Here is an example least
+// squares problem containing 3 parameter blocks of sizes 3, 4 and 5
+// respectively and two residual terms of size 2 and 6:
+//
+// double x1[] = { 1.0, 2.0, 3.0 };
+// double x2[] = { 1.0, 2.0, 3.0, 5.0 };
+// double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 };
+//
+// Problem problem;
+//
+// problem.AddResidualBlock(new MyUnaryCostFunction(...), NULL, x1);
+// problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x3);
+//
+// Please see cost_function.h for details of the CostFunction object.
+class CERES_EXPORT Problem {
+ public:
+ struct CERES_EXPORT Options {
+ // These flags control whether the Problem object owns the cost
+ // functions, loss functions, and parameterizations passed into
+ // the Problem. If set to TAKE_OWNERSHIP, then the problem object
+ // will delete the corresponding cost or loss functions on
+ // destruction. The destructor is careful to delete the pointers
+ // only once, since sharing cost/loss/parameterizations is
+ // allowed.
+ Ownership cost_function_ownership = TAKE_OWNERSHIP;
+ Ownership loss_function_ownership = TAKE_OWNERSHIP;
+ Ownership local_parameterization_ownership = TAKE_OWNERSHIP;
+
+ // If true, trades memory for faster RemoveResidualBlock() and
+ // RemoveParameterBlock() operations.
+ //
+ // By default, RemoveParameterBlock() and RemoveResidualBlock() take time
+ // proportional to the size of the entire problem. If you only ever remove
+ // parameters or residuals from the problem occasionally, this might be
+ // acceptable. However, if you have memory to spare, enable this option to
+ // make RemoveParameterBlock() take time proportional to the number of
+ // residual blocks that depend on it, and RemoveResidualBlock() take (on
+ // average) constant time.
+ //
+ // The increase in memory usage is twofold: an additional hash set per
+ // parameter block containing all the residuals that depend on the parameter
+ // block; and a hash set in the problem containing all residuals.
+ bool enable_fast_removal = false;
+
+ // By default, Ceres performs a variety of safety checks when constructing
+ // the problem. There is a small but measurable performance penalty to
+ // these checks, typically around 5% of construction time. If you are sure
+ // your problem construction is correct, and 5% of the problem construction
+ // time is truly an overhead you want to avoid, then you can set
+ // disable_all_safety_checks to true.
+ //
+ // WARNING: Do not set this to true, unless you are absolutely sure of what
+ // you are doing.
+ bool disable_all_safety_checks = false;
+
+ // A Ceres global context to use for solving this problem. This may help to
+ // reduce computation time as Ceres can reuse expensive objects to create.
+ // The context object can be NULL, in which case Ceres may create one.
+ //
+ // Ceres does NOT take ownership of the pointer.
+ Context* context = nullptr;
+ };
+
+ // The default constructor is equivalent to the
+ // invocation Problem(Problem::Options()).
+ Problem();
+ explicit Problem(const Options& options);
+ Problem(const Problem&) = delete;
+ void operator=(const Problem&) = delete;
+
+ ~Problem();
+
+ // Add a residual block to the overall cost function. The cost
+ // function carries with its information about the sizes of the
+ // parameter blocks it expects. The function checks that these match
+ // the sizes of the parameter blocks listed in parameter_blocks. The
+ // program aborts if a mismatch is detected. loss_function can be
+ // NULL, in which case the cost of the term is just the squared norm
+ // of the residuals.
+ //
+ // The user has the option of explicitly adding the parameter blocks
+ // using AddParameterBlock. This causes additional correctness
+ // checking; however, AddResidualBlock implicitly adds the parameter
+ // blocks if they are not present, so calling AddParameterBlock
+ // explicitly is not required.
+ //
+ // The Problem object by default takes ownership of the
+ // cost_function and loss_function pointers. These objects remain
+ // live for the life of the Problem object. If the user wishes to
+ // keep control over the destruction of these objects, then they can
+ // do this by setting the corresponding enums in the Options struct.
+ //
+ // Note: Even though the Problem takes ownership of cost_function
+ // and loss_function, it does not preclude the user from re-using
+ // them in another residual block. The destructor takes care to call
+ // delete on each cost_function or loss_function pointer only once,
+ // regardless of how many residual blocks refer to them.
+ //
+ // Example usage:
+ //
+ // double x1[] = {1.0, 2.0, 3.0};
+ // double x2[] = {1.0, 2.0, 5.0, 6.0};
+ // double x3[] = {3.0, 6.0, 2.0, 5.0, 1.0};
+ //
+ // Problem problem;
+ //
+ // problem.AddResidualBlock(new MyUnaryCostFunction(...), NULL, x1);
+ // problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x1);
+ //
+ // Add a residual block by listing the parameter block pointers
+ // directly instead of wapping them in a container.
+ template <typename... Ts>
+ ResidualBlockId AddResidualBlock(CostFunction* cost_function,
+ LossFunction* loss_function,
+ double* x0,
+ Ts*... xs) {
+ const std::array<double*, sizeof...(Ts) + 1> parameter_blocks{{x0, xs...}};
+ return AddResidualBlock(cost_function, loss_function,
+ parameter_blocks.data(),
+ static_cast<int>(parameter_blocks.size()));
+ }
+
+ // Add a residual block by providing a vector of parameter blocks.
+ ResidualBlockId AddResidualBlock(
+ CostFunction* cost_function,
+ LossFunction* loss_function,
+ const std::vector<double*>& parameter_blocks);
+
+ // Add a residual block by providing a pointer to the parameter block array
+ // and the number of parameter blocks.
+ ResidualBlockId AddResidualBlock(
+ CostFunction* cost_function,
+ LossFunction* loss_function,
+ double* const* const parameter_blocks,
+ int num_parameter_blocks);
+
+ // Add a parameter block with appropriate size to the problem.
+ // Repeated calls with the same arguments are ignored. Repeated
+ // calls with the same double pointer but a different size results
+ // in undefined behaviour.
+ void AddParameterBlock(double* values, int size);
+
+ // Add a parameter block with appropriate size and parameterization
+ // to the problem. Repeated calls with the same arguments are
+ // ignored. Repeated calls with the same double pointer but a
+ // different size results in undefined behaviour.
+ void AddParameterBlock(double* values,
+ int size,
+ LocalParameterization* local_parameterization);
+
+ // Remove a parameter block from the problem. The parameterization of the
+ // parameter block, if it exists, will persist until the deletion of the
+ // problem (similar to cost/loss functions in residual block removal). Any
+ // residual blocks that depend on the parameter are also removed, as
+ // described above in RemoveResidualBlock().
+ //
+ // If Problem::Options::enable_fast_removal is true, then the
+ // removal is fast (almost constant time). Otherwise, removing a parameter
+ // block will incur a scan of the entire Problem object.
+ //
+ // WARNING: Removing a residual or parameter block will destroy the implicit
+ // ordering, rendering the jacobian or residuals returned from the solver
+ // uninterpretable. If you depend on the evaluated jacobian, do not use
+ // remove! This may change in a future release.
+ void RemoveParameterBlock(double* values);
+
+ // Remove a residual block from the problem. Any parameters that the residual
+ // block depends on are not removed. The cost and loss functions for the
+ // residual block will not get deleted immediately; won't happen until the
+ // problem itself is deleted.
+ //
+ // WARNING: Removing a residual or parameter block will destroy the implicit
+ // ordering, rendering the jacobian or residuals returned from the solver
+ // uninterpretable. If you depend on the evaluated jacobian, do not use
+ // remove! This may change in a future release.
+ void RemoveResidualBlock(ResidualBlockId residual_block);
+
+ // Hold the indicated parameter block constant during optimization.
+ void SetParameterBlockConstant(double* values);
+
+ // Allow the indicated parameter block to vary during optimization.
+ void SetParameterBlockVariable(double* values);
+
+ // Returns true if a parameter block is set constant, and false otherwise.
+ bool IsParameterBlockConstant(double* values) const;
+
+ // Set the local parameterization for one of the parameter blocks.
+ // The local_parameterization is owned by the Problem by default. It
+ // is acceptable to set the same parameterization for multiple
+ // parameters; the destructor is careful to delete local
+ // parameterizations only once. The local parameterization can only
+ // be set once per parameter, and cannot be changed once set.
+ void SetParameterization(double* values,
+ LocalParameterization* local_parameterization);
+
+ // Get the local parameterization object associated with this
+ // parameter block. If there is no parameterization object
+ // associated then NULL is returned.
+ const LocalParameterization* GetParameterization(double* values) const;
+
+ // Set the lower/upper bound for the parameter at position "index".
+ void SetParameterLowerBound(double* values, int index, double lower_bound);
+ void SetParameterUpperBound(double* values, int index, double upper_bound);
+
+ // Get the lower/upper bound for the parameter at position
+ // "index". If the parameter is not bounded by the user, then its
+ // lower bound is -std::numeric_limits<double>::max() and upper
+ // bound is std::numeric_limits<double>::max().
+ double GetParameterLowerBound(double* values, int index) const;
+ double GetParameterUpperBound(double* values, int index) const;
+
+ // Number of parameter blocks in the problem. Always equals
+ // parameter_blocks().size() and parameter_block_sizes().size().
+ int NumParameterBlocks() const;
+
+ // The size of the parameter vector obtained by summing over the
+ // sizes of all the parameter blocks.
+ int NumParameters() const;
+
+ // Number of residual blocks in the problem. Always equals
+ // residual_blocks().size().
+ int NumResidualBlocks() const;
+
+ // The size of the residual vector obtained by summing over the
+ // sizes of all of the residual blocks.
+ int NumResiduals() const;
+
+ // The size of the parameter block.
+ int ParameterBlockSize(const double* values) const;
+
+ // The size of local parameterization for the parameter block. If
+ // there is no local parameterization associated with this parameter
+ // block, then ParameterBlockLocalSize = ParameterBlockSize.
+ int ParameterBlockLocalSize(const double* values) const;
+
+ // Is the given parameter block present in this problem or not?
+ bool HasParameterBlock(const double* values) const;
+
+ // Fills the passed parameter_blocks vector with pointers to the
+ // parameter blocks currently in the problem. After this call,
+ // parameter_block.size() == NumParameterBlocks.
+ void GetParameterBlocks(std::vector<double*>* parameter_blocks) const;
+
+ // Fills the passed residual_blocks vector with pointers to the
+ // residual blocks currently in the problem. After this call,
+ // residual_blocks.size() == NumResidualBlocks.
+ void GetResidualBlocks(std::vector<ResidualBlockId>* residual_blocks) const;
+
+ // Get all the parameter blocks that depend on the given residual block.
+ void GetParameterBlocksForResidualBlock(
+ const ResidualBlockId residual_block,
+ std::vector<double*>* parameter_blocks) const;
+
+ // Get the CostFunction for the given residual block.
+ const CostFunction* GetCostFunctionForResidualBlock(
+ const ResidualBlockId residual_block) const;
+
+ // Get the LossFunction for the given residual block. Returns NULL
+ // if no loss function is associated with this residual block.
+ const LossFunction* GetLossFunctionForResidualBlock(
+ const ResidualBlockId residual_block) const;
+
+ // Get all the residual blocks that depend on the given parameter block.
+ //
+ // If Problem::Options::enable_fast_removal is true, then
+ // getting the residual blocks is fast and depends only on the number of
+ // residual blocks. Otherwise, getting the residual blocks for a parameter
+ // block will incur a scan of the entire Problem object.
+ void GetResidualBlocksForParameterBlock(
+ const double* values,
+ std::vector<ResidualBlockId>* residual_blocks) const;
+
+ // Options struct to control Problem::Evaluate.
+ struct EvaluateOptions {
+ // The set of parameter blocks for which evaluation should be
+ // performed. This vector determines the order that parameter
+ // blocks occur in the gradient vector and in the columns of the
+ // jacobian matrix. If parameter_blocks is empty, then it is
+ // assumed to be equal to vector containing ALL the parameter
+ // blocks. Generally speaking the parameter blocks will occur in
+ // the order in which they were added to the problem. But, this
+ // may change if the user removes any parameter blocks from the
+ // problem.
+ //
+ // NOTE: This vector should contain the same pointers as the ones
+ // used to add parameter blocks to the Problem. These parameter
+ // block should NOT point to new memory locations. Bad things will
+ // happen otherwise.
+ std::vector<double*> parameter_blocks;
+
+ // The set of residual blocks to evaluate. This vector determines
+ // the order in which the residuals occur, and how the rows of the
+ // jacobian are ordered. If residual_blocks is empty, then it is
+ // assumed to be equal to the vector containing ALL the residual
+ // blocks. Generally speaking the residual blocks will occur in
+ // the order in which they were added to the problem. But, this
+ // may change if the user removes any residual blocks from the
+ // problem.
+ std::vector<ResidualBlockId> residual_blocks;
+
+ // Even though the residual blocks in the problem may contain loss
+ // functions, setting apply_loss_function to false will turn off
+ // the application of the loss function to the output of the cost
+ // function. This is of use for example if the user wishes to
+ // analyse the solution quality by studying the distribution of
+ // residuals before and after the solve.
+ bool apply_loss_function = true;
+
+ int num_threads = 1;
+ };
+
+ // Evaluate Problem. Any of the output pointers can be NULL. Which
+ // residual blocks and parameter blocks are used is controlled by
+ // the EvaluateOptions struct above.
+ //
+ // Note 1: The evaluation will use the values stored in the memory
+ // locations pointed to by the parameter block pointers used at the
+ // time of the construction of the problem. i.e.,
+ //
+ // Problem problem;
+ // double x = 1;
+ // problem.AddResidualBlock(new MyCostFunction, NULL, &x);
+ //
+ // double cost = 0.0;
+ // problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
+ //
+ // The cost is evaluated at x = 1. If you wish to evaluate the
+ // problem at x = 2, then
+ //
+ // x = 2;
+ // problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
+ //
+ // is the way to do so.
+ //
+ // Note 2: If no local parameterizations are used, then the size of
+ // the gradient vector (and the number of columns in the jacobian)
+ // is the sum of the sizes of all the parameter blocks. If a
+ // parameter block has a local parameterization, then it contributes
+ // "LocalSize" entries to the gradient vector (and the number of
+ // columns in the jacobian).
+ //
+ // Note 3: This function cannot be called while the problem is being
+ // solved, for example it cannot be called from an IterationCallback
+ // at the end of an iteration during a solve.
+ bool Evaluate(const EvaluateOptions& options,
+ double* cost,
+ std::vector<double>* residuals,
+ std::vector<double>* gradient,
+ CRSMatrix* jacobian);
+
+ private:
+ friend class Solver;
+ friend class Covariance;
+ std::unique_ptr<internal::ProblemImpl> problem_impl_;
+};
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_PROBLEM_H_
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
new file mode 100644
index 0000000..a0530dd
--- /dev/null
+++ b/include/ceres/rotation.h
@@ -0,0 +1,626 @@
+// 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)
+// sameeragarwal@google.com (Sameer Agarwal)
+//
+// Templated functions for manipulating rotations. The templated
+// functions are useful when implementing functors for automatic
+// differentiation.
+//
+// In the following, the Quaternions are laid out as 4-vectors, thus:
+//
+// q[0] scalar part.
+// q[1] coefficient of i.
+// q[2] coefficient of j.
+// q[3] coefficient of k.
+//
+// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
+
+#ifndef CERES_PUBLIC_ROTATION_H_
+#define CERES_PUBLIC_ROTATION_H_
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+namespace ceres {
+
+// Trivial wrapper to index linear arrays as matrices, given a fixed
+// column and row stride. When an array "T* array" is wrapped by a
+//
+// (const) MatrixAdapter<T, row_stride, col_stride> M"
+//
+// the expression M(i, j) is equivalent to
+//
+// arrary[i * row_stride + j * col_stride]
+//
+// Conversion functions to and from rotation matrices accept
+// MatrixAdapters to permit using row-major and column-major layouts,
+// and rotation matrices embedded in larger matrices (such as a 3x4
+// projection matrix).
+template <typename T, int row_stride, int col_stride>
+struct MatrixAdapter;
+
+// Convenience functions to create a MatrixAdapter that treats the
+// array pointed to by "pointer" as a 3x3 (contiguous) column-major or
+// row-major matrix.
+template <typename T>
+MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer);
+
+template <typename T>
+MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer);
+
+// Convert a value in combined axis-angle representation to a quaternion.
+// The value angle_axis is a triple whose norm is an angle in radians,
+// and whose direction is aligned with the axis of rotation,
+// and quaternion is a 4-tuple that will contain the resulting quaternion.
+// The implementation may be used with auto-differentiation up to the first
+// derivative, higher derivatives may have unexpected results near the origin.
+template<typename T>
+void AngleAxisToQuaternion(const T* angle_axis, T* quaternion);
+
+// Convert a quaternion to the equivalent combined axis-angle representation.
+// The value quaternion must be a unit quaternion - it is not normalized first,
+// and angle_axis will be filled with a value whose norm is the angle of
+// rotation in radians, and whose direction is the axis of rotation.
+// The implementation may be used with auto-differentiation up to the first
+// derivative, higher derivatives may have unexpected results near the origin.
+template<typename T>
+void QuaternionToAngleAxis(const T* quaternion, T* angle_axis);
+
+// Conversions between 3x3 rotation matrix (in column major order) and
+// quaternion rotation representations. Templated for use with
+// autodifferentiation.
+template <typename T>
+void RotationMatrixToQuaternion(const T* R, T* quaternion);
+
+template <typename T, int row_stride, int col_stride>
+void RotationMatrixToQuaternion(
+ const MatrixAdapter<const T, row_stride, col_stride>& R,
+ T* quaternion);
+
+// Conversions between 3x3 rotation matrix (in column major order) and
+// axis-angle rotation representations. Templated for use with
+// autodifferentiation.
+template <typename T>
+void RotationMatrixToAngleAxis(const T* R, T* angle_axis);
+
+template <typename T, int row_stride, int col_stride>
+void RotationMatrixToAngleAxis(
+ const MatrixAdapter<const T, row_stride, col_stride>& R,
+ T* angle_axis);
+
+template <typename T>
+void AngleAxisToRotationMatrix(const T* angle_axis, T* R);
+
+template <typename T, int row_stride, int col_stride>
+void AngleAxisToRotationMatrix(
+ const T* angle_axis,
+ const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Conversions between 3x3 rotation matrix (in row major order) and
+// Euler angle (in degrees) rotation representations.
+//
+// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z}
+// axes, respectively. They are applied in that same order, so the
+// total rotation R is Rz * Ry * Rx.
+template <typename T>
+void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R);
+
+template <typename T, int row_stride, int col_stride>
+void EulerAnglesToRotationMatrix(
+ const T* euler,
+ const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Convert a 4-vector to a 3x3 scaled rotation matrix.
+//
+// The choice of rotation is such that the quaternion [1 0 0 0] goes to an
+// identity matrix and for small a, b, c the quaternion [1 a b c] goes to
+// the matrix
+//
+// [ 0 -c b ]
+// I + 2 [ c 0 -a ] + higher order terms
+// [ -b a 0 ]
+//
+// which corresponds to a Rodrigues approximation, the last matrix being
+// the cross-product matrix of [a b c]. Together with the property that
+// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R.
+//
+// No normalization of the quaternion is performed, i.e.
+// R = ||q||^2 * Q, where Q is an orthonormal matrix
+// such that det(Q) = 1 and Q*Q' = I
+//
+// WARNING: The rotation matrix is ROW MAJOR
+template <typename T> inline
+void QuaternionToScaledRotation(const T q[4], T R[3 * 3]);
+
+template <typename T, int row_stride, int col_stride> inline
+void QuaternionToScaledRotation(
+ const T q[4],
+ const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Same as above except that the rotation matrix is normalized by the
+// Frobenius norm, so that R * R' = I (and det(R) = 1).
+//
+// WARNING: The rotation matrix is ROW MAJOR
+template <typename T> inline
+void QuaternionToRotation(const T q[4], T R[3 * 3]);
+
+template <typename T, int row_stride, int col_stride> inline
+void QuaternionToRotation(
+ const T q[4],
+ const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Rotates a point pt by a quaternion q:
+//
+// result = R(q) * pt
+//
+// Assumes the quaternion is unit norm. This assumption allows us to
+// write the transform as (something)*pt + pt, as is clear from the
+// formula below. If you pass in a quaternion with |q|^2 = 2 then you
+// WILL NOT get back 2 times the result you get for a unit quaternion.
+template <typename T> inline
+void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
+
+// With this function you do not need to assume that q has unit norm.
+// It does assume that the norm is non-zero.
+template <typename T> inline
+void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
+
+// zw = z * w, where * is the Quaternion product between 4 vectors.
+template<typename T> inline
+void QuaternionProduct(const T z[4], const T w[4], T zw[4]);
+
+// xy = x cross y;
+template<typename T> inline
+void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]);
+
+template<typename T> inline
+T DotProduct(const T x[3], const T y[3]);
+
+// y = R(angle_axis) * x;
+template<typename T> inline
+void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]);
+
+// --- IMPLEMENTATION
+
+template<typename T, int row_stride, int col_stride>
+struct MatrixAdapter {
+ T* pointer_;
+ explicit MatrixAdapter(T* pointer)
+ : pointer_(pointer)
+ {}
+
+ T& operator()(int r, int c) const {
+ return pointer_[r * row_stride + c * col_stride];
+ }
+};
+
+template <typename T>
+MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) {
+ return MatrixAdapter<T, 1, 3>(pointer);
+}
+
+template <typename T>
+MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) {
+ return MatrixAdapter<T, 3, 1>(pointer);
+}
+
+template<typename T>
+inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
+ const T& a0 = angle_axis[0];
+ const T& a1 = angle_axis[1];
+ const T& a2 = angle_axis[2];
+ const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
+
+ // For points not at the origin, the full conversion is numerically stable.
+ if (theta_squared > T(0.0)) {
+ const T theta = sqrt(theta_squared);
+ const T half_theta = theta * T(0.5);
+ const T k = sin(half_theta) / theta;
+ quaternion[0] = cos(half_theta);
+ quaternion[1] = a0 * k;
+ quaternion[2] = a1 * k;
+ quaternion[3] = a2 * k;
+ } else {
+ // At the origin, sqrt() will produce NaN in the derivative since
+ // the argument is zero. By approximating with a Taylor series,
+ // and truncating at one term, the value and first derivatives will be
+ // computed correctly when Jets are used.
+ const T k(0.5);
+ quaternion[0] = T(1.0);
+ quaternion[1] = a0 * k;
+ quaternion[2] = a1 * k;
+ quaternion[3] = a2 * k;
+ }
+}
+
+template<typename T>
+inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
+ const T& q1 = quaternion[1];
+ const T& q2 = quaternion[2];
+ const T& q3 = quaternion[3];
+ const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
+
+ // For quaternions representing non-zero rotation, the conversion
+ // is numerically stable.
+ if (sin_squared_theta > T(0.0)) {
+ const T sin_theta = sqrt(sin_squared_theta);
+ const T& cos_theta = quaternion[0];
+
+ // If cos_theta is negative, theta is greater than pi/2, which
+ // means that angle for the angle_axis vector which is 2 * theta
+ // would be greater than pi.
+ //
+ // While this will result in the correct rotation, it does not
+ // result in a normalized angle-axis vector.
+ //
+ // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
+ // which is equivalent saying
+ //
+ // theta - pi = atan(sin(theta - pi), cos(theta - pi))
+ // = atan(-sin(theta), -cos(theta))
+ //
+ const T two_theta =
+ T(2.0) * ((cos_theta < 0.0)
+ ? atan2(-sin_theta, -cos_theta)
+ : atan2(sin_theta, cos_theta));
+ const T k = two_theta / sin_theta;
+ angle_axis[0] = q1 * k;
+ angle_axis[1] = q2 * k;
+ angle_axis[2] = q3 * k;
+ } else {
+ // For zero rotation, sqrt() will produce NaN in the derivative since
+ // the argument is zero. By approximating with a Taylor series,
+ // and truncating at one term, the value and first derivatives will be
+ // computed correctly when Jets are used.
+ const T k(2.0);
+ angle_axis[0] = q1 * k;
+ angle_axis[1] = q2 * k;
+ angle_axis[2] = q3 * k;
+ }
+}
+
+template <typename T>
+void RotationMatrixToQuaternion(const T* R, T* angle_axis) {
+ RotationMatrixToQuaternion(ColumnMajorAdapter3x3(R), angle_axis);
+}
+
+// This algorithm comes from "Quaternion Calculus and Fast Animation",
+// Ken Shoemake, 1987 SIGGRAPH course notes
+template <typename T, int row_stride, int col_stride>
+void RotationMatrixToQuaternion(
+ const MatrixAdapter<const T, row_stride, col_stride>& R,
+ T* quaternion) {
+ const T trace = R(0, 0) + R(1, 1) + R(2, 2);
+ if (trace >= 0.0) {
+ T t = sqrt(trace + T(1.0));
+ quaternion[0] = T(0.5) * t;
+ t = T(0.5) / t;
+ quaternion[1] = (R(2, 1) - R(1, 2)) * t;
+ quaternion[2] = (R(0, 2) - R(2, 0)) * t;
+ quaternion[3] = (R(1, 0) - R(0, 1)) * t;
+ } else {
+ int i = 0;
+ if (R(1, 1) > R(0, 0)) {
+ i = 1;
+ }
+
+ if (R(2, 2) > R(i, i)) {
+ i = 2;
+ }
+
+ const int j = (i + 1) % 3;
+ const int k = (j + 1) % 3;
+ T t = sqrt(R(i, i) - R(j, j) - R(k, k) + T(1.0));
+ quaternion[i + 1] = T(0.5) * t;
+ t = T(0.5) / t;
+ quaternion[0] = (R(k, j) - R(j, k)) * t;
+ quaternion[j + 1] = (R(j, i) + R(i, j)) * t;
+ quaternion[k + 1] = (R(k, i) + R(i, k)) * t;
+ }
+}
+
+// The conversion of a rotation matrix to the angle-axis form is
+// numerically problematic when then rotation angle is close to zero
+// or to Pi. The following implementation detects when these two cases
+// occurs and deals with them by taking code paths that are guaranteed
+// to not perform division by a small number.
+template <typename T>
+inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) {
+ RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis);
+}
+
+template <typename T, int row_stride, int col_stride>
+void RotationMatrixToAngleAxis(
+ const MatrixAdapter<const T, row_stride, col_stride>& R,
+ T* angle_axis) {
+ T quaternion[4];
+ RotationMatrixToQuaternion(R, quaternion);
+ QuaternionToAngleAxis(quaternion, angle_axis);
+ return;
+}
+
+template <typename T>
+inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) {
+ AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R));
+}
+
+template <typename T, int row_stride, int col_stride>
+void AngleAxisToRotationMatrix(
+ const T* angle_axis,
+ const MatrixAdapter<T, row_stride, col_stride>& R) {
+ static const T kOne = T(1.0);
+ const T theta2 = DotProduct(angle_axis, angle_axis);
+ if (theta2 > T(std::numeric_limits<double>::epsilon())) {
+ // We want to be careful to only evaluate the square root if the
+ // norm of the angle_axis vector is greater than zero. Otherwise
+ // we get a division by zero.
+ const T theta = sqrt(theta2);
+ const T wx = angle_axis[0] / theta;
+ const T wy = angle_axis[1] / theta;
+ const T wz = angle_axis[2] / theta;
+
+ const T costheta = cos(theta);
+ const T sintheta = sin(theta);
+
+ R(0, 0) = costheta + wx*wx*(kOne - costheta);
+ R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta);
+ R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta);
+ R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta;
+ R(1, 1) = costheta + wy*wy*(kOne - costheta);
+ R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta);
+ R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta);
+ R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta);
+ R(2, 2) = costheta + wz*wz*(kOne - costheta);
+ } else {
+ // Near zero, we switch to using the first order Taylor expansion.
+ R(0, 0) = kOne;
+ R(1, 0) = angle_axis[2];
+ R(2, 0) = -angle_axis[1];
+ R(0, 1) = -angle_axis[2];
+ R(1, 1) = kOne;
+ R(2, 1) = angle_axis[0];
+ R(0, 2) = angle_axis[1];
+ R(1, 2) = -angle_axis[0];
+ R(2, 2) = kOne;
+ }
+}
+
+template <typename T>
+inline void EulerAnglesToRotationMatrix(const T* euler,
+ const int row_stride_parameter,
+ T* R) {
+ EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R));
+}
+
+template <typename T, int row_stride, int col_stride>
+void EulerAnglesToRotationMatrix(
+ const T* euler,
+ const MatrixAdapter<T, row_stride, col_stride>& R) {
+ const double kPi = 3.14159265358979323846;
+ const T degrees_to_radians(kPi / 180.0);
+
+ const T pitch(euler[0] * degrees_to_radians);
+ const T roll(euler[1] * degrees_to_radians);
+ const T yaw(euler[2] * degrees_to_radians);
+
+ const T c1 = cos(yaw);
+ const T s1 = sin(yaw);
+ const T c2 = cos(roll);
+ const T s2 = sin(roll);
+ const T c3 = cos(pitch);
+ const T s3 = sin(pitch);
+
+ R(0, 0) = c1*c2;
+ R(0, 1) = -s1*c3 + c1*s2*s3;
+ R(0, 2) = s1*s3 + c1*s2*c3;
+
+ R(1, 0) = s1*c2;
+ R(1, 1) = c1*c3 + s1*s2*s3;
+ R(1, 2) = -c1*s3 + s1*s2*c3;
+
+ R(2, 0) = -s2;
+ R(2, 1) = c2*s3;
+ R(2, 2) = c2*c3;
+}
+
+template <typename T> inline
+void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) {
+ QuaternionToScaledRotation(q, RowMajorAdapter3x3(R));
+}
+
+template <typename T, int row_stride, int col_stride> inline
+void QuaternionToScaledRotation(
+ const T q[4],
+ const MatrixAdapter<T, row_stride, col_stride>& R) {
+ // Make convenient names for elements of q.
+ T a = q[0];
+ T b = q[1];
+ T c = q[2];
+ T d = q[3];
+ // This is not to eliminate common sub-expression, but to
+ // make the lines shorter so that they fit in 80 columns!
+ T aa = a * a;
+ T ab = a * b;
+ T ac = a * c;
+ T ad = a * d;
+ T bb = b * b;
+ T bc = b * c;
+ T bd = b * d;
+ T cc = c * c;
+ T cd = c * d;
+ T dd = d * d;
+
+ R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT
+ R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT
+ R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT
+}
+
+template <typename T> inline
+void QuaternionToRotation(const T q[4], T R[3 * 3]) {
+ QuaternionToRotation(q, RowMajorAdapter3x3(R));
+}
+
+template <typename T, int row_stride, int col_stride> inline
+void QuaternionToRotation(const T q[4],
+ const MatrixAdapter<T, row_stride, col_stride>& R) {
+ QuaternionToScaledRotation(q, R);
+
+ T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
+ normalizer = T(1) / normalizer;
+
+ for (int i = 0; i < 3; ++i) {
+ for (int j = 0; j < 3; ++j) {
+ R(i, j) *= normalizer;
+ }
+ }
+}
+
+template <typename T> inline
+void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
+ const T t2 = q[0] * q[1];
+ const T t3 = q[0] * q[2];
+ const T t4 = q[0] * q[3];
+ const T t5 = -q[1] * q[1];
+ const T t6 = q[1] * q[2];
+ const T t7 = q[1] * q[3];
+ const T t8 = -q[2] * q[2];
+ const T t9 = q[2] * q[3];
+ const T t1 = -q[3] * q[3];
+ result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT
+ result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT
+ result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT
+}
+
+template <typename T> inline
+void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
+ // 'scale' is 1 / norm(q).
+ const T scale = T(1) / sqrt(q[0] * q[0] +
+ q[1] * q[1] +
+ q[2] * q[2] +
+ q[3] * q[3]);
+
+ // Make unit-norm version of q.
+ const T unit[4] = {
+ scale * q[0],
+ scale * q[1],
+ scale * q[2],
+ scale * q[3],
+ };
+
+ UnitQuaternionRotatePoint(unit, pt, result);
+}
+
+template<typename T> inline
+void QuaternionProduct(const T z[4], const T w[4], T zw[4]) {
+ zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
+ zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
+ zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
+ zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
+}
+
+// xy = x cross y;
+template<typename T> inline
+void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) {
+ x_cross_y[0] = x[1] * y[2] - x[2] * y[1];
+ x_cross_y[1] = x[2] * y[0] - x[0] * y[2];
+ x_cross_y[2] = x[0] * y[1] - x[1] * y[0];
+}
+
+template<typename T> inline
+T DotProduct(const T x[3], const T y[3]) {
+ return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
+}
+
+template<typename T> inline
+void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
+ const T theta2 = DotProduct(angle_axis, angle_axis);
+ if (theta2 > T(std::numeric_limits<double>::epsilon())) {
+ // Away from zero, use the rodriguez formula
+ //
+ // result = pt costheta +
+ // (w x pt) * sintheta +
+ // w (w . pt) (1 - costheta)
+ //
+ // We want to be careful to only evaluate the square root if the
+ // norm of the angle_axis vector is greater than zero. Otherwise
+ // we get a division by zero.
+ //
+ const T theta = sqrt(theta2);
+ const T costheta = cos(theta);
+ const T sintheta = sin(theta);
+ const T theta_inverse = T(1.0) / theta;
+
+ const T w[3] = { angle_axis[0] * theta_inverse,
+ angle_axis[1] * theta_inverse,
+ angle_axis[2] * theta_inverse };
+
+ // Explicitly inlined evaluation of the cross product for
+ // performance reasons.
+ const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
+ w[2] * pt[0] - w[0] * pt[2],
+ w[0] * pt[1] - w[1] * pt[0] };
+ const T tmp =
+ (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
+
+ result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
+ result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
+ result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
+ } else {
+ // Near zero, the first order Taylor approximation of the rotation
+ // matrix R corresponding to a vector w and angle w is
+ //
+ // R = I + hat(w) * sin(theta)
+ //
+ // But sintheta ~ theta and theta * w = angle_axis, which gives us
+ //
+ // R = I + hat(w)
+ //
+ // and actually performing multiplication with the point pt, gives us
+ // R * pt = pt + w x pt.
+ //
+ // Switching to the Taylor expansion near zero provides meaningful
+ // derivatives when evaluated using Jets.
+ //
+ // Explicitly inlined evaluation of the cross product for
+ // performance reasons.
+ const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
+ angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
+ angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };
+
+ result[0] = pt[0] + w_cross_pt[0];
+ result[1] = pt[1] + w_cross_pt[1];
+ result[2] = pt[2] + w_cross_pt[2];
+ }
+}
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_ROTATION_H_
diff --git a/include/ceres/sized_cost_function.h b/include/ceres/sized_cost_function.h
new file mode 100644
index 0000000..50d0363
--- /dev/null
+++ b/include/ceres/sized_cost_function.h
@@ -0,0 +1,71 @@
+// 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 convenience class for cost functions which are statically sized.
+// Compared to the dynamically-sized base class, this reduces boilerplate.
+//
+// The kNumResiduals template parameter can be a constant such as 2 or 5, or it
+// can be ceres::DYNAMIC. If kNumResiduals is ceres::DYNAMIC, then subclasses
+// are responsible for calling set_num_residuals() at runtime.
+
+#ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_
+#define CERES_PUBLIC_SIZED_COST_FUNCTION_H_
+
+#include "ceres/cost_function.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+#include "internal/parameter_dims.h"
+
+namespace ceres {
+
+template <int kNumResiduals, int... Ns>
+class SizedCostFunction : public CostFunction {
+ public:
+ static_assert(kNumResiduals > 0 || kNumResiduals == DYNAMIC,
+ "Cost functions must have at least one residual block.");
+ static_assert(internal::StaticParameterDims<Ns...>::kIsValid,
+ "Invalid parameter block dimension detected. Each parameter "
+ "block dimension must be bigger than zero.");
+
+ using ParameterDims = internal::StaticParameterDims<Ns...>;
+
+ SizedCostFunction() {
+ set_num_residuals(kNumResiduals);
+ *mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};
+ }
+
+ virtual ~SizedCostFunction() { }
+
+ // Subclasses must implement Evaluate().
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_SIZED_COST_FUNCTION_H_
diff --git a/include/ceres/solver.h b/include/ceres/solver.h
new file mode 100644
index 0000000..83077e2
--- /dev/null
+++ b/include/ceres/solver.h
@@ -0,0 +1,1053 @@
+// 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_PUBLIC_SOLVER_H_
+#define CERES_PUBLIC_SOLVER_H_
+
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+#include "ceres/crs_matrix.h"
+#include "ceres/evaluation_callback.h"
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/port.h"
+#include "ceres/iteration_callback.h"
+#include "ceres/ordered_groups.h"
+#include "ceres/types.h"
+
+namespace ceres {
+
+class Problem;
+
+// Interface for non-linear least squares solvers.
+class CERES_EXPORT Solver {
+ public:
+ virtual ~Solver();
+
+ // The options structure contains, not surprisingly, options that control how
+ // the solver operates. The defaults should be suitable for a wide range of
+ // problems; however, better performance is often obtainable with tweaking.
+ //
+ // The constants are defined inside types.h
+ struct CERES_EXPORT Options {
+ // Returns true if the options struct has a valid
+ // configuration. Returns false otherwise, and fills in *error
+ // with a message describing the problem.
+ bool IsValid(std::string* error) const;
+
+ // Minimizer options ----------------------------------------
+
+ // Ceres supports the two major families of optimization strategies -
+ // Trust Region and Line Search.
+ //
+ // 1. The line search approach first finds a descent direction
+ // along which the objective function will be reduced and then
+ // computes a step size that decides how far should move along
+ // that direction. The descent direction can be computed by
+ // various methods, such as gradient descent, Newton's method and
+ // Quasi-Newton method. The step size can be determined either
+ // exactly or inexactly.
+ //
+ // 2. The trust region approach approximates the objective
+ // function using a model function (often a quadratic) over
+ // a subset of the search space known as the trust region. If the
+ // model function succeeds in minimizing the true objective
+ // function the trust region is expanded; conversely, otherwise it
+ // is contracted and the model optimization problem is solved
+ // again.
+ //
+ // Trust region methods are in some sense dual to line search methods:
+ // trust region methods first choose a step size (the size of the
+ // trust region) and then a step direction while line search methods
+ // first choose a step direction and then a step size.
+ MinimizerType minimizer_type = TRUST_REGION;
+
+ LineSearchDirectionType line_search_direction_type = LBFGS;
+ LineSearchType line_search_type = WOLFE;
+ NonlinearConjugateGradientType nonlinear_conjugate_gradient_type =
+ FLETCHER_REEVES;
+
+ // The LBFGS hessian approximation is a low rank approximation to
+ // the inverse of the Hessian matrix. The rank of the
+ // approximation determines (linearly) the space and time
+ // complexity of using the approximation. Higher the rank, the
+ // better is the quality of the approximation. The increase in
+ // quality is however is bounded for a number of reasons.
+ //
+ // 1. The method only uses secant information and not actual
+ // derivatives.
+ //
+ // 2. The Hessian approximation is constrained to be positive
+ // definite.
+ //
+ // So increasing this rank to a large number will cost time and
+ // space complexity without the corresponding increase in solution
+ // quality. There are no hard and fast rules for choosing the
+ // maximum rank. The best choice usually requires some problem
+ // specific experimentation.
+ //
+ // For more theoretical and implementation details of the LBFGS
+ // method, please see:
+ //
+ // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with
+ // Limited Storage". Mathematics of Computation 35 (151): 773–782.
+ int max_lbfgs_rank = 20;
+
+ // As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS),
+ // the initial inverse Hessian approximation is taken to be the Identity.
+ // However, Oren showed that using instead I * \gamma, where \gamma is
+ // chosen to approximate an eigenvalue of the true inverse Hessian can
+ // result in improved convergence in a wide variety of cases. Setting
+ // use_approximate_eigenvalue_bfgs_scaling to true enables this scaling.
+ //
+ // It is important to note that approximate eigenvalue scaling does not
+ // always improve convergence, and that it can in fact significantly degrade
+ // performance for certain classes of problem, which is why it is disabled
+ // by default. In particular it can degrade performance when the
+ // sensitivity of the problem to different parameters varies significantly,
+ // as in this case a single scalar factor fails to capture this variation
+ // and detrimentally downscales parts of the jacobian approximation which
+ // correspond to low-sensitivity parameters. It can also reduce the
+ // robustness of the solution to errors in the jacobians.
+ //
+ // Oren S.S., Self-scaling variable metric (SSVM) algorithms
+ // Part II: Implementation and experiments, Management Science,
+ // 20(5), 863-874, 1974.
+ bool use_approximate_eigenvalue_bfgs_scaling = false;
+
+ // Degree of the polynomial used to approximate the objective
+ // function. Valid values are BISECTION, QUADRATIC and CUBIC.
+ //
+ // BISECTION corresponds to pure backtracking search with no
+ // interpolation.
+ LineSearchInterpolationType line_search_interpolation_type = CUBIC;
+
+ // If during the line search, the step_size falls below this
+ // value, it is truncated to zero.
+ double min_line_search_step_size = 1e-9;
+
+ // Line search parameters.
+
+ // Solving the line search problem exactly is computationally
+ // prohibitive. Fortunately, line search based optimization
+ // algorithms can still guarantee convergence if instead of an
+ // exact solution, the line search algorithm returns a solution
+ // which decreases the value of the objective function
+ // sufficiently. More precisely, we are looking for a step_size
+ // s.t.
+ //
+ // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size
+ //
+ double line_search_sufficient_function_decrease = 1e-4;
+
+ // In each iteration of the line search,
+ //
+ // new_step_size >= max_line_search_step_contraction * step_size
+ //
+ // Note that by definition, for contraction:
+ //
+ // 0 < max_step_contraction < min_step_contraction < 1
+ //
+ double max_line_search_step_contraction = 1e-3;
+
+ // In each iteration of the line search,
+ //
+ // new_step_size <= min_line_search_step_contraction * step_size
+ //
+ // Note that by definition, for contraction:
+ //
+ // 0 < max_step_contraction < min_step_contraction < 1
+ //
+ double min_line_search_step_contraction = 0.6;
+
+ // Maximum number of trial step size iterations during each line search,
+ // if a step size satisfying the search conditions cannot be found within
+ // this number of trials, the line search will terminate.
+ int max_num_line_search_step_size_iterations = 20;
+
+ // Maximum number of restarts of the line search direction algorithm before
+ // terminating the optimization. Restarts of the line search direction
+ // algorithm occur when the current algorithm fails to produce a new descent
+ // direction. This typically indicates a numerical failure, or a breakdown
+ // in the validity of the approximations used.
+ int max_num_line_search_direction_restarts = 5;
+
+ // The strong Wolfe conditions consist of the Armijo sufficient
+ // decrease condition, and an additional requirement that the
+ // step-size be chosen s.t. the _magnitude_ ('strong' Wolfe
+ // conditions) of the gradient along the search direction
+ // decreases sufficiently. Precisely, this second condition
+ // is that we seek a step_size s.t.
+ //
+ // |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)|
+ //
+ // Where f() is the line search objective and f'() is the derivative
+ // of f w.r.t step_size (d f / d step_size).
+ double line_search_sufficient_curvature_decrease = 0.9;
+
+ // During the bracketing phase of the Wolfe search, the step size is
+ // increased until either a point satisfying the Wolfe conditions is
+ // found, or an upper bound for a bracket containing a point satisfying
+ // the conditions is found. Precisely, at each iteration of the
+ // expansion:
+ //
+ // new_step_size <= max_step_expansion * step_size.
+ //
+ // By definition for expansion, max_step_expansion > 1.0.
+ double max_line_search_step_expansion = 10.0;
+
+ TrustRegionStrategyType trust_region_strategy_type = LEVENBERG_MARQUARDT;
+
+ // Type of dogleg strategy to use.
+ DoglegType dogleg_type = TRADITIONAL_DOGLEG;
+
+ // The classical trust region methods are descent methods, in that
+ // they only accept a point if it strictly reduces the value of
+ // the objective function.
+ //
+ // Relaxing this requirement allows the algorithm to be more
+ // efficient in the long term at the cost of some local increase
+ // in the value of the objective function.
+ //
+ // This is because allowing for non-decreasing objective function
+ // values in a principled manner allows the algorithm to "jump over
+ // boulders" as the method is not restricted to move into narrow
+ // valleys while preserving its convergence properties.
+ //
+ // Setting use_nonmonotonic_steps to true enables the
+ // non-monotonic trust region algorithm as described by Conn,
+ // Gould & Toint in "Trust Region Methods", Section 10.1.
+ //
+ // The parameter max_consecutive_nonmonotonic_steps controls the
+ // window size used by the step selection algorithm to accept
+ // non-monotonic steps.
+ //
+ // Even though the value of the objective function may be larger
+ // than the minimum value encountered over the course of the
+ // optimization, the final parameters returned to the user are the
+ // ones corresponding to the minimum cost over all iterations.
+ bool use_nonmonotonic_steps = false;
+ int max_consecutive_nonmonotonic_steps = 5;
+
+ // Maximum number of iterations for the minimizer to run for.
+ int max_num_iterations = 50;
+
+ // Maximum time for which the minimizer should run for.
+ double max_solver_time_in_seconds = 1e9;
+
+ // Number of threads used by Ceres for evaluating the cost and
+ // jacobians.
+ int num_threads = 1;
+
+ // Trust region minimizer settings.
+ double initial_trust_region_radius = 1e4;
+ double max_trust_region_radius = 1e16;
+
+ // Minimizer terminates when the trust region radius becomes
+ // smaller than this value.
+ double min_trust_region_radius = 1e-32;
+
+ // Lower bound for the relative decrease before a step is
+ // accepted.
+ double min_relative_decrease = 1e-3;
+
+ // For the Levenberg-Marquadt algorithm, the scaled diagonal of
+ // the normal equations J'J is used to control the size of the
+ // trust region. Extremely small and large values along the
+ // diagonal can make this regularization scheme
+ // fail. max_lm_diagonal and min_lm_diagonal, clamp the values of
+ // diag(J'J) from above and below. In the normal course of
+ // operation, the user should not have to modify these parameters.
+ double min_lm_diagonal = 1e-6;
+ double max_lm_diagonal = 1e32;
+
+ // Sometimes due to numerical conditioning problems or linear
+ // solver flakiness, the trust region strategy may return a
+ // numerically invalid step that can be fixed by reducing the
+ // trust region size. So the TrustRegionMinimizer allows for a few
+ // successive invalid steps before it declares NUMERICAL_FAILURE.
+ int max_num_consecutive_invalid_steps = 5;
+
+ // Minimizer terminates when
+ //
+ // (new_cost - old_cost) < function_tolerance * old_cost;
+ //
+ double function_tolerance = 1e-6;
+
+ // Minimizer terminates when
+ //
+ // max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance
+ //
+ // This value should typically be 1e-4 * function_tolerance.
+ double gradient_tolerance = 1e-10;
+
+ // Minimizer terminates when
+ //
+ // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance)
+ //
+ double parameter_tolerance = 1e-8;
+
+ // Linear least squares solver options -------------------------------------
+
+ LinearSolverType linear_solver_type =
+#if defined(CERES_NO_SPARSE)
+ DENSE_QR;
+#else
+ SPARSE_NORMAL_CHOLESKY;
+#endif
+
+ // Type of preconditioner to use with the iterative linear solvers.
+ PreconditionerType preconditioner_type = JACOBI;
+
+ // Type of clustering algorithm to use for visibility based
+ // preconditioning. This option is used only when the
+ // preconditioner_type is CLUSTER_JACOBI or CLUSTER_TRIDIAGONAL.
+ VisibilityClusteringType visibility_clustering_type = CANONICAL_VIEWS;
+
+ // Ceres supports using multiple dense linear algebra libraries
+ // for dense matrix factorizations. Currently EIGEN and LAPACK are
+ // the valid choices. EIGEN is always available, LAPACK refers to
+ // the system BLAS + LAPACK library which may or may not be
+ // available.
+ //
+ // This setting affects the DENSE_QR, DENSE_NORMAL_CHOLESKY and
+ // DENSE_SCHUR solvers. For small to moderate sized problem EIGEN
+ // is a fine choice but for large problems, an optimized LAPACK +
+ // BLAS implementation can make a substantial difference in
+ // performance.
+ DenseLinearAlgebraLibraryType dense_linear_algebra_library_type = EIGEN;
+
+ // Ceres supports using multiple sparse linear algebra libraries
+ // for sparse matrix ordering and factorizations. Currently,
+ // SUITE_SPARSE and CX_SPARSE are the valid choices, depending on
+ // whether they are linked into Ceres at build time.
+ SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type =
+#if !defined(CERES_NO_SUITESPARSE)
+ SUITE_SPARSE;
+#elif !defined(CERES_NO_ACCELERATE_SPARSE)
+ ACCELERATE_SPARSE;
+#elif !defined(CERES_NO_CXSPARSE)
+ CX_SPARSE;
+#elif defined(CERES_USE_EIGEN_SPARSE)
+ EIGEN_SPARSE;
+#else
+ NO_SPARSE;
+#endif
+
+ // The order in which variables are eliminated in a linear solver
+ // can have a significant of impact on the efficiency and accuracy
+ // of the method. e.g., when doing sparse Cholesky factorization,
+ // there are matrices for which a good ordering will give a
+ // Cholesky factor with O(n) storage, where as a bad ordering will
+ // result in an completely dense factor.
+ //
+ // Ceres allows the user to provide varying amounts of hints to
+ // the solver about the variable elimination ordering to use. This
+ // can range from no hints, where the solver is free to decide the
+ // best possible ordering based on the user's choices like the
+ // linear solver being used, to an exact order in which the
+ // variables should be eliminated, and a variety of possibilities
+ // in between.
+ //
+ // Instances of the ParameterBlockOrdering class are used to
+ // communicate this information to Ceres.
+ //
+ // Formally an ordering is an ordered partitioning of the
+ // parameter blocks, i.e, each parameter block belongs to exactly
+ // one group, and each group has a unique non-negative integer
+ // associated with it, that determines its order in the set of
+ // groups.
+ //
+ // Given such an ordering, Ceres ensures that the parameter blocks in
+ // the lowest numbered group are eliminated first, and then the
+ // parameter blocks in the next lowest numbered group and so on. Within
+ // each group, Ceres is free to order the parameter blocks as it
+ // chooses.
+ //
+ // If NULL, then all parameter blocks are assumed to be in the
+ // same group and the solver is free to decide the best
+ // ordering.
+ //
+ // e.g. Consider the linear system
+ //
+ // x + y = 3
+ // 2x + 3y = 7
+ //
+ // There are two ways in which it can be solved. First eliminating x
+ // from the two equations, solving for y and then back substituting
+ // for x, or first eliminating y, solving for x and back substituting
+ // for y. The user can construct three orderings here.
+ //
+ // {0: x}, {1: y} - eliminate x first.
+ // {0: y}, {1: x} - eliminate y first.
+ // {0: x, y} - Solver gets to decide the elimination order.
+ //
+ // Thus, to have Ceres determine the ordering automatically using
+ // heuristics, put all the variables in group 0 and to control the
+ // ordering for every variable, create groups 0..N-1, one per
+ // variable, in the desired order.
+ //
+ // Bundle Adjustment
+ // -----------------
+ //
+ // A particular case of interest is bundle adjustment, where the user
+ // has two options. The default is to not specify an ordering at all,
+ // the solver will see that the user wants to use a Schur type solver
+ // and figure out the right elimination ordering.
+ //
+ // But if the user already knows what parameter blocks are points and
+ // what are cameras, they can save preprocessing time by partitioning
+ // the parameter blocks into two groups, one for the points and one
+ // for the cameras, where the group containing the points has an id
+ // smaller than the group containing cameras.
+ std::shared_ptr<ParameterBlockOrdering> linear_solver_ordering;
+
+ // Use an explicitly computed Schur complement matrix with
+ // ITERATIVE_SCHUR.
+ //
+ // By default this option is disabled and ITERATIVE_SCHUR
+ // evaluates matrix-vector products between the Schur
+ // complement and a vector implicitly by exploiting the algebraic
+ // expression for the Schur complement.
+ //
+ // The cost of this evaluation scales with the number of non-zeros
+ // in the Jacobian.
+ //
+ // For small to medium sized problems there is a sweet spot where
+ // computing the Schur complement is cheap enough that it is much
+ // more efficient to explicitly compute it and use it for evaluating
+ // the matrix-vector products.
+ //
+ // Enabling this option tells ITERATIVE_SCHUR to use an explicitly
+ // computed Schur complement.
+ //
+ // NOTE: This option can only be used with the SCHUR_JACOBI
+ // preconditioner.
+ bool use_explicit_schur_complement = false;
+
+ // Sparse Cholesky factorization algorithms use a fill-reducing
+ // ordering to permute the columns of the Jacobian matrix. There
+ // are two ways of doing this.
+
+ // 1. Compute the Jacobian matrix in some order and then have the
+ // factorization algorithm permute the columns of the Jacobian.
+
+ // 2. Compute the Jacobian with its columns already permuted.
+
+ // The first option incurs a significant memory penalty. The
+ // factorization algorithm has to make a copy of the permuted
+ // Jacobian matrix, thus Ceres pre-permutes the columns of the
+ // Jacobian matrix and generally speaking, there is no performance
+ // penalty for doing so.
+
+ // In some rare cases, it is worth using a more complicated
+ // reordering algorithm which has slightly better runtime
+ // performance at the expense of an extra copy of the Jacobian
+ // matrix. Setting use_postordering to true enables this tradeoff.
+ bool use_postordering = false;
+
+ // Some non-linear least squares problems are symbolically dense but
+ // numerically sparse. i.e. at any given state only a small number
+ // of jacobian entries are non-zero, but the position and number of
+ // non-zeros is different depending on the state. For these problems
+ // it can be useful to factorize the sparse jacobian at each solver
+ // iteration instead of including all of the zero entries in a single
+ // general factorization.
+ //
+ // If your problem does not have this property (or you do not know),
+ // then it is probably best to keep this false, otherwise it will
+ // likely lead to worse performance.
+
+ // This settings only affects the SPARSE_NORMAL_CHOLESKY solver.
+ bool dynamic_sparsity = false;
+
+ // TODO(sameeragarwal): Further expand the documentation for the
+ // following two options.
+
+ // NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK.
+ //
+ // If use_mixed_precision_solves is true, the Gauss-Newton matrix
+ // is computed in double precision, but its factorization is
+ // computed in single precision. This can result in significant
+ // time and memory savings at the cost of some accuracy in the
+ // Gauss-Newton step. Iterative refinement is used to recover some
+ // of this accuracy back.
+ //
+ // If use_mixed_precision_solves is true, we recommend setting
+ // max_num_refinement_iterations to 2-3.
+ //
+ // NOTE2: The following two options are currently only applicable
+ // if sparse_linear_algebra_library_type is EIGEN_SPARSE and
+ // linear_solver_type is SPARSE_NORMAL_CHOLESKY, or SPARSE_SCHUR.
+ bool use_mixed_precision_solves = false;
+
+ // Number steps of the iterative refinement process to run when
+ // computing the Gauss-Newton step.
+ int max_num_refinement_iterations = 0;
+
+ // Some non-linear least squares problems have additional
+ // structure in the way the parameter blocks interact that it is
+ // beneficial to modify the way the trust region step is computed.
+ //
+ // e.g., consider the following regression problem
+ //
+ // y = a_1 exp(b_1 x) + a_2 exp(b_3 x^2 + c_1)
+ //
+ // Given a set of pairs{(x_i, y_i)}, the user wishes to estimate
+ // a_1, a_2, b_1, b_2, and c_1.
+ //
+ // Notice here that the expression on the left is linear in a_1
+ // and a_2, and given any value for b_1, b_2 and c_1, it is
+ // possible to use linear regression to estimate the optimal
+ // values of a_1 and a_2. Indeed, its possible to analytically
+ // eliminate the variables a_1 and a_2 from the problem all
+ // together. Problems like these are known as separable least
+ // squares problem and the most famous algorithm for solving them
+ // is the Variable Projection algorithm invented by Golub &
+ // Pereyra.
+ //
+ // Similar structure can be found in the matrix factorization with
+ // missing data problem. There the corresponding algorithm is
+ // known as Wiberg's algorithm.
+ //
+ // Ruhe & Wedin (Algorithms for Separable Nonlinear Least Squares
+ // Problems, SIAM Reviews, 22(3), 1980) present an analysis of
+ // various algorithms for solving separable non-linear least
+ // squares problems and refer to "Variable Projection" as
+ // Algorithm I in their paper.
+ //
+ // Implementing Variable Projection is tedious and expensive, and
+ // they present a simpler algorithm, which they refer to as
+ // Algorithm II, where once the Newton/Trust Region step has been
+ // computed for the whole problem (a_1, a_2, b_1, b_2, c_1) and
+ // additional optimization step is performed to estimate a_1 and
+ // a_2 exactly.
+ //
+ // This idea can be generalized to cases where the residual is not
+ // linear in a_1 and a_2, i.e., Solve for the trust region step
+ // for the full problem, and then use it as the starting point to
+ // further optimize just a_1 and a_2. For the linear case, this
+ // amounts to doing a single linear least squares solve. For
+ // non-linear problems, any method for solving the a_1 and a_2
+ // optimization problems will do. The only constraint on a_1 and
+ // a_2 is that they do not co-occur in any residual block.
+ //
+ // This idea can be further generalized, by not just optimizing
+ // (a_1, a_2), but decomposing the graph corresponding to the
+ // Hessian matrix's sparsity structure in a collection of
+ // non-overlapping independent sets and optimizing each of them.
+ //
+ // Setting "use_inner_iterations" to true enables the use of this
+ // non-linear generalization of Ruhe & Wedin's Algorithm II. This
+ // version of Ceres has a higher iteration complexity, but also
+ // displays better convergence behaviour per iteration. Setting
+ // Solver::Options::num_threads to the maximum number possible is
+ // highly recommended.
+ bool use_inner_iterations = false;
+
+ // If inner_iterations is true, then the user has two choices.
+ //
+ // 1. Let the solver heuristically decide which parameter blocks
+ // to optimize in each inner iteration. To do this leave
+ // Solver::Options::inner_iteration_ordering untouched.
+ //
+ // 2. Specify a collection of of ordered independent sets. Where
+ // the lower numbered groups are optimized before the higher
+ // number groups. Each group must be an independent set. Not
+ // all parameter blocks need to be present in the ordering.
+ std::shared_ptr<ParameterBlockOrdering> inner_iteration_ordering;
+
+ // Generally speaking, inner iterations make significant progress
+ // in the early stages of the solve and then their contribution
+ // drops down sharply, at which point the time spent doing inner
+ // iterations is not worth it.
+ //
+ // Once the relative decrease in the objective function due to
+ // inner iterations drops below inner_iteration_tolerance, the use
+ // of inner iterations in subsequent trust region minimizer
+ // iterations is disabled.
+ double inner_iteration_tolerance = 1e-3;
+
+ // Minimum number of iterations for which the linear solver should
+ // run, even if the convergence criterion is satisfied.
+ int min_linear_solver_iterations = 0;
+
+ // Maximum number of iterations for which the linear solver should
+ // run. If the solver does not converge in less than
+ // max_linear_solver_iterations, then it returns MAX_ITERATIONS,
+ // as its termination type.
+ int max_linear_solver_iterations = 500;
+
+ // Forcing sequence parameter. The truncated Newton solver uses
+ // this number to control the relative accuracy with which the
+ // Newton step is computed.
+ //
+ // This constant is passed to ConjugateGradientsSolver which uses
+ // it to terminate the iterations when
+ //
+ // (Q_i - Q_{i-1})/Q_i < eta/i
+ double eta = 1e-1;
+
+ // Normalize the jacobian using Jacobi scaling before calling
+ // the linear least squares solver.
+ bool jacobi_scaling = true;
+
+ // Logging options ---------------------------------------------------------
+
+ LoggingType logging_type = PER_MINIMIZER_ITERATION;
+
+ // By default the Minimizer progress is logged to VLOG(1), which
+ // is sent to STDERR depending on the vlog level. If this flag is
+ // set to true, and logging_type is not SILENT, the logging output
+ // is sent to STDOUT.
+ bool minimizer_progress_to_stdout = false;
+
+ // List of iterations at which the minimizer should dump the trust
+ // region problem. Useful for testing and benchmarking. If empty
+ // (default), no problems are dumped.
+ std::vector<int> trust_region_minimizer_iterations_to_dump;
+
+ // Directory to which the problems should be written to. Should be
+ // non-empty if trust_region_minimizer_iterations_to_dump is
+ // non-empty and trust_region_problem_dump_format_type is not
+ // CONSOLE.
+ std::string trust_region_problem_dump_directory = "/tmp";
+ DumpFormatType trust_region_problem_dump_format_type = TEXTFILE;
+
+ // Finite differences options ----------------------------------------------
+
+ // Check all jacobians computed by each residual block with finite
+ // differences. This is expensive since it involves computing the
+ // derivative by normal means (e.g. user specified, autodiff,
+ // etc), then also computing it using finite differences. The
+ // results are compared, and if they differ substantially, details
+ // are printed to the log.
+ bool check_gradients = false;
+
+ // Relative precision to check for in the gradient checker. If the
+ // relative difference between an element in a jacobian exceeds
+ // this number, then the jacobian for that cost term is dumped.
+ double gradient_check_relative_precision = 1e-8;
+
+ // WARNING: This option only applies to the to the numeric
+ // differentiation used for checking the user provided derivatives
+ // when when Solver::Options::check_gradients is true. If you are
+ // using NumericDiffCostFunction and are interested in changing
+ // the step size for numeric differentiation in your cost
+ // function, please have a look at
+ // include/ceres/numeric_diff_options.h.
+ //
+ // Relative shift used for taking numeric derivatives when
+ // Solver::Options::check_gradients is true.
+ //
+ // For finite differencing, each dimension is evaluated at
+ // slightly shifted values; for the case of central difference,
+ // this is what gets evaluated:
+ //
+ // delta = gradient_check_numeric_derivative_relative_step_size;
+ // f_initial = f(x)
+ // f_forward = f((1 + delta) * x)
+ // f_backward = f((1 - delta) * x)
+ //
+ // The finite differencing is done along each dimension. The
+ // reason to use a relative (rather than absolute) step size is
+ // that this way, numeric differentiation works for functions where
+ // the arguments are typically large (e.g. 1e9) and when the
+ // values are small (e.g. 1e-5). It is possible to construct
+ // "torture cases" which break this finite difference heuristic,
+ // but they do not come up often in practice.
+ //
+ // TODO(keir): Pick a smarter number than the default above! In
+ // theory a good choice is sqrt(eps) * x, which for doubles means
+ // about 1e-8 * x. However, I have found this number too
+ // optimistic. This number should be exposed for users to change.
+ double gradient_check_numeric_derivative_relative_step_size = 1e-6;
+
+ // If true, the user's parameter blocks are updated at the end of
+ // every Minimizer iteration, otherwise they are updated when the
+ // Minimizer terminates. This is useful if, for example, the user
+ // wishes to visualize the state of the optimization every iteration
+ // (in combination with an IterationCallback).
+ //
+ // NOTE: If an evaluation_callback is provided, then the behaviour
+ // of this flag is slightly different in each case:
+ //
+ // (1) If update_state_every_iteration = false, then the user's
+ // state is changed at every residual and/or jacobian evaluation.
+ // Any user provided IterationCallbacks should NOT inspect and
+ // depend on the user visible state while the solver is running,
+ // since there will be undefined contents.
+ //
+ // (2) If update_state_every_iteration is true, then the user's
+ // state is changed at every residual and/or jacobian evaluation,
+ // BUT the solver will ensure that before the user provided
+ // IterationCallbacks are called, the user visible state will be
+ // updated to the current best point found by the solver.
+ bool update_state_every_iteration = false;
+
+ // Callbacks that are executed at the end of each iteration of the
+ // Minimizer. An iteration may terminate midway, either due to
+ // numerical failures or because one of the convergence tests has
+ // been satisfied. In this case none of the callbacks are
+ // executed.
+
+ // Callbacks are executed in the order that they are specified in
+ // this vector. By default, parameter blocks are updated only at the
+ // end of the optimization, i.e when the Minimizer terminates. This
+ // behaviour is controlled by update_state_every_iteration. If the
+ // user wishes to have access to the updated parameter blocks when
+ // his/her callbacks are executed, then set
+ // update_state_every_iteration to true.
+ //
+ // The solver does NOT take ownership of these pointers.
+ std::vector<IterationCallback*> callbacks;
+
+ // If non-NULL, gets notified when Ceres is about to evaluate the
+ // residuals and/or Jacobians. This enables sharing computation
+ // between residuals, which in some cases is important for efficient
+ // cost evaluation. See evaluation_callback.h for details.
+ //
+ // NOTE: Evaluation callbacks are incompatible with inner iterations.
+ //
+ // WARNING: This interacts with update_state_every_iteration. See
+ // the documentation for that option for more details.
+ //
+ // The solver does NOT take ownership of the pointer.
+ EvaluationCallback* evaluation_callback = nullptr;
+ };
+
+ struct CERES_EXPORT Summary {
+ // A brief one line description of the state of the solver after
+ // termination.
+ std::string BriefReport() const;
+
+ // A full multiline description of the state of the solver after
+ // termination.
+ std::string FullReport() const;
+
+ bool IsSolutionUsable() const;
+
+ // Minimizer summary -------------------------------------------------
+ MinimizerType minimizer_type = TRUST_REGION;
+
+ TerminationType termination_type = FAILURE;
+
+ // Reason why the solver terminated.
+ std::string message = "ceres::Solve was not called.";
+
+ // Cost of the problem (value of the objective function) before
+ // the optimization.
+ double initial_cost = -1.0;
+
+ // Cost of the problem (value of the objective function) after the
+ // optimization.
+ double final_cost = -1.0;
+
+ // The part of the total cost that comes from residual blocks that
+ // were held fixed by the preprocessor because all the parameter
+ // blocks that they depend on were fixed.
+ double fixed_cost = -1.0;
+
+ // IterationSummary for each minimizer iteration in order.
+ std::vector<IterationSummary> iterations;
+
+ // Number of minimizer iterations in which the step was
+ // accepted. Unless use_non_monotonic_steps is true this is also
+ // the number of steps in which the objective function value/cost
+ // went down.
+ int num_successful_steps = -1.0;
+
+ // Number of minimizer iterations in which the step was rejected
+ // either because it did not reduce the cost enough or the step
+ // was not numerically valid.
+ int num_unsuccessful_steps = -1.0;
+
+ // Number of times inner iterations were performed.
+ int num_inner_iteration_steps = -1.0;
+
+ // Total number of iterations inside the line search algorithm
+ // across all invocations. We call these iterations "steps" to
+ // distinguish them from the outer iterations of the line search
+ // and trust region minimizer algorithms which call the line
+ // search algorithm as a subroutine.
+ int num_line_search_steps = -1.0;
+
+ // All times reported below are wall times.
+
+ // When the user calls Solve, before the actual optimization
+ // occurs, Ceres performs a number of preprocessing steps. These
+ // include error checks, memory allocations, and reorderings. This
+ // time is accounted for as preprocessing time.
+ double preprocessor_time_in_seconds = -1.0;
+
+ // Time spent in the TrustRegionMinimizer.
+ double minimizer_time_in_seconds = -1.0;
+
+ // After the Minimizer is finished, some time is spent in
+ // re-evaluating residuals etc. This time is accounted for in the
+ // postprocessor time.
+ double postprocessor_time_in_seconds = -1.0;
+
+ // Some total of all time spent inside Ceres when Solve is called.
+ double total_time_in_seconds = -1.0;
+
+ // Time (in seconds) spent in the linear solver computing the
+ // trust region step.
+ double linear_solver_time_in_seconds = -1.0;
+
+ // Number of times the Newton step was computed by solving a
+ // linear system. This does not include linear solves used by
+ // inner iterations.
+ int num_linear_solves = -1;
+
+ // Time (in seconds) spent evaluating the residual vector.
+ double residual_evaluation_time_in_seconds = 1.0;
+
+ // Number of residual only evaluations.
+ int num_residual_evaluations = -1;
+
+ // Time (in seconds) spent evaluating the jacobian matrix.
+ double jacobian_evaluation_time_in_seconds = -1.0;
+
+ // Number of Jacobian (and residual) evaluations.
+ int num_jacobian_evaluations = -1;
+
+ // Time (in seconds) spent doing inner iterations.
+ double inner_iteration_time_in_seconds = -1.0;
+
+ // Cumulative timing information for line searches performed as part of the
+ // solve. Note that in addition to the case when the Line Search minimizer
+ // is used, the Trust Region minimizer also uses a line search when
+ // solving a constrained problem.
+
+ // Time (in seconds) spent evaluating the univariate cost function as part
+ // of a line search.
+ double line_search_cost_evaluation_time_in_seconds = -1.0;
+
+ // Time (in seconds) spent evaluating the gradient of the univariate cost
+ // function as part of a line search.
+ double line_search_gradient_evaluation_time_in_seconds = -1.0;
+
+ // Time (in seconds) spent minimizing the interpolating polynomial
+ // to compute the next candidate step size as part of a line search.
+ double line_search_polynomial_minimization_time_in_seconds = -1.0;
+
+ // Total time (in seconds) spent performing line searches.
+ double line_search_total_time_in_seconds = -1.0;
+
+ // Number of parameter blocks in the problem.
+ int num_parameter_blocks = -1;
+
+ // Number of parameters in the problem.
+ int num_parameters = -1;
+
+ // Dimension of the tangent space of the problem (or the number of
+ // columns in the Jacobian for the problem). This is different
+ // from num_parameters if a parameter block is associated with a
+ // LocalParameterization
+ int num_effective_parameters = -1;
+
+ // Number of residual blocks in the problem.
+ int num_residual_blocks = -1;
+
+ // Number of residuals in the problem.
+ int num_residuals = -1;
+
+ // Number of parameter blocks in the problem after the inactive
+ // and constant parameter blocks have been removed. A parameter
+ // block is inactive if no residual block refers to it.
+ int num_parameter_blocks_reduced = -1;
+
+ // Number of parameters in the reduced problem.
+ int num_parameters_reduced = -1;
+
+ // Dimension of the tangent space of the reduced problem (or the
+ // number of columns in the Jacobian for the reduced
+ // problem). This is different from num_parameters_reduced if a
+ // parameter block in the reduced problem is associated with a
+ // LocalParameterization.
+ int num_effective_parameters_reduced = -1;
+
+ // Number of residual blocks in the reduced problem.
+ int num_residual_blocks_reduced = -1;
+
+ // Number of residuals in the reduced problem.
+ int num_residuals_reduced = -1;
+
+ // Is the reduced problem bounds constrained.
+ bool is_constrained = false;
+
+ // Number of threads specified by the user for Jacobian and
+ // residual evaluation.
+ int num_threads_given = -1;
+
+ // Number of threads actually used by the solver for Jacobian and
+ // residual evaluation. This number is not equal to
+ // num_threads_given if OpenMP is not available.
+ int num_threads_used = -1;
+
+ // Type of the linear solver requested by the user.
+ LinearSolverType linear_solver_type_given =
+#if defined(CERES_NO_SPARSE)
+ DENSE_QR;
+#else
+ SPARSE_NORMAL_CHOLESKY;
+#endif
+ // Type of the linear solver actually used. This may be different
+ // from linear_solver_type_given if Ceres determines that the
+ // problem structure is not compatible with the linear solver
+ // requested or if the linear solver requested by the user is not
+ // available, e.g. The user requested SPARSE_NORMAL_CHOLESKY but
+ // no sparse linear algebra library was available.
+ LinearSolverType linear_solver_type_used =
+#if defined(CERES_NO_SPARSE)
+ DENSE_QR;
+#else
+ SPARSE_NORMAL_CHOLESKY;
+#endif
+
+ // Size of the elimination groups given by the user as hints to
+ // the linear solver.
+ std::vector<int> linear_solver_ordering_given;
+
+ // Size of the parameter groups used by the solver when ordering
+ // the columns of the Jacobian. This maybe different from
+ // linear_solver_ordering_given if the user left
+ // linear_solver_ordering_given blank and asked for an automatic
+ // ordering, or if the problem contains some constant or inactive
+ // parameter blocks.
+ std::vector<int> linear_solver_ordering_used;
+
+ // For Schur type linear solvers, this string describes the
+ // template specialization which was detected in the problem and
+ // should be used.
+ std::string schur_structure_given;
+
+ // This is the Schur template specialization that was actually
+ // instantiated and used. The reason this will be different from
+ // schur_structure_given is because the corresponding template
+ // specialization does not exist.
+ //
+ // Template specializations can be added to ceres by editing
+ // internal/ceres/generate_template_specializations.py
+ std::string schur_structure_used;
+
+ // True if the user asked for inner iterations to be used as part
+ // of the optimization.
+ bool inner_iterations_given = false;
+
+ // True if the user asked for inner iterations to be used as part
+ // of the optimization and the problem structure was such that
+ // they were actually performed. e.g., in a problem with just one
+ // parameter block, inner iterations are not performed.
+ bool inner_iterations_used = false;
+
+ // Size of the parameter groups given by the user for performing
+ // inner iterations.
+ std::vector<int> inner_iteration_ordering_given;
+
+ // Size of the parameter groups given used by the solver for
+ // performing inner iterations. This maybe different from
+ // inner_iteration_ordering_given if the user left
+ // inner_iteration_ordering_given blank and asked for an automatic
+ // ordering, or if the problem contains some constant or inactive
+ // parameter blocks.
+ std::vector<int> inner_iteration_ordering_used;
+
+ // Type of the preconditioner requested by the user.
+ PreconditionerType preconditioner_type_given = IDENTITY;
+
+ // Type of the preconditioner actually used. This may be different
+ // from linear_solver_type_given if Ceres determines that the
+ // problem structure is not compatible with the linear solver
+ // requested or if the linear solver requested by the user is not
+ // available.
+ PreconditionerType preconditioner_type_used = IDENTITY;
+
+ // Type of clustering algorithm used for visibility based
+ // preconditioning. Only meaningful when the preconditioner_type
+ // is CLUSTER_JACOBI or CLUSTER_TRIDIAGONAL.
+ VisibilityClusteringType visibility_clustering_type = CANONICAL_VIEWS;
+
+ // Type of trust region strategy.
+ TrustRegionStrategyType trust_region_strategy_type = LEVENBERG_MARQUARDT;
+
+ // Type of dogleg strategy used for solving the trust region
+ // problem.
+ DoglegType dogleg_type = TRADITIONAL_DOGLEG;
+
+ // Type of the dense linear algebra library used.
+ DenseLinearAlgebraLibraryType dense_linear_algebra_library_type = EIGEN;
+
+ // Type of the sparse linear algebra library used.
+ SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type =
+ NO_SPARSE;
+
+ // Type of line search direction used.
+ LineSearchDirectionType line_search_direction_type = LBFGS;
+
+ // Type of the line search algorithm used.
+ LineSearchType line_search_type = WOLFE;
+
+ // When performing line search, the degree of the polynomial used
+ // to approximate the objective function.
+ LineSearchInterpolationType line_search_interpolation_type = CUBIC;
+
+ // If the line search direction is NONLINEAR_CONJUGATE_GRADIENT,
+ // then this indicates the particular variant of non-linear
+ // conjugate gradient used.
+ NonlinearConjugateGradientType nonlinear_conjugate_gradient_type =
+ FLETCHER_REEVES;
+
+ // If the type of the line search direction is LBFGS, then this
+ // indicates the rank of the Hessian approximation.
+ int max_lbfgs_rank = -1;
+ };
+
+ // Once a least squares problem has been built, this function takes
+ // the problem and optimizes it based on the values of the options
+ // parameters. Upon return, a detailed summary of the work performed
+ // by the preprocessor, the non-linear minimizer and the linear
+ // solver are reported in the summary object.
+ virtual void Solve(const Options& options,
+ Problem* problem,
+ Solver::Summary* summary);
+};
+
+// Helper function which avoids going through the interface.
+CERES_EXPORT void Solve(const Solver::Options& options, Problem* problem,
+ Solver::Summary* summary);
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_SOLVER_H_
diff --git a/include/ceres/tiny_solver.h b/include/ceres/tiny_solver.h
new file mode 100644
index 0000000..b6484fe
--- /dev/null
+++ b/include/ceres/tiny_solver.h
@@ -0,0 +1,364 @@
+// 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: mierle@gmail.com (Keir Mierle)
+//
+// WARNING WARNING WARNING
+// WARNING WARNING WARNING Tiny solver is experimental and will change.
+// WARNING WARNING WARNING
+//
+// A tiny least squares solver using Levenberg-Marquardt, intended for solving
+// small dense problems with low latency and low overhead. The implementation
+// takes care to do all allocation up front, so that no memory is allocated
+// during solving. This is especially useful when solving many similar problems;
+// for example, inverse pixel distortion for every pixel on a grid.
+//
+// Note: This code has no dependencies beyond Eigen, including on other parts of
+// Ceres, so it is possible to take this file alone and put it in another
+// project without the rest of Ceres.
+//
+// Algorithm based off of:
+//
+// [1] K. Madsen, H. Nielsen, O. Tingleoff.
+// Methods for Non-linear Least Squares Problems.
+// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
+
+#ifndef CERES_PUBLIC_TINY_SOLVER_H_
+#define CERES_PUBLIC_TINY_SOLVER_H_
+
+#include <cassert>
+#include <cmath>
+
+#include "Eigen/Dense"
+
+namespace ceres {
+
+// To use tiny solver, create a class or struct that allows computing the cost
+// function (described below). This is similar to a ceres::CostFunction, but is
+// different to enable statically allocating all memory for the solver
+// (specifically, enum sizes). Key parts are the Scalar typedef, the enums to
+// describe problem sizes (needed to remove all heap allocations), and the
+// operator() overload to evaluate the cost and (optionally) jacobians.
+//
+// struct TinySolverCostFunctionTraits {
+// typedef double Scalar;
+// enum {
+// NUM_RESIDUALS = <int> OR Eigen::Dynamic,
+// NUM_PARAMETERS = <int> OR Eigen::Dynamic,
+// };
+// bool operator()(const double* parameters,
+// double* residuals,
+// double* jacobian) const;
+//
+// int NumResiduals() const; -- Needed if NUM_RESIDUALS == Eigen::Dynamic.
+// int NumParameters() const; -- Needed if NUM_PARAMETERS == Eigen::Dynamic.
+// };
+//
+// For operator(), the size of the objects is:
+//
+// double* parameters -- NUM_PARAMETERS or NumParameters()
+// double* residuals -- NUM_RESIDUALS or NumResiduals()
+// double* jacobian -- NUM_RESIDUALS * NUM_PARAMETERS in column-major format
+// (Eigen's default); or NULL if no jacobian requested.
+//
+// An example (fully statically sized):
+//
+// struct MyCostFunctionExample {
+// typedef double Scalar;
+// enum {
+// NUM_RESIDUALS = 2,
+// NUM_PARAMETERS = 3,
+// };
+// bool operator()(const double* parameters,
+// double* residuals,
+// double* jacobian) const {
+// residuals[0] = x + 2*y + 4*z;
+// residuals[1] = y * z;
+// if (jacobian) {
+// jacobian[0 * 2 + 0] = 1; // First column (x).
+// jacobian[0 * 2 + 1] = 0;
+//
+// jacobian[1 * 2 + 0] = 2; // Second column (y).
+// jacobian[1 * 2 + 1] = z;
+//
+// jacobian[2 * 2 + 0] = 4; // Third column (z).
+// jacobian[2 * 2 + 1] = y;
+// }
+// return true;
+// }
+// };
+//
+// The solver supports either statically or dynamically sized cost
+// functions. If the number of residuals is dynamic then the Function
+// must define:
+//
+// int NumResiduals() const;
+//
+// If the number of parameters is dynamic then the Function must
+// define:
+//
+// int NumParameters() const;
+//
+template<typename Function,
+ typename LinearSolver = Eigen::LDLT<
+ Eigen::Matrix<typename Function::Scalar,
+ Function::NUM_PARAMETERS,
+ Function::NUM_PARAMETERS>>>
+class TinySolver {
+ public:
+ enum {
+ NUM_RESIDUALS = Function::NUM_RESIDUALS,
+ NUM_PARAMETERS = Function::NUM_PARAMETERS
+ };
+ typedef typename Function::Scalar Scalar;
+ typedef typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1> Parameters;
+
+ enum Status {
+ GRADIENT_TOO_SMALL, // eps > max(J'*f(x))
+ RELATIVE_STEP_SIZE_TOO_SMALL, // eps > ||dx|| / (||x|| + eps)
+ COST_TOO_SMALL, // eps > ||f(x)||^2 / 2
+ HIT_MAX_ITERATIONS,
+
+ // TODO(sameeragarwal): Deal with numerical failures.
+ };
+
+ struct Options {
+ Scalar gradient_tolerance = 1e-10; // eps > max(J'*f(x))
+ Scalar parameter_tolerance = 1e-8; // eps > ||dx|| / ||x||
+ Scalar cost_threshold = // eps > ||f(x)||
+ std::numeric_limits<Scalar>::epsilon();
+ Scalar initial_trust_region_radius = 1e4;
+ int max_num_iterations = 50;
+ };
+
+ struct Summary {
+ Scalar initial_cost = -1; // 1/2 ||f(x)||^2
+ Scalar final_cost = -1; // 1/2 ||f(x)||^2
+ Scalar gradient_max_norm = -1; // max(J'f(x))
+ int iterations = -1;
+ Status status = HIT_MAX_ITERATIONS;
+ };
+
+ bool Update(const Function& function, const Parameters &x) {
+ if (!function(x.data(), error_.data(), jacobian_.data())) {
+ return false;
+ }
+
+ error_ = -error_;
+
+ // On the first iteration, compute a diagonal (Jacobi) scaling
+ // matrix, which we store as a vector.
+ if (summary.iterations == 0) {
+ // jacobi_scaling = 1 / (1 + diagonal(J'J))
+ //
+ // 1 is added to the denominator to regularize small diagonal
+ // entries.
+ jacobi_scaling_ = 1.0 / (1.0 + jacobian_.colwise().norm().array());
+ }
+
+ // This explicitly computes the normal equations, which is numerically
+ // unstable. Nevertheless, it is often good enough and is fast.
+ //
+ // TODO(sameeragarwal): Refactor this to allow for DenseQR
+ // factorization.
+ jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal();
+ jtj_ = jacobian_.transpose() * jacobian_;
+ g_ = jacobian_.transpose() * error_;
+ summary.gradient_max_norm = g_.array().abs().maxCoeff();
+ cost_ = error_.squaredNorm() / 2;
+ return true;
+ }
+
+ const Summary& Solve(const Function& function, Parameters* x_and_min) {
+ Initialize<NUM_RESIDUALS, NUM_PARAMETERS>(function);
+ assert(x_and_min);
+ Parameters& x = *x_and_min;
+ summary = Summary();
+ summary.iterations = 0;
+
+ // TODO(sameeragarwal): Deal with failure here.
+ Update(function, x);
+ summary.initial_cost = cost_;
+ summary.final_cost = cost_;
+
+ if (summary.gradient_max_norm < options.gradient_tolerance) {
+ summary.status = GRADIENT_TOO_SMALL;
+ return summary;
+ }
+
+ if (cost_ < options.cost_threshold) {
+ summary.status = COST_TOO_SMALL;
+ return summary;
+ }
+
+ Scalar u = 1.0 / options.initial_trust_region_radius;
+ Scalar v = 2;
+
+ for (summary.iterations = 1;
+ summary.iterations < options.max_num_iterations;
+ summary.iterations++) {
+ jtj_regularized_ = jtj_;
+ const Scalar min_diagonal = 1e-6;
+ const Scalar max_diagonal = 1e32;
+ for (int i = 0; i < lm_diagonal_.rows(); ++i) {
+ lm_diagonal_[i] = std::sqrt(
+ u * std::min(std::max(jtj_(i, i), min_diagonal), max_diagonal));
+ jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i];
+ }
+
+ // TODO(sameeragarwal): Check for failure and deal with it.
+ linear_solver_.compute(jtj_regularized_);
+ lm_step_ = linear_solver_.solve(g_);
+ dx_ = jacobi_scaling_.asDiagonal() * lm_step_;
+
+ // Adding parameter_tolerance to x.norm() ensures that this
+ // works if x is near zero.
+ const Scalar parameter_tolerance =
+ options.parameter_tolerance *
+ (x.norm() + options.parameter_tolerance);
+ if (dx_.norm() < parameter_tolerance) {
+ summary.status = RELATIVE_STEP_SIZE_TOO_SMALL;
+ break;
+ }
+ x_new_ = x + dx_;
+
+ // TODO(keir): Add proper handling of errors from user eval of cost
+ // functions.
+ function(&x_new_[0], &f_x_new_[0], NULL);
+
+ const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm());
+
+ // TODO(sameeragarwal): Better more numerically stable evaluation.
+ const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_);
+
+ // rho is the ratio of the actual reduction in error to the reduction
+ // in error that would be obtained if the problem was linear. See [1]
+ // for details.
+ Scalar rho(cost_change / model_cost_change);
+ if (rho > 0) {
+ // Accept the Levenberg-Marquardt step because the linear
+ // model fits well.
+ x = x_new_;
+
+ // TODO(sameeragarwal): Deal with failure.
+ Update(function, x);
+ if (summary.gradient_max_norm < options.gradient_tolerance) {
+ summary.status = GRADIENT_TOO_SMALL;
+ break;
+ }
+
+ if (cost_ < options.cost_threshold) {
+ summary.status = COST_TOO_SMALL;
+ break;
+ }
+
+ Scalar tmp = Scalar(2 * rho - 1);
+ u = u * std::max(1 / 3., 1 - tmp * tmp * tmp);
+ v = 2;
+ continue;
+ }
+
+ // Reject the update because either the normal equations failed to solve
+ // or the local linear model was not good (rho < 0). Instead, increase u
+ // to move closer to gradient descent.
+ u *= v;
+ v *= 2;
+ }
+
+ summary.final_cost = cost_;
+ return summary;
+ }
+
+ Options options;
+ Summary summary;
+
+ private:
+ // Preallocate everything, including temporary storage needed for solving the
+ // linear system. This allows reusing the intermediate storage across solves.
+ LinearSolver linear_solver_;
+ Scalar cost_;
+ Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_;
+ Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> error_, f_x_new_;
+ Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_;
+ Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_;
+
+ // The following definitions are needed for template metaprogramming.
+ template <bool Condition, typename T>
+ struct enable_if;
+
+ template <typename T>
+ struct enable_if<true, T> {
+ typedef T type;
+ };
+
+ // The number of parameters and residuals are dynamically sized.
+ template <int R, int P>
+ typename enable_if<(R == Eigen::Dynamic && P == Eigen::Dynamic), void>::type
+ Initialize(const Function& function) {
+ Initialize(function.NumResiduals(), function.NumParameters());
+ }
+
+ // The number of parameters is dynamically sized and the number of
+ // residuals is statically sized.
+ template <int R, int P>
+ typename enable_if<(R == Eigen::Dynamic && P != Eigen::Dynamic), void>::type
+ Initialize(const Function& function) {
+ Initialize(function.NumResiduals(), P);
+ }
+
+ // The number of parameters is statically sized and the number of
+ // residuals is dynamically sized.
+ template <int R, int P>
+ typename enable_if<(R != Eigen::Dynamic && P == Eigen::Dynamic), void>::type
+ Initialize(const Function& function) {
+ Initialize(R, function.NumParameters());
+ }
+
+ // The number of parameters and residuals are statically sized.
+ template <int R, int P>
+ typename enable_if<(R != Eigen::Dynamic && P != Eigen::Dynamic), void>::type
+ Initialize(const Function& /* function */) {}
+
+ void Initialize(int num_residuals, int num_parameters) {
+ dx_.resize(num_parameters);
+ x_new_.resize(num_parameters);
+ g_.resize(num_parameters);
+ jacobi_scaling_.resize(num_parameters);
+ lm_diagonal_.resize(num_parameters);
+ lm_step_.resize(num_parameters);
+ error_.resize(num_residuals);
+ f_x_new_.resize(num_residuals);
+ jacobian_.resize(num_residuals, num_parameters);
+ jtj_.resize(num_parameters, num_parameters);
+ jtj_regularized_.resize(num_parameters, num_parameters);
+ }
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_TINY_SOLVER_H_
diff --git a/include/ceres/tiny_solver_autodiff_function.h b/include/ceres/tiny_solver_autodiff_function.h
new file mode 100644
index 0000000..cc93d73
--- /dev/null
+++ b/include/ceres/tiny_solver_autodiff_function.h
@@ -0,0 +1,206 @@
+// 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: mierle@gmail.com (Keir Mierle)
+//
+// WARNING WARNING WARNING
+// WARNING WARNING WARNING Tiny solver is experimental and will change.
+// WARNING WARNING WARNING
+
+#ifndef CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_
+#define CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_
+
+#include <memory>
+#include <type_traits>
+#include "Eigen/Core"
+
+#include "ceres/jet.h"
+#include "ceres/types.h" // For kImpossibleValue.
+
+namespace ceres {
+
+// An adapter around autodiff-style CostFunctors to enable easier use of
+// TinySolver. See the example below showing how to use it:
+//
+// // Example for cost functor with static residual size.
+// // Same as an autodiff cost functor, but taking only 1 parameter.
+// struct MyFunctor {
+// template<typename T>
+// bool operator()(const T* const parameters, T* residuals) const {
+// const T& x = parameters[0];
+// const T& y = parameters[1];
+// const T& z = parameters[2];
+// residuals[0] = x + 2.*y + 4.*z;
+// residuals[1] = y * z;
+// return true;
+// }
+// };
+//
+// typedef TinySolverAutoDiffFunction<MyFunctor, 2, 3>
+// AutoDiffFunction;
+//
+// MyFunctor my_functor;
+// AutoDiffFunction f(my_functor);
+//
+// Vec3 x = ...;
+// TinySolver<AutoDiffFunction> solver;
+// solver.Solve(f, &x);
+//
+// // Example for cost functor with dynamic residual size.
+// // NumResiduals() supplies dynamic size of residuals.
+// // Same functionality as in tiny_solver.h but with autodiff.
+// struct MyFunctorWithDynamicResiduals {
+// int NumResiduals() const {
+// return 2;
+// }
+//
+// template<typename T>
+// bool operator()(const T* const parameters, T* residuals) const {
+// const T& x = parameters[0];
+// const T& y = parameters[1];
+// const T& z = parameters[2];
+// residuals[0] = x + static_cast<T>(2.)*y + static_cast<T>(4.)*z;
+// residuals[1] = y * z;
+// return true;
+// }
+// };
+//
+// typedef TinySolverAutoDiffFunction<MyFunctorWithDynamicResiduals,
+// Eigen::Dynamic,
+// 3>
+// AutoDiffFunctionWithDynamicResiduals;
+//
+// MyFunctorWithDynamicResiduals my_functor_dyn;
+// AutoDiffFunctionWithDynamicResiduals f(my_functor_dyn);
+//
+// Vec3 x = ...;
+// TinySolver<AutoDiffFunctionWithDynamicResiduals> solver;
+// solver.Solve(f, &x);
+//
+// WARNING: The cost function adapter is not thread safe.
+template<typename CostFunctor,
+ int kNumResiduals,
+ int kNumParameters,
+ typename T = double>
+class TinySolverAutoDiffFunction {
+ public:
+ TinySolverAutoDiffFunction(const CostFunctor& cost_functor)
+ : cost_functor_(cost_functor) {
+ Initialize<kNumResiduals>(cost_functor);
+ }
+
+ typedef T Scalar;
+ enum {
+ NUM_PARAMETERS = kNumParameters,
+ NUM_RESIDUALS = kNumResiduals,
+ };
+
+ // This is similar to AutoDifferentiate(), but since there is only one
+ // parameter block it is easier to inline to avoid overhead.
+ bool operator()(const T* parameters,
+ T* residuals,
+ T* jacobian) const {
+ if (jacobian == NULL) {
+ // No jacobian requested, so just directly call the cost function with
+ // doubles, skipping jets and derivatives.
+ return cost_functor_(parameters, residuals);
+ }
+ // Initialize the input jets with passed parameters.
+ for (int i = 0; i < kNumParameters; ++i) {
+ jet_parameters_[i].a = parameters[i]; // Scalar part.
+ jet_parameters_[i].v.setZero(); // Derivative part.
+ jet_parameters_[i].v[i] = T(1.0);
+ }
+
+ // Initialize the output jets such that we can detect user errors.
+ for (int i = 0; i < num_residuals_; ++i) {
+ jet_residuals_[i].a = kImpossibleValue;
+ jet_residuals_[i].v.setConstant(kImpossibleValue);
+ }
+
+ // Execute the cost function, but with jets to find the derivative.
+ if (!cost_functor_(jet_parameters_, jet_residuals_.data())) {
+ return false;
+ }
+
+ // Copy the jacobian out of the derivative part of the residual jets.
+ Eigen::Map<Eigen::Matrix<T, kNumResiduals, kNumParameters>> jacobian_matrix(
+ jacobian,
+ num_residuals_,
+ kNumParameters);
+ for (int r = 0; r < num_residuals_; ++r) {
+ residuals[r] = jet_residuals_[r].a;
+ // Note that while this looks like a fast vectorized write, in practice it
+ // unfortunately thrashes the cache since the writes to the column-major
+ // jacobian are strided (e.g. rows are non-contiguous).
+ jacobian_matrix.row(r) = jet_residuals_[r].v;
+ }
+ return true;
+ }
+
+ int NumResiduals() const {
+ return num_residuals_; // Set by Initialize.
+ }
+
+ private:
+ const CostFunctor& cost_functor_;
+
+ // The number of residuals at runtime.
+ // This will be overriden if NUM_RESIDUALS == Eigen::Dynamic.
+ int num_residuals_ = kNumResiduals;
+
+ // To evaluate the cost function with jets, temporary storage is needed. These
+ // are the buffers that are used during evaluation; parameters for the input,
+ // and jet_residuals_ are where the final cost and derivatives end up.
+ //
+ // Since this buffer is used for evaluation, the adapter is not thread safe.
+ using JetType = Jet<T, kNumParameters>;
+ mutable JetType jet_parameters_[kNumParameters];
+ // Eigen::Matrix serves as static or dynamic container.
+ mutable Eigen::Matrix<JetType, kNumResiduals, 1> jet_residuals_;
+
+ // The number of residuals is dynamically sized and the number of
+ // parameters is statically sized.
+ template<int R>
+ typename std::enable_if<(R == Eigen::Dynamic), void>::type Initialize(
+ const CostFunctor& function) {
+ jet_residuals_.resize(function.NumResiduals());
+ num_residuals_ = function.NumResiduals();
+ }
+
+ // The number of parameters and residuals are statically sized.
+ template<int R>
+ typename std::enable_if<(R != Eigen::Dynamic), void>::type Initialize(
+ const CostFunctor& /* function */) {
+ num_residuals_ = kNumResiduals;
+ }
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_
diff --git a/include/ceres/tiny_solver_cost_function_adapter.h b/include/ceres/tiny_solver_cost_function_adapter.h
new file mode 100644
index 0000000..d44bdeb
--- /dev/null
+++ b/include/ceres/tiny_solver_cost_function_adapter.h
@@ -0,0 +1,137 @@
+// 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)
+
+
+#ifndef CERES_PUBLIC_TINY_SOLVER_COST_FUNCTION_ADAPTER_H_
+#define CERES_PUBLIC_TINY_SOLVER_COST_FUNCTION_ADAPTER_H_
+
+#include "Eigen/Core"
+#include "ceres/cost_function.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// An adapter class that lets users of TinySolver use
+// ceres::CostFunction objects that have exactly one parameter block.
+//
+// The adapter allows for the number of residuals and the size of the
+// parameter block to be specified at compile or run-time.
+//
+// WARNING: This object is not thread-safe.
+//
+// Example usage:
+//
+// CostFunction* cost_function = ...
+//
+// Number of residuals and parameter block size known at compile time:
+//
+// TinySolverCostFunctionAdapter<kNumResiduals, kNumParameters>
+// cost_function_adapter(*cost_function);
+//
+// Number of residuals known at compile time and the parameter block
+// size not known at compile time.
+//
+// TinySolverCostFunctionAdapter<kNumResiduals, Eigen::Dynamic>
+// cost_function_adapter(*cost_function);
+//
+// Number of residuals not known at compile time and the parameter
+// block size known at compile time.
+//
+// TinySolverCostFunctionAdapter<Eigen::Dynamic, kParameterBlockSize>
+// cost_function_adapter(*cost_function);
+//
+// Number of residuals not known at compile time and the parameter
+// block size not known at compile time.
+//
+// TinySolverCostFunctionAdapter cost_function_adapter(*cost_function);
+//
+template <int kNumResiduals = Eigen::Dynamic, int kNumParameters = Eigen::Dynamic>
+class TinySolverCostFunctionAdapter {
+ public:
+ typedef double Scalar;
+ enum ComponentSizeType {
+ NUM_PARAMETERS = kNumParameters,
+ NUM_RESIDUALS = kNumResiduals
+ };
+
+ TinySolverCostFunctionAdapter(const CostFunction& cost_function)
+ : cost_function_(cost_function) {
+ CHECK_EQ(cost_function_.parameter_block_sizes().size(), 1)
+ << "Only CostFunctions with exactly one parameter blocks are allowed.";
+
+ const int parameter_block_size = cost_function_.parameter_block_sizes()[0];
+ if (NUM_PARAMETERS == Eigen::Dynamic || NUM_RESIDUALS == Eigen::Dynamic) {
+ if (NUM_RESIDUALS != Eigen::Dynamic) {
+ CHECK_EQ(cost_function_.num_residuals(), NUM_RESIDUALS);
+ }
+ if (NUM_PARAMETERS != Eigen::Dynamic) {
+ CHECK_EQ(parameter_block_size, NUM_PARAMETERS);
+ }
+
+ row_major_jacobian_.resize(cost_function_.num_residuals(),
+ parameter_block_size);
+ }
+ }
+
+ bool operator()(const double* parameters,
+ double* residuals,
+ double* jacobian) const {
+ if (!jacobian) {
+ return cost_function_.Evaluate(¶meters, residuals, NULL);
+ }
+
+ double* jacobians[1] = {row_major_jacobian_.data()};
+ if (!cost_function_.Evaluate(¶meters, residuals, jacobians)) {
+ return false;
+ }
+
+ // The Function object used by TinySolver takes its Jacobian in a
+ // column-major layout, and the CostFunction objects use row-major
+ // Jacobian matrices. So the following bit of code does the
+ // conversion from row-major Jacobians to column-major Jacobians.
+ Eigen::Map<Eigen::Matrix<double, NUM_RESIDUALS, NUM_PARAMETERS>>
+ col_major_jacobian(jacobian, NumResiduals(), NumParameters());
+ col_major_jacobian = row_major_jacobian_;
+ return true;
+ }
+
+ int NumResiduals() const { return cost_function_.num_residuals(); }
+ int NumParameters() const {
+ return cost_function_.parameter_block_sizes()[0];
+ }
+
+ const CostFunction& cost_function_;
+ mutable Eigen::Matrix<double, NUM_RESIDUALS, NUM_PARAMETERS, Eigen::RowMajor>
+ row_major_jacobian_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_TINY_SOLVER_COST_FUNCTION_ADAPTER_H_
diff --git a/include/ceres/types.h b/include/ceres/types.h
new file mode 100644
index 0000000..8e5da6a
--- /dev/null
+++ b/include/ceres/types.h
@@ -0,0 +1,513 @@
+// 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)
+//
+// Enums and other top level class definitions.
+//
+// Note: internal/types.cc defines stringification routines for some
+// of these enums. Please update those routines if you extend or
+// remove enums from here.
+
+#ifndef CERES_PUBLIC_TYPES_H_
+#define CERES_PUBLIC_TYPES_H_
+
+#include <string>
+
+#include "ceres/internal/port.h"
+#include "ceres/internal/disable_warnings.h"
+
+namespace ceres {
+
+// Argument type used in interfaces that can optionally take ownership
+// of a passed in argument. If TAKE_OWNERSHIP is passed, the called
+// object takes ownership of the pointer argument, and will call
+// delete on it upon completion.
+enum Ownership {
+ DO_NOT_TAKE_OWNERSHIP,
+ TAKE_OWNERSHIP
+};
+
+// TODO(keir): Considerably expand the explanations of each solver type.
+enum LinearSolverType {
+ // These solvers are for general rectangular systems formed from the
+ // normal equations A'A x = A'b. They are direct solvers and do not
+ // assume any special problem structure.
+
+ // Solve the normal equations using a dense Cholesky solver; based
+ // on Eigen.
+ DENSE_NORMAL_CHOLESKY,
+
+ // Solve the normal equations using a dense QR solver; based on
+ // Eigen.
+ DENSE_QR,
+
+ // Solve the normal equations using a sparse cholesky solver; requires
+ // SuiteSparse or CXSparse.
+ SPARSE_NORMAL_CHOLESKY,
+
+ // Specialized solvers, specific to problems with a generalized
+ // bi-partitite structure.
+
+ // Solves the reduced linear system using a dense Cholesky solver;
+ // based on Eigen.
+ DENSE_SCHUR,
+
+ // Solves the reduced linear system using a sparse Cholesky solver;
+ // based on CHOLMOD.
+ SPARSE_SCHUR,
+
+ // Solves the reduced linear system using Conjugate Gradients, based
+ // on a new Ceres implementation. Suitable for large scale
+ // problems.
+ ITERATIVE_SCHUR,
+
+ // Conjugate gradients on the normal equations.
+ CGNR
+};
+
+enum PreconditionerType {
+ // Trivial preconditioner - the identity matrix.
+ IDENTITY,
+
+ // Block diagonal of the Gauss-Newton Hessian.
+ JACOBI,
+
+ // Note: The following three preconditioners can only be used with
+ // the ITERATIVE_SCHUR solver. They are well suited for Structure
+ // from Motion problems.
+
+ // Block diagonal of the Schur complement. This preconditioner may
+ // only be used with the ITERATIVE_SCHUR solver.
+ SCHUR_JACOBI,
+
+ // Visibility clustering based preconditioners.
+ //
+ // The following two preconditioners use the visibility structure of
+ // the scene to determine the sparsity structure of the
+ // preconditioner. This is done using a clustering algorithm. The
+ // available visibility clustering algorithms are described below.
+ //
+ // Note: Requires SuiteSparse.
+ CLUSTER_JACOBI,
+ CLUSTER_TRIDIAGONAL
+};
+
+enum VisibilityClusteringType {
+ // Canonical views algorithm as described in
+ //
+ // "Scene Summarization for Online Image Collections", Ian Simon, Noah
+ // Snavely, Steven M. Seitz, ICCV 2007.
+ //
+ // This clustering algorithm can be quite slow, but gives high
+ // quality clusters. The original visibility based clustering paper
+ // used this algorithm.
+ CANONICAL_VIEWS,
+
+ // The classic single linkage algorithm. It is extremely fast as
+ // compared to CANONICAL_VIEWS, but can give slightly poorer
+ // results. For problems with large number of cameras though, this
+ // is generally a pretty good option.
+ //
+ // If you are using SCHUR_JACOBI preconditioner and have SuiteSparse
+ // available, CLUSTER_JACOBI and CLUSTER_TRIDIAGONAL in combination
+ // with the SINGLE_LINKAGE algorithm will generally give better
+ // results.
+ SINGLE_LINKAGE
+};
+
+enum SparseLinearAlgebraLibraryType {
+ // High performance sparse Cholesky factorization and approximate
+ // minimum degree ordering.
+ SUITE_SPARSE,
+
+ // A lightweight replacement for SuiteSparse, which does not require
+ // a LAPACK/BLAS implementation. Consequently, its performance is
+ // also a bit lower than SuiteSparse.
+ CX_SPARSE,
+
+ // Eigen's sparse linear algebra routines. In particular Ceres uses
+ // the Simplicial LDLT routines.
+ EIGEN_SPARSE,
+
+ // Apple's Accelerate framework sparse linear algebra routines.
+ ACCELERATE_SPARSE,
+
+ // No sparse linear solver should be used. This does not necessarily
+ // imply that Ceres was built without any sparse library, although that
+ // is the likely use case, merely that one should not be used.
+ NO_SPARSE
+};
+
+enum DenseLinearAlgebraLibraryType {
+ EIGEN,
+ LAPACK
+};
+
+// Logging options
+// The options get progressively noisier.
+enum LoggingType {
+ SILENT,
+ PER_MINIMIZER_ITERATION
+};
+
+enum MinimizerType {
+ LINE_SEARCH,
+ TRUST_REGION
+};
+
+enum LineSearchDirectionType {
+ // Negative of the gradient.
+ STEEPEST_DESCENT,
+
+ // A generalization of the Conjugate Gradient method to non-linear
+ // functions. The generalization can be performed in a number of
+ // different ways, resulting in a variety of search directions. The
+ // precise choice of the non-linear conjugate gradient algorithm
+ // used is determined by NonlinerConjuateGradientType.
+ NONLINEAR_CONJUGATE_GRADIENT,
+
+ // BFGS, and it's limited memory approximation L-BFGS, are quasi-Newton
+ // algorithms that approximate the Hessian matrix by iteratively refining
+ // an initial estimate with rank-one updates using the gradient at each
+ // iteration. They are a generalisation of the Secant method and satisfy
+ // the Secant equation. The Secant equation has an infinium of solutions
+ // in multiple dimensions, as there are N*(N+1)/2 degrees of freedom in a
+ // symmetric matrix but only N conditions are specified by the Secant
+ // equation. The requirement that the Hessian approximation be positive
+ // definite imposes another N additional constraints, but that still leaves
+ // remaining degrees-of-freedom. (L)BFGS methods uniquely determine the
+ // approximate Hessian by imposing the additional constraints that the
+ // approximation at the next iteration must be the 'closest' to the current
+ // approximation (the nature of how this proximity is measured is actually
+ // the defining difference between a family of quasi-Newton methods including
+ // (L)BFGS & DFP). (L)BFGS is currently regarded as being the best known
+ // general quasi-Newton method.
+ //
+ // The principal difference between BFGS and L-BFGS is that whilst BFGS
+ // maintains a full, dense approximation to the (inverse) Hessian, L-BFGS
+ // maintains only a window of the last M observations of the parameters and
+ // gradients. Using this observation history, the calculation of the next
+ // search direction can be computed without requiring the construction of the
+ // full dense inverse Hessian approximation. This is particularly important
+ // for problems with a large number of parameters, where storage of an N-by-N
+ // matrix in memory would be prohibitive.
+ //
+ // For more details on BFGS see:
+ //
+ // Broyden, C.G., "The Convergence of a Class of Double-rank Minimization
+ // Algorithms,"; J. Inst. Maths. Applics., Vol. 6, pp 76–90, 1970.
+ //
+ // Fletcher, R., "A New Approach to Variable Metric Algorithms,"
+ // Computer Journal, Vol. 13, pp 317–322, 1970.
+ //
+ // Goldfarb, D., "A Family of Variable Metric Updates Derived by Variational
+ // Means," Mathematics of Computing, Vol. 24, pp 23–26, 1970.
+ //
+ // Shanno, D.F., "Conditioning of Quasi-Newton Methods for Function
+ // Minimization," Mathematics of Computing, Vol. 24, pp 647–656, 1970.
+ //
+ // For more details on L-BFGS see:
+ //
+ // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with Limited
+ // Storage". Mathematics of Computation 35 (151): 773–782.
+ //
+ // Byrd, R. H.; Nocedal, J.; Schnabel, R. B. (1994).
+ // "Representations of Quasi-Newton Matrices and their use in
+ // Limited Memory Methods". Mathematical Programming 63 (4):
+ // 129–156.
+ //
+ // A general reference for both methods:
+ //
+ // Nocedal J., Wright S., Numerical Optimization, 2nd Ed. Springer, 1999.
+ LBFGS,
+ BFGS,
+};
+
+// Nonlinear conjugate gradient methods are a generalization of the
+// method of Conjugate Gradients for linear systems. The
+// generalization can be carried out in a number of different ways
+// leading to number of different rules for computing the search
+// direction. Ceres provides a number of different variants. For more
+// details see Numerical Optimization by Nocedal & Wright.
+enum NonlinearConjugateGradientType {
+ FLETCHER_REEVES,
+ POLAK_RIBIERE,
+ HESTENES_STIEFEL,
+};
+
+enum LineSearchType {
+ // Backtracking line search with polynomial interpolation or
+ // bisection.
+ ARMIJO,
+ WOLFE,
+};
+
+// Ceres supports different strategies for computing the trust region
+// step.
+enum TrustRegionStrategyType {
+ // The default trust region strategy is to use the step computation
+ // used in the Levenberg-Marquardt algorithm. For more details see
+ // levenberg_marquardt_strategy.h
+ LEVENBERG_MARQUARDT,
+
+ // Powell's dogleg algorithm interpolates between the Cauchy point
+ // and the Gauss-Newton step. It is particularly useful if the
+ // LEVENBERG_MARQUARDT algorithm is making a large number of
+ // unsuccessful steps. For more details see dogleg_strategy.h.
+ //
+ // NOTES:
+ //
+ // 1. This strategy has not been experimented with or tested as
+ // extensively as LEVENBERG_MARQUARDT, and therefore it should be
+ // considered EXPERIMENTAL for now.
+ //
+ // 2. For now this strategy should only be used with exact
+ // factorization based linear solvers, i.e., SPARSE_SCHUR,
+ // DENSE_SCHUR, DENSE_QR and SPARSE_NORMAL_CHOLESKY.
+ DOGLEG
+};
+
+// Ceres supports two different dogleg strategies.
+// The "traditional" dogleg method by Powell and the
+// "subspace" method described in
+// R. H. Byrd, R. B. Schnabel, and G. A. Shultz,
+// "Approximate solution of the trust region problem by minimization
+// over two-dimensional subspaces", Mathematical Programming,
+// 40 (1988), pp. 247--263
+enum DoglegType {
+ // The traditional approach constructs a dogleg path
+ // consisting of two line segments and finds the furthest
+ // point on that path that is still inside the trust region.
+ TRADITIONAL_DOGLEG,
+
+ // The subspace approach finds the exact minimum of the model
+ // constrained to the subspace spanned by the dogleg path.
+ SUBSPACE_DOGLEG
+};
+
+enum TerminationType {
+ // Minimizer terminated because one of the convergence criterion set
+ // by the user was satisfied.
+ //
+ // 1. (new_cost - old_cost) < function_tolerance * old_cost;
+ // 2. max_i |gradient_i| < gradient_tolerance
+ // 3. |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance)
+ //
+ // The user's parameter blocks will be updated with the solution.
+ CONVERGENCE,
+
+ // The solver ran for maximum number of iterations or maximum amount
+ // of time specified by the user, but none of the convergence
+ // criterion specified by the user were met. The user's parameter
+ // blocks will be updated with the solution found so far.
+ NO_CONVERGENCE,
+
+ // The minimizer terminated because of an error. The user's
+ // parameter blocks will not be updated.
+ FAILURE,
+
+ // Using an IterationCallback object, user code can control the
+ // minimizer. The following enums indicate that the user code was
+ // responsible for termination.
+ //
+ // Minimizer terminated successfully because a user
+ // IterationCallback returned SOLVER_TERMINATE_SUCCESSFULLY.
+ //
+ // The user's parameter blocks will be updated with the solution.
+ USER_SUCCESS,
+
+ // Minimizer terminated because because a user IterationCallback
+ // returned SOLVER_ABORT.
+ //
+ // The user's parameter blocks will not be updated.
+ USER_FAILURE
+};
+
+// Enums used by the IterationCallback instances to indicate to the
+// solver whether it should continue solving, the user detected an
+// error or the solution is good enough and the solver should
+// terminate.
+enum CallbackReturnType {
+ // Continue solving to next iteration.
+ SOLVER_CONTINUE,
+
+ // Terminate solver, and do not update the parameter blocks upon
+ // return. Unless the user has set
+ // Solver:Options:::update_state_every_iteration, in which case the
+ // state would have been updated every iteration
+ // anyways. Solver::Summary::termination_type is set to USER_ABORT.
+ SOLVER_ABORT,
+
+ // Terminate solver, update state and
+ // return. Solver::Summary::termination_type is set to USER_SUCCESS.
+ SOLVER_TERMINATE_SUCCESSFULLY
+};
+
+// The format in which linear least squares problems should be logged
+// when Solver::Options::lsqp_iterations_to_dump is non-empty.
+enum DumpFormatType {
+ // Print the linear least squares problem in a human readable format
+ // to stderr. The Jacobian is printed as a dense matrix. The vectors
+ // D, x and f are printed as dense vectors. This should only be used
+ // for small problems.
+ CONSOLE,
+
+ // Write out the linear least squares problem to the directory
+ // pointed to by Solver::Options::lsqp_dump_directory as text files
+ // which can be read into MATLAB/Octave. The Jacobian is dumped as a
+ // text file containing (i,j,s) triplets, the vectors D, x and f are
+ // dumped as text files containing a list of their values.
+ //
+ // A MATLAB/octave script called lm_iteration_???.m is also output,
+ // which can be used to parse and load the problem into memory.
+ TEXTFILE
+};
+
+// For SizedCostFunction and AutoDiffCostFunction, DYNAMIC can be
+// specified for the number of residuals. If specified, then the
+// number of residuas for that cost function can vary at runtime.
+enum DimensionType {
+ DYNAMIC = -1
+};
+
+// The differentiation method used to compute numerical derivatives in
+// NumericDiffCostFunction and DynamicNumericDiffCostFunction.
+enum NumericDiffMethodType {
+ // Compute central finite difference: f'(x) ~ (f(x+h) - f(x-h)) / 2h.
+ CENTRAL,
+
+ // Compute forward finite difference: f'(x) ~ (f(x+h) - f(x)) / h.
+ FORWARD,
+
+ // Adaptive numerical differentiation using Ridders' method. Provides more
+ // accurate and robust derivatives at the expense of additional cost
+ // function evaluations.
+ RIDDERS
+};
+
+enum LineSearchInterpolationType {
+ BISECTION,
+ QUADRATIC,
+ CUBIC
+};
+
+enum CovarianceAlgorithmType {
+ DENSE_SVD,
+ SPARSE_QR,
+};
+
+// It is a near impossibility that user code generates this exact
+// value in normal operation, thus we will use it to fill arrays
+// before passing them to user code. If on return an element of the
+// array still contains this value, we will assume that the user code
+// did not write to that memory location.
+const double kImpossibleValue = 1e302;
+
+CERES_EXPORT const char* LinearSolverTypeToString(
+ LinearSolverType type);
+CERES_EXPORT bool StringToLinearSolverType(std::string value,
+ LinearSolverType* type);
+
+CERES_EXPORT const char* PreconditionerTypeToString(PreconditionerType type);
+CERES_EXPORT bool StringToPreconditionerType(std::string value,
+ PreconditionerType* type);
+
+CERES_EXPORT const char* VisibilityClusteringTypeToString(
+ VisibilityClusteringType type);
+CERES_EXPORT bool StringToVisibilityClusteringType(std::string value,
+ VisibilityClusteringType* type);
+
+CERES_EXPORT const char* SparseLinearAlgebraLibraryTypeToString(
+ SparseLinearAlgebraLibraryType type);
+CERES_EXPORT bool StringToSparseLinearAlgebraLibraryType(
+ std::string value,
+ SparseLinearAlgebraLibraryType* type);
+
+CERES_EXPORT const char* DenseLinearAlgebraLibraryTypeToString(
+ DenseLinearAlgebraLibraryType type);
+CERES_EXPORT bool StringToDenseLinearAlgebraLibraryType(
+ std::string value,
+ DenseLinearAlgebraLibraryType* type);
+
+CERES_EXPORT const char* TrustRegionStrategyTypeToString(
+ TrustRegionStrategyType type);
+CERES_EXPORT bool StringToTrustRegionStrategyType(std::string value,
+ TrustRegionStrategyType* type);
+
+CERES_EXPORT const char* DoglegTypeToString(DoglegType type);
+CERES_EXPORT bool StringToDoglegType(std::string value, DoglegType* type);
+
+CERES_EXPORT const char* MinimizerTypeToString(MinimizerType type);
+CERES_EXPORT bool StringToMinimizerType(std::string value, MinimizerType* type);
+
+CERES_EXPORT const char* LineSearchDirectionTypeToString(
+ LineSearchDirectionType type);
+CERES_EXPORT bool StringToLineSearchDirectionType(std::string value,
+ LineSearchDirectionType* type);
+
+CERES_EXPORT const char* LineSearchTypeToString(LineSearchType type);
+CERES_EXPORT bool StringToLineSearchType(std::string value, LineSearchType* type);
+
+CERES_EXPORT const char* NonlinearConjugateGradientTypeToString(
+ NonlinearConjugateGradientType type);
+CERES_EXPORT bool StringToNonlinearConjugateGradientType(
+ std::string value,
+ NonlinearConjugateGradientType* type);
+
+CERES_EXPORT const char* LineSearchInterpolationTypeToString(
+ LineSearchInterpolationType type);
+CERES_EXPORT bool StringToLineSearchInterpolationType(
+ std::string value,
+ LineSearchInterpolationType* type);
+
+CERES_EXPORT const char* CovarianceAlgorithmTypeToString(
+ CovarianceAlgorithmType type);
+CERES_EXPORT bool StringToCovarianceAlgorithmType(
+ std::string value,
+ CovarianceAlgorithmType* type);
+
+CERES_EXPORT const char* NumericDiffMethodTypeToString(
+ NumericDiffMethodType type);
+CERES_EXPORT bool StringToNumericDiffMethodType(
+ std::string value,
+ NumericDiffMethodType* type);
+
+CERES_EXPORT const char* TerminationTypeToString(TerminationType type);
+
+CERES_EXPORT bool IsSchurType(LinearSolverType type);
+CERES_EXPORT bool IsSparseLinearAlgebraLibraryTypeAvailable(
+ SparseLinearAlgebraLibraryType type);
+CERES_EXPORT bool IsDenseLinearAlgebraLibraryTypeAvailable(
+ DenseLinearAlgebraLibraryType type);
+
+} // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif // CERES_PUBLIC_TYPES_H_
diff --git a/include/ceres/version.h b/include/ceres/version.h
new file mode 100644
index 0000000..20fbe22
--- /dev/null
+++ b/include/ceres/version.h
@@ -0,0 +1,48 @@
+// 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)
+
+#ifndef CERES_PUBLIC_VERSION_H_
+#define CERES_PUBLIC_VERSION_H_
+
+#define CERES_VERSION_MAJOR 2
+#define CERES_VERSION_MINOR 0
+#define CERES_VERSION_REVISION 0
+
+// Classic CPP stringifcation; the extra level of indirection allows the
+// preprocessor to expand the macro before being converted to a string.
+#define CERES_TO_STRING_HELPER(x) #x
+#define CERES_TO_STRING(x) CERES_TO_STRING_HELPER(x)
+
+// The Ceres version as a string; for example "1.9.0".
+#define CERES_VERSION_STRING CERES_TO_STRING(CERES_VERSION_MAJOR) "." \
+ CERES_TO_STRING(CERES_VERSION_MINOR) "." \
+ CERES_TO_STRING(CERES_VERSION_REVISION)
+
+#endif // CERES_PUBLIC_VERSION_H_