|  | // Ceres Solver - A fast non-linear least squares minimizer | 
|  | // Copyright 2015 Google Inc. All rights reserved. | 
|  | // http://ceres-solver.org/ | 
|  | // | 
|  | // Redistribution and use in source and binary forms, with or without | 
|  | // modification, are permitted provided that the following conditions are met: | 
|  | // | 
|  | // * Redistributions of source code must retain the above copyright notice, | 
|  | //   this list of conditions and the following disclaimer. | 
|  | // * Redistributions in binary form must reproduce the above copyright notice, | 
|  | //   this list of conditions and the following disclaimer in the documentation | 
|  | //   and/or other materials provided with the distribution. | 
|  | // * Neither the name of Google Inc. nor the names of its contributors may be | 
|  | //   used to endorse or promote products derived from this software without | 
|  | //   specific prior written permission. | 
|  | // | 
|  | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" | 
|  | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | 
|  | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | 
|  | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE | 
|  | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | 
|  | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | 
|  | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | 
|  | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | 
|  | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | 
|  | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | 
|  | // POSSIBILITY OF SUCH DAMAGE. | 
|  | // | 
|  | // Author: keir@google.com (Keir Mierle) | 
|  | //         sameeragarwal@google.com (Sameer Agarwal) | 
|  |  | 
|  | #ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ | 
|  | #define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ | 
|  |  | 
|  | #include <vector> | 
|  | #include "ceres/internal/port.h" | 
|  | #include "ceres/internal/scoped_ptr.h" | 
|  | #include "ceres/internal/disable_warnings.h" | 
|  |  | 
|  | namespace ceres { | 
|  |  | 
|  | // Purpose: Sometimes parameter blocks x can overparameterize a problem | 
|  | // | 
|  | //   min f(x) | 
|  | //    x | 
|  | // | 
|  | // In that case it is desirable to choose a parameterization for the | 
|  | // block itself to remove the null directions of the cost. More | 
|  | // generally, if x lies on a manifold of a smaller dimension than the | 
|  | // ambient space that it is embedded in, then it is numerically and | 
|  | // computationally more effective to optimize it using a | 
|  | // parameterization that lives in the tangent space of that manifold | 
|  | // at each point. | 
|  | // | 
|  | // For example, a sphere in three dimensions is a 2 dimensional | 
|  | // manifold, embedded in a three dimensional space. At each point on | 
|  | // the sphere, the plane tangent to it defines a two dimensional | 
|  | // tangent space. For a cost function defined on this sphere, given a | 
|  | // point x, moving in the direction normal to the sphere at that point | 
|  | // is not useful. Thus a better way to do a local optimization is to | 
|  | // optimize over two dimensional vector delta in the tangent space at | 
|  | // that point and then "move" to the point x + delta, where the move | 
|  | // operation involves projecting back onto the sphere. Doing so | 
|  | // removes a redundent dimension from the optimization, making it | 
|  | // numerically more robust and efficient. | 
|  | // | 
|  | // More generally we can define a function | 
|  | // | 
|  | //   x_plus_delta = Plus(x, delta), | 
|  | // | 
|  | // where x_plus_delta has the same size as x, and delta is of size | 
|  | // less than or equal to x. The function Plus, generalizes the | 
|  | // definition of vector addition. Thus it satisfies the identify | 
|  | // | 
|  | //   Plus(x, 0) = x, for all x. | 
|  | // | 
|  | // A trivial version of Plus is when delta is of the same size as x | 
|  | // and | 
|  | // | 
|  | //   Plus(x, delta) = x + delta | 
|  | // | 
|  | // A more interesting case if x is two dimensional vector, and the | 
|  | // user wishes to hold the first coordinate constant. Then, delta is a | 
|  | // scalar and Plus is defined as | 
|  | // | 
|  | //   Plus(x, delta) = x + [0] * delta | 
|  | //                        [1] | 
|  | // | 
|  | // An example that occurs commonly in Structure from Motion problems | 
|  | // is when camera rotations are parameterized using Quaternion. There, | 
|  | // it is useful only make updates orthogonal to that 4-vector defining | 
|  | // the quaternion. One way to do this is to let delta be a 3 | 
|  | // dimensional vector and define Plus to be | 
|  | // | 
|  | //   Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x | 
|  | // | 
|  | // The multiplication between the two 4-vectors on the RHS is the | 
|  | // standard quaternion product. | 
|  | // | 
|  | // Given g and a point x, optimizing f can now be restated as | 
|  | // | 
|  | //     min  f(Plus(x, delta)) | 
|  | //    delta | 
|  | // | 
|  | // Given a solution delta to this problem, the optimal value is then | 
|  | // given by | 
|  | // | 
|  | //   x* = Plus(x, delta) | 
|  | // | 
|  | // The class LocalParameterization defines the function Plus and its | 
|  | // Jacobian which is needed to compute the Jacobian of f w.r.t delta. | 
|  | class CERES_EXPORT LocalParameterization { | 
|  | public: | 
|  | virtual ~LocalParameterization(); | 
|  |  | 
|  | // Generalization of the addition operation, | 
|  | // | 
|  | //   x_plus_delta = Plus(x, delta) | 
|  | // | 
|  | // with the condition that Plus(x, 0) = x. | 
|  | virtual bool Plus(const double* x, | 
|  | const double* delta, | 
|  | double* x_plus_delta) const = 0; | 
|  |  | 
|  | // The jacobian of Plus(x, delta) w.r.t delta at delta = 0. | 
|  | // | 
|  | // jacobian is a row-major GlobalSize() x LocalSize() matrix. | 
|  | virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; | 
|  |  | 
|  | // local_matrix = global_matrix * jacobian | 
|  | // | 
|  | // global_matrix is a num_rows x GlobalSize  row major matrix. | 
|  | // local_matrix is a num_rows x LocalSize row major matrix. | 
|  | // jacobian(x) is the matrix returned by ComputeJacobian at x. | 
|  | // | 
|  | // This is only used by GradientProblem. For most normal uses, it is | 
|  | // okay to use the default implementation. | 
|  | virtual bool MultiplyByJacobian(const double* x, | 
|  | const int num_rows, | 
|  | const double* global_matrix, | 
|  | double* local_matrix) const; | 
|  |  | 
|  | // Size of x. | 
|  | virtual int GlobalSize() const = 0; | 
|  |  | 
|  | // Size of delta. | 
|  | virtual int LocalSize() const = 0; | 
|  | }; | 
|  |  | 
|  | // Some basic parameterizations | 
|  |  | 
|  | // Identity Parameterization: Plus(x, delta) = x + delta | 
|  | class CERES_EXPORT IdentityParameterization : public LocalParameterization { | 
|  | public: | 
|  | explicit IdentityParameterization(int size); | 
|  | virtual ~IdentityParameterization() {} | 
|  | virtual bool Plus(const double* x, | 
|  | const double* delta, | 
|  | double* x_plus_delta) const; | 
|  | virtual bool ComputeJacobian(const double* x, | 
|  | double* jacobian) const; | 
|  | virtual bool MultiplyByJacobian(const double* x, | 
|  | const int num_cols, | 
|  | const double* global_matrix, | 
|  | double* local_matrix) const; | 
|  | virtual int GlobalSize() const { return size_; } | 
|  | virtual int LocalSize() const { return size_; } | 
|  |  | 
|  | private: | 
|  | const int size_; | 
|  | }; | 
|  |  | 
|  | // Hold a subset of the parameters inside a parameter block constant. | 
|  | class CERES_EXPORT SubsetParameterization : public LocalParameterization { | 
|  | public: | 
|  | explicit SubsetParameterization(int size, | 
|  | const std::vector<int>& constant_parameters); | 
|  | virtual ~SubsetParameterization() {} | 
|  | virtual bool Plus(const double* x, | 
|  | const double* delta, | 
|  | double* x_plus_delta) const; | 
|  | virtual bool ComputeJacobian(const double* x, | 
|  | double* jacobian) const; | 
|  | virtual bool MultiplyByJacobian(const double* x, | 
|  | const int num_cols, | 
|  | const double* global_matrix, | 
|  | double* local_matrix) const; | 
|  | virtual int GlobalSize() const { | 
|  | return static_cast<int>(constancy_mask_.size()); | 
|  | } | 
|  | virtual int LocalSize() const { return local_size_; } | 
|  |  | 
|  | private: | 
|  | const int local_size_; | 
|  | std::vector<char> constancy_mask_; | 
|  | }; | 
|  |  | 
|  | // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x | 
|  | // with * being the quaternion multiplication operator. Here we assume | 
|  | // that the first element of the quaternion vector is the real (cos | 
|  | // theta) part. | 
|  | class CERES_EXPORT QuaternionParameterization : public LocalParameterization { | 
|  | public: | 
|  | virtual ~QuaternionParameterization() {} | 
|  | virtual bool Plus(const double* x, | 
|  | const double* delta, | 
|  | double* x_plus_delta) const; | 
|  | virtual bool ComputeJacobian(const double* x, | 
|  | double* jacobian) const; | 
|  | virtual int GlobalSize() const { return 4; } | 
|  | virtual int LocalSize() const { return 3; } | 
|  | }; | 
|  |  | 
|  | // Implements the quaternion local parameterization for Eigen's representation | 
|  | // of the quaternion. Eigen uses a different internal memory layout for the | 
|  | // elements of the quaternion than what is commonly used. Specifically, Eigen | 
|  | // stores the elements in memory as [x, y, z, w] where the real part is last | 
|  | // whereas it is typically stored first. Note, when creating an Eigen quaternion | 
|  | // through the constructor the elements are accepted in w, x, y, z order. Since | 
|  | // Ceres operates on parameter blocks which are raw double pointers this | 
|  | // difference is important and requires a different parameterization. | 
|  | // | 
|  | // Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x | 
|  | // with * being the quaternion multiplication operator. | 
|  | class CERES_EXPORT EigenQuaternionParameterization | 
|  | : public ceres::LocalParameterization { | 
|  | public: | 
|  | virtual ~EigenQuaternionParameterization() {} | 
|  | virtual bool Plus(const double* x, | 
|  | const double* delta, | 
|  | double* x_plus_delta) const; | 
|  | virtual bool ComputeJacobian(const double* x, double* jacobian) const; | 
|  | virtual int GlobalSize() const { return 4; } | 
|  | virtual int LocalSize() const { return 3; } | 
|  | }; | 
|  |  | 
|  | // This provides a parameterization for homogeneous vectors which are commonly | 
|  | // used in Structure for Motion problems.  One example where they are used is | 
|  | // in representing points whose triangulation is ill-conditioned. Here | 
|  | // it is advantageous to use an over-parameterization since homogeneous vectors | 
|  | // can represent points at infinity. | 
|  | // | 
|  | // The plus operator is defined as | 
|  | // Plus(x, delta) = | 
|  | //    [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x | 
|  | // with * defined as an operator which applies the update orthogonal to x to | 
|  | // remain on the sphere. We assume that the last element of x is the scalar | 
|  | // component. The size of the homogeneous vector is required to be greater than | 
|  | // 1. | 
|  | class CERES_EXPORT HomogeneousVectorParameterization : | 
|  | public LocalParameterization { | 
|  | public: | 
|  | explicit HomogeneousVectorParameterization(int size); | 
|  | virtual ~HomogeneousVectorParameterization() {} | 
|  | virtual bool Plus(const double* x, | 
|  | const double* delta, | 
|  | double* x_plus_delta) const; | 
|  | virtual bool ComputeJacobian(const double* x, | 
|  | double* jacobian) const; | 
|  | virtual int GlobalSize() const { return size_; } | 
|  | virtual int LocalSize() const { return size_ - 1; } | 
|  |  | 
|  | private: | 
|  | const int size_; | 
|  | }; | 
|  |  | 
|  | // Construct a local parameterization by taking the Cartesian product | 
|  | // of a number of other local parameterizations. This is useful, when | 
|  | // a parameter block is the cartesian product of two or more | 
|  | // manifolds. For example the parameters of a camera consist of a | 
|  | // rotation and a translation, i.e., SO(3) x R^3. | 
|  | // | 
|  | // Currently this class supports taking the cartesian product of up to | 
|  | // four local parameterizations. | 
|  | // | 
|  | // Example usage: | 
|  | // | 
|  | // ProductParameterization product_param(new QuaterionionParameterization(), | 
|  | //                                       new IdentityParameterization(3)); | 
|  | // | 
|  | // is the local parameterization for a rigid transformation, where the | 
|  | // rotation is represented using a quaternion. | 
|  | class CERES_EXPORT ProductParameterization : public LocalParameterization { | 
|  | public: | 
|  | // | 
|  | // NOTE: All the constructors take ownership of the input local | 
|  | // parameterizations. | 
|  | // | 
|  | ProductParameterization(LocalParameterization* local_param1, | 
|  | LocalParameterization* local_param2); | 
|  |  | 
|  | ProductParameterization(LocalParameterization* local_param1, | 
|  | LocalParameterization* local_param2, | 
|  | LocalParameterization* local_param3); | 
|  |  | 
|  | ProductParameterization(LocalParameterization* local_param1, | 
|  | LocalParameterization* local_param2, | 
|  | LocalParameterization* local_param3, | 
|  | LocalParameterization* local_param4); | 
|  |  | 
|  | virtual ~ProductParameterization(); | 
|  | virtual bool Plus(const double* x, | 
|  | const double* delta, | 
|  | double* x_plus_delta) const; | 
|  | virtual bool ComputeJacobian(const double* x, | 
|  | double* jacobian) const; | 
|  | virtual int GlobalSize() const { return global_size_; } | 
|  | virtual int LocalSize() const { return local_size_; } | 
|  |  | 
|  | private: | 
|  | void Init(); | 
|  |  | 
|  | std::vector<LocalParameterization*> local_params_; | 
|  | int local_size_; | 
|  | int global_size_; | 
|  | int buffer_size_; | 
|  | }; | 
|  |  | 
|  | }  // namespace ceres | 
|  |  | 
|  | #include "ceres/internal/reenable_warnings.h" | 
|  |  | 
|  | #endif  // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ |