LocalParameterization -> Manifold #1
This is the first in a series of changes that will eventually
replace the LocalParameterization interface with the richer
Manifold interface.
1. Add the Manifold interface.
2. Add implementations and test for:
a. EuclideanManifold (formerly the IdentityParameterization)
b. SubsetManifold (formerly the SubsetParameterization)
c. ProductManifold (formerly the ProductParameterization)
The testing has been completely re-done, where instead of adhoc
testing, we now define a number of matchers which explicitly
enforce the invariants demanded by the Manifold interface.
Change-Id: I3f296d0964388d52b027c99dc86b7730d24d55fa
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
