diff options
Diffstat (limited to 'extern/ceres/include/ceres/manifold.h')
| -rw-r--r-- | extern/ceres/include/ceres/manifold.h | 411 |
1 files changed, 411 insertions, 0 deletions
diff --git a/extern/ceres/include/ceres/manifold.h b/extern/ceres/include/ceres/manifold.h new file mode 100644 index 00000000000..4d6e9fa0f59 --- /dev/null +++ b/extern/ceres/include/ceres/manifold.h @@ -0,0 +1,411 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_MANIFOLD_H_ +#define CERES_PUBLIC_MANIFOLD_H_ + +#include <Eigen/Core> +#include <algorithm> +#include <array> +#include <memory> +#include <utility> +#include <vector> + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +// In sensor fusion problems, often we have to model quantities that live in +// spaces known as Manifolds, for example the rotation/orientation of a sensor +// that is represented by a quaternion. +// +// Manifolds are spaces which locally look like Euclidean spaces. More +// precisely, at each point on the manifold there is a linear space that is +// tangent to the manifold. It has dimension equal to the intrinsic dimension of +// the manifold itself, which is less than or equal to the ambient space in +// which the manifold is embedded. +// +// For example, the tangent space to a point on a sphere in three dimensions is +// the two dimensional plane that is tangent to the sphere at that point. There +// are two reasons tangent spaces are interesting: +// +// 1. They are Eucliean spaces so the usual vector space operations apply there, +// which makes numerical operations easy. +// 2. Movement in the tangent space translate into movements along the manifold. +// Movements perpendicular to the tangent space do not translate into +// movements on the manifold. +// +// Returning to our sphere example, moving in the 2 dimensional plane +// tangent to the sphere and projecting back onto the sphere will move you away +// from the point you started from but moving along the normal at the same point +// and the projecting back onto the sphere brings you back to the point. +// +// The Manifold interface defines two operations (and their derivatives) +// involving the tangent space, allowing filtering and optimization to be +// performed on said manifold: +// +// 1. x_plus_delta = Plus(x, delta) +// 2. delta = Minus(x_plus_delta, x) +// +// "Plus" computes the result of moving along delta in the tangent space at x, +// and then projecting back onto the manifold that x belongs to. In Differential +// Geometry this is known as a "Retraction". It is a generalization of vector +// addition in Euclidean spaces. +// +// Given two points on the manifold, "Minus" computes the change delta to x in +// the tangent space at x, that will take it to x_plus_delta. +// +// Let us now consider two examples. +// +// The Euclidean space R^n is the simplest example of a manifold. It has +// dimension n (and so does its tangent space) and Plus and Minus are the +// familiar vector sum and difference operations. +// +// Plus(x, delta) = x + delta = y, +// Minus(y, x) = y - x = delta. +// +// A more interesting case is SO(3), the special orthogonal group in three +// dimensions - the space of 3x3 rotation matrices. SO(3) is a three dimensional +// manifold embedded in R^9 or R^(3x3). So points on SO(3) are represented using +// 9 dimensional vectors or 3x3 matrices, and points in its tangent spaces are +// represented by 3 dimensional vectors. +// +// Defining Plus and Minus are defined in terms of the matrix Exp and Log +// operations as follows: +// +// Let Exp(p, q, r) = [cos(theta) + cp^2, -sr + cpq , sq + cpr ] +// [sr + cpq , cos(theta) + cq^2, -sp + cqr ] +// [-sq + cpr , sp + cqr , cos(theta) + cr^2] +// +// where: theta = sqrt(p^2 + q^2 + r^2) +// s = sinc(theta) +// c = (1 - cos(theta))/theta^2 +// +// and Log(x) = 1/(2 sinc(theta))[x_32 - x_23, x_13 - x_31, x_21 - x_12] +// +// where: theta = acos((Trace(x) - 1)/2) +// +// Then, +// +// Plus(x, delta) = x Exp(delta) +// Minus(y, x) = Log(x^T y) +// +// For Plus and Minus to be mathematically consistent, the following identities +// must be satisfied at all points x on the manifold: +// +// 1. Plus(x, 0) = x. +// 2. For all y, Plus(x, Minus(y, x)) = y. +// 3. For all delta, Minus(Plus(x, delta), x) = delta. +// 4. For all delta_1, delta_2 +// |Minus(Plus(x, delta_1), Plus(x, delta_2)) <= |delta_1 - delta_2| +// +// Briefly: +// (1) Ensures that the tangent space is "centered" at x, and the zero vector is +// the identity element. +// (2) Ensures that any y can be reached from x. +// (3) Ensures that Plus is an injective (one-to-one) map. +// (4) Allows us to define a metric on the manifold. +// +// Additionally we require that Plus and Minus be sufficiently smooth. In +// particular they need to be differentiable everywhere on the manifold. +// +// For more details, please see +// +// "Integrating Generic Sensor Fusion Algorithms with Sound State +// Representations through Encapsulation of Manifolds" +// By C. Hertzberg, R. Wagner, U. Frese and L. Schroder +// https://arxiv.org/pdf/1107.1119.pdf +class CERES_EXPORT Manifold { + public: + virtual ~Manifold(); + + // Dimension of the ambient space in which the manifold is embedded. + virtual int AmbientSize() const = 0; + + // Dimension of the manifold/tangent space. + virtual int TangentSize() const = 0; + + // x_plus_delta = Plus(x, delta), + // + // A generalization of vector addition in Euclidean space, Plus computes the + // result of moving along delta in the tangent space at x, and then projecting + // back onto the manifold that x belongs to. + // + // x and x_plus_delta are AmbientSize() vectors. + // delta is a TangentSize() vector. + // + // Return value indicates if the operation was successful or not. + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const = 0; + + // Compute the derivative of Plus(x, delta) w.r.t delta at delta = 0, i.e. + // + // (D_2 Plus)(x, 0) + // + // jacobian is a row-major AmbientSize() x TangentSize() matrix. + // + // Return value indicates whether the operation was successful or not. + virtual bool PlusJacobian(const double* x, double* jacobian) const = 0; + + // tangent_matrix = ambient_matrix * (D_2 Plus)(x, 0) + // + // ambient_matrix is a row-major num_rows x AmbientSize() matrix. + // tangent_matrix is a row-major num_rows x TangentSize() matrix. + // + // Return value indicates whether the operation was successful or not. + // + // This function is only used by the GradientProblemSolver, where the + // dimension of the parameter block can be large and it may be more efficient + // to compute this product directly rather than first evaluating the Jacobian + // into a matrix and then doing a matrix vector product. + // + // Because this is not an often used function, we provide a default + // implementation for convenience. If performance becomes an issue then the + // user should consider implementing a specialization. + virtual bool RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const; + + // y_minus_x = Minus(y, x) + // + // Given two points on the manifold, Minus computes the change to x in the + // tangent space at x, that will take it to y. + // + // x and y are AmbientSize() vectors. + // y_minus_x is a TangentSize() vector. + // + // Return value indicates if the operation was successful or not. + virtual bool Minus(const double* y, + const double* x, + double* y_minus_x) const = 0; + + // Compute the derivative of Minus(y, x) w.r.t y at y = x, i.e + // + // (D_1 Minus) (x, x) + // + // Jacobian is a row-major TangentSize() x AmbientSize() matrix. + // + // Return value indicates whether the operation was successful or not. + virtual bool MinusJacobian(const double* x, double* jacobian) const = 0; +}; + +// The Euclidean manifold is another name for the ordinary vector space R^size, +// where the plus and minus operations are the usual vector addition and +// subtraction: +// Plus(x, delta) = x + delta +// Minus(y, x) = y - x. +// +// The class works with dynamic and static ambient space dimensions. If the +// ambient space dimensions is know at compile time use +// +// EuclideanManifold<3> manifold; +// +// If the ambient space dimensions is not known at compile time the template +// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs +// to be provided as a constructor argument: +// +// EuclideanManifold<ceres::DYNAMIC> manifold(ambient_dim); +template <int Size> +class EuclideanManifold final : public Manifold { + public: + static_assert(Size == ceres::DYNAMIC || Size >= 0, + "The size of the manifold needs to be non-negative."); + static_assert(ceres::DYNAMIC == Eigen::Dynamic, + "ceres::DYNAMIC needs to be the same as Eigen::Dynamic."); + + EuclideanManifold() : size_{Size} { + static_assert( + Size != ceres::DYNAMIC, + "The size is set to dynamic. Please call the constructor with a size."); + } + + explicit EuclideanManifold(int size) : size_(size) { + if (Size != ceres::DYNAMIC) { + CHECK_EQ(Size, size) + << "Specified size by template parameter differs from the supplied " + "one."; + } else { + CHECK_GE(size_, 0) + << "The size of the manifold needs to be non-negative."; + } + } + + int AmbientSize() const override { return size_; } + int TangentSize() const override { return size_; } + + bool Plus(const double* x_ptr, + const double* delta_ptr, + double* x_plus_delta_ptr) const override { + Eigen::Map<const AmbientVector> x(x_ptr, size_); + Eigen::Map<const AmbientVector> delta(delta_ptr, size_); + Eigen::Map<AmbientVector> x_plus_delta(x_plus_delta_ptr, size_); + x_plus_delta = x + delta; + return true; + } + + bool PlusJacobian(const double* x_ptr, double* jacobian_ptr) const override { + Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, size_, size_); + jacobian.setIdentity(); + return true; + } + + bool RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const override { + std::copy_n(ambient_matrix, num_rows * size_, tangent_matrix); + return true; + } + + bool Minus(const double* y_ptr, + const double* x_ptr, + double* y_minus_x_ptr) const override { + Eigen::Map<const AmbientVector> x(x_ptr, size_); + Eigen::Map<const AmbientVector> y(y_ptr, size_); + Eigen::Map<AmbientVector> y_minus_x(y_minus_x_ptr, size_); + y_minus_x = y - x; + return true; + } + + bool MinusJacobian(const double* x_ptr, double* jacobian_ptr) const override { + Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, size_, size_); + jacobian.setIdentity(); + return true; + } + + private: + static constexpr bool IsDynamic = (Size == ceres::DYNAMIC); + using AmbientVector = Eigen::Matrix<double, Size, 1>; + using MatrixJacobian = Eigen::Matrix<double, Size, Size, Eigen::RowMajor>; + + int size_{}; +}; + +// Hold a subset of the parameters inside a parameter block constant. +class CERES_EXPORT SubsetManifold final : public Manifold { + public: + SubsetManifold(int size, const std::vector<int>& constant_parameters); + int AmbientSize() const override; + int TangentSize() const override; + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool PlusJacobian(const double* x, double* jacobian) const override; + bool RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const override; + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override; + bool MinusJacobian(const double* x, double* jacobian) const override; + + private: + const int tangent_size_ = 0; + std::vector<bool> constancy_mask_; +}; + +// Implements the manifold for a Hamilton quaternion as defined in +// https://en.wikipedia.org/wiki/Quaternion. Quaternions are represented as +// unit norm 4-vectors, i.e. +// +// q = [q0; q1; q2; q3], |q| = 1 +// +// is the ambient space representation. +// +// q0 scalar part. +// q1 coefficient of i. +// q2 coefficient of j. +// q3 coefficient of k. +// +// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. +// +// The tangent space is R^3, which relates to the ambient space through the +// Plus and Minus operations defined as: +// +// Plus(x, delta) = [cos(|delta|); sin(|delta|) * delta / |delta|] * x +// Minus(y, x) = to_delta(y * x^{-1}) +// +// where "*" is the quaternion product and because q is a unit quaternion +// (|q|=1), q^-1 = [q0; -q1; -q2; -q3] +// +// and to_delta( [q0; u_{3x1}] ) = u / |u| * atan2(|u|, q0) +class CERES_EXPORT QuaternionManifold final : public Manifold { + public: + int AmbientSize() const override { return 4; } + int TangentSize() const override { return 3; } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool PlusJacobian(const double* x, double* jacobian) const override; + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override; + bool MinusJacobian(const double* x, double* jacobian) const override; +}; + +// Implements the quaternion manifold for Eigen's representation of the +// Hamilton quaternion. Geometrically it is exactly the same as the +// QuaternionManifold defined above. However, Eigen uses a different internal +// memory layout for the elements of the quaternion than what is commonly +// used. It stores the quaternion in memory as [q1, q2, q3, q0] or +// [x, y, z, w] where the real (scalar) part is last. +// +// Since Ceres operates on parameter blocks which are raw double pointers this +// difference is important and requires a different manifold. +class CERES_EXPORT EigenQuaternionManifold final : public Manifold { + public: + int AmbientSize() const override { return 4; } + int TangentSize() const override { return 3; } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool PlusJacobian(const double* x, double* jacobian) const override; + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override; + bool MinusJacobian(const double* x, double* jacobian) const override; +}; + +} // namespace ceres + +// clang-format off +#include "ceres/internal/reenable_warnings.h" +// clang-format on + +#endif // CERES_PUBLIC_MANIFOLD_H_ |