diff --git a/include/ceres/manifold.h b/include/ceres/manifold.h
new file mode 100644
index 0000000..5525069
--- /dev/null
+++ b/include/ceres/manifold.h
@@ -0,0 +1,345 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2021 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 <vector>
+
+#include "ceres/internal/disable_warnings.h"
+#include "ceres/internal/port.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 point 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
+//
+// TODO(sameeragarwal): Add documentation about how this class replaces
+// LocalParameterization once the transition starts happening.
+
+class CERES_EXPORT Manifold {
+ public:
+  virtual ~Manifold() = default;
+
+  // 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),
+  //
+  // 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.
+  //
+  // 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.
+class CERES_EXPORT EuclideanManifold : public Manifold {
+ public:
+  EuclideanManifold(int size);
+  virtual ~EuclideanManifold() = default;
+  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:
+  int size_ = 0;
+};
+
+// Hold a subset of the parameters inside a parameter block constant.
+class CERES_EXPORT SubsetManifold : public Manifold {
+ public:
+  SubsetManifold(int size, const std::vector<int>& constant_parameters);
+  virtual ~SubsetManifold() = default;
+  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_;
+};
+
+// Construct a manifold by taking the Cartesian product of a number of other
+// manifolds. 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_manifold(new Quaternion(),
+//                                          new EuclideanManifold(3));
+//
+// is the manifold for a rigid transformation, where the
+// rotation is represented using a quaternion.
+class CERES_EXPORT ProductManifold : public Manifold {
+ public:
+  ProductManifold(const ProductManifold&) = delete;
+  ProductManifold& operator=(const ProductManifold&) = delete;
+  virtual ~ProductManifold() {}
+
+  // NOTE: The constructor takes ownership of the input
+  // manifolds.
+  //
+  template <typename... Manifolds>
+  ProductManifold(Manifolds*... manifolds) : manifolds_(sizeof...(Manifolds)) {
+    constexpr int kNumManifolds = sizeof...(Manifolds);
+    static_assert(kNumManifolds >= 2,
+                  "At least two manifolds must be specified.");
+
+    using ManifoldPtr = std::unique_ptr<Manifold>;
+
+    // Wrap all raw pointers into std::unique_ptr for exception safety.
+    std::array<ManifoldPtr, kNumManifolds> manifolds_array{
+        ManifoldPtr(manifolds)...};
+
+    // Initialize internal state.
+    for (int i = 0; i < kNumManifolds; ++i) {
+      ManifoldPtr& manifold = manifolds_[i];
+      manifold = std::move(manifolds_array[i]);
+
+      buffer_size_ = std::max(
+          buffer_size_, manifold->TangentSize() * manifold->AmbientSize());
+      ambient_size_ += manifold->AmbientSize();
+      tangent_size_ += manifold->TangentSize();
+    }
+  }
+
+  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 Minus(const double* y,
+             const double* x,
+             double* y_minus_x) const override;
+  bool MinusJacobian(const double* x, double* jacobian) const override;
+
+ private:
+  std::vector<std::unique_ptr<Manifold>> manifolds_;
+  int ambient_size_ = 0;
+  int tangent_size_ = 0;
+  int buffer_size_ = 0;
+};
+
+}  // namespace ceres
+
+// clang-format off
+#include "ceres/internal/reenable_warnings.h"
+
+#endif  // CERES_PUBLIC_MANIFOLD_H_
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index 7911aee..3d39458 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -107,6 +107,7 @@
     local_parameterization.cc
     loss_function.cc
     low_rank_inverse_hessian.cc
+    manifold.cc
     minimizer.cc
     normal_prior.cc
     parallel_utils.cc
@@ -482,6 +483,7 @@
   ceres_test(line_search_preprocessor)
   ceres_test(local_parameterization)
   ceres_test(loss_function)
+  ceres_test(manifold)
   ceres_test(minimizer)
   ceres_test(normal_prior)
   ceres_test(numeric_diff_cost_function)
diff --git a/internal/ceres/manifold.cc b/internal/ceres/manifold.cc
new file mode 100644
index 0000000..b0fead2
--- /dev/null
+++ b/internal/ceres/manifold.cc
@@ -0,0 +1,275 @@
+#include "ceres/manifold.h"
+
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/fixed_array.h"
+#include "glog/logging.h"
+
+namespace ceres {
+
+bool Manifold::RightMultiplyByPlusJacobian(const double* x,
+                                           const int num_rows,
+                                           const double* ambient_matrix,
+                                           double* tangent_matrix) const {
+  const int tangent_size = TangentSize();
+  if (tangent_size == 0) {
+    return true;
+  }
+
+  const int ambient_size = AmbientSize();
+  Matrix plus_jacobian(ambient_size, tangent_size);
+  if (!PlusJacobian(x, plus_jacobian.data())) {
+    return false;
+  }
+
+  MatrixRef(tangent_matrix, num_rows, tangent_size) =
+      ConstMatrixRef(ambient_matrix, num_rows, ambient_size) * plus_jacobian;
+  return true;
+}
+
+EuclideanManifold::EuclideanManifold(int size) : size_(size) {
+  CHECK_GE(size, 0);
+}
+
+int EuclideanManifold::AmbientSize() const { return size_; }
+
+int EuclideanManifold::TangentSize() const { return size_; }
+
+bool EuclideanManifold::Plus(const double* x,
+                             const double* delta,
+                             double* x_plus_delta) const {
+  for (int i = 0; i < size_; ++i) {
+    x_plus_delta[i] = x[i] + delta[i];
+  }
+  return true;
+}
+
+bool EuclideanManifold::PlusJacobian(const double* x, double* jacobian) const {
+  MatrixRef(jacobian, size_, size_).setIdentity();
+  return true;
+}
+
+bool EuclideanManifold::RightMultiplyByPlusJacobian(
+    const double* x,
+    const int num_rows,
+    const double* ambient_matrix,
+    double* tangent_matrix) const {
+  std::copy_n(ambient_matrix, num_rows * size_, tangent_matrix);
+  return true;
+}
+
+bool EuclideanManifold::Minus(const double* y,
+                              const double* x,
+                              double* y_minus_x) const {
+  for (int i = 0; i < size_; ++i) {
+    y_minus_x[i] = y[i] - x[i];
+  }
+  return true;
+};
+
+bool EuclideanManifold::MinusJacobian(const double* x, double* jacobian) const {
+  MatrixRef(jacobian, size_, size_).setIdentity();
+  return true;
+}
+
+SubsetManifold::SubsetManifold(const int size,
+                               const std::vector<int>& constant_parameters)
+
+    : tangent_size_(size - constant_parameters.size()),
+      constancy_mask_(size, false) {
+  if (constant_parameters.empty()) {
+    return;
+  }
+
+  std::vector<int> constant = constant_parameters;
+  std::sort(constant.begin(), constant.end());
+  CHECK_GE(constant.front(), 0) << "Indices indicating constant parameter must "
+                                   "be greater than equal to zero.";
+  CHECK_LT(constant.back(), size)
+      << "Indices indicating constant parameter must be less than the size "
+      << "of the parameter block.";
+  CHECK(std::adjacent_find(constant.begin(), constant.end()) == constant.end())
+      << "The set of constant parameters cannot contain duplicates";
+
+  for (auto index : constant_parameters) {
+    constancy_mask_[index] = true;
+  }
+}
+
+int SubsetManifold::AmbientSize() const { return constancy_mask_.size(); }
+
+int SubsetManifold::TangentSize() const { return tangent_size_; }
+
+bool SubsetManifold::Plus(const double* x,
+                          const double* delta,
+                          double* x_plus_delta) const {
+  const int ambient_size = AmbientSize();
+  for (int i = 0, j = 0; i < ambient_size; ++i) {
+    if (constancy_mask_[i]) {
+      x_plus_delta[i] = x[i];
+    } else {
+      x_plus_delta[i] = x[i] + delta[j++];
+    }
+  }
+  return true;
+}
+
+bool SubsetManifold::PlusJacobian(const double* x,
+                                  double* plus_jacobian) const {
+  if (tangent_size_ == 0) {
+    return true;
+  }
+
+  const int ambient_size = AmbientSize();
+  MatrixRef m(plus_jacobian, ambient_size, tangent_size_);
+  m.setZero();
+  for (int r = 0, c = 0; r < ambient_size; ++r) {
+    if (!constancy_mask_[r]) {
+      m(r, c++) = 1.0;
+    }
+  }
+  return true;
+}
+
+bool SubsetManifold::RightMultiplyByPlusJacobian(const double* x,
+                                                 const int num_rows,
+                                                 const double* ambient_matrix,
+                                                 double* tangent_matrix) const {
+  if (tangent_size_ == 0) {
+    return true;
+  }
+
+  const int ambient_size = AmbientSize();
+  for (int r = 0; r < num_rows; ++r) {
+    for (int idx = 0, c = 0; idx < ambient_size; ++idx) {
+      if (!constancy_mask_[idx]) {
+        tangent_matrix[r * tangent_size_ + c++] =
+            ambient_matrix[r * ambient_size + idx];
+      }
+    }
+  }
+  return true;
+}
+
+bool SubsetManifold::Minus(const double* y,
+                           const double* x,
+                           double* y_minus_x) const {
+  if (tangent_size_ == 0) {
+    return true;
+  }
+
+  const int ambient_size = AmbientSize();
+  for (int i = 0, j = 0; i < ambient_size; ++i) {
+    if (!constancy_mask_[i]) {
+      y_minus_x[j++] = y[i] - x[i];
+    }
+  }
+  return true;
+}
+
+bool SubsetManifold::MinusJacobian(const double* x,
+                                   double* minus_jacobian) const {
+  const int ambient_size = AmbientSize();
+  const int tangent_size = TangentSize();
+  MatrixRef m(minus_jacobian, tangent_size_, ambient_size);
+  m.setZero();
+  for (int c = 0, r = 0; c < ambient_size; ++c) {
+    if (!constancy_mask_[c]) {
+      m(r++, c) = 1.0;
+    }
+  }
+  return true;
+}
+
+int ProductManifold::AmbientSize() const { return ambient_size_; }
+
+int ProductManifold::TangentSize() const { return tangent_size_; }
+
+bool ProductManifold::Plus(const double* x,
+                           const double* delta,
+                           double* x_plus_delta) const {
+  int ambient_cursor = 0;
+  int tangent_cursor = 0;
+  for (const auto& m : manifolds_) {
+    if (!m->Plus(x + ambient_cursor,
+                 delta + tangent_cursor,
+                 x_plus_delta + ambient_cursor)) {
+      return false;
+    }
+    tangent_cursor += m->TangentSize();
+    ambient_cursor += m->AmbientSize();
+  }
+
+  return true;
+}
+
+bool ProductManifold::PlusJacobian(const double* x,
+                                   double* jacobian_ptr) const {
+  MatrixRef jacobian(jacobian_ptr, AmbientSize(), TangentSize());
+  jacobian.setZero();
+  internal::FixedArray<double> buffer(buffer_size_);
+
+  int ambient_cursor = 0;
+  int tangent_cursor = 0;
+  for (const auto& m : manifolds_) {
+    const int ambient_size = m->AmbientSize();
+    const int tangent_size = m->TangentSize();
+
+    if (!m->PlusJacobian(x + ambient_cursor, buffer.data())) {
+      return false;
+    }
+
+    jacobian.block(ambient_cursor, tangent_cursor, ambient_size, tangent_size) =
+        MatrixRef(buffer.data(), ambient_size, tangent_size);
+
+    ambient_cursor += ambient_size;
+    tangent_cursor += tangent_size;
+  }
+
+  return true;
+}
+
+bool ProductManifold::Minus(const double* y,
+                            const double* x,
+                            double* y_minus_x) const {
+  int ambient_cursor = 0;
+  int tangent_cursor = 0;
+  for (const auto& m : manifolds_) {
+    if (!m->Minus(y + ambient_cursor,
+                  x + ambient_cursor,
+                  y_minus_x + tangent_cursor)) {
+      return false;
+    }
+    tangent_cursor += m->TangentSize();
+    ambient_cursor += m->AmbientSize();
+  }
+
+  return true;
+}
+
+bool ProductManifold::MinusJacobian(const double* x,
+                                    double* jacobian_ptr) const {
+  MatrixRef jacobian(jacobian_ptr, TangentSize(), AmbientSize());
+  jacobian.setZero();
+  internal::FixedArray<double> buffer(buffer_size_);
+
+  int ambient_cursor = 0;
+  int tangent_cursor = 0;
+  for (const auto& m : manifolds_) {
+    const int ambient_size = m->AmbientSize();
+    const int tangent_size = m->TangentSize();
+
+    if (!m->MinusJacobian(x + ambient_cursor, buffer.data())) {
+      return false;
+    }
+
+    jacobian.block(tangent_cursor, ambient_cursor, tangent_size, ambient_size) =
+        MatrixRef(buffer.data(), tangent_size, ambient_size);
+
+    ambient_cursor += ambient_size;
+    tangent_cursor += tangent_size;
+  }
+
+  return true;
+}
+
+}  // namespace ceres
diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc
new file mode 100644
index 0000000..40c36be
--- /dev/null
+++ b/internal/ceres/manifold_test.cc
@@ -0,0 +1,444 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2021 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/manifold.h"
+
+#include <cmath>
+#include <limits>
+#include <memory>
+
+#include "Eigen/Geometry"
+#include "ceres/dynamic_numeric_diff_cost_function.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/numeric_diff_options.h"
+#include "ceres/random.h"
+#include "ceres/types.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+// TODO(sameeragarwal): Once these helpers and matchers converge, it would be
+// helpful to expose them as testing utilities which can be used by the user
+// when implementing their own manifold objects.
+
+constexpr int kNumTrials = 10;
+constexpr double kEpsilon = 1e-10;
+
+// Helper struct to curry Plus(x, .) so that it can be numerically
+// differentiated.
+struct PlusFunctor {
+  PlusFunctor(const Manifold& manifold, 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(HasCorrectPlusJacobian, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+
+  for (int trial = 0; trial < kNumTrials; ++trial) {
+    Vector x = Vector::Random(ambient_size);
+
+    NumericDiffOptions options;
+    DynamicNumericDiffCostFunction<PlusFunctor, RIDDERS> cost_function(
+        new PlusFunctor(arg, x.data()));
+    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()};
+
+    CHECK(cost_function.Evaluate(parameters, x_plus_zero.data(), jacobians));
+
+    Matrix actual = Matrix::Random(ambient_size, tangent_size);
+    arg.PlusJacobian(x.data(), actual.data());
+
+    const double n = (actual - expected).norm();
+    const double d = expected.norm();
+    const bool result = (d == 0.0) ? (n == 0.0) : (n <= kEpsilon * d);
+    if (!result) {
+      *result_listener << "\nx: " << x.transpose() << "\nexpected: \n"
+                       << expected << "\nactual:\n"
+                       << actual << "\ndiff:\n"
+                       << expected - actual;
+
+      return false;
+    }
+  }
+  return true;
+}
+
+// Checks that the invariant Minus(Plus(x, delta), x) == delta holds.
+MATCHER_P(HasCorrectMinusAt, x, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+  for (int trial = 0; trial < kNumTrials; ++trial) {
+    Vector expected = Vector::Random(tangent_size);
+
+    Vector x_plus_expected = Vector::Zero(ambient_size);
+    arg.Plus(x.data(), expected.data(), x_plus_expected.data());
+
+    Vector actual = Vector::Zero(tangent_size);
+    arg.Minus(x_plus_expected.data(), x.data(), actual.data());
+
+    const double n = (actual - expected).norm();
+    const double d = expected.norm();
+    const bool result = (d == 0.0) ? (n == 0.0) : (n <= kEpsilon * d);
+    if (!result) {
+      *result_listener << "\nx: " << x.transpose()
+                       << "\nexpected: " << expected.transpose()
+                       << "\nactual:" << actual.transpose()
+                       << "\ndiff:" << (expected - actual).transpose();
+
+      return false;
+    }
+  }
+  return true;
+}
+
+// Helper struct to curry Minus(., x) so that it can be numerically
+// differentiated.
+struct MinusFunctor {
+  MinusFunctor(const Manifold& manifold, 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(HasCorrectMinusJacobian, "") {
+  const int ambient_size = arg.AmbientSize();
+  const int tangent_size = arg.TangentSize();
+
+  for (int trial = 0; trial < kNumTrials; ++trial) {
+    Vector y = Vector::Random(ambient_size);
+    Vector x = y;
+    Vector y_minus_x = Vector::Zero(tangent_size);
+
+    NumericDiffOptions options;
+    DynamicNumericDiffCostFunction<MinusFunctor, RIDDERS> cost_function(
+        new MinusFunctor(arg, x.data()));
+    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()};
+
+    CHECK(cost_function.Evaluate(parameters, y_minus_x.data(), jacobians));
+
+    Matrix actual = Matrix::Random(tangent_size, ambient_size);
+    arg.MinusJacobian(x.data(), actual.data());
+
+    const double n = (actual - expected).norm();
+    const double d = expected.norm();
+    const bool result = (d == 0.0) ? (n == 0.0) : (n <= kEpsilon * d);
+    if (!result) {
+      *result_listener << "\nx: " << x.transpose() << "\nexpected: \n"
+                       << expected << "\nactual:\n"
+                       << actual << "\ndiff:\n"
+                       << expected - actual;
+
+      return false;
+    }
+  }
+  return true;
+}
+
+// Verify that the output of RightMultiplyByPlusJacobian is ambient_matrix *
+// plus_jacobian.
+MATCHER(HasCorrectRightMultiplyByPlusJacobian, "") {
+  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) {
+    Vector x = Vector::Random(ambient_size);
+    Matrix plus_jacobian = Matrix::Random(ambient_size, tangent_size);
+    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);
+    arg.RightMultiplyByPlusJacobian(
+        x.data(), num_rows, ambient_matrix.data(), actual.data());
+    const double n = (actual - expected).norm();
+    const double d = expected.norm();
+    const bool result = (d == 0.0) ? (n == 0.0) : (n <= kEpsilon * d);
+    if (!result) {
+      *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;
+
+      return false;
+    }
+  }
+  return true;
+}
+
+TEST(EuclideanManifold, NormalFunctionTest) {
+  EuclideanManifold manifold(3);
+  EXPECT_EQ(manifold.AmbientSize(), 3);
+  EXPECT_EQ(manifold.TangentSize(), 3);
+
+  Vector x = Vector::Random(3);
+  Vector delta = Vector::Random(3);
+  Vector x_plus_delta = Vector::Zero(3);
+
+  manifold.Plus(x.data(), delta.data(), x_plus_delta.data());
+  EXPECT_NEAR(
+      (x_plus_delta - x - delta).norm() / (x + delta).norm(), 0.0, kEpsilon);
+  EXPECT_THAT(manifold, HasCorrectMinusAt(x));
+  EXPECT_THAT(manifold, HasCorrectPlusJacobian());
+  EXPECT_THAT(manifold, HasCorrectMinusJacobian());
+  EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian());
+}
+
+TEST(SubsetManifold, EmptyConstantParameters) {
+  SubsetManifold manifold(3, {});
+
+  Vector x = Vector::Random(3);
+  Vector delta = Vector::Random(3);
+  Vector x_plus_delta = Vector::Zero(3);
+
+  manifold.Plus(x.data(), delta.data(), x_plus_delta.data());
+  EXPECT_NEAR(
+      (x_plus_delta - x - delta).norm() / (x + delta).norm(), 0.0, kEpsilon);
+  EXPECT_THAT(manifold, HasCorrectMinusAt(x));
+  EXPECT_THAT(manifold, HasCorrectPlusJacobian());
+  EXPECT_THAT(manifold, HasCorrectMinusJacobian());
+  EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian());
+}
+
+TEST(SubsetManifold, NegativeParameterIndexDeathTest) {
+  EXPECT_DEATH_IF_SUPPORTED(SubsetManifold manifold(2, {-1}),
+                            "greater than equal to zero");
+}
+
+TEST(SubsetManifold, GreaterThanSizeParameterIndexDeathTest) {
+  EXPECT_DEATH_IF_SUPPORTED(SubsetManifold manifold(2, {2}),
+                            "less than the size");
+}
+
+TEST(SubsetManifold, DuplicateParametersDeathTest) {
+  EXPECT_DEATH_IF_SUPPORTED(SubsetManifold manifold(2, {1, 1}), "duplicates");
+}
+
+TEST(SubsetManifold, NormalFunctionTest) {
+  const int kAmbientSize = 4;
+  const int kTangentSize = 3;
+  Vector x = Vector::Random(kAmbientSize);
+  Vector delta = Vector::Random(kTangentSize);
+  Vector x_plus_delta = Vector::Zero(kAmbientSize);
+
+  for (int i = 0; i < kAmbientSize; ++i) {
+    SubsetManifold manifold(kAmbientSize, {i});
+    x_plus_delta.setZero();
+    manifold.Plus(x.data(), delta.data(), x_plus_delta.data());
+    int k = 0;
+    for (int j = 0; j < kAmbientSize; ++j) {
+      if (j == i) {
+        EXPECT_EQ(x_plus_delta[j], x[j]);
+      } else {
+        EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]);
+      }
+    }
+    EXPECT_THAT(manifold, HasCorrectMinusAt(x));
+    EXPECT_THAT(manifold, HasCorrectPlusJacobian());
+    EXPECT_THAT(manifold, HasCorrectMinusJacobian());
+    EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian());
+  }
+}
+
+TEST(ProductManifold, Size2) {
+  Manifold* manifold1 = new SubsetManifold(5, {2});
+  Manifold* manifold2 = new SubsetManifold(3, {0, 1});
+  ProductManifold manifold(manifold1, manifold2);
+
+  EXPECT_EQ(manifold.AmbientSize(),
+            manifold1->AmbientSize() + manifold2->AmbientSize());
+  EXPECT_EQ(manifold.TangentSize(),
+            manifold1->TangentSize() + manifold2->TangentSize());
+}
+
+TEST(ProductManifold, Size3) {
+  Manifold* manifold1 = new SubsetManifold(5, {2});
+  Manifold* manifold2 = new SubsetManifold(3, {0, 1});
+  Manifold* manifold3 = new SubsetManifold(4, {1});
+
+  ProductManifold manifold(manifold1, manifold2, manifold3);
+
+  EXPECT_EQ(manifold.AmbientSize(),
+            manifold1->AmbientSize() + manifold2->AmbientSize() +
+                manifold3->AmbientSize());
+  EXPECT_EQ(manifold.TangentSize(),
+            manifold1->TangentSize() + manifold2->TangentSize() +
+                manifold3->TangentSize());
+}
+
+TEST(ProductManifold, Size4) {
+  Manifold* manifold1 = new SubsetManifold(5, {2});
+  Manifold* manifold2 = new SubsetManifold(3, {0, 1});
+  Manifold* manifold3 = new SubsetManifold(4, {1});
+  Manifold* manifold4 = new SubsetManifold(2, {0});
+
+  ProductManifold manifold(manifold1, manifold2, manifold3, manifold4);
+
+  EXPECT_EQ(manifold.AmbientSize(),
+            manifold1->AmbientSize() + manifold2->AmbientSize() +
+                manifold3->AmbientSize() + manifold4->AmbientSize());
+  EXPECT_EQ(manifold.TangentSize(),
+            manifold1->TangentSize() + manifold2->TangentSize() +
+                manifold3->TangentSize() + manifold4->TangentSize());
+}
+
+TEST(ProductManifold, NormalFunctionTest) {
+  Manifold* manifold1 = new SubsetManifold(5, {2});
+  Manifold* manifold2 = new SubsetManifold(3, {0, 1});
+  Manifold* manifold3 = new SubsetManifold(4, {1});
+  Manifold* manifold4 = new SubsetManifold(2, {0});
+
+  ProductManifold manifold(manifold1, manifold2, manifold3, manifold4);
+
+  Vector x = Vector::Random(manifold.AmbientSize());
+  Vector delta = Vector::Random(manifold.TangentSize());
+  Vector x_plus_delta = Vector::Zero(manifold.AmbientSize());
+  Vector x_plus_delta_expected = Vector::Zero(manifold.AmbientSize());
+
+  EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data()));
+
+  int ambient_cursor = 0;
+  int tangent_cursor = 0;
+
+  EXPECT_TRUE(manifold1->Plus(&x[ambient_cursor],
+                              &delta[tangent_cursor],
+                              &x_plus_delta_expected[ambient_cursor]));
+  ambient_cursor += manifold1->AmbientSize();
+  tangent_cursor += manifold1->TangentSize();
+
+  EXPECT_TRUE(manifold2->Plus(&x[ambient_cursor],
+                              &delta[tangent_cursor],
+                              &x_plus_delta_expected[ambient_cursor]));
+  ambient_cursor += manifold2->AmbientSize();
+  tangent_cursor += manifold2->TangentSize();
+
+  EXPECT_TRUE(manifold3->Plus(&x[ambient_cursor],
+                              &delta[tangent_cursor],
+                              &x_plus_delta_expected[ambient_cursor]));
+  ambient_cursor += manifold3->AmbientSize();
+  tangent_cursor += manifold3->TangentSize();
+
+  EXPECT_TRUE(manifold4->Plus(&x[ambient_cursor],
+                              &delta[tangent_cursor],
+                              &x_plus_delta_expected[ambient_cursor]));
+  ambient_cursor += manifold4->AmbientSize();
+  tangent_cursor += manifold4->TangentSize();
+
+  for (int i = 0; i < x.size(); ++i) {
+    EXPECT_EQ(x_plus_delta[i], x_plus_delta_expected[i]);
+  }
+
+  EXPECT_THAT(manifold, HasCorrectMinusAt(x));
+  EXPECT_THAT(manifold, HasCorrectPlusJacobian());
+  EXPECT_THAT(manifold, HasCorrectMinusJacobian());
+  EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian());
+}
+
+TEST(ProductManifold, ZeroTangentSizeAndEuclidean) {
+  Manifold* subset_manifold = new SubsetManifold(1, {0});
+  Manifold* euclidean_manifold = new EuclideanManifold(2);
+  ProductManifold manifold(subset_manifold, euclidean_manifold);
+  EXPECT_EQ(manifold.AmbientSize(), 3);
+  EXPECT_EQ(manifold.TangentSize(), 2);
+
+  Vector x = Vector::Random(3);
+  Vector delta = Vector::Random(2);
+  Vector x_plus_delta = Vector::Zero(3);
+
+  EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data()));
+
+  EXPECT_EQ(x_plus_delta[0], x[0]);
+  EXPECT_EQ(x_plus_delta[1], x[1] + delta[0]);
+  EXPECT_EQ(x_plus_delta[2], x[2] + delta[1]);
+
+  EXPECT_THAT(manifold, HasCorrectMinusAt(x));
+  EXPECT_THAT(manifold, HasCorrectPlusJacobian());
+  EXPECT_THAT(manifold, HasCorrectMinusJacobian());
+  EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian());
+}
+
+TEST(ProductManifold, EuclideanAndZeroTangentSize) {
+  Manifold* subset_manifold = new SubsetManifold(1, {0});
+  Manifold* euclidean_manifold = new EuclideanManifold(2);
+  ProductManifold manifold(euclidean_manifold, subset_manifold);
+  EXPECT_EQ(manifold.AmbientSize(), 3);
+  EXPECT_EQ(manifold.TangentSize(), 2);
+
+  Vector x = Vector::Random(3);
+  Vector delta = Vector::Random(2);
+  Vector x_plus_delta = Vector::Zero(3);
+
+  EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data()));
+
+  EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]);
+  EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]);
+  EXPECT_EQ(x_plus_delta[2], x[2]);
+
+  EXPECT_THAT(manifold, HasCorrectMinusAt(x));
+  EXPECT_THAT(manifold, HasCorrectPlusJacobian());
+  EXPECT_THAT(manifold, HasCorrectMinusJacobian());
+  EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobian());
+}
+
+}  // namespace internal
+}  // namespace ceres