1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
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.
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.
29 // Author: keir@google.com (Keir Mierle)
30 // sameeragarwal@google.com (Sameer Agarwal)
32 #ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
33 #define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
36 #include "ceres/internal/port.h"
37 #include "ceres/internal/scoped_ptr.h"
38 #include "ceres/internal/disable_warnings.h"
42 // Purpose: Sometimes parameter blocks x can overparameterize a problem
47 // In that case it is desirable to choose a parameterization for the
48 // block itself to remove the null directions of the cost. More
49 // generally, if x lies on a manifold of a smaller dimension than the
50 // ambient space that it is embedded in, then it is numerically and
51 // computationally more effective to optimize it using a
52 // parameterization that lives in the tangent space of that manifold
55 // For example, a sphere in three dimensions is a 2 dimensional
56 // manifold, embedded in a three dimensional space. At each point on
57 // the sphere, the plane tangent to it defines a two dimensional
58 // tangent space. For a cost function defined on this sphere, given a
59 // point x, moving in the direction normal to the sphere at that point
60 // is not useful. Thus a better way to do a local optimization is to
61 // optimize over two dimensional vector delta in the tangent space at
62 // that point and then "move" to the point x + delta, where the move
63 // operation involves projecting back onto the sphere. Doing so
64 // removes a redundent dimension from the optimization, making it
65 // numerically more robust and efficient.
67 // More generally we can define a function
69 // x_plus_delta = Plus(x, delta),
71 // where x_plus_delta has the same size as x, and delta is of size
72 // less than or equal to x. The function Plus, generalizes the
73 // definition of vector addition. Thus it satisfies the identify
75 // Plus(x, 0) = x, for all x.
77 // A trivial version of Plus is when delta is of the same size as x
80 // Plus(x, delta) = x + delta
82 // A more interesting case if x is two dimensional vector, and the
83 // user wishes to hold the first coordinate constant. Then, delta is a
84 // scalar and Plus is defined as
86 // Plus(x, delta) = x + [0] * delta
89 // An example that occurs commonly in Structure from Motion problems
90 // is when camera rotations are parameterized using Quaternion. There,
91 // it is useful only make updates orthogonal to that 4-vector defining
92 // the quaternion. One way to do this is to let delta be a 3
93 // dimensional vector and define Plus to be
95 // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x
97 // The multiplication between the two 4-vectors on the RHS is the
98 // standard quaternion product.
100 // Given g and a point x, optimizing f can now be restated as
102 // min f(Plus(x, delta))
105 // Given a solution delta to this problem, the optimal value is then
108 // x* = Plus(x, delta)
110 // The class LocalParameterization defines the function Plus and its
111 // Jacobian which is needed to compute the Jacobian of f w.r.t delta.
112 class CERES_EXPORT LocalParameterization {
114 virtual ~LocalParameterization();
116 // Generalization of the addition operation,
118 // x_plus_delta = Plus(x, delta)
120 // with the condition that Plus(x, 0) = x.
121 virtual bool Plus(const double* x,
123 double* x_plus_delta) const = 0;
125 // The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
127 // jacobian is a row-major GlobalSize() x LocalSize() matrix.
128 virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;
130 // local_matrix = global_matrix * jacobian
132 // global_matrix is a num_rows x GlobalSize row major matrix.
133 // local_matrix is a num_rows x LocalSize row major matrix.
134 // jacobian(x) is the matrix returned by ComputeJacobian at x.
136 // This is only used by GradientProblem. For most normal uses, it is
137 // okay to use the default implementation.
138 virtual bool MultiplyByJacobian(const double* x,
140 const double* global_matrix,
141 double* local_matrix) const;
144 virtual int GlobalSize() const = 0;
147 virtual int LocalSize() const = 0;
150 // Some basic parameterizations
152 // Identity Parameterization: Plus(x, delta) = x + delta
153 class CERES_EXPORT IdentityParameterization : public LocalParameterization {
155 explicit IdentityParameterization(int size);
156 virtual ~IdentityParameterization() {}
157 virtual bool Plus(const double* x,
159 double* x_plus_delta) const;
160 virtual bool ComputeJacobian(const double* x,
161 double* jacobian) const;
162 virtual bool MultiplyByJacobian(const double* x,
164 const double* global_matrix,
165 double* local_matrix) const;
166 virtual int GlobalSize() const { return size_; }
167 virtual int LocalSize() const { return size_; }
173 // Hold a subset of the parameters inside a parameter block constant.
174 class CERES_EXPORT SubsetParameterization : public LocalParameterization {
176 explicit SubsetParameterization(int size,
177 const std::vector<int>& constant_parameters);
178 virtual ~SubsetParameterization() {}
179 virtual bool Plus(const double* x,
181 double* x_plus_delta) const;
182 virtual bool ComputeJacobian(const double* x,
183 double* jacobian) const;
184 virtual bool MultiplyByJacobian(const double* x,
186 const double* global_matrix,
187 double* local_matrix) const;
188 virtual int GlobalSize() const {
189 return static_cast<int>(constancy_mask_.size());
191 virtual int LocalSize() const { return local_size_; }
194 const int local_size_;
195 std::vector<char> constancy_mask_;
198 // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x
199 // with * being the quaternion multiplication operator. Here we assume
200 // that the first element of the quaternion vector is the real (cos
202 class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
204 virtual ~QuaternionParameterization() {}
205 virtual bool Plus(const double* x,
207 double* x_plus_delta) const;
208 virtual bool ComputeJacobian(const double* x,
209 double* jacobian) const;
210 virtual int GlobalSize() const { return 4; }
211 virtual int LocalSize() const { return 3; }
214 // Implements the quaternion local parameterization for Eigen's representation
215 // of the quaternion. Eigen uses a different internal memory layout for the
216 // elements of the quaternion than what is commonly used. Specifically, Eigen
217 // stores the elements in memory as [x, y, z, w] where the real part is last
218 // whereas it is typically stored first. Note, when creating an Eigen quaternion
219 // through the constructor the elements are accepted in w, x, y, z order. Since
220 // Ceres operates on parameter blocks which are raw double pointers this
221 // difference is important and requires a different parameterization.
223 // Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x
224 // with * being the quaternion multiplication operator.
225 class CERES_EXPORT EigenQuaternionParameterization
226 : public ceres::LocalParameterization {
228 virtual ~EigenQuaternionParameterization() {}
229 virtual bool Plus(const double* x,
231 double* x_plus_delta) const;
232 virtual bool ComputeJacobian(const double* x, double* jacobian) const;
233 virtual int GlobalSize() const { return 4; }
234 virtual int LocalSize() const { return 3; }
237 // This provides a parameterization for homogeneous vectors which are commonly
238 // used in Structure for Motion problems. One example where they are used is
239 // in representing points whose triangulation is ill-conditioned. Here
240 // it is advantageous to use an over-parameterization since homogeneous vectors
241 // can represent points at infinity.
243 // The plus operator is defined as
245 // [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x
246 // with * defined as an operator which applies the update orthogonal to x to
247 // remain on the sphere. We assume that the last element of x is the scalar
248 // component. The size of the homogeneous vector is required to be greater than
250 class CERES_EXPORT HomogeneousVectorParameterization :
251 public LocalParameterization {
253 explicit HomogeneousVectorParameterization(int size);
254 virtual ~HomogeneousVectorParameterization() {}
255 virtual bool Plus(const double* x,
257 double* x_plus_delta) const;
258 virtual bool ComputeJacobian(const double* x,
259 double* jacobian) const;
260 virtual int GlobalSize() const { return size_; }
261 virtual int LocalSize() const { return size_ - 1; }
267 // Construct a local parameterization by taking the Cartesian product
268 // of a number of other local parameterizations. This is useful, when
269 // a parameter block is the cartesian product of two or more
270 // manifolds. For example the parameters of a camera consist of a
271 // rotation and a translation, i.e., SO(3) x R^3.
273 // Currently this class supports taking the cartesian product of up to
274 // four local parameterizations.
278 // ProductParameterization product_param(new QuaterionionParameterization(),
279 // new IdentityParameterization(3));
281 // is the local parameterization for a rigid transformation, where the
282 // rotation is represented using a quaternion.
283 class CERES_EXPORT ProductParameterization : public LocalParameterization {
286 // NOTE: All the constructors take ownership of the input local
287 // parameterizations.
289 ProductParameterization(LocalParameterization* local_param1,
290 LocalParameterization* local_param2);
292 ProductParameterization(LocalParameterization* local_param1,
293 LocalParameterization* local_param2,
294 LocalParameterization* local_param3);
296 ProductParameterization(LocalParameterization* local_param1,
297 LocalParameterization* local_param2,
298 LocalParameterization* local_param3,
299 LocalParameterization* local_param4);
301 virtual ~ProductParameterization();
302 virtual bool Plus(const double* x,
304 double* x_plus_delta) const;
305 virtual bool ComputeJacobian(const double* x,
306 double* jacobian) const;
307 virtual int GlobalSize() const { return global_size_; }
308 virtual int LocalSize() const { return local_size_; }
313 std::vector<LocalParameterization*> local_params_;
321 #include "ceres/internal/reenable_warnings.h"
323 #endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_