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/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_