ceres-solver-v2

.gitattributes

@@ -50,3 +50,4 @@ colmap/lib/libpba.a filter=lfs diff=lfs merge=lfs -text
 colmap/lib/libpoisson_recon.a filter=lfs diff=lfs merge=lfs -text
 colmap/lib/libsift_gpu.a filter=lfs diff=lfs merge=lfs -text
 ceres/lib/libceres.a filter=lfs diff=lfs merge=lfs -text
+ceres-v2/lib/libceres.a filter=lfs diff=lfs merge=lfs -text

ceres-v2/include/autodiff_cost_function.h

@@ -0,0 +1,228 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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'y)^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 final
+    : public SizedCostFunction<kNumResiduals, Ns...> {
+ public:
+  // Takes ownership of functor by default. Uses the template-provided
+  // value for the number of residuals ("kNumResiduals").
+  explicit AutoDiffCostFunction(CostFunctor* functor,
+                                Ownership ownership = TAKE_OWNERSHIP)
+      : functor_(functor), ownership_(ownership) {
+    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 by default. 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,
+                       Ownership ownership = TAKE_OWNERSHIP)
+      : functor_(functor), ownership_(ownership) {
+    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);
+  }
+
+  AutoDiffCostFunction(AutoDiffCostFunction&& other)
+      : functor_(std::move(other.functor_)), ownership_(other.ownership_) {}
+
+  virtual ~AutoDiffCostFunction() {
+    // Manually release pointer if configured to not take ownership rather than
+    // deleting only if ownership is taken.
+    // This is to stay maximally compatible to old user code which may have
+    // forgotten to implement a virtual destructor, from when the
+    // AutoDiffCostFunction always took ownership.
+    if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
+      functor_.release();
+    }
+  }
+
+  // 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.
+  bool Evaluate(double const* const* parameters,
+                double* residuals,
+                double** jacobians) const override {
+    using ParameterDims =
+        typename SizedCostFunction<kNumResiduals, Ns...>::ParameterDims;
+
+    if (!jacobians) {
+      return internal::VariadicEvaluate<ParameterDims>(
+          *functor_, parameters, residuals);
+    }
+    return internal::AutoDifferentiate<kNumResiduals, ParameterDims>(
+        *functor_,
+        parameters,
+        SizedCostFunction<kNumResiduals, Ns...>::num_residuals(),
+        residuals,
+        jacobians);
+  };
+
+  const CostFunctor& functor() const { return *functor_; }
+
+ private:
+  std::unique_ptr<CostFunctor> functor_;
+  Ownership ownership_;
+};
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_

ceres-v2/include/autodiff_first_order_function.h

@@ -0,0 +1,151 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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_AUTODIFF_FIRST_ORDER_FUNCTION_H_
+#define CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_
+
+#include <memory>
+
+#include "ceres/first_order_function.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/jet.h"
+#include "ceres/types.h"
+
+namespace ceres {
+
+// Create FirstOrderFunctions as needed by the GradientProblem
+// framework, with gradients 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 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.
+//
+// For example, consider a scalar error e = x'y - a, where both x and y are
+// two-dimensional column vector parameters, the prime sign indicates
+// transposition, and a is a constant.
+//
+// To write an auto-differentiable FirstOrderFunction for the above model, first
+// define the object
+//
+//  class QuadraticCostFunctor {
+//   public:
+//    explicit QuadraticCostFunctor(double a) : a_(a) {}
+//    template <typename T>
+//    bool operator()(const T* const xy, T* cost) const {
+//      const T* const x = xy;
+//      const T* const y = xy + 2;
+//      *cost = x[0] * y[0] + x[1] * y[1] - T(a_);
+//      return true;
+//    }
+//
+//   private:
+//    double a_;
+//  };
+//
+// Note that in the declaration of operator() the input parameters xy come
+// first, and are passed as const pointers to arrays of T. The
+// output is the last parameter.
+//
+// Then given this class definition, the auto differentiated FirstOrderFunction
+// for it can be constructed as follows.
+//
+//    FirstOrderFunction* function =
+//      new AutoDiffFirstOrderFunction<QuadraticCostFunctor, 4>(
+//          new QuadraticCostFunctor(1.0)));
+//
+// In the instantiation above, the template parameters following
+// "QuadraticCostFunctor", "4", describe the functor as computing a
+// 1-dimensional output from a four dimensional vector.
+//
+// WARNING: 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 a_ directly, a_ is wrapped with T(a_).
+
+template <typename FirstOrderFunctor, int kNumParameters>
+class AutoDiffFirstOrderFunction final : public FirstOrderFunction {
+ public:
+  // Takes ownership of functor.
+  explicit AutoDiffFirstOrderFunction(FirstOrderFunctor* functor)
+      : functor_(functor) {
+    static_assert(kNumParameters > 0, "kNumParameters must be positive");
+  }
+
+  bool Evaluate(const double* const parameters,
+                double* cost,
+                double* gradient) const override {
+    if (gradient == nullptr) {
+      return (*functor_)(parameters, cost);
+    }
+
+    using JetT = Jet<double, kNumParameters>;
+    internal::FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(kNumParameters);
+    for (int i = 0; i < kNumParameters; ++i) {
+      x[i].a = parameters[i];
+      x[i].v.setZero();
+      x[i].v[i] = 1.0;
+    }
+
+    JetT output;
+    output.a = kImpossibleValue;
+    output.v.setConstant(kImpossibleValue);
+
+    if (!(*functor_)(x.data(), &output)) {
+      return false;
+    }
+
+    *cost = output.a;
+    VectorRef(gradient, kNumParameters) = output.v;
+    return true;
+  }
+
+  int NumParameters() const override { return kNumParameters; }
+
+  const FirstOrderFunctor& functor() const { return *functor_; }
+
+ private:
+  std::unique_ptr<FirstOrderFunctor> functor_;
+};
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_

ceres-v2/include/autodiff_local_parameterization.h

@@ -0,0 +1,158 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/internal/autodiff.h"
+#include "ceres/local_parameterization.h"
+
+namespace ceres {
+
+// WARNING: LocalParameterizations are deprecated, so is
+// AutoDiffLocalParameterization. They will be removed from Ceres Solver in
+// version 2.2.0. Please use Manifolds and AutoDiffManifold instead.
+
+// 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 CERES_DEPRECATED_WITH_MSG("Use AutoDiffManifold instead.")
+    AutoDiffLocalParameterization : public LocalParameterization {
+ public:
+  AutoDiffLocalParameterization() : functor_(new Functor()) {}
+
+  // Takes ownership of functor.
+  explicit AutoDiffLocalParameterization(Functor* functor)
+      : functor_(functor) {}
+
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override {
+    return (*functor_)(x, delta, x_plus_delta);
+  }
+
+  bool ComputeJacobian(const double* x, double* jacobian) const override {
+    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] = {nullptr, jacobian};
+    return internal::AutoDifferentiate<
+        kGlobalSize,
+        internal::StaticParameterDims<kGlobalSize, kLocalSize>>(
+        *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs);
+  }
+
+  int GlobalSize() const override { return kGlobalSize; }
+  int LocalSize() const override { return kLocalSize; }
+
+  const Functor& functor() const { return *functor_; }
+
+ private:
+  std::unique_ptr<Functor> functor_;
+};
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_

ceres-v2/include/autodiff_manifold.h

@@ -0,0 +1,259 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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_AUTODIFF_MANIFOLD_H_
+#define CERES_PUBLIC_AUTODIFF_MANIFOLD_H_
+
+#include <memory>
+
+#include "ceres/internal/autodiff.h"
+#include "ceres/manifold.h"
+
+namespace ceres {
+
+// Create a Manifold with Jacobians computed via automatic differentiation. For
+// more information on manifolds, see include/ceres/manifold.h
+//
+// To get an auto differentiated manifold, you must define a class/struct with
+// templated Plus and Minus functions that compute
+//
+//   x_plus_delta = Plus(x, delta);
+//   y_minus_x    = Minus(y, x);
+//
+// Where, x, y and x_plus_y are vectors on the manifold in the ambient space (so
+// they are kAmbientSize vectors) and delta, y_minus_x are vectors in the
+// tangent space (so they are kTangentSize vectors).
+//
+// The Functor should have the signature:
+//
+// struct Functor {
+//   template <typename T>
+//   bool Plus(const T* x, const T* delta, T* x_plus_delta) const;
+//
+//   template <typename T>
+//   bool Minus(const T* y, const T* x, T* y_minus_x) const;
+// };
+//
+// Observe that the Plus and Minus operations are templated on the parameter T.
+// The autodiff framework substitutes appropriate "Jet" objects for T in order
+// to compute the derivative when necessary. This is the same mechanism that is
+// used to compute derivatives when using AutoDiffCostFunction.
+//
+// Plus and Minus should return true if the computation is successful and false
+// otherwise, in which case the result will not be used.
+//
+// Given this Functor, the corresponding Manifold can be constructed as:
+//
+// AutoDiffManifold<Functor, kAmbientSize, kTangentSize> manifold;
+//
+// As a concrete example consider the case of Quaternions. Quaternions form a
+// three dimensional manifold embedded in R^4, i.e. they have an ambient
+// dimension of 4 and their tangent space has dimension 3. The following Functor
+// (taken from autodiff_manifold_test.cc) defines the Plus and Minus operations
+// on the Quaternion manifold:
+//
+// NOTE: The following is only used for illustration purposes. Ceres Solver
+// ships with optimized production grade QuaternionManifold implementation. See
+// manifold.h.
+//
+// This functor assumes that the quaternions are laid out as [w,x,y,z] in
+// memory, i.e. the real or scalar part is the first coordinate.
+//
+// struct QuaternionFunctor {
+//   template <typename T>
+//   bool Plus(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;
+//   }
+//
+//   template <typename T>
+//   bool Minus(const T* y, const T* x, T* y_minus_x) const {
+//     T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
+//     T ambient_y_minus_x[4];
+//     QuaternionProduct(y, minus_x, ambient_y_minus_x);
+//     T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] +
+//                     ambient_y_minus_x[2] * ambient_y_minus_x[2] +
+//                     ambient_y_minus_x[3] * ambient_y_minus_x[3]);
+//     if (u_norm > 0.0) {
+//       T theta = atan2(u_norm, ambient_y_minus_x[0]);
+//       y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
+//       y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;
+//       y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm;
+//     } else {
+//       // We do not use [0,0,0] here because even though the value part is
+//       // a constant, the derivative part is not.
+//       y_minus_x[0] = ambient_y_minus_x[1];
+//       y_minus_x[1] = ambient_y_minus_x[2];
+//       y_minus_x[2] = ambient_y_minus_x[3];
+//     }
+//     return true;
+//   }
+// };
+//
+// Then given this struct, the auto differentiated Quaternion Manifold can now
+// be constructed as
+//
+//   Manifold* manifold = new AutoDiffManifold<QuaternionFunctor, 4, 3>;
+
+template <typename Functor, int kAmbientSize, int kTangentSize>
+class AutoDiffManifold final : public Manifold {
+ public:
+  AutoDiffManifold() : functor_(std::make_unique<Functor>()) {}
+
+  // Takes ownership of functor.
+  explicit AutoDiffManifold(Functor* functor) : functor_(functor) {}
+
+  int AmbientSize() const override { return kAmbientSize; }
+  int TangentSize() const override { return kTangentSize; }
+
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override {
+    return functor_->Plus(x, delta, x_plus_delta);
+  }
+
+  bool PlusJacobian(const double* x, double* jacobian) const override;
+
+  bool Minus(const double* y,
+             const double* x,
+             double* y_minus_x) const override {
+    return functor_->Minus(y, x, y_minus_x);
+  }
+
+  bool MinusJacobian(const double* x, double* jacobian) const override;
+
+  const Functor& functor() const { return *functor_; }
+
+ private:
+  std::unique_ptr<Functor> functor_;
+};
+
+namespace internal {
+
+// The following two helper structs are needed to interface the Plus and Minus
+// methods of the ManifoldFunctor with the automatic differentiation which
+// expects a Functor with operator().
+template <typename Functor>
+struct PlusWrapper {
+  explicit PlusWrapper(const Functor& functor) : functor(functor) {}
+  template <typename T>
+  bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
+    return functor.Plus(x, delta, x_plus_delta);
+  }
+  const Functor& functor;
+};
+
+template <typename Functor>
+struct MinusWrapper {
+  explicit MinusWrapper(const Functor& functor) : functor(functor) {}
+  template <typename T>
+  bool operator()(const T* y, const T* x, T* y_minus_x) const {
+    return functor.Minus(y, x, y_minus_x);
+  }
+  const Functor& functor;
+};
+}  // namespace internal
+
+template <typename Functor, int kAmbientSize, int kTangentSize>
+bool AutoDiffManifold<Functor, kAmbientSize, kTangentSize>::PlusJacobian(
+    const double* x, double* jacobian) const {
+  double zero_delta[kTangentSize];
+  for (int i = 0; i < kTangentSize; ++i) {
+    zero_delta[i] = 0.0;
+  }
+
+  double x_plus_delta[kAmbientSize];
+  for (int i = 0; i < kAmbientSize; ++i) {
+    x_plus_delta[i] = 0.0;
+  }
+
+  const double* parameter_ptrs[2] = {x, zero_delta};
+
+  // PlusJacobian is D_2 Plus(x,0) so we only need to compute the Jacobian
+  // w.r.t. the second argument.
+  double* jacobian_ptrs[2] = {nullptr, jacobian};
+  return internal::AutoDifferentiate<
+      kAmbientSize,
+      internal::StaticParameterDims<kAmbientSize, kTangentSize>>(
+      internal::PlusWrapper<Functor>(*functor_),
+      parameter_ptrs,
+      kAmbientSize,
+      x_plus_delta,
+      jacobian_ptrs);
+}
+
+template <typename Functor, int kAmbientSize, int kTangentSize>
+bool AutoDiffManifold<Functor, kAmbientSize, kTangentSize>::MinusJacobian(
+    const double* x, double* jacobian) const {
+  double y_minus_x[kTangentSize];
+  for (int i = 0; i < kTangentSize; ++i) {
+    y_minus_x[i] = 0.0;
+  }
+
+  const double* parameter_ptrs[2] = {x, x};
+
+  // MinusJacobian is D_1 Minus(x,x), so we only need to compute the Jacobian
+  // w.r.t. the first argument.
+  double* jacobian_ptrs[2] = {jacobian, nullptr};
+  return internal::AutoDifferentiate<
+      kTangentSize,
+      internal::StaticParameterDims<kAmbientSize, kAmbientSize>>(
+      internal::MinusWrapper<Functor>(*functor_),
+      parameter_ptrs,
+      kTangentSize,
+      y_minus_x,
+      jacobian_ptrs);
+}
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_AUTODIFF_MANIFOLD_H_

ceres-v2/include/c_api.h

@@ -0,0 +1,148 @@
+/* Ceres Solver - A fast non-linear least squares minimizer
+ * Copyright 2019 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_
+
+// clang-format off
+#include "ceres/internal/export.h"
+#include "ceres/internal/disable_warnings.h"
+// clang-format on
+
+#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_ */

ceres-v2/include/ceres.h

@@ -0,0 +1,74 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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_first_order_function.h"
+#include "ceres/autodiff_local_parameterization.h"
+#include "ceres/autodiff_manifold.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/evaluation_callback.h"
+#include "ceres/first_order_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/line_manifold.h"
+#include "ceres/local_parameterization.h"
+#include "ceres/loss_function.h"
+#include "ceres/manifold.h"
+#include "ceres/numeric_diff_cost_function.h"
+#include "ceres/numeric_diff_first_order_function.h"
+#include "ceres/numeric_diff_options.h"
+#include "ceres/ordered_groups.h"
+#include "ceres/problem.h"
+#include "ceres/product_manifold.h"
+#include "ceres/sized_cost_function.h"
+#include "ceres/solver.h"
+#include "ceres/sphere_manifold.h"
+#include "ceres/types.h"
+#include "ceres/version.h"
+
+#endif  // CERES_PUBLIC_CERES_H_

ceres-v2/include/conditioned_cost_function.h

@@ -0,0 +1,101 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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 <memory>
+#include <vector>
+
+#include "ceres/cost_function.h"
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/types.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 final : 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 nullptr, 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);
+  ~ConditionedCostFunction() override;
+
+  bool Evaluate(double const* const* parameters,
+                double* residuals,
+                double** jacobians) const override;
+
+ 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_

ceres-v2/include/context.h

@@ -0,0 +1,58 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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_
+
+#include "ceres/internal/export.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 CERES_EXPORT 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_

ceres-v2/include/cost_function.h

@@ -0,0 +1,144 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/disable_warnings.h"
+#include "ceres/internal/export.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();
+  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 nullptr, then no derivatives are returned; this is
+  // the case when computing cost only. If jacobians[i] is nullptr, 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 &parameter_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_

ceres-v2/include/cost_function_to_functor.h

@@ -0,0 +1,171 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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);
+//      bool Evaluate(double const* const* parameters,
+//                    double* residuals,
+//                    double** jacobians) const override;
+//  };
+//
+// 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 <utility>
+#include <vector>
+
+#include "ceres/cost_function.h"
+#include "ceres/dynamic_cost_function_to_functor.h"
+#include "ceres/internal/export.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/parameter_dims.h"
+#include "ceres/types.h"
+#include "glog/logging.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 =
+        std::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,
+                       std::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_

ceres-v2/include/covariance.h

@@ -0,0 +1,459 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/config.h"
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.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 it.
+//
+//
+// 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) - y|^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/Manifold. 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 * D * V' = J
+    //
+    //    and then uses it to compute the pseudo inverse of J'J as
+    //
+    //      pseudoinverse[J'J] = V * pseudoinverse[D^2] * 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*>& parameter_blocks,
+                           double* covariance_matrix) const;
+
+  // 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) const;
+
+ private:
+  std::unique_ptr<internal::CovarianceImpl> impl_;
+};
+
+}  // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif  // CERES_PUBLIC_COVARIANCE_H_

ceres-v2/include/crs_matrix.h

@@ -0,0 +1,87 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/disable_warnings.h"
+#include "ceres/internal/export.h"
+
+namespace ceres {
+
+// A compressed row sparse matrix used primarily for communicating the
+// Jacobian matrix to the user.
+struct CERES_EXPORT CRSMatrix {
+  CRSMatrix() = default;
+
+  int num_rows{0};
+  int num_cols{0};
+
+  // 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_

ceres-v2/include/cubic_interpolation.h

@@ -0,0 +1,436 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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 "Eigen/Core"
+#include "ceres/internal/export.h"
+#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 nullptr will contain the interpolated function values.
+// dfdx if not nullptr 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) {
+  using VType = Eigen::Matrix<double, kDataDimension, 1>;
+  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 != nullptr) {
+    Eigen::Map<VType>(f, kDataDimension) = d + x * (c + x * (b + x * a));
+  }
+
+  // dfdx = 3ax^2 + 2bx + c
+  if (dfdx != nullptr) {
+    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(data, 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, nullptr); }
+
+  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. Uses the nearest point on the grid boundary 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 != nullptr) {
+      // Interpolate vertically the derivative along the columns.
+      CubicHermiteSpline<Grid::DATA_DIMENSION>(
+          df0dc, df1dc, df2dc, df3dc, r - row, dfdc, nullptr);
+    }
+  }
+
+  // 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, nullptr, nullptr);
+  }
+
+  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_

ceres-v2/include/dynamic_autodiff_cost_function.h

@@ -0,0 +1,274 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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 <memory>
+#include <numeric>
+#include <vector>
+
+#include "ceres/dynamic_cost_function.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/jet.h"
+#include "ceres/types.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 final : public DynamicCostFunction {
+ public:
+  // Takes ownership by default.
+  explicit DynamicAutoDiffCostFunction(CostFunctor* functor,
+                                       Ownership ownership = TAKE_OWNERSHIP)
+      : functor_(functor), ownership_(ownership) {}
+
+  DynamicAutoDiffCostFunction(DynamicAutoDiffCostFunction&& other)
+      : functor_(std::move(other.functor_)), ownership_(other.ownership_) {}
+
+  ~DynamicAutoDiffCostFunction() override {
+    // Manually release pointer if configured to not take ownership
+    // rather than deleting only if ownership is taken.  This is to
+    // stay maximally compatible to old user code which may have
+    // forgotten to implement a virtual destructor, from when the
+    // AutoDiffCostFunction always took ownership.
+    if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
+      functor_.release();
+    }
+  }
+
+  bool Evaluate(double const* const* parameters,
+                double* residuals,
+                double** jacobians) const override {
+    CHECK_GT(num_residuals(), 0)
+        << "You must call DynamicAutoDiffCostFunction::SetNumResiduals() "
+        << "before DynamicAutoDiffCostFunction::Evaluate().";
+
+    if (jacobians == nullptr) {
+      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.
+    using JetT = Jet<double, Stride>;
+    internal::FixedArray<JetT, (256 * 7) / sizeof(JetT)> input_jets(
+        num_parameters);
+    internal::FixedArray<JetT, (256 * 7) / sizeof(JetT)> output_jets(
+        num_residuals());
+
+    // Make the parameter pack that is sent to the functor (reused).
+    internal::FixedArray<Jet<double, Stride>*> jet_parameters(
+        num_parameter_blocks, nullptr);
+    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] != nullptr) {
+        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];
+      }
+    }
+
+    if (num_active_parameters == 0) {
+      return (*functor_)(parameters, residuals);
+    }
+    // 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] != nullptr) {
+              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] != nullptr) {
+              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_;
+  Ownership ownership_;
+};
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_

ceres-v2/include/dynamic_cost_function.h

@@ -0,0 +1,57 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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"
+#include "ceres/internal/disable_warnings.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:
+  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
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif  // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_

ceres-v2/include/dynamic_cost_function_to_functor.h

@@ -0,0 +1,194 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/internal/fixed_array.h"
+#include "glog/logging.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);
+//      bool Evaluate(double const* const* parameters,
+//                    double* residuals,
+//                    double** jacobians) const override;
+//  };
+//
+// 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 CERES_EXPORT 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, nullptr);
+  }
+
+  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.data();
+    double* jacobian_ptr = jacobians.data();
+    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.data(),
+                                  residuals.data(),
+                                  jacobian_blocks.data())) {
+      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
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif  // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_

ceres-v2/include/dynamic_numeric_diff_cost_function.h

@@ -0,0 +1,164 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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 "ceres/types.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 final : public DynamicCostFunction {
+ public:
+  explicit DynamicNumericDiffCostFunction(
+      const CostFunctor* functor,
+      Ownership ownership = TAKE_OWNERSHIP,
+      const NumericDiffOptions& options = NumericDiffOptions())
+      : functor_(functor), ownership_(ownership), options_(options) {}
+
+  DynamicNumericDiffCostFunction(DynamicNumericDiffCostFunction&& other)
+      : functor_(std::move(other.functor_)), ownership_(other.ownership_) {}
+
+  ~DynamicNumericDiffCostFunction() override {
+    if (ownership_ != TAKE_OWNERSHIP) {
+      functor_.release();
+    }
+  }
+
+  bool Evaluate(double const* const* parameters,
+                double* residuals,
+                double** jacobians) const override {
+    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 == nullptr || !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] = &parameters_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] != nullptr &&
+          !NumericDiff<CostFunctor,
+                       method,
+                       ceres::DYNAMIC,
+                       internal::DynamicParameterDims,
+                       ceres::DYNAMIC,
+                       ceres::DYNAMIC>::
+              EvaluateJacobianForParameterBlock(functor_.get(),
+                                                residuals,
+                                                options_,
+                                                this->num_residuals(),
+                                                block,
+                                                block_sizes[block],
+                                                &parameters_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_

ceres-v2/include/evaluation_callback.h

@@ -0,0 +1,80 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/export.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(). 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_

ceres-v2/include/first_order_function.h

@@ -0,0 +1,54 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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_FIRST_ORDER_FUNCTION_H_
+#define CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_
+
+#include "ceres/internal/export.h"
+
+namespace ceres {
+
+// 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. The return value
+  // indicates whether the evaluation was successful or not.
+  virtual bool Evaluate(const double* const parameters,
+                        double* cost,
+                        double* gradient) const = 0;
+  virtual int NumParameters() const = 0;
+};
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_

ceres-v2/include/gradient_checker.h

@@ -0,0 +1,189 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/disable_warnings.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/export.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/local_parameterization.h"
+#include "ceres/manifold.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 constructor will not take ownership of the cost function or local
+  // parameterizations.
+  //
+  // function: The cost function to probe.
+  //
+  // local_parameterizations: A vector of local parameterizations, one for each
+  // parameter block. May be nullptr or contain nullptrs to indicate that the
+  // respective parameter does not have a local parameterization.
+  //
+  // options: Options to use for numerical differentiation.
+  //
+  // NOTE: This constructor is deprecated and will be removed in the next public
+  // release of Ceres Solver. Please transition to using the Manifold based
+  // version.
+  CERES_DEPRECATED_WITH_MSG(
+      "Local Parameterizations are deprecated. Use the constructor that uses "
+      "Manifolds instead.")
+  GradientChecker(
+      const CostFunction* function,
+      const std::vector<const LocalParameterization*>* local_parameterizations,
+      const NumericDiffOptions& options);
+
+  // This will not take ownership of the cost function or manifolds.
+  //
+  // function: The cost function to probe.
+  //
+  // manifolds: A vector of manifolds for each parameter. May be nullptr or
+  // contain nullptrs to indicate that the respective parameter blocks are
+  // Euclidean.
+  //
+  // options: Options to use for numerical differentiation.
+  GradientChecker(const CostFunction* function,
+                  const std::vector<const Manifold*>* manifolds,
+                  const NumericDiffOptions& options);
+  ~GradientChecker();
+
+  // 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
+    // manifold associated with it, the size of the "local" Jacobian will be
+    // determined by the dimension of the manifold (which is the same as the
+    // dimension of the tangent space) 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 manifolds are given, the Jacobians
+  // will be multiplied by the manifold Jacobians before performing the check,
+  // which effectively means that all errors along the null space of the
+  // manifold will be ignored.  Returns false if the Jacobians don't match, the
+  // cost function return false, or if a cost function returns a 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 nullptr.
+  //
+  // 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;
+
+  // This bool is used to determine whether the constructor with the
+  // LocalParameterizations is called or the one with Manifolds is called. If
+  // the former, then the vector of manifolds is a vector of ManifoldAdapter
+  // objects which we own and should be deleted. If the latter then they are
+  // real Manifold objects owned by the caller and will not be deleted.
+  //
+  // This bool is only needed during the LocalParameterization to Manifold
+  // transition, once this transition is complete the LocalParameterization
+  // based constructor and this bool will be removed.
+  const bool delete_manifolds_ = false;
+
+  std::vector<const Manifold*> manifolds_;
+  const CostFunction* function_;
+  std::unique_ptr<CostFunction> finite_diff_cost_function_;
+};
+
+}  // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif  // CERES_PUBLIC_GRADIENT_CHECKER_H_

ceres-v2/include/gradient_problem.h

@@ -0,0 +1,185 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/first_order_function.h"
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/local_parameterization.h"
+#include "ceres/manifold.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 are not restricted in the form
+// of the objective function.
+//
+// Structurally GradientProblem is a composition of a FirstOrderFunction and
+// optionally a Manifold.
+//
+// The FirstOrderFunction is responsible for evaluating the cost and gradient of
+// the objective function.
+//
+// The Manifold is responsible for going back and forth between the ambient
+// space and the local tangent space. (See manifold.h for more details). When a
+// Manifold 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 != nullptr) {
+//       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());
+//
+// NOTE: We are currently in the process of transitioning from
+// LocalParameterization to Manifolds in the Ceres API. During this period,
+// GradientProblem will support using both Manifold and LocalParameterization
+// objects interchangably. For methods in the API affected by this change, see
+// their documentation below.
+class CERES_EXPORT GradientProblem {
+ public:
+  // Takes ownership of the function.
+  explicit GradientProblem(FirstOrderFunction* function);
+
+  // Takes ownership of the function and the parameterization.
+  //
+  // NOTE: This constructor is deprecated and will be removed in the next public
+  // release of Ceres Solver. Please move to using the Manifold based
+  // constructor.
+  CERES_DEPRECATED_WITH_MSG(
+      "LocalParameterizations are deprecated. Please use the constructor that "
+      "uses Manifold instead.")
+  GradientProblem(FirstOrderFunction* function,
+                  LocalParameterization* parameterization);
+
+  // Takes ownership of the function and the manifold.
+  GradientProblem(FirstOrderFunction* function, Manifold* manifold);
+
+  int NumParameters() const;
+
+  // Dimension of the manifold (and its tangent space).
+  //
+  // During the transition from LocalParameterization to Manifold, this method
+  // reports the LocalSize of the LocalParameterization or the TangentSize of
+  // the Manifold object associated with this problem.
+  int NumTangentParameters() const;
+
+  // Dimension of the manifold (and its tangent space).
+  //
+  // NOTE: This method is deprecated and will be removed in the next public
+  // release of Ceres Solver. Please move to using NumTangentParameters()
+  // instead.
+  int NumLocalParameters() const { return NumTangentParameters(); }
+
+  // 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;
+
+  const FirstOrderFunction* function() const { return function_.get(); }
+  FirstOrderFunction* mutable_function() { return function_.get(); }
+
+  // NOTE: During the transition from LocalParameterization to Manifold we need
+  // to support both The LocalParameterization and Manifold based constructors.
+  //
+  // When the user uses the LocalParameterization, internally the solver will
+  // wrap it in a ManifoldAdapter object and return it when manifold or
+  // mutable_manifold are called.
+  //
+  // As a result this method will return a non-nullptr result if a Manifold or a
+  // LocalParameterization was used when constructing the GradientProblem.
+  const Manifold* manifold() const { return manifold_.get(); }
+  Manifold* mutable_manifold() { return manifold_.get(); }
+
+  // If the problem is constructed without a LocalParameterization or with a
+  // Manifold this method will return a nullptr.
+  //
+  // NOTE: This method is deprecated and will be removed in the next public
+  // release of Ceres Solver.
+  CERES_DEPRECATED_WITH_MSG("Use Manifolds instead.")
+  const LocalParameterization* parameterization() const {
+    return parameterization_.get();
+  }
+
+  // If the problem is constructed without a LocalParameterization or with a
+  // Manifold this method will return a nullptr.
+  //
+  // NOTE: This method is deprecated and will be removed in the next public
+  // release of Ceres Solver.
+  CERES_DEPRECATED_WITH_MSG("Use Manifolds instead.")
+  LocalParameterization* mutable_parameterization() {
+    return parameterization_.get();
+  }
+
+ private:
+  std::unique_ptr<FirstOrderFunction> function_;
+  CERES_DEPRECATED_WITH_MSG("")
+  std::unique_ptr<LocalParameterization> parameterization_;
+  std::unique_ptr<Manifold> manifold_;
+  std::unique_ptr<double[]> scratch_;
+};
+
+}  // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif  // CERES_PUBLIC_GRADIENT_PROBLEM_H_

ceres-v2/include/gradient_problem_solver.h

@@ -0,0 +1,357 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/internal/port.h"
+#include "ceres/iteration_callback.h"
+#include "ceres/types.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.
+    CERES_DEPRECATED_WITH_MSG("Use num_tangent_parameters.")
+    int num_local_parameters = -1;
+
+    // Dimension of the tangent space of the problem.
+    int num_tangent_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_

ceres-v2/include/internal/array_selector.h

@@ -0,0 +1,97 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2020 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: darius.rueckert@fau.de (Darius Rueckert)
+//
+
+#ifndef CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_
+#define CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_
+
+#include <array>
+#include <vector>
+
+#include "ceres/internal/fixed_array.h"
+#include "ceres/types.h"
+
+namespace ceres {
+namespace internal {
+
+// StaticFixedArray selects the best array implementation based on template
+// arguments. If the size is not known at compile-time, pass
+// ceres::DYNAMIC as a size-template argument.
+//
+// Three different containers are selected in different scenarios:
+//
+//   num_elements == DYNAMIC:
+//      -> ceres::internal::FixedArray<T, max_stack_size>(size)
+
+//   num_elements != DYNAMIC  &&  num_elements <= max_stack_size
+//      -> std::array<T,num_elements>
+
+//   num_elements != DYNAMIC  &&  num_elements >  max_stack_size
+//      -> std::vector<T>(num_elements)
+//
+template <typename T,
+          int num_elements,
+          int max_num_elements_on_stack,
+          bool dynamic = (num_elements == DYNAMIC),
+          bool fits_on_stack = (num_elements <= max_num_elements_on_stack)>
+struct ArraySelector {};
+
+template <typename T,
+          int num_elements,
+          int max_num_elements_on_stack,
+          bool fits_on_stack>
+struct ArraySelector<T,
+                     num_elements,
+                     max_num_elements_on_stack,
+                     true,
+                     fits_on_stack>
+    : ceres::internal::FixedArray<T, max_num_elements_on_stack> {
+  explicit ArraySelector(int s)
+      : ceres::internal::FixedArray<T, max_num_elements_on_stack>(s) {}
+};
+
+template <typename T, int num_elements, int max_num_elements_on_stack>
+struct ArraySelector<T, num_elements, max_num_elements_on_stack, false, true>
+    : std::array<T, num_elements> {
+  explicit ArraySelector(int s) { CHECK_EQ(s, num_elements); }
+};
+
+template <typename T, int num_elements, int max_num_elements_on_stack>
+struct ArraySelector<T, num_elements, max_num_elements_on_stack, false, false>
+    : std::vector<T> {
+  explicit ArraySelector(int s) : std::vector<T>(s) {
+    CHECK_EQ(s, num_elements);
+  }
+};
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_

ceres-v2/include/internal/autodiff.h

@@ -0,0 +1,365 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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 (nullptr)
+// --------------------------------
+// In general, the functions below will accept nullptr 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 <array>
+#include <cstddef>
+#include <utility>
+
+#include "ceres/internal/array_selector.h"
+#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"
+
+// If the number of parameters exceeds this values, the corresponding jets are
+// placed on the heap. This will reduce performance by a factor of 2-5 on
+// current compilers.
+#ifndef CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK
+#define CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK 50
+#endif
+
+#ifndef CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK
+#define CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK 20
+#endif
+
+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 j, int N, int Offset, typename T, typename JetT>
+struct Make1stOrderPerturbation {
+ public:
+  inline static void Apply(const T* src, JetT* dst) {
+    if (j == 0) {
+      DCHECK(src);
+      DCHECK(dst);
+    }
+    dst[j] = JetT(src[j], j + Offset);
+    Make1stOrderPerturbation<j + 1, N, Offset, T, JetT>::Apply(src, dst);
+  }
+};
+
+template <int N, int Offset, typename T, typename JetT>
+struct Make1stOrderPerturbation<N, N, Offset, T, JetT> {
+ public:
+  static void Apply(const T* /* NOT USED */, JetT* /* NOT USED */) {}
+};
+
+// 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, 0>::Apply(params[0], x + 0);
+// Make1stOrderPerturbation<0, 2, 3>::Apply(params[1], x + 3);
+// Make1stOrderPerturbation<0, 4, 5>::Apply(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<std::integer_sequence<int, N, Ns...>,
+                                 ParameterIdx,
+                                 Offset> {
+  template <typename T, typename JetT>
+  inline static void Apply(T const* const* parameters, JetT* x) {
+    Make1stOrderPerturbation<0, N, Offset, T, JetT>::Apply(
+        parameters[ParameterIdx], x + Offset);
+    Make1stOrderPerturbations<std::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<std::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<std::integer_sequence<int, N, Ns...>,
+                         ParameterIdx,
+                         Offset> {
+  template <typename JetT, typename T>
+  inline static void Apply(int num_outputs, JetT* output, T** jacobians) {
+    if (jacobians[ParameterIdx]) {
+      Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]);
+    }
+    Take1stOrderParts<std::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<std::integer_sequence<int>, ParameterIdx, Offset> {
+  template <typename T, typename JetT>
+  static void Apply(int /* NOT USED*/,
+                    JetT* /* NOT USED*/,
+                    T** /* NOT USED */) {}
+};
+
+template <int kNumResiduals,
+          typename ParameterDims,
+          typename Functor,
+          typename T>
+inline bool AutoDifferentiate(const Functor& functor,
+                              T const* const* parameters,
+                              int dynamic_num_outputs,
+                              T* function_value,
+                              T** jacobians) {
+  using JetT = Jet<T, ParameterDims::kNumParameters>;
+  using Parameters = typename ParameterDims::Parameters;
+
+  if (kNumResiduals != DYNAMIC) {
+    DCHECK_EQ(kNumResiduals, dynamic_num_outputs);
+  }
+
+  ArraySelector<JetT,
+                ParameterDims::kNumParameters,
+                CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK>
+      parameters_as_jets(ParameterDims::kNumParameters);
+
+  // Pointers to the beginning of each parameter block
+  std::array<JetT*, ParameterDims::kNumParameterBlocks> unpacked_parameters =
+      ParameterDims::GetUnpackedParameters(parameters_as_jets.data());
+
+  // If the number of residuals is fixed, we use the template argument as the
+  // number of outputs. Otherwise we use the num_outputs parameter. Note: The
+  // ?-operator here is compile-time evaluated, therefore num_outputs is also
+  // a compile-time constant for functors with fixed residuals.
+  const int num_outputs =
+      kNumResiduals == DYNAMIC ? dynamic_num_outputs : kNumResiduals;
+  DCHECK_GT(num_outputs, 0);
+
+  ArraySelector<JetT, kNumResiduals, CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK>
+      residuals_as_jets(num_outputs);
+
+  // 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) {
+    residuals_as_jets[i].a = kImpossibleValue;
+    residuals_as_jets[i].v.setConstant(kImpossibleValue);
+  }
+
+  Make1stOrderPerturbations<Parameters>::Apply(parameters,
+                                               parameters_as_jets.data());
+
+  if (!VariadicEvaluate<ParameterDims>(
+          functor, unpacked_parameters.data(), residuals_as_jets.data())) {
+    return false;
+  }
+
+  Take0thOrderPart(num_outputs, residuals_as_jets.data(), function_value);
+  Take1stOrderParts<Parameters>::Apply(
+      num_outputs, residuals_as_jets.data(), jacobians);
+
+  return true;
+}
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_AUTODIFF_H_

ceres-v2/include/internal/config.h

@@ -0,0 +1,123 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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: alexs.mac@gmail.com (Alex Stewart)
+
+// Configuration options for Ceres.
+//
+// Do not edit this file, it was automatically configured by CMake when
+// Ceres was compiled with the relevant configuration for the machine
+// on which Ceres was compiled.
+//
+// Ceres Developers: All options should have the same name as their mapped
+//                   CMake options, in the preconfigured version of this file
+//                   all options should be enclosed in '@'.
+
+#ifndef CERES_PUBLIC_INTERNAL_CONFIG_H_
+#define CERES_PUBLIC_INTERNAL_CONFIG_H_
+
+// If defined, use the LGPL code in Eigen.
+#define CERES_USE_EIGEN_SPARSE
+
+// If defined, Ceres was compiled without LAPACK.
+// #define CERES_NO_LAPACK
+
+// If defined, Ceres was compiled without SuiteSparse.
+// #define CERES_NO_SUITESPARSE
+
+// If defined, Ceres was compiled without CXSparse.
+// #define CERES_NO_CXSPARSE
+
+// If defined, Ceres was compiled without CUDA.
+// #define CERES_NO_CUDA
+
+// If defined, Ceres was compiled without Apple's Accelerate framework solvers.
+#define CERES_NO_ACCELERATE_SPARSE
+
+#if defined(CERES_NO_SUITESPARSE) &&              \
+    defined(CERES_NO_ACCELERATE_SPARSE) &&        \
+    defined(CERES_NO_CXSPARSE) &&                 \
+    !defined(CERES_USE_EIGEN_SPARSE)  // NOLINT
+// If defined Ceres was compiled without any sparse linear algebra support.
+#define CERES_NO_SPARSE
+#endif
+
+// If defined, Ceres was compiled without Schur specializations.
+// #define CERES_RESTRICT_SCHUR_SPECIALIZATION
+
+// If defined, Ceres was compiled to use Eigen instead of hardcoded BLAS
+// routines.
+// #define CERES_NO_CUSTOM_BLAS
+
+// If defined, Ceres was compiled without multithreading support.
+// #define CERES_NO_THREADS
+// If defined Ceres was compiled with OpenMP multithreading.
+// #define CERES_USE_OPENMP
+// If defined Ceres was compiled with modern C++ multithreading.
+#define CERES_USE_CXX_THREADS
+
+// If defined, Ceres was compiled with a version MSVC >= 2005 which
+// deprecated the standard POSIX names for bessel functions, replacing them
+// with underscore prefixed versions (e.g. j0() -> _j0()).
+// #define CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS
+
+#if defined(CERES_USE_OPENMP)
+#if defined(CERES_USE_CXX_THREADS) || defined(CERES_NO_THREADS)
+#error CERES_USE_OPENMP is mutually exclusive to CERES_USE_CXX_THREADS and CERES_NO_THREADS
+#endif
+#elif defined(CERES_USE_CXX_THREADS)
+#if defined(CERES_USE_OPENMP) || defined(CERES_NO_THREADS)
+#error CERES_USE_CXX_THREADS is mutually exclusive to CERES_USE_OPENMP, CERES_USE_CXX_THREADS and CERES_NO_THREADS
+#endif
+#elif defined(CERES_NO_THREADS)
+#if defined(CERES_USE_OPENMP) || defined(CERES_USE_CXX_THREADS)
+#error CERES_NO_THREADS is mutually exclusive to CERES_USE_OPENMP and CERES_USE_CXX_THREADS
+#endif
+#else
+#  error One of CERES_USE_OPENMP, CERES_USE_CXX_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
+
+#endif  // CERES_PUBLIC_INTERNAL_CONFIG_H_

ceres-v2/include/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

ceres-v2/include/internal/eigen.h

@@ -0,0 +1,75 @@
+// 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 {
+
+using Vector = Eigen::Matrix<double, Eigen::Dynamic, 1>;
+using Matrix =
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+using VectorRef = Eigen::Map<Vector>;
+using MatrixRef = Eigen::Map<Matrix>;
+using ConstVectorRef = Eigen::Map<const Vector>;
+using ConstMatrixRef = Eigen::Map<const Matrix>;
+
+// Column major matrices for DenseSparseMatrix/DenseQRSolver
+using ColMajorMatrix =
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
+
+using ColMajorMatrixRef =
+    Eigen::Map<ColMajorMatrix, 0, Eigen::Stride<Eigen::Dynamic, 1>>;
+
+using ConstColMajorMatrixRef =
+    Eigen::Map<const ColMajorMatrix, 0, Eigen::Stride<Eigen::Dynamic, 1>>;
+
+// 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 {
+  using Matrix =
+      Eigen::Matrix<double,
+                    num_rows,
+                    num_cols,
+                    num_cols == 1 ? Eigen::ColMajor : Eigen::RowMajor>;
+
+  using MatrixRef = Eigen::Map<Matrix>;
+  using ConstMatrixRef = Eigen::Map<const Matrix>;
+  using Vector = Eigen::Matrix<double, num_rows, 1>;
+  using VectorRef = Eigen::Map<Eigen::Matrix<double, num_rows, 1>>;
+  using ConstVectorRef = Eigen::Map<const Eigen::Matrix<double, num_rows, 1>>;
+};
+
+}  // namespace ceres
+
+#endif  // CERES_INTERNAL_EIGEN_H_

ceres-v2/include/internal/export.h

@@ -0,0 +1,42 @@
+
+#ifndef CERES_EXPORT_H
+#define CERES_EXPORT_H
+
+#ifdef CERES_STATIC_DEFINE
+#  define CERES_EXPORT
+#  define CERES_NO_EXPORT
+#else
+#  ifndef CERES_EXPORT
+#    ifdef ceres_EXPORTS
+        /* We are building this library */
+#      define CERES_EXPORT 
+#    else
+        /* We are using this library */
+#      define CERES_EXPORT 
+#    endif
+#  endif
+
+#  ifndef CERES_NO_EXPORT
+#    define CERES_NO_EXPORT 
+#  endif
+#endif
+
+#ifndef CERES_DEPRECATED
+#  define CERES_DEPRECATED __attribute__ ((__deprecated__))
+#endif
+
+#ifndef CERES_DEPRECATED_EXPORT
+#  define CERES_DEPRECATED_EXPORT CERES_EXPORT CERES_DEPRECATED
+#endif
+
+#ifndef CERES_DEPRECATED_NO_EXPORT
+#  define CERES_DEPRECATED_NO_EXPORT CERES_NO_EXPORT CERES_DEPRECATED
+#endif
+
+#if 0 /* DEFINE_NO_DEPRECATED */
+#  ifndef CERES_NO_DEPRECATED
+#    define CERES_NO_DEPRECATED
+#  endif
+#endif
+
+#endif /* CERES_EXPORT_H */

ceres-v2/include/internal/fixed_array.h

@@ -0,0 +1,467 @@
+// Copyright 2018 The Abseil Authors.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// -----------------------------------------------------------------------------
+// File: fixed_array.h
+// -----------------------------------------------------------------------------
+//
+// A `FixedArray<T>` represents a non-resizable array of `T` where the length of
+// the array can be determined at run-time. It is a good replacement for
+// non-standard and deprecated uses of `alloca()` and variable length arrays
+// within the GCC extension. (See
+// https://gcc.gnu.org/onlinedocs/gcc/Variable-Length.html).
+//
+// `FixedArray` allocates small arrays inline, keeping performance fast by
+// avoiding heap operations. It also helps reduce the chances of
+// accidentally overflowing your stack if large input is passed to
+// your function.
+
+#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
+#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
+
+#include <Eigen/Core>  // For Eigen::aligned_allocator
+#include <algorithm>
+#include <array>
+#include <cstddef>
+#include <memory>
+#include <tuple>
+#include <type_traits>
+
+#include "ceres/internal/memory.h"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+constexpr static auto kFixedArrayUseDefault = static_cast<size_t>(-1);
+
+// The default fixed array allocator.
+//
+// As one can not easily detect if a struct contains or inherits from a fixed
+// size Eigen type, to be safe the Eigen::aligned_allocator is used by default.
+// But trivial types can never contain Eigen types, so std::allocator is used to
+// safe some heap memory.
+template <typename T>
+using FixedArrayDefaultAllocator =
+    typename std::conditional<std::is_trivial<T>::value,
+                              std::allocator<T>,
+                              Eigen::aligned_allocator<T>>::type;
+
+// -----------------------------------------------------------------------------
+// FixedArray
+// -----------------------------------------------------------------------------
+//
+// A `FixedArray` provides a run-time fixed-size array, allocating a small array
+// inline for efficiency.
+//
+// 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 use inline storage for arrays with a
+// length <= `inline_elements`.
+//
+// Note that a `FixedArray` constructed with a `size_type` argument will
+// default-initialize its values by leaving trivially constructible types
+// uninitialized (e.g. int, int[4], double), and others default-constructed.
+// This matches the behavior of c-style arrays and `std::array`, but not
+// `std::vector`.
+//
+// Note that `FixedArray` does not provide a public allocator; if it requires a
+// heap allocation, it will do so with global `::operator new[]()` and
+// `::operator delete[]()`, even if T provides class-scope overrides for these
+// operators.
+template <typename T,
+          size_t N = kFixedArrayUseDefault,
+          typename A = FixedArrayDefaultAllocator<T>>
+class FixedArray {
+  static_assert(!std::is_array<T>::value || std::extent<T>::value > 0,
+                "Arrays with unknown bounds cannot be used with FixedArray.");
+
+  static constexpr size_t kInlineBytesDefault = 256;
+
+  using AllocatorTraits = std::allocator_traits<A>;
+  // std::iterator_traits isn't guaranteed to be SFINAE-friendly until C++17,
+  // but this seems to be mostly pedantic.
+  template <typename Iterator>
+  using EnableIfForwardIterator = typename std::enable_if<std::is_convertible<
+      typename std::iterator_traits<Iterator>::iterator_category,
+      std::forward_iterator_tag>::value>::type;
+  static constexpr bool DefaultConstructorIsNonTrivial() {
+    return !std::is_trivially_default_constructible<StorageElement>::value;
+  }
+
+ public:
+  using allocator_type = typename AllocatorTraits::allocator_type;
+  using value_type = typename AllocatorTraits::value_type;
+  using pointer = typename AllocatorTraits::pointer;
+  using const_pointer = typename AllocatorTraits::const_pointer;
+  using reference = value_type&;
+  using const_reference = const value_type&;
+  using size_type = typename AllocatorTraits::size_type;
+  using difference_type = typename AllocatorTraits::difference_type;
+  using iterator = pointer;
+  using const_iterator = const_pointer;
+  using reverse_iterator = std::reverse_iterator<iterator>;
+  using const_reverse_iterator = std::reverse_iterator<const_iterator>;
+
+  static constexpr size_type inline_elements =
+      (N == kFixedArrayUseDefault ? kInlineBytesDefault / sizeof(value_type)
+                                  : static_cast<size_type>(N));
+
+  FixedArray(const FixedArray& other,
+             const allocator_type& a = allocator_type())
+      : FixedArray(other.begin(), other.end(), a) {}
+
+  FixedArray(FixedArray&& other, const allocator_type& a = allocator_type())
+      : FixedArray(std::make_move_iterator(other.begin()),
+                   std::make_move_iterator(other.end()),
+                   a) {}
+
+  // Creates an array object that can store `n` elements.
+  // Note that trivially constructible elements will be uninitialized.
+  explicit FixedArray(size_type n, const allocator_type& a = allocator_type())
+      : storage_(n, a) {
+    if (DefaultConstructorIsNonTrivial()) {
+      ConstructRange(storage_.alloc(), storage_.begin(), storage_.end());
+    }
+  }
+
+  // Creates an array initialized with `n` copies of `val`.
+  FixedArray(size_type n,
+             const value_type& val,
+             const allocator_type& a = allocator_type())
+      : storage_(n, a) {
+    ConstructRange(storage_.alloc(), storage_.begin(), storage_.end(), val);
+  }
+
+  // Creates an array initialized with the size and contents of `init_list`.
+  FixedArray(std::initializer_list<value_type> init_list,
+             const allocator_type& a = allocator_type())
+      : FixedArray(init_list.begin(), init_list.end(), a) {}
+
+  // Creates an array initialized with the elements from the input
+  // range. The array's size will always be `std::distance(first, last)`.
+  // REQUIRES: Iterator must be a forward_iterator or better.
+  template <typename Iterator, EnableIfForwardIterator<Iterator>* = nullptr>
+  FixedArray(Iterator first,
+             Iterator last,
+             const allocator_type& a = allocator_type())
+      : storage_(std::distance(first, last), a) {
+    CopyRange(storage_.alloc(), storage_.begin(), first, last);
+  }
+
+  ~FixedArray() noexcept {
+    for (auto* cur = storage_.begin(); cur != storage_.end(); ++cur) {
+      AllocatorTraits::destroy(storage_.alloc(), cur);
+    }
+  }
+
+  // Assignments are deleted because they break the invariant that the size of a
+  // `FixedArray` never changes.
+  void operator=(FixedArray&&) = delete;
+  void operator=(const FixedArray&) = delete;
+
+  // FixedArray::size()
+  //
+  // Returns the length of the fixed array.
+  size_type size() const { return storage_.size(); }
+
+  // FixedArray::max_size()
+  //
+  // Returns the largest possible value of `std::distance(begin(), end())` for a
+  // `FixedArray<T>`. This is equivalent to the most possible addressable bytes
+  // over the number of bytes taken by T.
+  constexpr size_type max_size() const {
+    return (std::numeric_limits<difference_type>::max)() / sizeof(value_type);
+  }
+
+  // FixedArray::empty()
+  //
+  // Returns whether or not the fixed array is empty.
+  bool empty() const { return size() == 0; }
+
+  // FixedArray::memsize()
+  //
+  // Returns the memory size of the fixed array in bytes.
+  size_t memsize() const { return size() * sizeof(value_type); }
+
+  // FixedArray::data()
+  //
+  // Returns a const T* pointer to elements of the `FixedArray`. This pointer
+  // can be used to access (but not modify) the contained elements.
+  const_pointer data() const { return AsValueType(storage_.begin()); }
+
+  // Overload of FixedArray::data() to return a T* pointer to elements of the
+  // fixed array. This pointer can be used to access and modify the contained
+  // elements.
+  pointer data() { return AsValueType(storage_.begin()); }
+
+  // FixedArray::operator[]
+  //
+  // Returns a reference the ith element of the fixed array.
+  // REQUIRES: 0 <= i < size()
+  reference operator[](size_type i) {
+    DCHECK_LT(i, size());
+    return data()[i];
+  }
+
+  // Overload of FixedArray::operator()[] to return a const reference to the
+  // ith element of the fixed array.
+  // REQUIRES: 0 <= i < size()
+  const_reference operator[](size_type i) const {
+    DCHECK_LT(i, size());
+    return data()[i];
+  }
+
+  // FixedArray::front()
+  //
+  // Returns a reference to the first element of the fixed array.
+  reference front() { return *begin(); }
+
+  // Overload of FixedArray::front() to return a reference to the first element
+  // of a fixed array of const values.
+  const_reference front() const { return *begin(); }
+
+  // FixedArray::back()
+  //
+  // Returns a reference to the last element of the fixed array.
+  reference back() { return *(end() - 1); }
+
+  // Overload of FixedArray::back() to return a reference to the last element
+  // of a fixed array of const values.
+  const_reference back() const { return *(end() - 1); }
+
+  // FixedArray::begin()
+  //
+  // Returns an iterator to the beginning of the fixed array.
+  iterator begin() { return data(); }
+
+  // Overload of FixedArray::begin() to return a const iterator to the
+  // beginning of the fixed array.
+  const_iterator begin() const { return data(); }
+
+  // FixedArray::cbegin()
+  //
+  // Returns a const iterator to the beginning of the fixed array.
+  const_iterator cbegin() const { return begin(); }
+
+  // FixedArray::end()
+  //
+  // Returns an iterator to the end of the fixed array.
+  iterator end() { return data() + size(); }
+
+  // Overload of FixedArray::end() to return a const iterator to the end of the
+  // fixed array.
+  const_iterator end() const { return data() + size(); }
+
+  // FixedArray::cend()
+  //
+  // Returns a const iterator to the end of the fixed array.
+  const_iterator cend() const { return end(); }
+
+  // FixedArray::rbegin()
+  //
+  // Returns a reverse iterator from the end of the fixed array.
+  reverse_iterator rbegin() { return reverse_iterator(end()); }
+
+  // Overload of FixedArray::rbegin() to return a const reverse iterator from
+  // the end of the fixed array.
+  const_reverse_iterator rbegin() const {
+    return const_reverse_iterator(end());
+  }
+
+  // FixedArray::crbegin()
+  //
+  // Returns a const reverse iterator from the end of the fixed array.
+  const_reverse_iterator crbegin() const { return rbegin(); }
+
+  // FixedArray::rend()
+  //
+  // Returns a reverse iterator from the beginning of the fixed array.
+  reverse_iterator rend() { return reverse_iterator(begin()); }
+
+  // Overload of FixedArray::rend() for returning a const reverse iterator
+  // from the beginning of the fixed array.
+  const_reverse_iterator rend() const {
+    return const_reverse_iterator(begin());
+  }
+
+  // FixedArray::crend()
+  //
+  // Returns a reverse iterator from the beginning of the fixed array.
+  const_reverse_iterator crend() const { return rend(); }
+
+  // FixedArray::fill()
+  //
+  // Assigns the given `value` to all elements in the fixed array.
+  void fill(const value_type& val) { std::fill(begin(), end(), val); }
+
+  // Relational operators. Equality operators are elementwise using
+  // `operator==`, while order operators order FixedArrays lexicographically.
+  friend bool operator==(const FixedArray& lhs, const FixedArray& rhs) {
+    return std::equal(lhs.begin(), lhs.end(), rhs.begin(), rhs.end());
+  }
+
+  friend bool operator!=(const FixedArray& lhs, const FixedArray& rhs) {
+    return !(lhs == rhs);
+  }
+
+  friend bool operator<(const FixedArray& lhs, const FixedArray& rhs) {
+    return std::lexicographical_compare(
+        lhs.begin(), lhs.end(), rhs.begin(), rhs.end());
+  }
+
+  friend bool operator>(const FixedArray& lhs, const FixedArray& rhs) {
+    return rhs < lhs;
+  }
+
+  friend bool operator<=(const FixedArray& lhs, const FixedArray& rhs) {
+    return !(rhs < lhs);
+  }
+
+  friend bool operator>=(const FixedArray& lhs, const FixedArray& rhs) {
+    return !(lhs < rhs);
+  }
+
+ private:
+  // StorageElement
+  //
+  // For FixedArrays with a C-style-array value_type, StorageElement is a POD
+  // wrapper struct called StorageElementWrapper that holds the value_type
+  // instance inside. This is needed for construction and destruction of the
+  // entire array regardless of how many dimensions it has. For all other cases,
+  // StorageElement is just an alias of value_type.
+  //
+  // Maintainer's Note: The simpler solution would be to simply wrap value_type
+  // in a struct whether it's an array or not. That causes some paranoid
+  // diagnostics to misfire, believing that 'data()' returns a pointer to a
+  // single element, rather than the packed array that it really is.
+  // e.g.:
+  //
+  //     FixedArray<char> buf(1);
+  //     sprintf(buf.data(), "foo");
+  //
+  //     error: call to int __builtin___sprintf_chk(etc...)
+  //     will always overflow destination buffer [-Werror]
+  //
+  template <typename OuterT,
+            typename InnerT = typename std::remove_extent<OuterT>::type,
+            size_t InnerN = std::extent<OuterT>::value>
+  struct StorageElementWrapper {
+    InnerT array[InnerN];
+  };
+
+  using StorageElement =
+      typename std::conditional<std::is_array<value_type>::value,
+                                StorageElementWrapper<value_type>,
+                                value_type>::type;
+
+  static pointer AsValueType(pointer ptr) { return ptr; }
+  static pointer AsValueType(StorageElementWrapper<value_type>* ptr) {
+    return std::addressof(ptr->array);
+  }
+
+  static_assert(sizeof(StorageElement) == sizeof(value_type), "");
+  static_assert(alignof(StorageElement) == alignof(value_type), "");
+
+  class NonEmptyInlinedStorage {
+   public:
+    StorageElement* data() { return reinterpret_cast<StorageElement*>(buff_); }
+    void AnnotateConstruct(size_type) {}
+    void AnnotateDestruct(size_type) {}
+
+    // #ifdef ADDRESS_SANITIZER
+    //     void* RedzoneBegin() { return &redzone_begin_; }
+    //     void* RedzoneEnd() { return &redzone_end_ + 1; }
+    // #endif  // ADDRESS_SANITIZER
+
+   private:
+    // ADDRESS_SANITIZER_REDZONE(redzone_begin_);
+    alignas(StorageElement) char buff_[sizeof(StorageElement[inline_elements])];
+    // ADDRESS_SANITIZER_REDZONE(redzone_end_);
+  };
+
+  class EmptyInlinedStorage {
+   public:
+    StorageElement* data() { return nullptr; }
+    void AnnotateConstruct(size_type) {}
+    void AnnotateDestruct(size_type) {}
+  };
+
+  using InlinedStorage =
+      typename std::conditional<inline_elements == 0,
+                                EmptyInlinedStorage,
+                                NonEmptyInlinedStorage>::type;
+
+  // Storage
+  //
+  // An instance of Storage manages the inline and out-of-line memory for
+  // instances of FixedArray. This guarantees that even when construction of
+  // individual elements fails in the FixedArray constructor body, the
+  // destructor for Storage will still be called and out-of-line memory will be
+  // properly deallocated.
+  //
+  class Storage : public InlinedStorage {
+   public:
+    Storage(size_type n, const allocator_type& a)
+        : size_alloc_(n, a), data_(InitializeData()) {}
+
+    ~Storage() noexcept {
+      if (UsingInlinedStorage(size())) {
+        InlinedStorage::AnnotateDestruct(size());
+      } else {
+        AllocatorTraits::deallocate(alloc(), AsValueType(begin()), size());
+      }
+    }
+
+    size_type size() const { return std::get<0>(size_alloc_); }
+    StorageElement* begin() const { return data_; }
+    StorageElement* end() const { return begin() + size(); }
+    allocator_type& alloc() { return std::get<1>(size_alloc_); }
+
+   private:
+    static bool UsingInlinedStorage(size_type n) {
+      return n <= inline_elements;
+    }
+
+    StorageElement* InitializeData() {
+      if (UsingInlinedStorage(size())) {
+        InlinedStorage::AnnotateConstruct(size());
+        return InlinedStorage::data();
+      } else {
+        return reinterpret_cast<StorageElement*>(
+            AllocatorTraits::allocate(alloc(), size()));
+      }
+    }
+
+    // Using std::tuple and not absl::CompressedTuple, as it has a lot of
+    // dependencies to other absl headers.
+    std::tuple<size_type, allocator_type> size_alloc_;
+    StorageElement* data_;
+  };
+
+  Storage storage_;
+};
+
+template <typename T, size_t N, typename A>
+constexpr size_t FixedArray<T, N, A>::kInlineBytesDefault;
+
+template <typename T, size_t N, typename A>
+constexpr typename FixedArray<T, N, A>::size_type
+    FixedArray<T, N, A>::inline_elements;
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_

ceres-v2/include/internal/householder_vector.h

@@ -0,0 +1,96 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_
+#define CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_
+
+#include "Eigen/Core"
+#include "glog/logging.h"
+
+namespace ceres {
+namespace internal {
+
+// Algorithm 5.1.1 from 'Matrix Computations' by Golub et al. (Johns Hopkins
+// Studies in Mathematical Sciences) but using the nth element of the input
+// vector as pivot instead of first. This computes the vector v with v(n) = 1
+// and beta such that H = I - beta * v * v^T is orthogonal and
+// H * x = ||x||_2 * e_n.
+//
+// NOTE: Some versions of MSVC have trouble deducing the type of v if
+// you do not specify all the template arguments explicitly.
+template <typename XVectorType, typename Scalar, int N>
+void ComputeHouseholderVector(const XVectorType& x,
+                              Eigen::Matrix<Scalar, N, 1>* v,
+                              Scalar* beta) {
+  CHECK(beta != nullptr);
+  CHECK(v != nullptr);
+  CHECK_GT(x.rows(), 1);
+  CHECK_EQ(x.rows(), v->rows());
+
+  Scalar sigma = x.head(x.rows() - 1).squaredNorm();
+  *v = x;
+  (*v)(v->rows() - 1) = Scalar(1.0);
+
+  *beta = Scalar(0.0);
+  const Scalar& x_pivot = x(x.rows() - 1);
+
+  if (sigma <= Scalar(std::numeric_limits<double>::epsilon())) {
+    if (x_pivot < Scalar(0.0)) {
+      *beta = Scalar(2.0);
+    }
+    return;
+  }
+
+  const Scalar mu = sqrt(x_pivot * x_pivot + sigma);
+  Scalar v_pivot = Scalar(1.0);
+
+  if (x_pivot <= Scalar(0.0)) {
+    v_pivot = x_pivot - mu;
+  } else {
+    v_pivot = -sigma / (x_pivot + mu);
+  }
+
+  *beta = Scalar(2.0) * v_pivot * v_pivot / (sigma + v_pivot * v_pivot);
+
+  v->head(v->rows() - 1) /= v_pivot;
+}
+
+template <typename XVectorType, typename Derived>
+typename Derived::PlainObject ApplyHouseholderVector(
+    const XVectorType& y,
+    const Eigen::MatrixBase<Derived>& v,
+    const typename Derived::Scalar& beta) {
+  return (y - v * (beta * (v.transpose() * y)));
+}
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_

ceres-v2/include/internal/integer_sequence_algorithm.h

@@ -0,0 +1,291 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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)
+//         sergiu.deitsch@gmail.com (Sergiu Deitsch)
+//
+// 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 <utility>
+
+#include "ceres/jet_fwd.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<std::integer_sequence<T, N, Ns...>> {
+  static constexpr T Value =
+      N + SumImpl<std::integer_sequence<T, Ns...>>::Value;
+};
+
+// Strip of and sum the first two numbers.
+template <typename T, T N1, T N2, T... Ns>
+struct SumImpl<std::integer_sequence<T, N1, N2, Ns...>> {
+  static constexpr T Value =
+      N1 + N2 + SumImpl<std::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<std::integer_sequence<T, N1, N2, N3, N4, Ns...>> {
+  static constexpr T Value =
+      N1 + N2 + N3 + N4 + SumImpl<std::integer_sequence<T, Ns...>>::Value;
+};
+
+// Only one number is left. 'Value' is just that number ('recursion' ends).
+template <typename T, T N>
+struct SumImpl<std::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<std::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,
+                         std::integer_sequence<T, N, Ns...>,
+                         std::integer_sequence<T, Rs...>> {
+  using Type =
+      typename ExclusiveScanImpl<T,
+                                 Sum + N,
+                                 std::integer_sequence<T, Ns...>,
+                                 std::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, std::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, std::integer_sequence<T>>::Type;
+};
+
+// Helper to use exclusive scan without typename.
+template <typename Seq>
+using ExclusiveScan = typename ExclusiveScanT<Seq>::Type;
+
+// Removes all elements from a integer sequence corresponding to specified
+// ValueToRemove.
+//
+// This type should not be used directly but instead RemoveValue.
+template <typename T, T ValueToRemove, typename... Sequence>
+struct RemoveValueImpl;
+
+// Final filtered sequence
+template <typename T, T ValueToRemove, T... Values>
+struct RemoveValueImpl<T,
+                       ValueToRemove,
+                       std::integer_sequence<T, Values...>,
+                       std::integer_sequence<T>> {
+  using type = std::integer_sequence<T, Values...>;
+};
+
+// Found a matching value
+template <typename T, T ValueToRemove, T... Head, T... Tail>
+struct RemoveValueImpl<T,
+                       ValueToRemove,
+                       std::integer_sequence<T, Head...>,
+                       std::integer_sequence<T, ValueToRemove, Tail...>>
+    : RemoveValueImpl<T,
+                      ValueToRemove,
+                      std::integer_sequence<T, Head...>,
+                      std::integer_sequence<T, Tail...>> {};
+
+// Move one element from the tail to the head
+template <typename T, T ValueToRemove, T... Head, T MiddleValue, T... Tail>
+struct RemoveValueImpl<T,
+                       ValueToRemove,
+                       std::integer_sequence<T, Head...>,
+                       std::integer_sequence<T, MiddleValue, Tail...>>
+    : RemoveValueImpl<T,
+                      ValueToRemove,
+                      std::integer_sequence<T, Head..., MiddleValue>,
+                      std::integer_sequence<T, Tail...>> {};
+
+// Start recursion by splitting the integer sequence into two separate ones
+template <typename T, T ValueToRemove, T... Tail>
+struct RemoveValueImpl<T, ValueToRemove, std::integer_sequence<T, Tail...>>
+    : RemoveValueImpl<T,
+                      ValueToRemove,
+                      std::integer_sequence<T>,
+                      std::integer_sequence<T, Tail...>> {};
+
+// RemoveValue takes an integer Sequence of arbitrary type and removes all
+// elements matching ValueToRemove.
+//
+// In contrast to RemoveValueImpl, this implementation deduces the value type
+// eliminating the need to specify it explicitly.
+//
+// As an example, RemoveValue<std::integer_sequence<int, 1, 2, 3>, 4>::type will
+// not transform the type of the original sequence. However,
+// RemoveValue<std::integer_sequence<int, 0, 0, 2>, 2>::type will generate a new
+// sequence of type std::integer_sequence<int, 0, 0> by removing the value 2.
+template <typename Sequence, typename Sequence::value_type ValueToRemove>
+struct RemoveValue
+    : RemoveValueImpl<typename Sequence::value_type, ValueToRemove, Sequence> {
+};
+
+// Convenience template alias for RemoveValue.
+template <typename Sequence, typename Sequence::value_type ValueToRemove>
+using RemoveValue_t = typename RemoveValue<Sequence, ValueToRemove>::type;
+
+// Determines whether the values of an integer sequence are all the same.
+//
+// The integer sequence must contain at least one value. The predicate is
+// undefined for empty sequences. The evaluation result of the predicate for a
+// sequence containing only one value is defined to be true.
+template <typename... Sequence>
+struct AreAllEqual;
+
+// The predicate result for a sequence containing one element is defined to be
+// true.
+template <typename T, T Value>
+struct AreAllEqual<std::integer_sequence<T, Value>> : std::true_type {};
+
+// Recursion end.
+template <typename T, T Value1, T Value2>
+struct AreAllEqual<std::integer_sequence<T, Value1, Value2>>
+    : std::integral_constant<bool, Value1 == Value2> {};
+
+// Recursion for sequences containing at least two elements.
+template <typename T, T Value1, T Value2, T... Values>
+// clang-format off
+struct AreAllEqual<std::integer_sequence<T, Value1, Value2, Values...> >
+    : std::integral_constant
+<
+    bool,
+    AreAllEqual<std::integer_sequence<T, Value1, Value2> >::value &&
+    AreAllEqual<std::integer_sequence<T, Value2, Values...> >::value
+>
+// clang-format on
+{};
+
+// Convenience variable template for AreAllEqual.
+template <class Sequence>
+constexpr bool AreAllEqual_v = AreAllEqual<Sequence>::value;
+
+// Predicate determining whether an integer sequence is either empty or all
+// values are equal.
+template <typename Sequence>
+struct IsEmptyOrAreAllEqual;
+
+// Empty case.
+template <typename T>
+struct IsEmptyOrAreAllEqual<std::integer_sequence<T>> : std::true_type {};
+
+// General case for sequences containing at least one value.
+template <typename T, T HeadValue, T... Values>
+struct IsEmptyOrAreAllEqual<std::integer_sequence<T, HeadValue, Values...>>
+    : AreAllEqual<std::integer_sequence<T, HeadValue, Values...>> {};
+
+// Convenience variable template for IsEmptyOrAreAllEqual.
+template <class Sequence>
+constexpr bool IsEmptyOrAreAllEqual_v = IsEmptyOrAreAllEqual<Sequence>::value;
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_

ceres-v2/include/internal/jet_traits.h

@@ -0,0 +1,223 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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: sergiu.deitsch@gmail.com (Sergiu Deitsch)
+//
+
+#ifndef CERES_PUBLIC_INTERNAL_JET_TRAITS_H_
+#define CERES_PUBLIC_INTERNAL_JET_TRAITS_H_
+
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#include "ceres/internal/integer_sequence_algorithm.h"
+#include "ceres/jet_fwd.h"
+
+namespace ceres {
+namespace internal {
+
+// Predicate that determines whether T is a Jet.
+template <typename T, typename E = void>
+struct IsJet : std::false_type {};
+
+template <typename T, int N>
+struct IsJet<Jet<T, N>> : std::true_type {};
+
+// Convenience variable template for IsJet.
+template <typename T>
+constexpr bool IsJet_v = IsJet<T>::value;
+
+// Predicate that determines whether any of the Types is a Jet.
+template <typename... Types>
+struct AreAnyJet : std::false_type {};
+
+template <typename T, typename... Types>
+struct AreAnyJet<T, Types...> : AreAnyJet<Types...> {};
+
+template <typename T, int N, typename... Types>
+struct AreAnyJet<Jet<T, N>, Types...> : std::true_type {};
+
+// Convenience variable template for AreAnyJet.
+template <typename... Types>
+constexpr bool AreAnyJet_v = AreAnyJet<Types...>::value;
+
+// Extracts the underlying floating-point from a type T.
+template <typename T, typename E = void>
+struct UnderlyingScalar {
+  using type = T;
+};
+
+template <typename T, int N>
+struct UnderlyingScalar<Jet<T, N>> : UnderlyingScalar<T> {};
+
+// Convenience template alias for UnderlyingScalar type trait.
+template <typename T>
+using UnderlyingScalar_t = typename UnderlyingScalar<T>::type;
+
+// Predicate determining whether all Types in the pack are the same.
+//
+// Specifically, the predicate applies std::is_same recursively to pairs of
+// Types in the pack.
+//
+// The predicate is defined only for template packs containing at least two
+// types.
+template <typename T1, typename T2, typename... Types>
+// clang-format off
+struct AreAllSame : std::integral_constant
+<
+    bool,
+    AreAllSame<T1, T2>::value &&
+    AreAllSame<T2, Types...>::value
+>
+// clang-format on
+{};
+
+// AreAllSame pairwise test.
+template <typename T1, typename T2>
+struct AreAllSame<T1, T2> : std::is_same<T1, T2> {};
+
+// Convenience variable template for AreAllSame.
+template <typename... Types>
+constexpr bool AreAllSame_v = AreAllSame<Types...>::value;
+
+// Determines the rank of a type. This allows to ensure that types passed as
+// arguments are compatible to each other. The rank of Jet is determined by the
+// dimensions of the dual part. The rank of a scalar is always 0.
+// Non-specialized types default to a rank of -1.
+template <typename T, typename E = void>
+struct Rank : std::integral_constant<int, -1> {};
+
+// The rank of a scalar is 0.
+template <typename T>
+struct Rank<T, std::enable_if_t<std::is_scalar<T>::value>>
+    : std::integral_constant<int, 0> {};
+
+// The rank of a Jet is given by its dimensionality.
+template <typename T, int N>
+struct Rank<Jet<T, N>> : std::integral_constant<int, N> {};
+
+// Convenience variable template for Rank.
+template <typename T>
+constexpr int Rank_v = Rank<T>::value;
+
+// Constructs an integer sequence of ranks for each of the Types in the pack.
+template <typename... Types>
+using Ranks_t = std::integer_sequence<int, Rank_v<Types>...>;
+
+// Returns the scalar part of a type. This overload acts as an identity.
+template <typename T>
+constexpr decltype(auto) AsScalar(T&& value) noexcept {
+  return std::forward<T>(value);
+}
+
+// Recursively unwraps the scalar part of a Jet until a non-Jet scalar type is
+// encountered.
+template <typename T, int N>
+constexpr decltype(auto) AsScalar(const Jet<T, N>& value) noexcept(
+    noexcept(AsScalar(value.a))) {
+  return AsScalar(value.a);
+}
+
+}  // namespace internal
+
+// Type trait ensuring at least one of the types is a Jet,
+// the underlying scalar types are the same and Jet dimensions match.
+//
+// The type trait can be further specialized if necessary.
+//
+// This trait is a candidate for a concept definition once C++20 features can
+// be used.
+template <typename... Types>
+// clang-format off
+struct CompatibleJetOperands : std::integral_constant
+<
+    bool,
+    // At least one of the types is a Jet
+    internal::AreAnyJet_v<Types...> &&
+    // The underlying floating-point types are exactly the same
+    internal::AreAllSame_v<internal::UnderlyingScalar_t<Types>...> &&
+    // Non-zero ranks of types are equal
+    internal::IsEmptyOrAreAllEqual_v<internal::RemoveValue_t<internal::Ranks_t<Types...>, 0>>
+>
+// clang-format on
+{};
+
+// Single Jet operand is always compatible.
+template <typename T, int N>
+struct CompatibleJetOperands<Jet<T, N>> : std::true_type {};
+
+// Single non-Jet operand is always incompatible.
+template <typename T>
+struct CompatibleJetOperands<T> : std::false_type {};
+
+// Empty operands are always incompatible.
+template <>
+struct CompatibleJetOperands<> : std::false_type {};
+
+// Convenience variable template ensuring at least one of the types is a Jet,
+// the underlying scalar types are the same and Jet dimensions match.
+//
+// This trait is a candidate for a concept definition once C++20 features can
+// be used.
+template <typename... Types>
+constexpr bool CompatibleJetOperands_v = CompatibleJetOperands<Types...>::value;
+
+// Type trait ensuring at least one of the types is a Jet,
+// the underlying scalar types are compatible among each other and Jet
+// dimensions match.
+//
+// The type trait can be further specialized if necessary.
+//
+// This trait is a candidate for a concept definition once C++20 features can
+// be used.
+template <typename... Types>
+// clang-format off
+struct PromotableJetOperands : std::integral_constant
+<
+    bool,
+    // Types can be compatible among each other
+    internal::AreAnyJet_v<Types...> &&
+    // Non-zero ranks of types are equal
+    internal::IsEmptyOrAreAllEqual_v<internal::RemoveValue_t<internal::Ranks_t<Types...>, 0>>
+>
+// clang-format on
+{};
+
+// Convenience variable template ensuring at least one of the types is a Jet,
+// the underlying scalar types are compatible among each other and Jet
+// dimensions match.
+//
+// This trait is a candidate for a concept definition once C++20 features can
+// be used.
+template <typename... Types>
+constexpr bool PromotableJetOperands_v = PromotableJetOperands<Types...>::value;
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_JET_TRAITS_H_

ceres-v2/include/internal/line_parameterization.h

@@ -0,0 +1,183 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2020 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_LINE_PARAMETERIZATION_H_
+#define CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_
+
+#include "householder_vector.h"
+
+namespace ceres {
+
+template <int AmbientSpaceDimension>
+bool LineParameterization<AmbientSpaceDimension>::Plus(
+    const double* x_ptr,
+    const double* delta_ptr,
+    double* x_plus_delta_ptr) const {
+  // We seek a box plus operator of the form
+  //
+  //   [o*, d*] = Plus([o, d], [delta_o, delta_d])
+  //
+  // where o is the origin point, d is the direction vector, delta_o is
+  // the delta of the origin point and delta_d the delta of the direction and
+  // o* and d* is the updated origin point and direction.
+  //
+  // We separate the Plus operator into the origin point and directional part
+  //   d* = Plus_d(d, delta_d)
+  //   o* = Plus_o(o, d, delta_o)
+  //
+  // The direction update function Plus_d is the same as for the homogeneous
+  // vector parameterization:
+  //
+  //   d* = H_{v(d)} [0.5 sinc(0.5 |delta_d|) delta_d, cos(0.5 |delta_d|)]^T
+  //
+  // where H is the householder matrix
+  //   H_{v} = I - (2 / |v|^2) v v^T
+  // and
+  //   v(d) = d - sign(d_n) |d| e_n.
+  //
+  // The origin point update function Plus_o is defined as
+  //
+  //   o* = o + H_{v(d)} [0.5 delta_o, 0]^T.
+
+  static constexpr int kDim = AmbientSpaceDimension;
+  using AmbientVector = Eigen::Matrix<double, kDim, 1>;
+  using AmbientVectorRef = Eigen::Map<Eigen::Matrix<double, kDim, 1>>;
+  using ConstAmbientVectorRef =
+      Eigen::Map<const Eigen::Matrix<double, kDim, 1>>;
+  using ConstTangentVectorRef =
+      Eigen::Map<const Eigen::Matrix<double, kDim - 1, 1>>;
+
+  ConstAmbientVectorRef o(x_ptr);
+  ConstAmbientVectorRef d(x_ptr + kDim);
+
+  ConstTangentVectorRef delta_o(delta_ptr);
+  ConstTangentVectorRef delta_d(delta_ptr + kDim - 1);
+  AmbientVectorRef o_plus_delta(x_plus_delta_ptr);
+  AmbientVectorRef d_plus_delta(x_plus_delta_ptr + kDim);
+
+  const double norm_delta_d = delta_d.norm();
+
+  o_plus_delta = o;
+
+  // Shortcut for zero delta direction.
+  if (norm_delta_d == 0.0) {
+    d_plus_delta = d;
+
+    if (delta_o.isZero(0.0)) {
+      return true;
+    }
+  }
+
+  // Calculate the householder transformation which is needed for f_d and f_o.
+  AmbientVector v;
+  double beta;
+
+  // NOTE: The explicit template arguments are needed here because
+  // ComputeHouseholderVector is templated and some versions of MSVC
+  // have trouble deducing the type of v automatically.
+  internal::ComputeHouseholderVector<ConstAmbientVectorRef, double, kDim>(
+      d, &v, &beta);
+
+  if (norm_delta_d != 0.0) {
+    // Map the delta from the minimum representation to the over parameterized
+    // homogeneous vector. See section A6.9.2 on page 624 of Hartley & Zisserman
+    // (2nd Edition) for a detailed description.  Note there is a typo on Page
+    // 625, line 4 so check the book errata.
+    const double norm_delta_div_2 = 0.5 * norm_delta_d;
+    const double sin_delta_by_delta =
+        std::sin(norm_delta_div_2) / norm_delta_div_2;
+
+    // Apply the delta update to remain on the unit sphere. See section A6.9.3
+    // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed
+    // description.
+    AmbientVector y;
+    y.template head<kDim - 1>() = 0.5 * sin_delta_by_delta * delta_d;
+    y[kDim - 1] = std::cos(norm_delta_div_2);
+
+    d_plus_delta = d.norm() * (y - v * (beta * (v.transpose() * y)));
+  }
+
+  // The null space is in the direction of the line, so the tangent space is
+  // perpendicular to the line direction. This is achieved by using the
+  // householder matrix of the direction and allow only movements
+  // perpendicular to e_n.
+  //
+  // The factor of 0.5 is used to be consistent with the line direction
+  // update.
+  AmbientVector y;
+  y << 0.5 * delta_o, 0;
+  o_plus_delta += y - v * (beta * (v.transpose() * y));
+
+  return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineParameterization<AmbientSpaceDimension>::ComputeJacobian(
+    const double* x_ptr, double* jacobian_ptr) const {
+  static constexpr int kDim = AmbientSpaceDimension;
+  using AmbientVector = Eigen::Matrix<double, kDim, 1>;
+  using ConstAmbientVectorRef =
+      Eigen::Map<const Eigen::Matrix<double, kDim, 1>>;
+  using MatrixRef = Eigen::Map<
+      Eigen::Matrix<double, 2 * kDim, 2 * (kDim - 1), Eigen::RowMajor>>;
+
+  ConstAmbientVectorRef d(x_ptr + kDim);
+  MatrixRef jacobian(jacobian_ptr);
+
+  // Clear the Jacobian as only half of the matrix is not zero.
+  jacobian.setZero();
+
+  AmbientVector v;
+  double beta;
+
+  // NOTE: The explicit template arguments are needed here because
+  // ComputeHouseholderVector is templated and some versions of MSVC
+  // have trouble deducing the type of v automatically.
+  internal::ComputeHouseholderVector<ConstAmbientVectorRef, double, kDim>(
+      d, &v, &beta);
+
+  // The Jacobian is equal to J = 0.5 * H.leftCols(kDim - 1) where H is
+  // the Householder matrix (H = I - beta * v * v') for the origin point. For
+  // the line direction part the Jacobian is scaled by the norm of the
+  // direction.
+  for (int i = 0; i < kDim - 1; ++i) {
+    jacobian.block(0, i, kDim, 1) = -0.5 * beta * v(i) * v;
+    jacobian.col(i)(i) += 0.5;
+  }
+
+  jacobian.template block<kDim, kDim - 1>(kDim, kDim - 1) =
+      jacobian.template block<kDim, kDim - 1>(0, 0) * d.norm();
+  return true;
+}
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_

ceres-v2/include/internal/memory.h

@@ -0,0 +1,90 @@
+// Copyright 2017 The Abseil Authors.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// -----------------------------------------------------------------------------
+// File: memory.h
+// -----------------------------------------------------------------------------
+//
+// This header file contains utility functions for managing the creation and
+// conversion of smart pointers. This file is an extension to the C++
+// standard <memory> library header file.
+
+#ifndef CERES_PUBLIC_INTERNAL_MEMORY_H_
+#define CERES_PUBLIC_INTERNAL_MEMORY_H_
+
+#include <memory>
+
+#ifdef CERES_HAVE_EXCEPTIONS
+#define CERES_INTERNAL_TRY try
+#define CERES_INTERNAL_CATCH_ANY catch (...)
+#define CERES_INTERNAL_RETHROW \
+  do {                         \
+    throw;                     \
+  } while (false)
+#else  // CERES_HAVE_EXCEPTIONS
+#define CERES_INTERNAL_TRY if (true)
+#define CERES_INTERNAL_CATCH_ANY else if (false)
+#define CERES_INTERNAL_RETHROW \
+  do {                         \
+  } while (false)
+#endif  // CERES_HAVE_EXCEPTIONS
+
+namespace ceres {
+namespace internal {
+
+template <typename Allocator, typename Iterator, typename... Args>
+void ConstructRange(Allocator& alloc,
+                    Iterator first,
+                    Iterator last,
+                    const Args&... args) {
+  for (Iterator cur = first; cur != last; ++cur) {
+    CERES_INTERNAL_TRY {
+      std::allocator_traits<Allocator>::construct(
+          alloc, std::addressof(*cur), args...);
+    }
+    CERES_INTERNAL_CATCH_ANY {
+      while (cur != first) {
+        --cur;
+        std::allocator_traits<Allocator>::destroy(alloc, std::addressof(*cur));
+      }
+      CERES_INTERNAL_RETHROW;
+    }
+  }
+}
+
+template <typename Allocator, typename Iterator, typename InputIterator>
+void CopyRange(Allocator& alloc,
+               Iterator destination,
+               InputIterator first,
+               InputIterator last) {
+  for (Iterator cur = destination; first != last;
+       static_cast<void>(++cur), static_cast<void>(++first)) {
+    CERES_INTERNAL_TRY {
+      std::allocator_traits<Allocator>::construct(
+          alloc, std::addressof(*cur), *first);
+    }
+    CERES_INTERNAL_CATCH_ANY {
+      while (cur != destination) {
+        --cur;
+        std::allocator_traits<Allocator>::destroy(alloc, std::addressof(*cur));
+      }
+      CERES_INTERNAL_RETHROW;
+    }
+  }
+}
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_MEMORY_H_

ceres-v2/include/internal/numeric_diff.h

@@ -0,0 +1,508 @@
+// 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 <utility>
+
+#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::ColMajor;
+    using Eigen::Map;
+    using Eigen::Matrix;
+    using Eigen::RowMajor;
+
+    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);
+
+    using ResidualVector = Matrix<double, kNumResiduals, 1>;
+    using ParameterVector = Matrix<double, kParameterBlockSize, 1>;
+
+    // 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.
+    using JacobianMatrix =
+        Matrix<double,
+               kNumResiduals,
+               kParameterBlockSize,
+               (kParameterBlockSize == 1) ? ColMajor : RowMajor>;
+
+    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.data(),
+                                  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.data(),
+                                           residual_array.data())) {
+          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.data(),
+                                    residual_array.data())) {
+          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;
+
+    using ResidualVector = Matrix<double, kNumResiduals, 1>;
+    using ParameterVector = Matrix<double, kParameterBlockSize, 1>;
+
+    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::aligned_allocator;
+    using Eigen::Map;
+    using Eigen::Matrix;
+
+    using ResidualVector = Matrix<double, kNumResiduals, 1>;
+    using ResidualCandidateMatrix =
+        Matrix<double, kNumResiduals, Eigen::Dynamic>;
+    using ParameterVector = Matrix<double, kParameterBlockSize, 1>;
+
+    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,
+                                          std::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,
+                                              std::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,
+                                          std::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_

ceres-v2/include/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 <utility>
+
+#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(std::integer_sequence<int>) {
+  return true;
+}
+
+template <int N, int... Ts>
+constexpr bool IsValidParameterDimensionSequence(
+    std::integer_sequence<int, N, Ts...>) {
+  return (N <= 0) ? false
+                  : IsValidParameterDimensionSequence(
+                        std::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 = std::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<std::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, std::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_

ceres-v2/include/internal/port.h

@@ -0,0 +1,88 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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_
+
+// A macro to mark a function/variable/class as deprecated.
+// We use compiler specific attributes rather than the c++
+// attribute because they do not mix well with each other.
+#if defined(_MSC_VER)
+#define CERES_DEPRECATED_WITH_MSG(message) __declspec(deprecated(message))
+#elif defined(__GNUC__)
+#define CERES_DEPRECATED_WITH_MSG(message) __attribute__((deprecated(message)))
+#else
+// In the worst case fall back to c++ attribute.
+#define CERES_DEPRECATED_WITH_MSG(message) [[deprecated(message)]]
+#endif
+
+#ifndef CERES_GET_FLAG
+#define CERES_GET_FLAG(X) X
+#endif
+
+// Indicates whether C++17 is currently active
+#ifndef CERES_HAS_CPP17
+#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L)
+#define CERES_HAS_CPP17
+#endif  // __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >=
+        // 201703L)
+#endif  // !defined(CERES_HAS_CPP17)
+
+// Indicates whether C++20 is currently active
+#ifndef CERES_HAS_CPP20
+#if __cplusplus >= 202002L || (defined(_MSVC_LANG) && _MSVC_LANG >= 202002L)
+#define CERES_HAS_CPP20
+#endif  // __cplusplus >= 202002L || (defined(_MSVC_LANG) && _MSVC_LANG >=
+        // 202002L)
+#endif  // !defined(CERES_HAS_CPP20)
+
+// Prevents symbols from being substituted by the corresponding macro definition
+// under the same name. For instance, min and max are defined as macros on
+// Windows (unless NOMINMAX is defined) which causes compilation errors when
+// defining or referencing symbols under the same name.
+//
+// To be robust in all cases particularly when NOMINMAX cannot be used, use this
+// macro to annotate min/max declarations/definitions. Examples:
+//
+//   int max CERES_PREVENT_MACRO_SUBSTITUTION();
+//   min CERES_PREVENT_MACRO_SUBSTITUTION(a, b);
+//   max CERES_PREVENT_MACRO_SUBSTITUTION(a, b);
+//
+// NOTE: In case the symbols for which the substitution must be prevented are
+// used within another macro, the substitution must be inhibited using parens as
+//
+//   (std::numerical_limits<double>::max)()
+//
+// since the helper macro will not work here. Do not use this technique in
+// general case, because it will prevent argument-dependent lookup (ADL).
+//
+#define CERES_PREVENT_MACRO_SUBSTITUTION  // Yes, it's empty
+
+#endif  // CERES_PUBLIC_INTERNAL_PORT_H_

ceres-v2/include/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

ceres-v2/include/internal/sphere_manifold_functions.h

@@ -0,0 +1,162 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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 (Mike Vitus)
+//         jodebo_beck@gmx.de (Johannes Beck)
+
+#ifndef CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_HELPERS_H_
+#define CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_HELPERS_H_
+
+#include "ceres/internal/householder_vector.h"
+
+// This module contains functions to compute the SphereManifold plus and minus
+// operator and their Jacobians.
+//
+// As the parameters to these functions are shared between them, they are
+// described here: The following variable names are used:
+//  Plus(x, delta) = x + delta = x_plus_delta,
+//  Minus(y, x) = y - x = y_minus_x.
+//
+// The remaining ones are v and beta which describe the Householder
+// transformation of x, and norm_delta which is the norm of delta.
+//
+// The types of x, y, x_plus_delta and y_minus_x need to be equivalent to
+// Eigen::Matrix<double, AmbientSpaceDimension, 1> and the type of delta needs
+// to be equivalent to Eigen::Matrix<double, TangentSpaceDimension, 1>.
+//
+// The type of Jacobian plus needs to be equivalent to Eigen::Matrix<double,
+// AmbientSpaceDimension, TangentSpaceDimension, Eigen::RowMajor> and for
+// Jacobian minus Eigen::Matrix<double, TangentSpaceDimension,
+// AmbientSpaceDimension, Eigen::RowMajor>.
+//
+// For all vector / matrix inputs and outputs, template parameters are
+// used in order to allow also Eigen::Ref and Eigen block expressions to
+// be passed to the function.
+
+namespace ceres {
+namespace internal {
+
+template <typename VT, typename XT, typename DeltaT, typename XPlusDeltaT>
+inline void ComputeSphereManifoldPlus(const VT& v,
+                                      double beta,
+                                      const XT& x,
+                                      const DeltaT& delta,
+                                      double norm_delta,
+                                      XPlusDeltaT* x_plus_delta) {
+  constexpr int AmbientDim = VT::RowsAtCompileTime;
+
+  // Map the delta from the minimum representation to the over parameterized
+  // homogeneous vector. See B.2 p.25 equation (106) - (107) for more details.
+  const double norm_delta_div_2 = 0.5 * norm_delta;
+  const double sin_delta_by_delta =
+      std::sin(norm_delta_div_2) / norm_delta_div_2;
+
+  Eigen::Matrix<double, AmbientDim, 1> y(v.size());
+  y << 0.5 * sin_delta_by_delta * delta, std::cos(norm_delta_div_2);
+
+  // Apply the delta update to remain on the sphere.
+  *x_plus_delta = x.norm() * ApplyHouseholderVector(y, v, beta);
+}
+
+template <typename VT, typename JacobianT>
+inline void ComputeSphereManifoldPlusJacobian(const VT& x,
+                                              JacobianT* jacobian) {
+  constexpr int AmbientSpaceDim = VT::RowsAtCompileTime;
+  using AmbientVector = Eigen::Matrix<double, AmbientSpaceDim, 1>;
+  const int ambient_size = x.size();
+  const int tangent_size = x.size() - 1;
+
+  AmbientVector v(ambient_size);
+  double beta;
+
+  // NOTE: The explicit template arguments are needed here because
+  // ComputeHouseholderVector is templated and some versions of MSVC
+  // have trouble deducing the type of v automatically.
+  ComputeHouseholderVector<VT, double, AmbientSpaceDim>(x, &v, &beta);
+
+  // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the
+  // Householder matrix (H = I - beta * v * v').
+  for (int i = 0; i < tangent_size; ++i) {
+    (*jacobian).col(i) = -0.5 * beta * v(i) * v;
+    (*jacobian)(i, i) += 0.5;
+  }
+  (*jacobian) *= x.norm();
+}
+
+template <typename VT, typename XT, typename YT, typename YMinusXT>
+inline void ComputeSphereManifoldMinus(
+    const VT& v, double beta, const XT& x, const YT& y, YMinusXT* y_minus_x) {
+  constexpr int AmbientSpaceDim = VT::RowsAtCompileTime;
+  constexpr int TangentSpaceDim =
+      AmbientSpaceDim == Eigen::Dynamic ? Eigen::Dynamic : AmbientSpaceDim - 1;
+  using AmbientVector = Eigen::Matrix<double, AmbientSpaceDim, 1>;
+
+  const int tanget_size = v.size() - 1;
+
+  const AmbientVector hy = ApplyHouseholderVector(y, v, beta) / x.norm();
+
+  // Calculate y - x. See B.2 p.25 equation (108).
+  double y_last = hy[tanget_size];
+  double hy_norm = hy.template head<TangentSpaceDim>(tanget_size).norm();
+  if (hy_norm == 0.0) {
+    y_minus_x->setZero();
+  } else {
+    *y_minus_x = 2.0 * std::atan2(hy_norm, y_last) / hy_norm *
+                 hy.template head<TangentSpaceDim>(tanget_size);
+  }
+}
+
+template <typename VT, typename JacobianT>
+inline void ComputeSphereManifoldMinusJacobian(const VT& x,
+                                               JacobianT* jacobian) {
+  constexpr int AmbientSpaceDim = VT::RowsAtCompileTime;
+  using AmbientVector = Eigen::Matrix<double, AmbientSpaceDim, 1>;
+  const int ambient_size = x.size();
+  const int tangent_size = x.size() - 1;
+
+  AmbientVector v(ambient_size);
+  double beta;
+
+  // NOTE: The explicit template arguments are needed here because
+  // ComputeHouseholderVector is templated and some versions of MSVC
+  // have trouble deducing the type of v automatically.
+  ComputeHouseholderVector<VT, double, AmbientSpaceDim>(x, &v, &beta);
+
+  // The Jacobian is equal to J = 2.0 * H.leftCols(size_ - 1) where H is the
+  // Householder matrix (H = I - beta * v * v').
+  for (int i = 0; i < tangent_size; ++i) {
+    (*jacobian).row(i) = -2.0 * beta * v(i) * v;
+    (*jacobian)(i, i) += 2.0;
+  }
+  (*jacobian) /= x.norm();
+}
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif

ceres-v2/include/internal/variadic_evaluate.h

@@ -0,0 +1,113 @@
+// 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 <cstddef>
+#include <type_traits>
+#include <utility>
+
+#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*/,
+                                 std::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*/,
+                                 std::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 =
+      std::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_

ceres-v2/include/iteration_callback.h

@@ -0,0 +1,204 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/types.h"
+
+namespace ceres {
+
+// This struct describes the state of the optimizer after each
+// iteration of the minimization.
+struct CERES_EXPORT IterationSummary {
+  // Current iteration number.
+  int iteration = 0;
+
+  // 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 = false;
+
+  // 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 = false;
+
+  // 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 = false;
+
+  // Value of the objective function.
+  double cost = 0.0;
+
+  // Change in the value of the objective function in this
+  // iteration. This can be positive or negative.
+  double cost_change = 0.0;
+
+  // Infinity norm of the gradient vector.
+  double gradient_max_norm = 0.0;
+
+  // 2-norm of the gradient vector.
+  double gradient_norm = 0.0;
+
+  // 2-norm of the size of the step computed by the optimization
+  // algorithm.
+  double step_norm = 0.0;
+
+  // 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 = 0.0;
+
+  // 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 = 0.0;
+
+  // 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 = 0.0;
+
+  // Step sized computed by the line search algorithm.
+  double step_size = 0.0;
+
+  // Number of function value evaluations used by the line search algorithm.
+  int line_search_function_evaluations = 0;
+
+  // Number of function gradient evaluations used by the line search algorithm.
+  int line_search_gradient_evaluations = 0;
+
+  // Number of iterations taken by the line search algorithm.
+  int line_search_iterations = 0;
+
+  // Number of iterations taken by the linear solver to solve for the
+  // Newton step.
+  int linear_solver_iterations = 0;
+
+  // All times reported below are wall times.
+
+  // Time (in seconds) spent inside the minimizer loop in the current
+  // iteration.
+  double iteration_time_in_seconds = 0.0;
+
+  // Time (in seconds) spent inside the trust region step solver.
+  double step_solver_time_in_seconds = 0.0;
+
+  // Time (in seconds) since the user called Solve().
+  double cumulative_time_in_seconds = 0.0;
+};
+
+// 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) {}
+//
+//     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_

ceres-v2/include/jet.h

@@ -0,0 +1,1387 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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 <complex>
+#include <iosfwd>
+#include <iostream>  // NOLINT
+#include <limits>
+#include <numeric>
+#include <string>
+#include <type_traits>
+
+#include "Eigen/Core"
+#include "ceres/internal/jet_traits.h"
+#include "ceres/internal/port.h"
+#include "ceres/jet_fwd.h"
+
+// Here we provide partial specializations of std::common_type for the Jet class
+// to allow determining a Jet type with a common underlying arithmetic type.
+// Such an arithmetic type can be either a scalar or an another Jet. An example
+// for a common type, say, between a float and a Jet<double, N> is a Jet<double,
+// N> (i.e., std::common_type_t<float, ceres::Jet<double, N>> and
+// ceres::Jet<double, N> refer to the same type.)
+//
+// The partial specialization are also used for determining compatible types by
+// means of SFINAE and thus allow such types to be expressed as operands of
+// logical comparison operators. Missing (partial) specialization of
+// std::common_type for a particular (custom) type will therefore disable the
+// use of comparison operators defined by Ceres.
+//
+// Since these partial specializations are used as SFINAE constraints, they
+// enable standard promotion rules between various scalar types and consequently
+// their use in comparison against a Jet without providing implicit
+// conversions from a scalar, such as an int, to a Jet (see the implementation
+// of logical comparison operators below).
+
+template <typename T, int N, typename U>
+struct std::common_type<T, ceres::Jet<U, N>> {
+  using type = ceres::Jet<common_type_t<T, U>, N>;
+};
+
+template <typename T, int N, typename U>
+struct std::common_type<ceres::Jet<T, N>, U> {
+  using type = ceres::Jet<common_type_t<T, U>, N>;
+};
+
+template <typename T, int N, typename U>
+struct std::common_type<ceres::Jet<T, N>, ceres::Jet<U, N>> {
+  using type = ceres::Jet<common_type_t<T, U>, N>;
+};
+
+namespace ceres {
+
+template <typename T, int N>
+struct Jet {
+  enum { DIMENSION = N };
+  using Scalar = T;
+
+  // 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.setConstant(Scalar()); }
+
+  // Constructor from scalar: a + 0.
+  explicit Jet(const T& value) {
+    a = value;
+    v.setConstant(Scalar());
+  }
+
+  // Constructor from scalar plus variable: a + t_i.
+  Jet(const T& value, int k) {
+    a = value;
+    v.setConstant(Scalar());
+    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.
+  Eigen::Matrix<T, N, 1> v;
+
+  // This struct needs to have an Eigen aligned operator new as it contains
+  // fixed-size Eigen types.
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+// 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. At least one of the
+// operands must be a Jet. Promotable scalars (e.g., int, float, double etc.)
+// can appear on either side of the operator. std::common_type_t is used as an
+// SFINAE constraint to selectively enable compatible operand types. This allows
+// comparison, for instance, against int literals without implicit conversion.
+// In case the Jet arithmetic type is a Jet itself, a recursive expansion of Jet
+// value is performed.
+#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op)                            \
+  template <typename Lhs,                                                   \
+            typename Rhs,                                                   \
+            std::enable_if_t<PromotableJetOperands_v<Lhs, Rhs>>* = nullptr> \
+  constexpr bool operator op(const Lhs& f, const Rhs& g) noexcept(          \
+      noexcept(internal::AsScalar(f) op internal::AsScalar(g))) {           \
+    using internal::AsScalar;                                               \
+    return AsScalar(f) op AsScalar(g);                                      \
+  }
+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::copysign;
+using std::cos;
+using std::cosh;
+using std::erf;
+using std::erfc;
+using std::exp;
+using std::exp2;
+using std::expm1;
+using std::fdim;
+using std::floor;
+using std::fma;
+using std::fmax;
+using std::fmin;
+using std::fpclassify;
+using std::hypot;
+using std::isfinite;
+using std::isinf;
+using std::isnan;
+using std::isnormal;
+using std::log;
+using std::log10;
+using std::log1p;
+using std::log2;
+using std::norm;
+using std::pow;
+using std::signbit;
+using std::sin;
+using std::sinh;
+using std::sqrt;
+using std::tan;
+using std::tanh;
+
+// MSVC (up to 1930) defines quiet comparison functions as template functions
+// which causes compilation errors due to ambiguity in the template parameter
+// type resolution for using declarations in the ceres namespace. Workaround the
+// issue by defining specific overload and bypass MSVC standard library
+// definitions.
+#if defined(_MSC_VER)
+inline bool isgreater(double lhs,
+                      double rhs) noexcept(noexcept(std::isgreater(lhs, rhs))) {
+  return std::isgreater(lhs, rhs);
+}
+inline bool isless(double lhs,
+                   double rhs) noexcept(noexcept(std::isless(lhs, rhs))) {
+  return std::isless(lhs, rhs);
+}
+inline bool islessequal(double lhs,
+                        double rhs) noexcept(noexcept(std::islessequal(lhs,
+                                                                       rhs))) {
+  return std::islessequal(lhs, rhs);
+}
+inline bool isgreaterequal(double lhs, double rhs) noexcept(
+    noexcept(std::isgreaterequal(lhs, rhs))) {
+  return std::isgreaterequal(lhs, rhs);
+}
+inline bool islessgreater(double lhs, double rhs) noexcept(
+    noexcept(std::islessgreater(lhs, rhs))) {
+  return std::islessgreater(lhs, rhs);
+}
+inline bool isunordered(double lhs,
+                        double rhs) noexcept(noexcept(std::isunordered(lhs,
+                                                                       rhs))) {
+  return std::isunordered(lhs, rhs);
+}
+#else
+using std::isgreater;
+using std::isgreaterequal;
+using std::isless;
+using std::islessequal;
+using std::islessgreater;
+using std::isunordered;
+#endif
+
+#ifdef CERES_HAS_CPP20
+using std::lerp;
+using std::midpoint;
+#endif  // defined(CERES_HAS_CPP20)
+
+// Legacy names from pre-C++11 days.
+// clang-format off
+CERES_DEPRECATED_WITH_MSG("ceres::IsFinite will be removed in a future Ceres Solver release. Please use ceres::isfinite.")
+inline bool IsFinite(double x)   { return std::isfinite(x); }
+CERES_DEPRECATED_WITH_MSG("ceres::IsInfinite will be removed in a future Ceres Solver release. Please use ceres::isinf.")
+inline bool IsInfinite(double x) { return std::isinf(x);    }
+CERES_DEPRECATED_WITH_MSG("ceres::IsNaN will be removed in a future Ceres Solver release. Please use ceres::isnan.")
+inline bool IsNaN(double x)      { return std::isnan(x);    }
+CERES_DEPRECATED_WITH_MSG("ceres::IsNormal will be removed in a future Ceres Solver release. Please use ceres::isnormal.")
+inline bool IsNormal(double x)   { return std::isnormal(x); }
+// clang-format on
+
+// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
+
+// abs(x + h) ~= abs(x) + sgn(x)h
+template <typename T, int N>
+inline Jet<T, N> abs(const Jet<T, N>& f) {
+  return Jet<T, N>(abs(f.a), copysign(T(1), f.a) * f.v);
+}
+
+// copysign(a, b) composes a float with the magnitude of a and the sign of b.
+// Therefore, the function can be formally defined as
+//
+//   copysign(a, b) = sgn(b)|a|
+//
+// where
+//
+//   d/dx |x| = sgn(x)
+//   d/dx sgn(x) = 2δ(x)
+//
+// sgn(x) being the signum function. Differentiating copysign(a, b) with respect
+// to a and b gives:
+//
+//   d/da sgn(b)|a| = sgn(a) sgn(b)
+//   d/db sgn(b)|a| = 2|a|δ(b)
+//
+// with the dual representation given by
+//
+//   copysign(a + da, b + db) ~= sgn(b)|a| + (sgn(a)sgn(b) da + 2|a|δ(b) db)
+//
+// where δ(b) is the Dirac delta function.
+template <typename T, int N>
+inline Jet<T, N> copysign(const Jet<T, N>& f, const Jet<T, N> g) {
+  // The Dirac delta function  δ(b) is undefined at b=0 (here it's
+  // infinite) and 0 everywhere else.
+  T d = fpclassify(g) == FP_ZERO ? std::numeric_limits<T>::infinity() : T(0);
+  T sa = copysign(T(1), f.a);  // sgn(a)
+  T sb = copysign(T(1), g.a);  // sgn(b)
+  // The second part of the infinitesimal is 2|a|δ(b) which is either infinity
+  // or 0 unless a or any of the values of the b infinitesimal are 0. In the
+  // latter case, the corresponding values become NaNs (multiplying 0 by
+  // infinity gives NaN). We drop the constant factor 2 since it does not change
+  // the result (its values will still be either 0, infinity or NaN).
+  return Jet<T, N>(copysign(f.a, g.a), sa * sb * f.v + abs(f.a) * d * g.v);
+}
+
+// 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);
+}
+
+// log10(a + h) ~= log10(a) + h / (a log(10))
+template <typename T, int N>
+inline Jet<T, N> log10(const Jet<T, N>& f) {
+  // Most compilers will expand log(10) to a constant.
+  const T a_inverse = T(1.0) / (f.a * log(T(10.0)));
+  return Jet<T, N>(log10(f.a), f.v * a_inverse);
+}
+
+// log1p(a + h) ~= log1p(a) + h / (1 + a)
+template <typename T, int N>
+inline Jet<T, N> log1p(const Jet<T, N>& f) {
+  const T a_inverse = T(1.0) / (T(1.0) + f.a);
+  return Jet<T, N>(log1p(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);
+}
+
+// expm1(a + h) ~= expm1(a) + exp(a) h
+template <typename T, int N>
+inline Jet<T, N> expm1(const Jet<T, N>& f) {
+  const T tmp = expm1(f.a);
+  const T expa = tmp + T(1.0);  // exp(a) = expm1(a) + 1
+  return Jet<T, N>(tmp, expa * 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);
+}
+
+#ifdef CERES_HAS_CPP17
+// Like sqrt(x^2 + y^2 + z^2),
+// but acts to prevent underflow/overflow for small/large x/y/z.
+// Note that the function is non-smooth at x=y=z=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,
+                       const Jet<T, N>& z) {
+  // d/da sqrt(a) = 0.5 / sqrt(a)
+  // d/dx x^2 + y^2 + z^2 = 2x
+  // So by the chain rule:
+  // d/dx sqrt(x^2 + y^2 + z^2)
+  //    = 0.5 / sqrt(x^2 + y^2 + z^2) * 2x
+  //    = x / sqrt(x^2 + y^2 + z^2)
+  // d/dy sqrt(x^2 + y^2 + z^2) = y / sqrt(x^2 + y^2 + z^2)
+  // d/dz sqrt(x^2 + y^2 + z^2) = z / sqrt(x^2 + y^2 + z^2)
+  const T tmp = hypot(x.a, y.a, z.a);
+  return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v + z.a / tmp * z.v);
+}
+#endif  // defined(CERES_HAS_CPP17)
+
+// Like x * y + z but rounded only once.
+template <typename T, int N>
+inline Jet<T, N> fma(const Jet<T, N>& x,
+                     const Jet<T, N>& y,
+                     const Jet<T, N>& z) {
+  // d/dx fma(x, y, z) = y
+  // d/dy fma(x, y, z) = x
+  // d/dz fma(x, y, z) = 1
+  return Jet<T, N>(fma(x.a, y.a, z.a), y.a * x.v + x.a * y.v + z.v);
+}
+
+// Returns the larger of the two arguments. NaNs are treated as missing data.
+//
+// NOTE: This function is NOT subject to any of the error conditions specified
+// in `math_errhandling`.
+template <typename Lhs,
+          typename Rhs,
+          std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline decltype(auto) fmax(const Lhs& f, const Rhs& g) {
+  using J = std::common_type_t<Lhs, Rhs>;
+  return (isnan(g) || isgreater(f, g)) ? J{f} : J{g};
+}
+
+// Returns the smaller of the two arguments. NaNs are treated as missing data.
+//
+// NOTE: This function is NOT subject to any of the error conditions specified
+// in `math_errhandling`.
+template <typename Lhs,
+          typename Rhs,
+          std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline decltype(auto) fmin(const Lhs& f, const Rhs& g) {
+  using J = std::common_type_t<Lhs, Rhs>;
+  return (isnan(f) || isless(g, f)) ? J{g} : J{f};
+}
+
+// Returns the positive difference (f - g) of two arguments and zero if f <= g.
+// If at least one argument is NaN, a NaN is return.
+//
+// NOTE At least one of the argument types must be a Jet, the other one can be a
+// scalar. In case both arguments are Jets, their dimensionality must match.
+template <typename Lhs,
+          typename Rhs,
+          std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline decltype(auto) fdim(const Lhs& f, const Rhs& g) {
+  using J = std::common_type_t<Lhs, Rhs>;
+  if (isnan(f) || isnan(g)) {
+    return std::numeric_limits<J>::quiet_NaN();
+  }
+  return isgreater(f, g) ? J{f - g} : J{};
+}
+
+// erf is defined as an integral that cannot be expressed analytically
+// however, the derivative is trivial to compute
+// erf(x + h) = erf(x) + h * 2*exp(-x^2)/sqrt(pi)
+template <typename T, int N>
+inline Jet<T, N> erf(const Jet<T, N>& x) {
+  // We evaluate the constant as follows:
+  //   2 / sqrt(pi) = 1 / sqrt(atan(1.))
+  // On POSIX sytems it is defined as M_2_SQRTPI, but this is not
+  // portable and the type may not be T.  The above expression
+  // evaluates to full precision with IEEE arithmetic and, since it's
+  // constant, the compiler can generate exactly the same code.  gcc
+  // does so even at -O0.
+  return Jet<T, N>(erf(x.a), x.v * exp(-x.a * x.a) * (T(1) / sqrt(atan(T(1)))));
+}
+
+// erfc(x) = 1-erf(x)
+// erfc(x + h) = erfc(x) + h * (-2*exp(-x^2)/sqrt(pi))
+template <typename T, int N>
+inline Jet<T, N> erfc(const Jet<T, N>& x) {
+  // See in erf() above for the evaluation of the constant in the derivative.
+  return Jet<T, N>(erfc(x.a),
+                   -x.v * exp(-x.a * x.a) * (T(1) / sqrt(atan(T(1)))));
+}
+
+// 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);
+}
+
+// Classification and comparison functionality referencing only the scalar part
+// of a Jet. To classify the derivatives (e.g., for sanity checks), the dual
+// part should be referenced explicitly. For instance, to check whether the
+// derivatives of a Jet 'f' are reasonable, one can use
+//
+//  isfinite(f.v.array()).all()
+//  !isnan(f.v.array()).any()
+//
+// etc., depending on the desired semantics.
+//
+// NOTE: Floating-point classification and comparison functions and operators
+// should be used with care as no derivatives can be propagated by such
+// functions directly but only by expressions resulting from corresponding
+// conditional statements. At the same time, conditional statements can possibly
+// introduce a discontinuity in the cost function making it impossible to
+// evaluate its derivative and thus the optimization problem intractable.
+
+// Determines whether the scalar part of the Jet is finite.
+template <typename T, int N>
+inline bool isfinite(const Jet<T, N>& f) {
+  return isfinite(f.a);
+}
+
+// Determines whether the scalar part of the Jet is infinite.
+template <typename T, int N>
+inline bool isinf(const Jet<T, N>& f) {
+  return isinf(f.a);
+}
+
+// Determines whether the scalar part of the Jet is NaN.
+template <typename T, int N>
+inline bool isnan(const Jet<T, N>& f) {
+  return isnan(f.a);
+}
+
+// Determines whether the scalar part of the Jet is neither zero, subnormal,
+// infinite, nor NaN.
+template <typename T, int N>
+inline bool isnormal(const Jet<T, N>& f) {
+  return isnormal(f.a);
+}
+
+// Determines whether the scalar part of the Jet f is less than the scalar
+// part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+          typename Rhs,
+          std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool isless(const Lhs& f, const Rhs& g) {
+  using internal::AsScalar;
+  return isless(AsScalar(f), AsScalar(g));
+}
+
+// Determines whether the scalar part of the Jet f is greater than the scalar
+// part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+          typename Rhs,
+          std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool isgreater(const Lhs& f, const Rhs& g) {
+  using internal::AsScalar;
+  return isgreater(AsScalar(f), AsScalar(g));
+}
+
+// Determines whether the scalar part of the Jet f is less than or equal to the
+// scalar part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+          typename Rhs,
+          std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool islessequal(const Lhs& f, const Rhs& g) {
+  using internal::AsScalar;
+  return islessequal(AsScalar(f), AsScalar(g));
+}
+
+// Determines whether the scalar part of the Jet f is less than or greater than
+// (f < g || f > g) the scalar part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+          typename Rhs,
+          std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool islessgreater(const Lhs& f, const Rhs& g) {
+  using internal::AsScalar;
+  return islessgreater(AsScalar(f), AsScalar(g));
+}
+
+// Determines whether the scalar part of the Jet f is greater than or equal to
+// the scalar part of g.
+//
+// NOTE: This function does NOT set any floating-point exceptions.
+template <typename Lhs,
+          typename Rhs,
+          std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool isgreaterequal(const Lhs& f, const Rhs& g) {
+  using internal::AsScalar;
+  return isgreaterequal(AsScalar(f), AsScalar(g));
+}
+
+// Determines if either of the scalar parts of the arguments are NaN and
+// thus cannot be ordered with respect to each other.
+template <typename Lhs,
+          typename Rhs,
+          std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr>
+inline bool isunordered(const Lhs& f, const Rhs& g) {
+  using internal::AsScalar;
+  return isunordered(AsScalar(f), AsScalar(g));
+}
+
+// Categorize scalar part as zero, subnormal, normal, infinite, NaN, or
+// implementation-defined.
+template <typename T, int N>
+inline int fpclassify(const Jet<T, N>& f) {
+  return fpclassify(f.a);
+}
+
+// Determines whether the scalar part of the argument is negative.
+template <typename T, int N>
+inline bool signbit(const Jet<T, N>& f) {
+  return signbit(f.a);
+}
+
+// Legacy functions from the pre-C++11 days.
+template <typename T, int N>
+CERES_DEPRECATED_WITH_MSG(
+    "ceres::IsFinite will be removed in a future Ceres Solver release. Please "
+    "use ceres::isfinite.")
+inline bool IsFinite(const Jet<T, N>& f) {
+  return isfinite(f);
+}
+
+template <typename T, int N>
+CERES_DEPRECATED_WITH_MSG(
+    "ceres::IsNaN will be removed in a future Ceres Solver release. Please use "
+    "ceres::isnan.")
+inline bool IsNaN(const Jet<T, N>& f) {
+  return isnan(f);
+}
+
+template <typename T, int N>
+CERES_DEPRECATED_WITH_MSG(
+    "ceres::IsNormal will be removed in a future Ceres Solver release. Please "
+    "use ceres::isnormal.")
+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>
+CERES_DEPRECATED_WITH_MSG(
+    "ceres::IsInfinite will be removed in a future Ceres Solver release. "
+    "Please use ceres::isinf.")
+inline bool IsInfinite(const Jet<T, N>& f) {
+  return isinf(f);
+}
+
+#ifdef CERES_HAS_CPP20
+// Computes the linear interpolation a + t(b - a) between a and b at the value
+// t. For arguments outside of the range 0 <= t <= 1, the values are
+// extrapolated.
+//
+// Differentiating lerp(a, b, t) with respect to a, b, and t gives:
+//
+//   d/da lerp(a, b, t) = 1 - t
+//   d/db lerp(a, b, t) = t
+//   d/dt lerp(a, b, t) = b - a
+//
+// with the dual representation given by
+//
+//   lerp(a + da, b + db, t + dt)
+//      ~= lerp(a, b, t) + (1 - t) da + t db + (b - a) dt .
+template <typename T, int N>
+inline Jet<T, N> lerp(const Jet<T, N>& a,
+                      const Jet<T, N>& b,
+                      const Jet<T, N>& t) {
+  return Jet<T, N>{lerp(a.a, b.a, t.a),
+                   (T(1) - t.a) * a.v + t.a * b.v + (b.a - a.a) * t.v};
+}
+
+// Computes the midpoint a + (b - a) / 2.
+//
+// Differentiating midpoint(a, b) with respect to a and b gives:
+//
+//   d/da midpoint(a, b) = 1/2
+//   d/db midpoint(a, b) = 1/2
+//
+// with the dual representation given by
+//
+//   midpoint(a + da, b + db) ~= midpoint(a, b) + (da + db) / 2 .
+template <typename T, int N>
+inline Jet<T, N> midpoint(const Jet<T, N>& a, const Jet<T, N>& b) {
+  Jet<T, N> result{midpoint(a.a, b.a)};
+  // To avoid overflow in the differential, compute
+  // (da + db) / 2 using midpoint.
+  for (int i = 0; i < N; ++i) {
+    result.v[i] = midpoint(a.v[i], b.v[i]);
+  }
+  return result;
+}
+#endif  // defined(CERES_HAS_CPP20)
+
+// 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));
+}
+
+// Computes the square x^2 of a real number x (not the Euclidean L^2 norm as
+// the name might suggest).
+//
+// NOTE: While std::norm is primarily intended for computing the squared
+// magnitude of a std::complex<> number, the current Jet implementation does not
+// support mixing a scalar T in its real part and std::complex<T> and in the
+// infinitesimal. Mixed Jet support is necessary for the type decay from
+// std::complex<T> to T (the squared magnitude of a complex number is always
+// real) performed by std::norm.
+//
+// norm(x + h) ~= norm(x) + 2x h
+template <typename T, int N>
+inline Jet<T, N> norm(const Jet<T, N>& f) {
+  return Jet<T, N>(norm(f.a), T(2) * f.a * f.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(T f, const Jet<T, N>& g) {
+  Jet<T, N> result;
+
+  if (fpclassify(f) == FP_ZERO && g > 0) {
+    // Handle case 2.
+    result = Jet<T, N>(T(0.0));
+  } else {
+    if (f < 0 && g == floor(g.a)) {  // Handle case 3.
+      result = Jet<T, N>(pow(f, g.a));
+      for (int i = 0; i < N; i++) {
+        if (fpclassify(g.v[i]) != FP_ZERO) {
+          // Return a NaN when g.v != 0.
+          result.v[i] = std::numeric_limits<T>::quiet_NaN();
+        }
+      }
+    } else {
+      // Handle case 1.
+      T const tmp = pow(f, g.a);
+      result = Jet<T, N>(tmp, log(f) * tmp * g.v);
+    }
+  }
+
+  return result;
+}
+
+// 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) {
+  Jet<T, N> result;
+
+  if (fpclassify(f) == FP_ZERO && g >= 1) {
+    // Handle cases 2 and 3.
+    if (g > 1) {
+      result = Jet<T, N>(T(0.0));
+    } else {
+      result = f;
+    }
+
+  } else {
+    if (f < 0 && g == floor(g.a)) {
+      // Handle cases 7 and 8.
+      T const tmp = g.a * pow(f.a, g.a - T(1.0));
+      result = Jet<T, N>(pow(f.a, g.a), tmp * f.v);
+      for (int i = 0; i < N; i++) {
+        if (fpclassify(g.v[i]) != FP_ZERO) {
+          // Return a NaN when g.v != 0.
+          result.v[i] = T(std::numeric_limits<double>::quiet_NaN());
+        }
+      }
+    } else {
+      // 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);
+      result = Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
+    }
+  }
+
+  return result;
+}
+
+// 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 std {
+template <typename T, int N>
+struct numeric_limits<ceres::Jet<T, N>> {
+  static constexpr bool is_specialized = true;
+  static constexpr bool is_signed = std::numeric_limits<T>::is_signed;
+  static constexpr bool is_integer = std::numeric_limits<T>::is_integer;
+  static constexpr bool is_exact = std::numeric_limits<T>::is_exact;
+  static constexpr bool has_infinity = std::numeric_limits<T>::has_infinity;
+  static constexpr bool has_quiet_NaN = std::numeric_limits<T>::has_quiet_NaN;
+  static constexpr bool has_signaling_NaN =
+      std::numeric_limits<T>::has_signaling_NaN;
+  static constexpr bool is_iec559 = std::numeric_limits<T>::is_iec559;
+  static constexpr bool is_bounded = std::numeric_limits<T>::is_bounded;
+  static constexpr bool is_modulo = std::numeric_limits<T>::is_modulo;
+
+  static constexpr std::float_denorm_style has_denorm =
+      std::numeric_limits<T>::has_denorm;
+  static constexpr std::float_round_style round_style =
+      std::numeric_limits<T>::round_style;
+
+  static constexpr int digits = std::numeric_limits<T>::digits;
+  static constexpr int digits10 = std::numeric_limits<T>::digits10;
+  static constexpr int max_digits10 = std::numeric_limits<T>::max_digits10;
+  static constexpr int radix = std::numeric_limits<T>::radix;
+  static constexpr int min_exponent = std::numeric_limits<T>::min_exponent;
+  static constexpr int min_exponent10 = std::numeric_limits<T>::max_exponent10;
+  static constexpr int max_exponent = std::numeric_limits<T>::max_exponent;
+  static constexpr int max_exponent10 = std::numeric_limits<T>::max_exponent10;
+  static constexpr bool traps = std::numeric_limits<T>::traps;
+  static constexpr bool tinyness_before =
+      std::numeric_limits<T>::tinyness_before;
+
+  static constexpr ceres::Jet<T, N> min
+  CERES_PREVENT_MACRO_SUBSTITUTION() noexcept {
+    return ceres::Jet<T, N>((std::numeric_limits<T>::min)());
+  }
+  static constexpr ceres::Jet<T, N> lowest() noexcept {
+    return ceres::Jet<T, N>(std::numeric_limits<T>::lowest());
+  }
+  static constexpr ceres::Jet<T, N> epsilon() noexcept {
+    return ceres::Jet<T, N>(std::numeric_limits<T>::epsilon());
+  }
+  static constexpr ceres::Jet<T, N> round_error() noexcept {
+    return ceres::Jet<T, N>(std::numeric_limits<T>::round_error());
+  }
+  static constexpr ceres::Jet<T, N> infinity() noexcept {
+    return ceres::Jet<T, N>(std::numeric_limits<T>::infinity());
+  }
+  static constexpr ceres::Jet<T, N> quiet_NaN() noexcept {
+    return ceres::Jet<T, N>(std::numeric_limits<T>::quiet_NaN());
+  }
+  static constexpr ceres::Jet<T, N> signaling_NaN() noexcept {
+    return ceres::Jet<T, N>(std::numeric_limits<T>::signaling_NaN());
+  }
+  static constexpr ceres::Jet<T, N> denorm_min() noexcept {
+    return ceres::Jet<T, N>(std::numeric_limits<T>::denorm_min());
+  }
+
+  static constexpr ceres::Jet<T, N> max
+  CERES_PREVENT_MACRO_SUBSTITUTION() noexcept {
+    return ceres::Jet<T, N>((std::numeric_limits<T>::max)());
+  }
+};
+
+}  // namespace std
+
+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>> {
+  using Real = ceres::Jet<T, N>;
+  using NonInteger = ceres::Jet<T, N>;
+  using Nested = ceres::Jet<T, N>;
+  using Literal = ceres::Jet<T, N>;
+
+  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)()); }
+};
+
+// 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> {
+  using ReturnType = ceres::Jet<T, N>;
+};
+template <typename BinaryOp, typename T, int N>
+struct ScalarBinaryOpTraits<T, ceres::Jet<T, N>, BinaryOp> {
+  using ReturnType = ceres::Jet<T, N>;
+};
+
+}  // namespace Eigen
+
+#endif  // CERES_PUBLIC_JET_H_

ceres-v2/include/jet_fwd.h

@@ -0,0 +1,44 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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: sergiu.deitsch@gmail.com (Sergiu Deitsch)
+//
+
+#ifndef CERES_PUBLIC_JET_FWD_H_
+#define CERES_PUBLIC_JET_FWD_H_
+
+namespace ceres {
+
+// Jet forward declaration necessary for the following partial specialization of
+// std::common_type and type traits.
+template <typename T, int N>
+struct Jet;
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_JET_FWD_H_

ceres-v2/include/line_manifold.h

@@ -0,0 +1,304 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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_LINE_MANIFOLD_H_
+#define CERES_PUBLIC_LINE_MANIFOLD_H_
+
+#include <Eigen/Core>
+#include <algorithm>
+#include <array>
+#include <memory>
+#include <vector>
+
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/internal/householder_vector.h"
+#include "ceres/internal/sphere_manifold_functions.h"
+#include "ceres/manifold.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+// This provides a manifold for lines, where the line is
+// over-parameterized by an origin point and a direction vector. So the
+// parameter vector size needs to be two times the ambient space dimension,
+// where the first half is interpreted as the origin point and the second half
+// as the direction.
+//
+// The plus operator for the line direction is the same as for the
+// SphereManifold. The update of the origin point is
+// perpendicular to the line direction before the update.
+//
+// This manifold is a special case of the affine Grassmannian
+// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold))
+// for the case Graff_1(R^n).
+//
+// The class works with dynamic and static ambient space dimensions. If the
+// ambient space dimensions is known at compile time use
+//
+//    LineManifold<3> manifold;
+//
+// If the ambient space dimensions is not known at compile time the template
+// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs
+// to be provided as a constructor argument:
+//
+//    LineManifold<ceres::DYNAMIC> manifold(ambient_dim);
+//
+template <int AmbientSpaceDimension>
+class LineManifold final : public Manifold {
+ public:
+  static_assert(AmbientSpaceDimension == DYNAMIC || AmbientSpaceDimension >= 2,
+                "The ambient space must be at least 2.");
+  static_assert(ceres::DYNAMIC == Eigen::Dynamic,
+                "ceres::DYNAMIC needs to be the same as Eigen::Dynamic.");
+
+  LineManifold();
+  explicit LineManifold(int size);
+
+  int AmbientSize() const override { return 2 * size_; }
+  int TangentSize() const override { return 2 * (size_ - 1); }
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool PlusJacobian(const double* x, double* jacobian) const override;
+  bool Minus(const double* y,
+             const double* x,
+             double* y_minus_x) const override;
+  bool MinusJacobian(const double* x, double* jacobian) const override;
+
+ private:
+  static constexpr bool IsDynamic = (AmbientSpaceDimension == ceres::DYNAMIC);
+  static constexpr int TangentSpaceDimension =
+      IsDynamic ? ceres::DYNAMIC : AmbientSpaceDimension - 1;
+
+  static constexpr int DAmbientSpaceDimension =
+      IsDynamic ? ceres::DYNAMIC : 2 * AmbientSpaceDimension;
+  static constexpr int DTangentSpaceDimension =
+      IsDynamic ? ceres::DYNAMIC : 2 * TangentSpaceDimension;
+
+  using AmbientVector = Eigen::Matrix<double, AmbientSpaceDimension, 1>;
+  using TangentVector = Eigen::Matrix<double, TangentSpaceDimension, 1>;
+  using MatrixPlusJacobian = Eigen::Matrix<double,
+                                           DAmbientSpaceDimension,
+                                           DTangentSpaceDimension,
+                                           Eigen::RowMajor>;
+  using MatrixMinusJacobian = Eigen::Matrix<double,
+                                            DTangentSpaceDimension,
+                                            DAmbientSpaceDimension,
+                                            Eigen::RowMajor>;
+
+  const int size_{AmbientSpaceDimension};
+};
+
+template <int AmbientSpaceDimension>
+LineManifold<AmbientSpaceDimension>::LineManifold()
+    : size_{AmbientSpaceDimension} {
+  static_assert(
+      AmbientSpaceDimension != Eigen::Dynamic,
+      "The size is set to dynamic. Please call the constructor with a size.");
+}
+
+template <int AmbientSpaceDimension>
+LineManifold<AmbientSpaceDimension>::LineManifold(int size) : size_{size} {
+  if (AmbientSpaceDimension != Eigen::Dynamic) {
+    CHECK_EQ(AmbientSpaceDimension, size)
+        << "Specified size by template parameter differs from the supplied "
+           "one.";
+  } else {
+    CHECK_GT(size_, 1)
+        << "The size of the manifold needs to be greater than 1.";
+  }
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::Plus(const double* x_ptr,
+                                               const double* delta_ptr,
+                                               double* x_plus_delta_ptr) const {
+  // We seek a box plus operator of the form
+  //
+  //   [o*, d*] = Plus([o, d], [delta_o, delta_d])
+  //
+  // where o is the origin point, d is the direction vector, delta_o is
+  // the delta of the origin point and delta_d the delta of the direction and
+  // o* and d* is the updated origin point and direction.
+  //
+  // We separate the Plus operator into the origin point and directional part
+  //   d* = Plus_d(d, delta_d)
+  //   o* = Plus_o(o, d, delta_o)
+  //
+  // The direction update function Plus_d is the same as as the SphereManifold:
+  //
+  //   d* = H_{v(d)} [0.5 sinc(0.5 |delta_d|) delta_d, cos(0.5 |delta_d|)]^T
+  //
+  // where H is the householder matrix
+  //   H_{v} = I - (2 / |v|^2) v v^T
+  // and
+  //   v(d) = d - sign(d_n) |d| e_n.
+  //
+  // The origin point update function Plus_o is defined as
+  //
+  //   o* = o + H_{v(d)} [0.5 delta_o, 0]^T.
+
+  Eigen::Map<const AmbientVector> o(x_ptr, size_);
+  Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
+
+  Eigen::Map<const TangentVector> delta_o(delta_ptr, size_ - 1);
+  Eigen::Map<const TangentVector> delta_d(delta_ptr + size_ - 1, size_ - 1);
+  Eigen::Map<AmbientVector> o_plus_delta(x_plus_delta_ptr, size_);
+  Eigen::Map<AmbientVector> d_plus_delta(x_plus_delta_ptr + size_, size_);
+
+  const double norm_delta_d = delta_d.norm();
+
+  o_plus_delta = o;
+
+  // Shortcut for zero delta direction.
+  if (norm_delta_d == 0.0) {
+    d_plus_delta = d;
+
+    if (delta_o.isZero(0.0)) {
+      return true;
+    }
+  }
+
+  // Calculate the householder transformation which is needed for f_d and f_o.
+  AmbientVector v(size_);
+  double beta;
+
+  // NOTE: The explicit template arguments are needed here because
+  // ComputeHouseholderVector is templated and some versions of MSVC
+  // have trouble deducing the type of v automatically.
+  internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
+                                     double,
+                                     AmbientSpaceDimension>(d, &v, &beta);
+
+  if (norm_delta_d != 0.0) {
+    internal::ComputeSphereManifoldPlus(
+        v, beta, d, delta_d, norm_delta_d, &d_plus_delta);
+  }
+
+  // The null space is in the direction of the line, so the tangent space is
+  // perpendicular to the line direction. This is achieved by using the
+  // householder matrix of the direction and allow only movements
+  // perpendicular to e_n.
+  //
+  // The factor of 0.5 is used to be consistent with the line direction
+  // update.
+  AmbientVector y(size_);
+  y << 0.5 * delta_o, 0;
+  o_plus_delta += internal::ApplyHouseholderVector(y, v, beta);
+
+  return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::PlusJacobian(
+    const double* x_ptr, double* jacobian_ptr) const {
+  Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
+  Eigen::Map<MatrixPlusJacobian> jacobian(
+      jacobian_ptr, 2 * size_, 2 * (size_ - 1));
+
+  // Clear the Jacobian as only half of the matrix is not zero.
+  jacobian.setZero();
+
+  auto jacobian_d =
+      jacobian
+          .template topLeftCorner<AmbientSpaceDimension, TangentSpaceDimension>(
+              size_, size_ - 1);
+  auto jacobian_o = jacobian.template bottomRightCorner<AmbientSpaceDimension,
+                                                        TangentSpaceDimension>(
+      size_, size_ - 1);
+  internal::ComputeSphereManifoldPlusJacobian(d, &jacobian_d);
+  jacobian_o = jacobian_d;
+  return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::Minus(const double* y_ptr,
+                                                const double* x_ptr,
+                                                double* y_minus_x) const {
+  Eigen::Map<const AmbientVector> y_o(y_ptr, size_);
+  Eigen::Map<const AmbientVector> y_d(y_ptr + size_, size_);
+  Eigen::Map<const AmbientVector> x_o(x_ptr, size_);
+  Eigen::Map<const AmbientVector> x_d(x_ptr + size_, size_);
+
+  Eigen::Map<TangentVector> y_minus_x_o(y_minus_x, size_ - 1);
+  Eigen::Map<TangentVector> y_minus_x_d(y_minus_x + size_ - 1, size_ - 1);
+
+  AmbientVector v(size_);
+  double beta;
+
+  // NOTE: The explicit template arguments are needed here because
+  // ComputeHouseholderVector is templated and some versions of MSVC
+  // have trouble deducing the type of v automatically.
+  internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
+                                     double,
+                                     AmbientSpaceDimension>(x_d, &v, &beta);
+
+  internal::ComputeSphereManifoldMinus(v, beta, x_d, y_d, &y_minus_x_d);
+
+  AmbientVector delta_o = y_o - x_o;
+  const AmbientVector h_delta_o =
+      2.0 * internal::ApplyHouseholderVector(delta_o, v, beta);
+  y_minus_x_o = h_delta_o.template head<TangentSpaceDimension>(size_ - 1);
+
+  return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::MinusJacobian(
+    const double* x_ptr, double* jacobian_ptr) const {
+  Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
+  Eigen::Map<MatrixMinusJacobian> jacobian(
+      jacobian_ptr, 2 * (size_ - 1), 2 * size_);
+
+  // Clear the Jacobian as only half of the matrix is not zero.
+  jacobian.setZero();
+
+  auto jacobian_d =
+      jacobian
+          .template topLeftCorner<TangentSpaceDimension, AmbientSpaceDimension>(
+              size_ - 1, size_);
+  auto jacobian_o = jacobian.template bottomRightCorner<TangentSpaceDimension,
+                                                        AmbientSpaceDimension>(
+      size_ - 1, size_);
+  internal::ComputeSphereManifoldMinusJacobian(d, &jacobian_d);
+  jacobian_o = jacobian_d;
+
+  return true;
+}
+
+}  // namespace ceres
+
+// clang-format off
+#include "ceres/internal/reenable_warnings.h"
+// clang-format on
+
+#endif  // CERES_PUBLIC_LINE_MANIFOLD_H_

ceres-v2/include/local_parameterization.h

@@ -0,0 +1,371 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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 <array>
+#include <memory>
+#include <vector>
+
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/internal/port.h"
+
+namespace ceres {
+
+// WARNING: LocalParameterizations are deprecated. They will be removed from
+// Ceres Solver in version 2.2.0. Please use Manifolds instead.
+
+// 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 to 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 f 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_DEPRECATED_WITH_MSG(
+    "LocalParameterizations will be removed from the Ceres Solver API in "
+    "version 2.2.0. Use Manifolds instead.")
+    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_DEPRECATED_WITH_MSG("Use EuclideanManifold instead.")
+    CERES_EXPORT IdentityParameterization : public LocalParameterization {
+ public:
+  explicit IdentityParameterization(int size);
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool ComputeJacobian(const double* x, double* jacobian) const override;
+  bool MultiplyByJacobian(const double* x,
+                          const int num_cols,
+                          const double* global_matrix,
+                          double* local_matrix) const override;
+  int GlobalSize() const override { return size_; }
+  int LocalSize() const override { return size_; }
+
+ private:
+  const int size_;
+};
+
+// Hold a subset of the parameters inside a parameter block constant.
+class CERES_DEPRECATED_WITH_MSG("Use SubsetManifold instead.")
+    CERES_EXPORT SubsetParameterization : public LocalParameterization {
+ public:
+  explicit SubsetParameterization(int size,
+                                  const std::vector<int>& constant_parameters);
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool ComputeJacobian(const double* x, double* jacobian) const override;
+  bool MultiplyByJacobian(const double* x,
+                          const int num_cols,
+                          const double* global_matrix,
+                          double* local_matrix) const override;
+  int GlobalSize() const override {
+    return static_cast<int>(constancy_mask_.size());
+  }
+  int LocalSize() const override { 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_DEPRECATED_WITH_MSG("Use QuaternionManifold instead.")
+    CERES_EXPORT QuaternionParameterization : public LocalParameterization {
+ public:
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool ComputeJacobian(const double* x, double* jacobian) const override;
+  int GlobalSize() const override { return 4; }
+  int LocalSize() const override { 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_DEPRECATED_WITH_MSG("Use EigenQuaternionManifold instead.")
+    CERES_EXPORT EigenQuaternionParameterization
+    : public ceres::LocalParameterization {
+ public:
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool ComputeJacobian(const double* x, double* jacobian) const override;
+  int GlobalSize() const override { return 4; }
+  int LocalSize() const override { return 3; }
+};
+
+// This provides a parameterization for homogeneous vectors which are commonly
+// used in Structure from 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_DEPRECATED_WITH_MSG("Use SphereManifold instead.") CERES_EXPORT
+    HomogeneousVectorParameterization : public LocalParameterization {
+ public:
+  explicit HomogeneousVectorParameterization(int size);
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool ComputeJacobian(const double* x, double* jacobian) const override;
+  int GlobalSize() const override { return size_; }
+  int LocalSize() const override { return size_ - 1; }
+
+ private:
+  const int size_;
+};
+
+// This provides a parameterization for lines, where the line is
+// over-parameterized by an origin point and a direction vector. So the
+// parameter vector size needs to be two times the ambient space dimension,
+// where the first half is interpreted as the origin point and the second half
+// as the direction.
+//
+// The plus operator for the line direction is the same as for the
+// HomogeneousVectorParameterization. The update of the origin point is
+// perpendicular to the line direction before the update.
+//
+// This local parameterization is a special case of the affine Grassmannian
+// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold))
+// for the case Graff_1(R^n).
+template <int AmbientSpaceDimension>
+class CERES_DEPRECATED_WITH_MSG("Use LineManifold instead.")
+    LineParameterization : public LocalParameterization {
+ public:
+  static_assert(AmbientSpaceDimension >= 2,
+                "The ambient space must be at least 2");
+
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool ComputeJacobian(const double* x, double* jacobian) const override;
+  int GlobalSize() const override { return 2 * AmbientSpaceDimension; }
+  int LocalSize() const override { return 2 * (AmbientSpaceDimension - 1); }
+};
+
+// 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.
+//
+// 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_DEPRECATED_WITH_MSG("Use ProductManifold instead.")
+    CERES_EXPORT ProductParameterization : public LocalParameterization {
+ public:
+  ProductParameterization(const ProductParameterization&) = delete;
+  ProductParameterization& operator=(const ProductParameterization&) = delete;
+  //
+  // NOTE: The constructor takes ownership of the input local
+  // parameterizations.
+  //
+  template <typename... LocalParams>
+  explicit ProductParameterization(LocalParams*... local_params)
+      : local_params_(sizeof...(LocalParams)) {
+    constexpr int kNumLocalParams = sizeof...(LocalParams);
+    static_assert(kNumLocalParams >= 2,
+                  "At least two local parameterizations must be specified.");
+
+    using LocalParameterizationPtr = std::unique_ptr<LocalParameterization>;
+
+    // Wrap all raw pointers into std::unique_ptr for exception safety.
+    std::array<LocalParameterizationPtr, kNumLocalParams> local_params_array{
+        LocalParameterizationPtr(local_params)...};
+
+    // Initialize internal state.
+    for (int i = 0; i < kNumLocalParams; ++i) {
+      LocalParameterizationPtr& param = local_params_[i];
+      param = std::move(local_params_array[i]);
+
+      buffer_size_ =
+          std::max(buffer_size_, param->LocalSize() * param->GlobalSize());
+      global_size_ += param->GlobalSize();
+      local_size_ += param->LocalSize();
+    }
+  }
+
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool ComputeJacobian(const double* x, double* jacobian) const override;
+  int GlobalSize() const override { return global_size_; }
+  int LocalSize() const override { return local_size_; }
+
+ private:
+  std::vector<std::unique_ptr<LocalParameterization>> local_params_;
+  int local_size_{0};
+  int global_size_{0};
+  int buffer_size_{0};
+};
+
+}  // namespace ceres
+
+// clang-format off
+#include "ceres/internal/reenable_warnings.h"
+// clang-format on
+
+#include "ceres/internal/line_parameterization.h"
+
+#endif  // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_

ceres-v2/include/loss_function.h

@@ -0,0 +1,433 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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 nullptr 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 "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/types.h"
+#include "glog/logging.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 nullptr for the
+// loss function when building the problem accomplishes the same
+// thing.
+class CERES_EXPORT TrivialLoss final : public LossFunction {
+ public:
+  void Evaluate(double, double*) const override;
+};
+
+// 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 final : public LossFunction {
+ public:
+  explicit HuberLoss(double a) : a_(a), b_(a * a) {}
+  void Evaluate(double, double*) const override;
+
+ 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 * a^2)].
+class CERES_EXPORT SoftLOneLoss final : public LossFunction {
+ public:
+  explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) {}
+  void Evaluate(double, double*) const override;
+
+ 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 / a^2].
+class CERES_EXPORT CauchyLoss final : public LossFunction {
+ public:
+  explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) {}
+  void Evaluate(double, double*) const override;
+
+ 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 final : public LossFunction {
+ public:
+  explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) {}
+  void Evaluate(double, double*) const override;
+
+ 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 final : public LossFunction {
+ public:
+  explicit TolerantLoss(double a, double b);
+  void Evaluate(double, double*) const override;
+
+ 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 follows where the equations are scaled by a
+// factor of 2 because the cost function is given by 1/2 rho(s):
+//
+//   rho(s) = a^2 / 3 * (1 - (1 - s / a^2)^3 )   for s <= a^2,
+//   rho(s) = a^2 / 3                            for s >  a^2.
+//
+// At s = 0: rho = [0, 1, -2 / a^2]
+class CERES_EXPORT TukeyLoss final : public ceres::LossFunction {
+ public:
+  explicit TukeyLoss(double a) : a_squared_(a * a) {}
+  void Evaluate(double, double*) const override;
+
+ 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 nullptr.
+class CERES_EXPORT ComposedLoss final : public LossFunction {
+ public:
+  explicit ComposedLoss(const LossFunction* f,
+                        Ownership ownership_f,
+                        const LossFunction* g,
+                        Ownership ownership_g);
+  ~ComposedLoss() override;
+  void Evaluate(double, double*) const override;
+
+ 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 nullptr Loss function as the Identity loss
+// function, rho = nullptr 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 final : 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;
+
+  ~ScaledLoss() override {
+    if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
+      rho_.release();
+    }
+  }
+  void Evaluate(double, double*) const override;
+
+ 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 nullptr Loss function as the Identity loss
+// function, rho = nullptr 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 LossFunctionWrapper(
+//    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 final : public LossFunction {
+ public:
+  LossFunctionWrapper(LossFunction* rho, Ownership ownership)
+      : rho_(rho), ownership_(ownership) {}
+
+  LossFunctionWrapper(const LossFunctionWrapper&) = delete;
+  void operator=(const LossFunctionWrapper&) = delete;
+
+  ~LossFunctionWrapper() override {
+    if (ownership_ == DO_NOT_TAKE_OWNERSHIP) {
+      rho_.release();
+    }
+  }
+
+  void Evaluate(double sq_norm, double out[3]) const override {
+    if (rho_.get() == nullptr) {
+      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_

ceres-v2/include/manifold.h

@@ -0,0 +1,411 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 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_MANIFOLD_H_
+#define CERES_PUBLIC_MANIFOLD_H_
+
+#include <Eigen/Core>
+#include <algorithm>
+#include <array>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/export.h"
+#include "ceres/types.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+// In sensor fusion problems, often we have to model quantities that live in
+// spaces known as Manifolds, for example the rotation/orientation of a sensor
+// that is represented by a quaternion.
+//
+// Manifolds are spaces which locally look like Euclidean spaces. More
+// precisely, at each point on the manifold there is a linear space that is
+// tangent to the manifold. It has dimension equal to the intrinsic dimension of
+// the manifold itself, which is less than or equal to the ambient space in
+// which the manifold is embedded.
+//
+// For example, the tangent space to a point on a sphere in three dimensions is
+// the two dimensional plane that is tangent to the sphere at that point. There
+// are two reasons tangent spaces are interesting:
+//
+// 1. They are Eucliean spaces so the usual vector space operations apply there,
+//    which makes numerical operations easy.
+// 2. Movement in the tangent space translate into movements along the manifold.
+//    Movements perpendicular to the tangent space do not translate into
+//    movements on the manifold.
+//
+// Returning to our sphere example, moving in the 2 dimensional plane
+// tangent to the sphere and projecting back onto the sphere will move you away
+// from the point you started from but moving along the normal at the same point
+// and the projecting back onto the sphere brings you back to the point.
+//
+// The Manifold interface defines two operations (and their derivatives)
+// involving the tangent space, allowing filtering and optimization to be
+// performed on said manifold:
+//
+// 1. x_plus_delta = Plus(x, delta)
+// 2. delta = Minus(x_plus_delta, x)
+//
+// "Plus" computes the result of moving along delta in the tangent space at x,
+// and then projecting back onto the manifold that x belongs to. In Differential
+// Geometry this is known as a "Retraction". It is a generalization of vector
+// addition in Euclidean spaces.
+//
+// Given two points on the manifold, "Minus" computes the change delta to x in
+// the tangent space at x, that will take it to x_plus_delta.
+//
+// Let us now consider two examples.
+//
+// The Euclidean space R^n is the simplest example of a manifold. It has
+// dimension n (and so does its tangent space) and Plus and Minus are the
+// familiar vector sum and difference operations.
+//
+//  Plus(x, delta) = x + delta = y,
+//  Minus(y, x) = y - x = delta.
+//
+// A more interesting case is SO(3), the special orthogonal group in three
+// dimensions - the space of 3x3 rotation matrices. SO(3) is a three dimensional
+// manifold embedded in R^9 or R^(3x3). So points on SO(3) are represented using
+// 9 dimensional vectors or 3x3 matrices, and points in its tangent spaces are
+// represented by 3 dimensional vectors.
+//
+// Defining Plus and Minus are defined in terms of the matrix Exp and Log
+// operations as follows:
+//
+// Let Exp(p, q, r) = [cos(theta) + cp^2, -sr + cpq        ,  sq + cpr        ]
+//                    [sr + cpq         , cos(theta) + cq^2, -sp + cqr        ]
+//                    [-sq + cpr        , sp + cqr         , cos(theta) + cr^2]
+//
+// where: theta = sqrt(p^2 + q^2 + r^2)
+//            s = sinc(theta)
+//            c = (1 - cos(theta))/theta^2
+//
+// and Log(x) = 1/(2 sinc(theta))[x_32 - x_23, x_13 - x_31, x_21 - x_12]
+//
+// where: theta = acos((Trace(x) - 1)/2)
+//
+// Then,
+//
+// Plus(x, delta) = x Exp(delta)
+// Minus(y, x) = Log(x^T y)
+//
+// For Plus and Minus to be mathematically consistent, the following identities
+// must be satisfied at all points x on the manifold:
+//
+// 1.  Plus(x, 0) = x.
+// 2.  For all y, Plus(x, Minus(y, x)) = y.
+// 3.  For all delta, Minus(Plus(x, delta), x) = delta.
+// 4.  For all delta_1, delta_2
+//    |Minus(Plus(x, delta_1), Plus(x, delta_2)) <= |delta_1 - delta_2|
+//
+// Briefly:
+// (1) Ensures that the tangent space is "centered" at x, and the zero vector is
+//     the identity element.
+// (2) Ensures that any y can be reached from x.
+// (3) Ensures that Plus is an injective (one-to-one) map.
+// (4) Allows us to define a metric on the manifold.
+//
+// Additionally we require that Plus and Minus be sufficiently smooth. In
+// particular they need to be differentiable everywhere on the manifold.
+//
+// For more details, please see
+//
+// "Integrating Generic Sensor Fusion Algorithms with Sound State
+// Representations through Encapsulation of Manifolds"
+// By C. Hertzberg, R. Wagner, U. Frese and L. Schroder
+// https://arxiv.org/pdf/1107.1119.pdf
+class CERES_EXPORT Manifold {
+ public:
+  virtual ~Manifold();
+
+  // Dimension of the ambient space in which the manifold is embedded.
+  virtual int AmbientSize() const = 0;
+
+  // Dimension of the manifold/tangent space.
+  virtual int TangentSize() const = 0;
+
+  //   x_plus_delta = Plus(x, delta),
+  //
+  // A generalization of vector addition in Euclidean space, Plus computes the
+  // result of moving along delta in the tangent space at x, and then projecting
+  // back onto the manifold that x belongs to.
+  //
+  // x and x_plus_delta are AmbientSize() vectors.
+  // delta is a TangentSize() vector.
+  //
+  // Return value indicates if the operation was successful or not.
+  virtual bool Plus(const double* x,
+                    const double* delta,
+                    double* x_plus_delta) const = 0;
+
+  // Compute the derivative of Plus(x, delta) w.r.t delta at delta = 0, i.e.
+  //
+  // (D_2 Plus)(x, 0)
+  //
+  // jacobian is a row-major AmbientSize() x TangentSize() matrix.
+  //
+  // Return value indicates whether the operation was successful or not.
+  virtual bool PlusJacobian(const double* x, double* jacobian) const = 0;
+
+  // tangent_matrix = ambient_matrix * (D_2 Plus)(x, 0)
+  //
+  // ambient_matrix is a row-major num_rows x AmbientSize() matrix.
+  // tangent_matrix is a row-major num_rows x TangentSize() matrix.
+  //
+  // Return value indicates whether the operation was successful or not.
+  //
+  // This function is only used by the GradientProblemSolver, where the
+  // dimension of the parameter block can be large and it may be more efficient
+  // to compute this product directly rather than first evaluating the Jacobian
+  // into a matrix and then doing a matrix vector product.
+  //
+  // Because this is not an often used function, we provide a default
+  // implementation for convenience. If performance becomes an issue then the
+  // user should consider implementing a specialization.
+  virtual bool RightMultiplyByPlusJacobian(const double* x,
+                                           const int num_rows,
+                                           const double* ambient_matrix,
+                                           double* tangent_matrix) const;
+
+  // y_minus_x = Minus(y, x)
+  //
+  // Given two points on the manifold, Minus computes the change to x in the
+  // tangent space at x, that will take it to y.
+  //
+  // x and y are AmbientSize() vectors.
+  // y_minus_x is a TangentSize() vector.
+  //
+  // Return value indicates if the operation was successful or not.
+  virtual bool Minus(const double* y,
+                     const double* x,
+                     double* y_minus_x) const = 0;
+
+  // Compute the derivative of Minus(y, x) w.r.t y at y = x, i.e
+  //
+  //   (D_1 Minus) (x, x)
+  //
+  // Jacobian is a row-major TangentSize() x AmbientSize() matrix.
+  //
+  // Return value indicates whether the operation was successful or not.
+  virtual bool MinusJacobian(const double* x, double* jacobian) const = 0;
+};
+
+// The Euclidean manifold is another name for the ordinary vector space R^size,
+// where the plus and minus operations are the usual vector addition and
+// subtraction:
+//   Plus(x, delta) = x + delta
+//   Minus(y, x) = y - x.
+//
+// The class works with dynamic and static ambient space dimensions. If the
+// ambient space dimensions is know at compile time use
+//
+//    EuclideanManifold<3> manifold;
+//
+// If the ambient space dimensions is not known at compile time the template
+// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs
+// to be provided as a constructor argument:
+//
+//    EuclideanManifold<ceres::DYNAMIC> manifold(ambient_dim);
+template <int Size>
+class EuclideanManifold final : public Manifold {
+ public:
+  static_assert(Size == ceres::DYNAMIC || Size >= 0,
+                "The size of the manifold needs to be non-negative.");
+  static_assert(ceres::DYNAMIC == Eigen::Dynamic,
+                "ceres::DYNAMIC needs to be the same as Eigen::Dynamic.");
+
+  EuclideanManifold() : size_{Size} {
+    static_assert(
+        Size != ceres::DYNAMIC,
+        "The size is set to dynamic. Please call the constructor with a size.");
+  }
+
+  explicit EuclideanManifold(int size) : size_(size) {
+    if (Size != ceres::DYNAMIC) {
+      CHECK_EQ(Size, size)
+          << "Specified size by template parameter differs from the supplied "
+             "one.";
+    } else {
+      CHECK_GE(size_, 0)
+          << "The size of the manifold needs to be non-negative.";
+    }
+  }
+
+  int AmbientSize() const override { return size_; }
+  int TangentSize() const override { return size_; }
+
+  bool Plus(const double* x_ptr,
+            const double* delta_ptr,
+            double* x_plus_delta_ptr) const override {
+    Eigen::Map<const AmbientVector> x(x_ptr, size_);
+    Eigen::Map<const AmbientVector> delta(delta_ptr, size_);
+    Eigen::Map<AmbientVector> x_plus_delta(x_plus_delta_ptr, size_);
+    x_plus_delta = x + delta;
+    return true;
+  }
+
+  bool PlusJacobian(const double* x_ptr, double* jacobian_ptr) const override {
+    Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, size_, size_);
+    jacobian.setIdentity();
+    return true;
+  }
+
+  bool RightMultiplyByPlusJacobian(const double* x,
+                                   const int num_rows,
+                                   const double* ambient_matrix,
+                                   double* tangent_matrix) const override {
+    std::copy_n(ambient_matrix, num_rows * size_, tangent_matrix);
+    return true;
+  }
+
+  bool Minus(const double* y_ptr,
+             const double* x_ptr,
+             double* y_minus_x_ptr) const override {
+    Eigen::Map<const AmbientVector> x(x_ptr, size_);
+    Eigen::Map<const AmbientVector> y(y_ptr, size_);
+    Eigen::Map<AmbientVector> y_minus_x(y_minus_x_ptr, size_);
+    y_minus_x = y - x;
+    return true;
+  }
+
+  bool MinusJacobian(const double* x_ptr, double* jacobian_ptr) const override {
+    Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, size_, size_);
+    jacobian.setIdentity();
+    return true;
+  }
+
+ private:
+  static constexpr bool IsDynamic = (Size == ceres::DYNAMIC);
+  using AmbientVector = Eigen::Matrix<double, Size, 1>;
+  using MatrixJacobian = Eigen::Matrix<double, Size, Size, Eigen::RowMajor>;
+
+  int size_{};
+};
+
+// Hold a subset of the parameters inside a parameter block constant.
+class CERES_EXPORT SubsetManifold final : public Manifold {
+ public:
+  SubsetManifold(int size, const std::vector<int>& constant_parameters);
+  int AmbientSize() const override;
+  int TangentSize() const override;
+
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool PlusJacobian(const double* x, double* jacobian) const override;
+  bool RightMultiplyByPlusJacobian(const double* x,
+                                   const int num_rows,
+                                   const double* ambient_matrix,
+                                   double* tangent_matrix) const override;
+  bool Minus(const double* y,
+             const double* x,
+             double* y_minus_x) const override;
+  bool MinusJacobian(const double* x, double* jacobian) const override;
+
+ private:
+  const int tangent_size_ = 0;
+  std::vector<bool> constancy_mask_;
+};
+
+// Implements the manifold for a Hamilton quaternion as defined in
+// https://en.wikipedia.org/wiki/Quaternion. Quaternions are represented as
+// unit norm 4-vectors, i.e.
+//
+// q = [q0; q1; q2; q3], |q| = 1
+//
+// is the ambient space representation.
+//
+//   q0  scalar part.
+//   q1  coefficient of i.
+//   q2  coefficient of j.
+//   q3  coefficient of k.
+//
+// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
+//
+// The tangent space is R^3, which relates to the ambient space through the
+// Plus and Minus operations defined as:
+//
+// Plus(x, delta) = [cos(|delta|); sin(|delta|) * delta / |delta|] * x
+//    Minus(y, x) = to_delta(y * x^{-1})
+//
+// where "*" is the quaternion product and because q is a unit quaternion
+// (|q|=1), q^-1 = [q0; -q1; -q2; -q3]
+//
+// and to_delta( [q0; u_{3x1}] ) = u / |u| * atan2(|u|, q0)
+class CERES_EXPORT QuaternionManifold final : public Manifold {
+ public:
+  int AmbientSize() const override { return 4; }
+  int TangentSize() const override { return 3; }
+
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool PlusJacobian(const double* x, double* jacobian) const override;
+  bool Minus(const double* y,
+             const double* x,
+             double* y_minus_x) const override;
+  bool MinusJacobian(const double* x, double* jacobian) const override;
+};
+
+// Implements the quaternion manifold for Eigen's representation of the
+// Hamilton quaternion. Geometrically it is exactly the same as the
+// QuaternionManifold defined above. However, Eigen uses a different internal
+// memory layout for the elements of the quaternion than what is commonly
+// used. It stores the quaternion in memory as [q1, q2, q3, q0] or
+// [x, y, z, w] where the real (scalar) part is last.
+//
+// Since Ceres operates on parameter blocks which are raw double pointers this
+// difference is important and requires a different manifold.
+class CERES_EXPORT EigenQuaternionManifold final : public Manifold {
+ public:
+  int AmbientSize() const override { return 4; }
+  int TangentSize() const override { return 3; }
+
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool PlusJacobian(const double* x, double* jacobian) const override;
+  bool Minus(const double* y,
+             const double* x,
+             double* y_minus_x) const override;
+  bool MinusJacobian(const double* x, double* jacobian) const override;
+};
+
+}  // namespace ceres
+
+// clang-format off
+#include "ceres/internal/reenable_warnings.h"
+// clang-format on
+
+#endif  // CERES_PUBLIC_MANIFOLD_H_

ceres-v2/include/manifold_test_utils.h

@@ -0,0 +1,328 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include <cmath>
+#include <limits>
+#include <memory>
+
+#include "ceres/dynamic_numeric_diff_cost_function.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/manifold.h"
+#include "ceres/numeric_diff_options.h"
+#include "ceres/types.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+
+// Matchers and macros for help with testing Manifold objects.
+//
+// Testing a Manifold has two parts.
+//
+// 1. Checking that Manifold::Plus is correctly defined. This requires per
+// manifold tests.
+//
+// 2. The other methods of the manifold have mathematical properties that make
+// it compatible with Plus, as described in:
+//
+// "Integrating Generic Sensor Fusion Algorithms with Sound State
+// Representations through Encapsulation of Manifolds"
+// By C. Hertzberg, R. Wagner, U. Frese and L. Schroder
+// https://arxiv.org/pdf/1107.1119.pdf
+//
+// These tests are implemented using generic matchers defined below which can
+// all be called by the macro EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x,
+// delta, y, tolerance). See manifold_test.cc for example usage.
+
+// Checks that the invariant Plus(x, 0) == x holds.
+MATCHER_P2(XPlusZeroIsXAt, x, tolerance, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+
+  Vector actual = Vector::Zero(ambient_size);
+  Vector zero = Vector::Zero(tangent_size);
+  EXPECT_TRUE(arg.Plus(x.data(), zero.data(), actual.data()));
+  const double n = (actual - x).norm();
+  const double d = x.norm();
+  const double diffnorm = (d == 0.0) ? n : (n / d);
+  if (diffnorm > tolerance) {
+    *result_listener << "\nexpected (x): " << x.transpose()
+                     << "\nactual: " << actual.transpose()
+                     << "\ndiffnorm: " << diffnorm;
+    return false;
+  }
+  return true;
+}
+
+// Checks that the invariant Minus(x, x) == 0 holds.
+MATCHER_P2(XMinusXIsZeroAt, x, tolerance, "") {
+  const int tangent_size = arg.TangentSize();
+  Vector actual = Vector::Zero(tangent_size);
+  EXPECT_TRUE(arg.Minus(x.data(), x.data(), actual.data()));
+  const double diffnorm = actual.norm();
+  if (diffnorm > tolerance) {
+    *result_listener << "\nx: " << x.transpose()  //
+                     << "\nexpected: 0 0 0"
+                     << "\nactual: " << actual.transpose()
+                     << "\ndiffnorm: " << diffnorm;
+    return false;
+  }
+  return true;
+}
+
+// Helper struct to curry Plus(x, .) so that it can be numerically
+// differentiated.
+struct PlusFunctor {
+  PlusFunctor(const Manifold& manifold, const double* x)
+      : manifold(manifold), x(x) {}
+  bool operator()(double const* const* parameters, double* x_plus_delta) const {
+    return manifold.Plus(x, parameters[0], x_plus_delta);
+  }
+
+  const Manifold& manifold;
+  const double* x;
+};
+
+// Checks that the output of PlusJacobian matches the one obtained by
+// numerically evaluating D_2 Plus(x,0).
+MATCHER_P2(HasCorrectPlusJacobianAt, x, tolerance, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+
+  NumericDiffOptions options;
+  options.ridders_relative_initial_step_size = 1e-4;
+
+  DynamicNumericDiffCostFunction<PlusFunctor, RIDDERS> cost_function(
+      new PlusFunctor(arg, x.data()), TAKE_OWNERSHIP, options);
+  cost_function.AddParameterBlock(tangent_size);
+  cost_function.SetNumResiduals(ambient_size);
+
+  Vector zero = Vector::Zero(tangent_size);
+  double* parameters[1] = {zero.data()};
+
+  Vector x_plus_zero = Vector::Zero(ambient_size);
+  Matrix expected = Matrix::Zero(ambient_size, tangent_size);
+  double* jacobians[1] = {expected.data()};
+
+  EXPECT_TRUE(
+      cost_function.Evaluate(parameters, x_plus_zero.data(), jacobians));
+
+  Matrix actual = Matrix::Random(ambient_size, tangent_size);
+  EXPECT_TRUE(arg.PlusJacobian(x.data(), actual.data()));
+
+  const double n = (actual - expected).norm();
+  const double d = expected.norm();
+  const double diffnorm = (d == 0.0) ? n : n / d;
+  if (diffnorm > tolerance) {
+    *result_listener << "\nx: " << x.transpose() << "\nexpected: \n"
+                     << expected << "\nactual:\n"
+                     << actual << "\ndiff:\n"
+                     << expected - actual << "\ndiffnorm : " << diffnorm;
+    return false;
+  }
+  return true;
+}
+
+// Checks that the invariant Minus(Plus(x, delta), x) == delta holds.
+MATCHER_P3(MinusPlusIsIdentityAt, x, delta, tolerance, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+  Vector x_plus_delta = Vector::Zero(ambient_size);
+  EXPECT_TRUE(arg.Plus(x.data(), delta.data(), x_plus_delta.data()));
+  Vector actual = Vector::Zero(tangent_size);
+  EXPECT_TRUE(arg.Minus(x_plus_delta.data(), x.data(), actual.data()));
+
+  const double n = (actual - delta).norm();
+  const double d = delta.norm();
+  const double diffnorm = (d == 0.0) ? n : (n / d);
+  if (diffnorm > tolerance) {
+    *result_listener << "\nx: " << x.transpose()
+                     << "\nexpected: " << delta.transpose()
+                     << "\nactual:" << actual.transpose()
+                     << "\ndiff:" << (delta - actual).transpose()
+                     << "\ndiffnorm: " << diffnorm;
+    return false;
+  }
+  return true;
+}
+
+// Checks that the invariant Plus(Minus(y, x), x) == y holds.
+MATCHER_P3(PlusMinusIsIdentityAt, x, y, tolerance, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+
+  Vector y_minus_x = Vector::Zero(tangent_size);
+  EXPECT_TRUE(arg.Minus(y.data(), x.data(), y_minus_x.data()));
+
+  Vector actual = Vector::Zero(ambient_size);
+  EXPECT_TRUE(arg.Plus(x.data(), y_minus_x.data(), actual.data()));
+
+  const double n = (actual - y).norm();
+  const double d = y.norm();
+  const double diffnorm = (d == 0.0) ? n : (n / d);
+  if (diffnorm > tolerance) {
+    *result_listener << "\nx: " << x.transpose()
+                     << "\nexpected: " << y.transpose()
+                     << "\nactual:" << actual.transpose()
+                     << "\ndiff:" << (y - actual).transpose()
+                     << "\ndiffnorm: " << diffnorm;
+    return false;
+  }
+  return true;
+}
+
+// Helper struct to curry Minus(., x) so that it can be numerically
+// differentiated.
+struct MinusFunctor {
+  MinusFunctor(const Manifold& manifold, const double* x)
+      : manifold(manifold), x(x) {}
+  bool operator()(double const* const* parameters, double* y_minus_x) const {
+    return manifold.Minus(parameters[0], x, y_minus_x);
+  }
+
+  const Manifold& manifold;
+  const double* x;
+};
+
+// Checks that the output of MinusJacobian matches the one obtained by
+// numerically evaluating D_1 Minus(x,x).
+MATCHER_P2(HasCorrectMinusJacobianAt, x, tolerance, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+
+  Vector y = x;
+  Vector y_minus_x = Vector::Zero(tangent_size);
+
+  NumericDiffOptions options;
+  options.ridders_relative_initial_step_size = 1e-4;
+  DynamicNumericDiffCostFunction<MinusFunctor, RIDDERS> cost_function(
+      new MinusFunctor(arg, x.data()), TAKE_OWNERSHIP, options);
+  cost_function.AddParameterBlock(ambient_size);
+  cost_function.SetNumResiduals(tangent_size);
+
+  double* parameters[1] = {y.data()};
+
+  Matrix expected = Matrix::Zero(tangent_size, ambient_size);
+  double* jacobians[1] = {expected.data()};
+
+  EXPECT_TRUE(cost_function.Evaluate(parameters, y_minus_x.data(), jacobians));
+
+  Matrix actual = Matrix::Random(tangent_size, ambient_size);
+  EXPECT_TRUE(arg.MinusJacobian(x.data(), actual.data()));
+
+  const double n = (actual - expected).norm();
+  const double d = expected.norm();
+  const double diffnorm = (d == 0.0) ? n : (n / d);
+  if (diffnorm > tolerance) {
+    *result_listener << "\nx: " << x.transpose() << "\nexpected: \n"
+                     << expected << "\nactual:\n"
+                     << actual << "\ndiff:\n"
+                     << expected - actual << "\ndiffnorm: " << diffnorm;
+    return false;
+  }
+  return true;
+}
+
+// Checks that D_delta Minus(Plus(x, delta), x) at delta = 0 is an identity
+// matrix.
+MATCHER_P2(MinusPlusJacobianIsIdentityAt, x, tolerance, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+
+  Matrix plus_jacobian(ambient_size, tangent_size);
+  EXPECT_TRUE(arg.PlusJacobian(x.data(), plus_jacobian.data()));
+  Matrix minus_jacobian(tangent_size, ambient_size);
+  EXPECT_TRUE(arg.MinusJacobian(x.data(), minus_jacobian.data()));
+
+  const Matrix actual = minus_jacobian * plus_jacobian;
+  const Matrix expected = Matrix::Identity(tangent_size, tangent_size);
+
+  const double n = (actual - expected).norm();
+  const double d = expected.norm();
+  const double diffnorm = n / d;
+  if (diffnorm > tolerance) {
+    *result_listener << "\nx: " << x.transpose() << "\nexpected: \n"
+                     << expected << "\nactual:\n"
+                     << actual << "\ndiff:\n"
+                     << expected - actual << "\ndiffnorm: " << diffnorm;
+
+    return false;
+  }
+  return true;
+}
+
+// Verify that the output of RightMultiplyByPlusJacobian is ambient_matrix *
+// plus_jacobian.
+MATCHER_P2(HasCorrectRightMultiplyByPlusJacobianAt, x, tolerance, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+
+  constexpr int kMinNumRows = 0;
+  constexpr int kMaxNumRows = 3;
+  for (int num_rows = kMinNumRows; num_rows <= kMaxNumRows; ++num_rows) {
+    Matrix plus_jacobian = Matrix::Random(ambient_size, tangent_size);
+    EXPECT_TRUE(arg.PlusJacobian(x.data(), plus_jacobian.data()));
+
+    Matrix ambient_matrix = Matrix::Random(num_rows, ambient_size);
+    Matrix expected = ambient_matrix * plus_jacobian;
+
+    Matrix actual = Matrix::Random(num_rows, tangent_size);
+    EXPECT_TRUE(arg.RightMultiplyByPlusJacobian(
+        x.data(), num_rows, ambient_matrix.data(), actual.data()));
+    const double n = (actual - expected).norm();
+    const double d = expected.norm();
+    const double diffnorm = (d == 0.0) ? n : (n / d);
+    if (diffnorm > tolerance) {
+      *result_listener << "\nx: " << x.transpose() << "\nambient_matrix : \n"
+                       << ambient_matrix << "\nplus_jacobian : \n"
+                       << plus_jacobian << "\nexpected: \n"
+                       << expected << "\nactual:\n"
+                       << actual << "\ndiff:\n"
+                       << expected - actual << "\ndiffnorm : " << diffnorm;
+      return false;
+    }
+  }
+  return true;
+}
+
+#define EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, tolerance) \
+  Vector zero_tangent = Vector::Zero(manifold.TangentSize());                  \
+  EXPECT_THAT(manifold, XPlusZeroIsXAt(x, tolerance));                         \
+  EXPECT_THAT(manifold, XMinusXIsZeroAt(x, tolerance));                        \
+  EXPECT_THAT(manifold, MinusPlusIsIdentityAt(x, delta, tolerance));           \
+  EXPECT_THAT(manifold, MinusPlusIsIdentityAt(x, zero_tangent, tolerance));    \
+  EXPECT_THAT(manifold, PlusMinusIsIdentityAt(x, x, tolerance));               \
+  EXPECT_THAT(manifold, PlusMinusIsIdentityAt(x, y, tolerance));               \
+  EXPECT_THAT(manifold, HasCorrectPlusJacobianAt(x, tolerance));               \
+  EXPECT_THAT(manifold, HasCorrectMinusJacobianAt(x, tolerance));              \
+  EXPECT_THAT(manifold, MinusPlusJacobianIsIdentityAt(x, tolerance));          \
+  EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobianAt(x, tolerance));
+
+}  // namespace ceres

ceres-v2/include/normal_prior.h

@@ -0,0 +1,78 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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/disable_warnings.h"
+#include "ceres/internal/eigen.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 final : 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);
+  bool Evaluate(double const* const* parameters,
+                double* residuals,
+                double** jacobians) const override;
+
+ private:
+  Matrix A_;
+  Vector b_;
+};
+
+}  // namespace ceres
+
+#include "ceres/internal/reenable_warnings.h"
+
+#endif  // CERES_PUBLIC_NORMAL_PRIOR_H_