// 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_