include/ceres/local_parameterization.h - ceres-solver - Git at Google

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