1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3// http://code.google.com/p/ceres-solver/ 4// 5// Redistribution and use in source and binary forms, with or without 6// modification, are permitted provided that the following conditions are met: 7// 8// * Redistributions of source code must retain the above copyright notice, 9// this list of conditions and the following disclaimer. 10// * Redistributions in binary form must reproduce the above copyright notice, 11// this list of conditions and the following disclaimer in the documentation 12// and/or other materials provided with the distribution. 13// * Neither the name of Google Inc. nor the names of its contributors may be 14// used to endorse or promote products derived from this software without 15// specific prior written permission. 16// 17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27// POSSIBILITY OF SUCH DAMAGE. 28// 29// Author: keir@google.com (Keir Mierle) 30// sameeragarwal@google.com (Sameer Agarwal) 31 32#ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ 33#define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ 34 35#include <vector> 36#include "ceres/internal/port.h" 37#include "ceres/internal/disable_warnings.h" 38 39namespace ceres { 40 41// Purpose: Sometimes parameter blocks x can overparameterize a problem 42// 43// min f(x) 44// x 45// 46// In that case it is desirable to choose a parameterization for the 47// block itself to remove the null directions of the cost. More 48// generally, if x lies on a manifold of a smaller dimension than the 49// ambient space that it is embedded in, then it is numerically and 50// computationally more effective to optimize it using a 51// parameterization that lives in the tangent space of that manifold 52// at each point. 53// 54// For example, a sphere in three dimensions is a 2 dimensional 55// manifold, embedded in a three dimensional space. At each point on 56// the sphere, the plane tangent to it defines a two dimensional 57// tangent space. For a cost function defined on this sphere, given a 58// point x, moving in the direction normal to the sphere at that point 59// is not useful. Thus a better way to do a local optimization is to 60// optimize over two dimensional vector delta in the tangent space at 61// that point and then "move" to the point x + delta, where the move 62// operation involves projecting back onto the sphere. Doing so 63// removes a redundent dimension from the optimization, making it 64// numerically more robust and efficient. 65// 66// More generally we can define a function 67// 68// x_plus_delta = Plus(x, delta), 69// 70// where x_plus_delta has the same size as x, and delta is of size 71// less than or equal to x. The function Plus, generalizes the 72// definition of vector addition. Thus it satisfies the identify 73// 74// Plus(x, 0) = x, for all x. 75// 76// A trivial version of Plus is when delta is of the same size as x 77// and 78// 79// Plus(x, delta) = x + delta 80// 81// A more interesting case if x is two dimensional vector, and the 82// user wishes to hold the first coordinate constant. Then, delta is a 83// scalar and Plus is defined as 84// 85// Plus(x, delta) = x + [0] * delta 86// [1] 87// 88// An example that occurs commonly in Structure from Motion problems 89// is when camera rotations are parameterized using Quaternion. There, 90// it is useful only make updates orthogonal to that 4-vector defining 91// the quaternion. One way to do this is to let delta be a 3 92// dimensional vector and define Plus to be 93// 94// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x 95// 96// The multiplication between the two 4-vectors on the RHS is the 97// standard quaternion product. 98// 99// Given g and a point x, optimizing f can now be restated as 100// 101// min f(Plus(x, delta)) 102// delta 103// 104// Given a solution delta to this problem, the optimal value is then 105// given by 106// 107// x* = Plus(x, delta) 108// 109// The class LocalParameterization defines the function Plus and its 110// Jacobian which is needed to compute the Jacobian of f w.r.t delta. 111class CERES_EXPORT LocalParameterization { 112 public: 113 virtual ~LocalParameterization() {} 114 115 // Generalization of the addition operation, 116 // 117 // x_plus_delta = Plus(x, delta) 118 // 119 // with the condition that Plus(x, 0) = x. 120 virtual bool Plus(const double* x, 121 const double* delta, 122 double* x_plus_delta) const = 0; 123 124 // The jacobian of Plus(x, delta) w.r.t delta at delta = 0. 125 virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; 126 127 // Size of x. 128 virtual int GlobalSize() const = 0; 129 130 // Size of delta. 131 virtual int LocalSize() const = 0; 132}; 133 134// Some basic parameterizations 135 136// Identity Parameterization: Plus(x, delta) = x + delta 137class CERES_EXPORT IdentityParameterization : public LocalParameterization { 138 public: 139 explicit IdentityParameterization(int size); 140 virtual ~IdentityParameterization() {} 141 virtual bool Plus(const double* x, 142 const double* delta, 143 double* x_plus_delta) const; 144 virtual bool ComputeJacobian(const double* x, 145 double* jacobian) const; 146 virtual int GlobalSize() const { return size_; } 147 virtual int LocalSize() const { return size_; } 148 149 private: 150 const int size_; 151}; 152 153// Hold a subset of the parameters inside a parameter block constant. 154class CERES_EXPORT SubsetParameterization : public LocalParameterization { 155 public: 156 explicit SubsetParameterization(int size, 157 const vector<int>& constant_parameters); 158 virtual ~SubsetParameterization() {} 159 virtual bool Plus(const double* x, 160 const double* delta, 161 double* x_plus_delta) const; 162 virtual bool ComputeJacobian(const double* x, 163 double* jacobian) const; 164 virtual int GlobalSize() const { 165 return static_cast<int>(constancy_mask_.size()); 166 } 167 virtual int LocalSize() const { return local_size_; } 168 169 private: 170 const int local_size_; 171 vector<int> constancy_mask_; 172}; 173 174// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x 175// with * being the quaternion multiplication operator. Here we assume 176// that the first element of the quaternion vector is the real (cos 177// theta) part. 178class CERES_EXPORT QuaternionParameterization : public LocalParameterization { 179 public: 180 virtual ~QuaternionParameterization() {} 181 virtual bool Plus(const double* x, 182 const double* delta, 183 double* x_plus_delta) const; 184 virtual bool ComputeJacobian(const double* x, 185 double* jacobian) const; 186 virtual int GlobalSize() const { return 4; } 187 virtual int LocalSize() const { return 3; } 188}; 189 190} // namespace ceres 191 192#include "ceres/internal/reenable_warnings.h" 193 194#endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ 195