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 39 namespace 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. 111 class CERES_EXPORT LocalParameterization { 112 public: ~LocalParameterization()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 137 class CERES_EXPORT IdentityParameterization : public LocalParameterization { 138 public: 139 explicit IdentityParameterization(int size); ~IdentityParameterization()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; GlobalSize()146 virtual int GlobalSize() const { return size_; } LocalSize()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. 154 class CERES_EXPORT SubsetParameterization : public LocalParameterization { 155 public: 156 explicit SubsetParameterization(int size, 157 const vector<int>& constant_parameters); ~SubsetParameterization()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; GlobalSize()164 virtual int GlobalSize() const { 165 return static_cast<int>(constancy_mask_.size()); 166 } LocalSize()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. 178 class CERES_EXPORT QuaternionParameterization : public LocalParameterization { 179 public: ~QuaternionParameterization()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; GlobalSize()186 virtual int GlobalSize() const { return 4; } LocalSize()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