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 38 namespace ceres { 39 40 // Purpose: Sometimes parameter blocks x can overparameterize a problem 41 // 42 // min f(x) 43 // x 44 // 45 // In that case it is desirable to choose a parameterization for the 46 // block itself to remove the null directions of the cost. More 47 // generally, if x lies on a manifold of a smaller dimension than the 48 // ambient space that it is embedded in, then it is numerically and 49 // computationally more effective to optimize it using a 50 // parameterization that lives in the tangent space of that manifold 51 // at each point. 52 // 53 // For example, a sphere in three dimensions is a 2 dimensional 54 // manifold, embedded in a three dimensional space. At each point on 55 // the sphere, the plane tangent to it defines a two dimensional 56 // tangent space. For a cost function defined on this sphere, given a 57 // point x, moving in the direction normal to the sphere at that point 58 // is not useful. Thus a better way to do a local optimization is to 59 // optimize over two dimensional vector delta in the tangent space at 60 // that point and then "move" to the point x + delta, where the move 61 // operation involves projecting back onto the sphere. Doing so 62 // removes a redundent dimension from the optimization, making it 63 // numerically more robust and efficient. 64 // 65 // More generally we can define a function 66 // 67 // x_plus_delta = Plus(x, delta), 68 // 69 // where x_plus_delta has the same size as x, and delta is of size 70 // less than or equal to x. The function Plus, generalizes the 71 // definition of vector addition. Thus it satisfies the identify 72 // 73 // Plus(x, 0) = x, for all x. 74 // 75 // A trivial version of Plus is when delta is of the same size as x 76 // and 77 // 78 // Plus(x, delta) = x + delta 79 // 80 // A more interesting case if x is two dimensional vector, and the 81 // user wishes to hold the first coordinate constant. Then, delta is a 82 // scalar and Plus is defined as 83 // 84 // Plus(x, delta) = x + [0] * delta 85 // [1] 86 // 87 // An example that occurs commonly in Structure from Motion problems 88 // is when camera rotations are parameterized using Quaternion. There, 89 // it is useful only make updates orthogonal to that 4-vector defining 90 // the quaternion. One way to do this is to let delta be a 3 91 // dimensional vector and define Plus to be 92 // 93 // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x 94 // 95 // The multiplication between the two 4-vectors on the RHS is the 96 // standard quaternion product. 97 // 98 // Given g and a point x, optimizing f can now be restated as 99 // 100 // min f(Plus(x, delta)) 101 // delta 102 // 103 // Given a solution delta to this problem, the optimal value is then 104 // given by 105 // 106 // x* = Plus(x, delta) 107 // 108 // The class LocalParameterization defines the function Plus and its 109 // Jacobian which is needed to compute the Jacobian of f w.r.t delta. 110 class LocalParameterization { 111 public: ~LocalParameterization()112 virtual ~LocalParameterization() {} 113 114 // Generalization of the addition operation, 115 // 116 // x_plus_delta = Plus(x, delta) 117 // 118 // with the condition that Plus(x, 0) = x. 119 virtual bool Plus(const double* x, 120 const double* delta, 121 double* x_plus_delta) const = 0; 122 123 // The jacobian of Plus(x, delta) w.r.t delta at delta = 0. 124 virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; 125 126 // Size of x. 127 virtual int GlobalSize() const = 0; 128 129 // Size of delta. 130 virtual int LocalSize() const = 0; 131 }; 132 133 // Some basic parameterizations 134 135 // Identity Parameterization: Plus(x, delta) = x + delta 136 class IdentityParameterization : public LocalParameterization { 137 public: 138 explicit IdentityParameterization(int size); ~IdentityParameterization()139 virtual ~IdentityParameterization() {} 140 virtual bool Plus(const double* x, 141 const double* delta, 142 double* x_plus_delta) const; 143 virtual bool ComputeJacobian(const double* x, 144 double* jacobian) const; GlobalSize()145 virtual int GlobalSize() const { return size_; } LocalSize()146 virtual int LocalSize() const { return size_; } 147 148 private: 149 const int size_; 150 }; 151 152 // Hold a subset of the parameters inside a parameter block constant. 153 class SubsetParameterization : public LocalParameterization { 154 public: 155 explicit SubsetParameterization(int size, 156 const vector<int>& constant_parameters); ~SubsetParameterization()157 virtual ~SubsetParameterization() {} 158 virtual bool Plus(const double* x, 159 const double* delta, 160 double* x_plus_delta) const; 161 virtual bool ComputeJacobian(const double* x, 162 double* jacobian) const; GlobalSize()163 virtual int GlobalSize() const { return constancy_mask_.size(); } LocalSize()164 virtual int LocalSize() const { return local_size_; } 165 166 private: 167 const int local_size_; 168 vector<int> constancy_mask_; 169 }; 170 171 // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x 172 // with * being the quaternion multiplication operator. Here we assume 173 // that the first element of the quaternion vector is the real (cos 174 // theta) part. 175 class QuaternionParameterization : public LocalParameterization { 176 public: ~QuaternionParameterization()177 virtual ~QuaternionParameterization() {} 178 virtual bool Plus(const double* x, 179 const double* delta, 180 double* x_plus_delta) const; 181 virtual bool ComputeJacobian(const double* x, 182 double* jacobian) const; GlobalSize()183 virtual int GlobalSize() const { return 4; } LocalSize()184 virtual int LocalSize() const { return 3; } 185 }; 186 187 } // namespace ceres 188 189 #endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ 190