GoogleGit

blob: 67633de309f283d6b6e6a2c619bc2f8b69fc6eef [file] [log] [blame]
  1. // Ceres Solver - A fast non-linear least squares minimizer
  2. // Copyright 2015 Google Inc. All rights reserved.
  3. // http://ceres-solver.org/
  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. #ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
  32. #define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
  33. #include <vector>
  34. #include "ceres/internal/port.h"
  35. #include "ceres/internal/scoped_ptr.h"
  36. #include "ceres/internal/disable_warnings.h"
  37. namespace ceres {
  38. // Purpose: Sometimes parameter blocks x can overparameterize a problem
  39. //
  40. // min f(x)
  41. // x
  42. //
  43. // In that case it is desirable to choose a parameterization for the
  44. // block itself to remove the null directions of the cost. More
  45. // generally, if x lies on a manifold of a smaller dimension than the
  46. // ambient space that it is embedded in, then it is numerically and
  47. // computationally more effective to optimize it using a
  48. // parameterization that lives in the tangent space of that manifold
  49. // at each point.
  50. //
  51. // For example, a sphere in three dimensions is a 2 dimensional
  52. // manifold, embedded in a three dimensional space. At each point on
  53. // the sphere, the plane tangent to it defines a two dimensional
  54. // tangent space. For a cost function defined on this sphere, given a
  55. // point x, moving in the direction normal to the sphere at that point
  56. // is not useful. Thus a better way to do a local optimization is to
  57. // optimize over two dimensional vector delta in the tangent space at
  58. // that point and then "move" to the point x + delta, where the move
  59. // operation involves projecting back onto the sphere. Doing so
  60. // removes a redundent dimension from the optimization, making it
  61. // numerically more robust and efficient.
  62. //
  63. // More generally we can define a function
  64. //
  65. // x_plus_delta = Plus(x, delta),
  66. //
  67. // where x_plus_delta has the same size as x, and delta is of size
  68. // less than or equal to x. The function Plus, generalizes the
  69. // definition of vector addition. Thus it satisfies the identify
  70. //
  71. // Plus(x, 0) = x, for all x.
  72. //
  73. // A trivial version of Plus is when delta is of the same size as x
  74. // and
  75. //
  76. // Plus(x, delta) = x + delta
  77. //
  78. // A more interesting case if x is two dimensional vector, and the
  79. // user wishes to hold the first coordinate constant. Then, delta is a
  80. // scalar and Plus is defined as
  81. //
  82. // Plus(x, delta) = x + [0] * delta
  83. // [1]
  84. //
  85. // An example that occurs commonly in Structure from Motion problems
  86. // is when camera rotations are parameterized using Quaternion. There,
  87. // it is useful only make updates orthogonal to that 4-vector defining
  88. // the quaternion. One way to do this is to let delta be a 3
  89. // dimensional vector and define Plus to be
  90. //
  91. // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x
  92. //
  93. // The multiplication between the two 4-vectors on the RHS is the
  94. // standard quaternion product.
  95. //
  96. // Given g and a point x, optimizing f can now be restated as
  97. //
  98. // min f(Plus(x, delta))
  99. // delta
  100. //
  101. // Given a solution delta to this problem, the optimal value is then
  102. // given by
  103. //
  104. // x* = Plus(x, delta)
  105. //
  106. // The class LocalParameterization defines the function Plus and its
  107. // Jacobian which is needed to compute the Jacobian of f w.r.t delta.
  108. class CERES_EXPORT LocalParameterization {
  109. public:
  110. virtual ~LocalParameterization();
  111. // Generalization of the addition operation,
  112. //
  113. // x_plus_delta = Plus(x, delta)
  114. //
  115. // with the condition that Plus(x, 0) = x.
  116. virtual bool Plus(const double* x,
  117. const double* delta,
  118. double* x_plus_delta) const = 0;
  119. // The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  120. //
  121. // jacobian is a row-major GlobalSize() x LocalSize() matrix.
  122. virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;
  123. // local_matrix = global_matrix * jacobian
  124. //
  125. // global_matrix is a num_rows x GlobalSize row major matrix.
  126. // local_matrix is a num_rows x LocalSize row major matrix.
  127. // jacobian(x) is the matrix returned by ComputeJacobian at x.
  128. //
  129. // This is only used by GradientProblem. For most normal uses, it is
  130. // okay to use the default implementation.
  131. virtual bool MultiplyByJacobian(const double* x,
  132. const int num_rows,
  133. const double* global_matrix,
  134. double* local_matrix) const;
  135. // Size of x.
  136. virtual int GlobalSize() const = 0;
  137. // Size of delta.
  138. virtual int LocalSize() const = 0;
  139. };
  140. // Some basic parameterizations
  141. // Identity Parameterization: Plus(x, delta) = x + delta
  142. class CERES_EXPORT IdentityParameterization : public LocalParameterization {
  143. public:
  144. explicit IdentityParameterization(int size);
  145. virtual ~IdentityParameterization() {}
  146. virtual bool Plus(const double* x,
  147. const double* delta,
  148. double* x_plus_delta) const;
  149. virtual bool ComputeJacobian(const double* x,
  150. double* jacobian) const;
  151. virtual bool MultiplyByJacobian(const double* x,
  152. const int num_cols,
  153. const double* global_matrix,
  154. double* local_matrix) const;
  155. virtual int GlobalSize() const { return size_; }
  156. virtual int LocalSize() const { return size_; }
  157. private:
  158. const int size_;
  159. };
  160. // Hold a subset of the parameters inside a parameter block constant.
  161. class CERES_EXPORT SubsetParameterization : public LocalParameterization {
  162. public:
  163. explicit SubsetParameterization(int size,
  164. const std::vector<int>& constant_parameters);
  165. virtual ~SubsetParameterization() {}
  166. virtual bool Plus(const double* x,
  167. const double* delta,
  168. double* x_plus_delta) const;
  169. virtual bool ComputeJacobian(const double* x,
  170. double* jacobian) const;
  171. virtual bool MultiplyByJacobian(const double* x,
  172. const int num_cols,
  173. const double* global_matrix,
  174. double* local_matrix) const;
  175. virtual int GlobalSize() const {
  176. return static_cast<int>(constancy_mask_.size());
  177. }
  178. virtual int LocalSize() const { return local_size_; }
  179. private:
  180. const int local_size_;
  181. std::vector<char> constancy_mask_;
  182. };
  183. // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x
  184. // with * being the quaternion multiplication operator. Here we assume
  185. // that the first element of the quaternion vector is the real (cos
  186. // theta) part.
  187. class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
  188. public:
  189. virtual ~QuaternionParameterization() {}
  190. virtual bool Plus(const double* x,
  191. const double* delta,
  192. double* x_plus_delta) const;
  193. virtual bool ComputeJacobian(const double* x,
  194. double* jacobian) const;
  195. virtual int GlobalSize() const { return 4; }
  196. virtual int LocalSize() const { return 3; }
  197. };
  198. // This provides a parameterization for homogeneous vectors which are commonly
  199. // used in Structure for Motion problems. One example where they are used is
  200. // in representing points whose triangulation is ill-conditioned. Here
  201. // it is advantageous to use an over-parameterization since homogeneous vectors
  202. // can represent points at infinity.
  203. //
  204. // The plus operator is defined as
  205. // Plus(x, delta) =
  206. // [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x
  207. // with * defined as an operator which applies the update orthogonal to x to
  208. // remain on the sphere. We assume that the last element of x is the scalar
  209. // component. The size of the homogeneous vector is required to be greater than
  210. // 1.
  211. class CERES_EXPORT HomogeneousVectorParameterization :
  212. public LocalParameterization {
  213. public:
  214. explicit HomogeneousVectorParameterization(int size);
  215. virtual ~HomogeneousVectorParameterization() {}
  216. virtual bool Plus(const double* x,
  217. const double* delta,
  218. double* x_plus_delta) const;
  219. virtual bool ComputeJacobian(const double* x,
  220. double* jacobian) const;
  221. virtual int GlobalSize() const { return size_; }
  222. virtual int LocalSize() const { return size_ - 1; }
  223. private:
  224. const int size_;
  225. };
  226. // Construct a local parameterization by taking the Cartesian product
  227. // of a number of other local parameterizations. This is useful, when
  228. // a parameter block is the cartesian product of two or more
  229. // manifolds. For example the parameters of a camera consist of a
  230. // rotation and a translation, i.e., SO(3) x R^3.
  231. //
  232. // Currently this class supports taking the cartesian product of up to
  233. // four local parameterizations.
  234. //
  235. // Example usage:
  236. //
  237. // ProductParameterization product_param(new QuaterionionParameterization(),
  238. // new IdentityParameterization(3));
  239. //
  240. // is the local parameterization for a rigid transformation, where the
  241. // rotation is represented using a quaternion.
  242. class CERES_EXPORT ProductParameterization : public LocalParameterization {
  243. public:
  244. //
  245. // NOTE: All the constructors take ownership of the input local
  246. // parameterizations.
  247. //
  248. ProductParameterization(LocalParameterization* local_param1,
  249. LocalParameterization* local_param2);
  250. ProductParameterization(LocalParameterization* local_param1,
  251. LocalParameterization* local_param2,
  252. LocalParameterization* local_param3);
  253. ProductParameterization(LocalParameterization* local_param1,
  254. LocalParameterization* local_param2,
  255. LocalParameterization* local_param3,
  256. LocalParameterization* local_param4);
  257. virtual ~ProductParameterization();
  258. virtual bool Plus(const double* x,
  259. const double* delta,
  260. double* x_plus_delta) const;
  261. virtual bool ComputeJacobian(const double* x,
  262. double* jacobian) const;
  263. virtual int GlobalSize() const { return global_size_; }
  264. virtual int LocalSize() const { return local_size_; }
  265. private:
  266. void Init();
  267. std::vector<LocalParameterization*> local_params_;
  268. int local_size_;
  269. int global_size_;
  270. int buffer_size_;
  271. };
  272. } // namespace ceres
  273. #include "ceres/internal/reenable_warnings.h"
  274. #endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_