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