dd20dc36ba125bc59131cbcb6461af2a654b4c5d
[blender.git] / extern / ceres / include / ceres / covariance.h
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: sameeragarwal@google.com (Sameer Agarwal)
30
31 #ifndef CERES_PUBLIC_COVARIANCE_H_
32 #define CERES_PUBLIC_COVARIANCE_H_
33
34 #include <utility>
35 #include <vector>
36 #include "ceres/internal/port.h"
37 #include "ceres/internal/scoped_ptr.h"
38 #include "ceres/types.h"
39 #include "ceres/internal/disable_warnings.h"
40
41 namespace ceres {
42
43 class Problem;
44
45 namespace internal {
46 class CovarianceImpl;
47 }  // namespace internal
48
49 // WARNING
50 // =======
51 // It is very easy to use this class incorrectly without understanding
52 // the underlying mathematics. Please read and understand the
53 // documentation completely before attempting to use this class.
54 //
55 //
56 // This class allows the user to evaluate the covariance for a
57 // non-linear least squares problem and provides random access to its
58 // blocks
59 //
60 // Background
61 // ==========
62 // One way to assess the quality of the solution returned by a
63 // non-linear least squares solve is to analyze the covariance of the
64 // solution.
65 //
66 // Let us consider the non-linear regression problem
67 //
68 //   y = f(x) + N(0, I)
69 //
70 // i.e., the observation y is a random non-linear function of the
71 // independent variable x with mean f(x) and identity covariance. Then
72 // the maximum likelihood estimate of x given observations y is the
73 // solution to the non-linear least squares problem:
74 //
75 //  x* = arg min_x |f(x)|^2
76 //
77 // And the covariance of x* is given by
78 //
79 //  C(x*) = inverse[J'(x*)J(x*)]
80 //
81 // Here J(x*) is the Jacobian of f at x*. The above formula assumes
82 // that J(x*) has full column rank.
83 //
84 // If J(x*) is rank deficient, then the covariance matrix C(x*) is
85 // also rank deficient and is given by
86 //
87 //  C(x*) =  pseudoinverse[J'(x*)J(x*)]
88 //
89 // Note that in the above, we assumed that the covariance
90 // matrix for y was identity. This is an important assumption. If this
91 // is not the case and we have
92 //
93 //  y = f(x) + N(0, S)
94 //
95 // Where S is a positive semi-definite matrix denoting the covariance
96 // of y, then the maximum likelihood problem to be solved is
97 //
98 //  x* = arg min_x f'(x) inverse[S] f(x)
99 //
100 // and the corresponding covariance estimate of x* is given by
101 //
102 //  C(x*) = inverse[J'(x*) inverse[S] J(x*)]
103 //
104 // So, if it is the case that the observations being fitted to have a
105 // covariance matrix not equal to identity, then it is the user's
106 // responsibility that the corresponding cost functions are correctly
107 // scaled, e.g. in the above case the cost function for this problem
108 // should evaluate S^{-1/2} f(x) instead of just f(x), where S^{-1/2}
109 // is the inverse square root of the covariance matrix S.
110 //
111 // This class allows the user to evaluate the covariance for a
112 // non-linear least squares problem and provides random access to its
113 // blocks. The computation assumes that the CostFunctions compute
114 // residuals such that their covariance is identity.
115 //
116 // Since the computation of the covariance matrix requires computing
117 // the inverse of a potentially large matrix, this can involve a
118 // rather large amount of time and memory. However, it is usually the
119 // case that the user is only interested in a small part of the
120 // covariance matrix. Quite often just the block diagonal. This class
121 // allows the user to specify the parts of the covariance matrix that
122 // she is interested in and then uses this information to only compute
123 // and store those parts of the covariance matrix.
124 //
125 // Rank of the Jacobian
126 // --------------------
127 // As we noted above, if the jacobian is rank deficient, then the
128 // inverse of J'J is not defined and instead a pseudo inverse needs to
129 // be computed.
130 //
131 // The rank deficiency in J can be structural -- columns which are
132 // always known to be zero or numerical -- depending on the exact
133 // values in the Jacobian.
134 //
135 // Structural rank deficiency occurs when the problem contains
136 // parameter blocks that are constant. This class correctly handles
137 // structural rank deficiency like that.
138 //
139 // Numerical rank deficiency, where the rank of the matrix cannot be
140 // predicted by its sparsity structure and requires looking at its
141 // numerical values is more complicated. Here again there are two
142 // cases.
143 //
144 //   a. The rank deficiency arises from overparameterization. e.g., a
145 //   four dimensional quaternion used to parameterize SO(3), which is
146 //   a three dimensional manifold. In cases like this, the user should
147 //   use an appropriate LocalParameterization. Not only will this lead
148 //   to better numerical behaviour of the Solver, it will also expose
149 //   the rank deficiency to the Covariance object so that it can
150 //   handle it correctly.
151 //
152 //   b. More general numerical rank deficiency in the Jacobian
153 //   requires the computation of the so called Singular Value
154 //   Decomposition (SVD) of J'J. We do not know how to do this for
155 //   large sparse matrices efficiently. For small and moderate sized
156 //   problems this is done using dense linear algebra.
157 //
158 // Gauge Invariance
159 // ----------------
160 // In structure from motion (3D reconstruction) problems, the
161 // reconstruction is ambiguous upto a similarity transform. This is
162 // known as a Gauge Ambiguity. Handling Gauges correctly requires the
163 // use of SVD or custom inversion algorithms. For small problems the
164 // user can use the dense algorithm. For more details see
165 //
166 // Ken-ichi Kanatani, Daniel D. Morris: Gauges and gauge
167 // transformations for uncertainty description of geometric structure
168 // with indeterminacy. IEEE Transactions on Information Theory 47(5):
169 // 2017-2028 (2001)
170 //
171 // Example Usage
172 // =============
173 //
174 //  double x[3];
175 //  double y[2];
176 //
177 //  Problem problem;
178 //  problem.AddParameterBlock(x, 3);
179 //  problem.AddParameterBlock(y, 2);
180 //  <Build Problem>
181 //  <Solve Problem>
182 //
183 //  Covariance::Options options;
184 //  Covariance covariance(options);
185 //
186 //  std::vector<std::pair<const double*, const double*> > covariance_blocks;
187 //  covariance_blocks.push_back(make_pair(x, x));
188 //  covariance_blocks.push_back(make_pair(y, y));
189 //  covariance_blocks.push_back(make_pair(x, y));
190 //
191 //  CHECK(covariance.Compute(covariance_blocks, &problem));
192 //
193 //  double covariance_xx[3 * 3];
194 //  double covariance_yy[2 * 2];
195 //  double covariance_xy[3 * 2];
196 //  covariance.GetCovarianceBlock(x, x, covariance_xx)
197 //  covariance.GetCovarianceBlock(y, y, covariance_yy)
198 //  covariance.GetCovarianceBlock(x, y, covariance_xy)
199 //
200 class CERES_EXPORT Covariance {
201  public:
202   struct CERES_EXPORT Options {
203     Options()
204 #ifndef CERES_NO_SUITESPARSE
205         : algorithm_type(SUITE_SPARSE_QR),
206 #else
207         : algorithm_type(EIGEN_SPARSE_QR),
208 #endif
209           min_reciprocal_condition_number(1e-14),
210           null_space_rank(0),
211           num_threads(1),
212           apply_loss_function(true) {
213     }
214
215     // Ceres supports three different algorithms for covariance
216     // estimation, which represent different tradeoffs in speed,
217     // accuracy and reliability.
218     //
219     // 1. DENSE_SVD uses Eigen's JacobiSVD to perform the
220     //    computations. It computes the singular value decomposition
221     //
222     //      U * S * V' = J
223     //
224     //    and then uses it to compute the pseudo inverse of J'J as
225     //
226     //      pseudoinverse[J'J]^ = V * pseudoinverse[S] * V'
227     //
228     //    It is an accurate but slow method and should only be used
229     //    for small to moderate sized problems. It can handle
230     //    full-rank as well as rank deficient Jacobians.
231     //
232     // 2. EIGEN_SPARSE_QR uses the sparse QR factorization algorithm
233     //    in Eigen to compute the decomposition
234     //
235     //      Q * R = J
236     //
237     //    [J'J]^-1 = [R*R']^-1
238     //
239     //    It is a moderately fast algorithm for sparse matrices.
240     //
241     // 3. SUITE_SPARSE_QR uses the SuiteSparseQR sparse QR
242     //    factorization algorithm. It uses dense linear algebra and is
243     //    multi threaded, so for large sparse sparse matrices it is
244     //    significantly faster than EIGEN_SPARSE_QR.
245     //
246     // Neither EIGEN_SPARSE_QR not SUITE_SPARSE_QR are capable of
247     // computing the covariance if the Jacobian is rank deficient.
248     CovarianceAlgorithmType algorithm_type;
249
250     // If the Jacobian matrix is near singular, then inverting J'J
251     // will result in unreliable results, e.g, if
252     //
253     //   J = [1.0 1.0         ]
254     //       [1.0 1.0000001   ]
255     //
256     // which is essentially a rank deficient matrix, we have
257     //
258     //   inv(J'J) = [ 2.0471e+14  -2.0471e+14]
259     //              [-2.0471e+14   2.0471e+14]
260     //
261     // This is not a useful result. Therefore, by default
262     // Covariance::Compute will return false if a rank deficient
263     // Jacobian is encountered. How rank deficiency is detected
264     // depends on the algorithm being used.
265     //
266     // 1. DENSE_SVD
267     //
268     //      min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)
269     //
270     //    where min_sigma and max_sigma are the minimum and maxiumum
271     //    singular values of J respectively.
272     //
273     // 2. SUITE_SPARSE_QR and EIGEN_SPARSE_QR
274     //
275     //      rank(J) < num_col(J)
276     //
277     //   Here rank(J) is the estimate of the rank of J returned by the
278     //   sparse QR factorization algorithm. It is a fairly reliable
279     //   indication of rank deficiency.
280     //
281     double min_reciprocal_condition_number;
282
283     // When using DENSE_SVD, the user has more control in dealing with
284     // singular and near singular covariance matrices.
285     //
286     // As mentioned above, when the covariance matrix is near
287     // singular, instead of computing the inverse of J'J, the
288     // Moore-Penrose pseudoinverse of J'J should be computed.
289     //
290     // If J'J has the eigen decomposition (lambda_i, e_i), where
291     // lambda_i is the i^th eigenvalue and e_i is the corresponding
292     // eigenvector, then the inverse of J'J is
293     //
294     //   inverse[J'J] = sum_i e_i e_i' / lambda_i
295     //
296     // and computing the pseudo inverse involves dropping terms from
297     // this sum that correspond to small eigenvalues.
298     //
299     // How terms are dropped is controlled by
300     // min_reciprocal_condition_number and null_space_rank.
301     //
302     // If null_space_rank is non-negative, then the smallest
303     // null_space_rank eigenvalue/eigenvectors are dropped
304     // irrespective of the magnitude of lambda_i. If the ratio of the
305     // smallest non-zero eigenvalue to the largest eigenvalue in the
306     // truncated matrix is still below
307     // min_reciprocal_condition_number, then the Covariance::Compute()
308     // will fail and return false.
309     //
310     // Setting null_space_rank = -1 drops all terms for which
311     //
312     //   lambda_i / lambda_max < min_reciprocal_condition_number.
313     //
314     // This option has no effect on the SUITE_SPARSE_QR and
315     // EIGEN_SPARSE_QR algorithms.
316     int null_space_rank;
317
318     int num_threads;
319
320     // Even though the residual blocks in the problem may contain loss
321     // functions, setting apply_loss_function to false will turn off
322     // the application of the loss function to the output of the cost
323     // function and in turn its effect on the covariance.
324     //
325     // TODO(sameergaarwal): Expand this based on Jim's experiments.
326     bool apply_loss_function;
327   };
328
329   explicit Covariance(const Options& options);
330   ~Covariance();
331
332   // Compute a part of the covariance matrix.
333   //
334   // The vector covariance_blocks, indexes into the covariance matrix
335   // block-wise using pairs of parameter blocks. This allows the
336   // covariance estimation algorithm to only compute and store these
337   // blocks.
338   //
339   // Since the covariance matrix is symmetric, if the user passes
340   // (block1, block2), then GetCovarianceBlock can be called with
341   // block1, block2 as well as block2, block1.
342   //
343   // covariance_blocks cannot contain duplicates. Bad things will
344   // happen if they do.
345   //
346   // Note that the list of covariance_blocks is only used to determine
347   // what parts of the covariance matrix are computed. The full
348   // Jacobian is used to do the computation, i.e. they do not have an
349   // impact on what part of the Jacobian is used for computation.
350   //
351   // The return value indicates the success or failure of the
352   // covariance computation. Please see the documentation for
353   // Covariance::Options for more on the conditions under which this
354   // function returns false.
355   bool Compute(
356       const std::vector<std::pair<const double*,
357                                   const double*> >& covariance_blocks,
358       Problem* problem);
359
360   // Return the block of the cross-covariance matrix corresponding to
361   // parameter_block1 and parameter_block2.
362   //
363   // Compute must be called before the first call to
364   // GetCovarianceBlock and the pair <parameter_block1,
365   // parameter_block2> OR the pair <parameter_block2,
366   // parameter_block1> must have been present in the vector
367   // covariance_blocks when Compute was called. Otherwise
368   // GetCovarianceBlock will return false.
369   //
370   // covariance_block must point to a memory location that can store a
371   // parameter_block1_size x parameter_block2_size matrix. The
372   // returned covariance will be a row-major matrix.
373   bool GetCovarianceBlock(const double* parameter_block1,
374                           const double* parameter_block2,
375                           double* covariance_block) const;
376
377   // Return the block of the cross-covariance matrix corresponding to
378   // parameter_block1 and parameter_block2.
379   // Returns cross-covariance in the tangent space if a local
380   // parameterization is associated with either parameter block;
381   // else returns cross-covariance in the ambient space.
382   //
383   // Compute must be called before the first call to
384   // GetCovarianceBlock and the pair <parameter_block1,
385   // parameter_block2> OR the pair <parameter_block2,
386   // parameter_block1> must have been present in the vector
387   // covariance_blocks when Compute was called. Otherwise
388   // GetCovarianceBlock will return false.
389   //
390   // covariance_block must point to a memory location that can store a
391   // parameter_block1_local_size x parameter_block2_local_size matrix. The
392   // returned covariance will be a row-major matrix.
393   bool GetCovarianceBlockInTangentSpace(const double* parameter_block1,
394                                         const double* parameter_block2,
395                                         double* covariance_block) const;
396
397  private:
398   internal::scoped_ptr<internal::CovarianceImpl> impl_;
399 };
400
401 }  // namespace ceres
402
403 #include "ceres/internal/reenable_warnings.h"
404
405 #endif  // CERES_PUBLIC_COVARIANCE_H_