3e8302bed553440478a949cf0f274172b61688b3
[blender.git] / extern / ceres / internal / ceres / covariance_impl.cc
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 #include "ceres/covariance_impl.h"
32
33 #ifdef CERES_USE_OPENMP
34 #include <omp.h>
35 #endif
36
37 #include <algorithm>
38 #include <cstdlib>
39 #include <utility>
40 #include <vector>
41
42 #include "Eigen/SparseCore"
43 #include "Eigen/SparseQR"
44 #include "Eigen/SVD"
45
46 #include "ceres/compressed_col_sparse_matrix_utils.h"
47 #include "ceres/compressed_row_sparse_matrix.h"
48 #include "ceres/covariance.h"
49 #include "ceres/crs_matrix.h"
50 #include "ceres/internal/eigen.h"
51 #include "ceres/map_util.h"
52 #include "ceres/parameter_block.h"
53 #include "ceres/problem_impl.h"
54 #include "ceres/suitesparse.h"
55 #include "ceres/wall_time.h"
56 #include "glog/logging.h"
57
58 namespace ceres {
59 namespace internal {
60
61 using std::make_pair;
62 using std::map;
63 using std::pair;
64 using std::swap;
65 using std::vector;
66
67 typedef vector<pair<const double*, const double*> > CovarianceBlocks;
68
69 CovarianceImpl::CovarianceImpl(const Covariance::Options& options)
70     : options_(options),
71       is_computed_(false),
72       is_valid_(false) {
73 #ifndef CERES_USE_OPENMP
74   if (options_.num_threads > 1) {
75     LOG(WARNING)
76         << "OpenMP support is not compiled into this binary; "
77         << "only options.num_threads = 1 is supported. Switching "
78         << "to single threaded mode.";
79     options_.num_threads = 1;
80   }
81 #endif
82   evaluate_options_.num_threads = options_.num_threads;
83   evaluate_options_.apply_loss_function = options_.apply_loss_function;
84 }
85
86 CovarianceImpl::~CovarianceImpl() {
87 }
88
89 bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks,
90                              ProblemImpl* problem) {
91   problem_ = problem;
92   parameter_block_to_row_index_.clear();
93   covariance_matrix_.reset(NULL);
94   is_valid_ = (ComputeCovarianceSparsity(covariance_blocks, problem) &&
95                ComputeCovarianceValues());
96   is_computed_ = true;
97   return is_valid_;
98 }
99
100 bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace(
101     const double* original_parameter_block1,
102     const double* original_parameter_block2,
103     bool lift_covariance_to_ambient_space,
104     double* covariance_block) const {
105   CHECK(is_computed_)
106       << "Covariance::GetCovarianceBlock called before Covariance::Compute";
107   CHECK(is_valid_)
108       << "Covariance::GetCovarianceBlock called when Covariance::Compute "
109       << "returned false.";
110
111   // If either of the two parameter blocks is constant, then the
112   // covariance block is also zero.
113   if (constant_parameter_blocks_.count(original_parameter_block1) > 0 ||
114       constant_parameter_blocks_.count(original_parameter_block2) > 0) {
115     const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map();
116     ParameterBlock* block1 =
117         FindOrDie(parameter_map,
118                   const_cast<double*>(original_parameter_block1));
119
120     ParameterBlock* block2 =
121         FindOrDie(parameter_map,
122                   const_cast<double*>(original_parameter_block2));
123     const int block1_size = block1->Size();
124     const int block2_size = block2->Size();
125     MatrixRef(covariance_block, block1_size, block2_size).setZero();
126     return true;
127   }
128
129   const double* parameter_block1 = original_parameter_block1;
130   const double* parameter_block2 = original_parameter_block2;
131   const bool transpose = parameter_block1 > parameter_block2;
132   if (transpose) {
133     swap(parameter_block1, parameter_block2);
134   }
135
136   // Find where in the covariance matrix the block is located.
137   const int row_begin =
138       FindOrDie(parameter_block_to_row_index_, parameter_block1);
139   const int col_begin =
140       FindOrDie(parameter_block_to_row_index_, parameter_block2);
141   const int* rows = covariance_matrix_->rows();
142   const int* cols = covariance_matrix_->cols();
143   const int row_size = rows[row_begin + 1] - rows[row_begin];
144   const int* cols_begin = cols + rows[row_begin];
145
146   // The only part that requires work is walking the compressed column
147   // vector to determine where the set of columns correspnding to the
148   // covariance block begin.
149   int offset = 0;
150   while (cols_begin[offset] != col_begin && offset < row_size) {
151     ++offset;
152   }
153
154   if (offset == row_size) {
155     LOG(ERROR) << "Unable to find covariance block for "
156                << original_parameter_block1 << " "
157                << original_parameter_block2;
158     return false;
159   }
160
161   const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map();
162   ParameterBlock* block1 =
163       FindOrDie(parameter_map, const_cast<double*>(parameter_block1));
164   ParameterBlock* block2 =
165       FindOrDie(parameter_map, const_cast<double*>(parameter_block2));
166   const LocalParameterization* local_param1 = block1->local_parameterization();
167   const LocalParameterization* local_param2 = block2->local_parameterization();
168   const int block1_size = block1->Size();
169   const int block1_local_size = block1->LocalSize();
170   const int block2_size = block2->Size();
171   const int block2_local_size = block2->LocalSize();
172
173   ConstMatrixRef cov(covariance_matrix_->values() + rows[row_begin],
174                      block1_size,
175                      row_size);
176
177   // Fast path when there are no local parameterizations or if the
178   // user does not want it lifted to the ambient space.
179   if ((local_param1 == NULL && local_param2 == NULL) ||
180       !lift_covariance_to_ambient_space) {
181     if (transpose) {
182       MatrixRef(covariance_block, block2_local_size, block1_local_size) =
183           cov.block(0, offset, block1_local_size,
184                     block2_local_size).transpose();
185     } else {
186       MatrixRef(covariance_block, block1_local_size, block2_local_size) =
187           cov.block(0, offset, block1_local_size, block2_local_size);
188     }
189     return true;
190   }
191
192   // If local parameterizations are used then the covariance that has
193   // been computed is in the tangent space and it needs to be lifted
194   // back to the ambient space.
195   //
196   // This is given by the formula
197   //
198   //  C'_12 = J_1 C_12 J_2'
199   //
200   // Where C_12 is the local tangent space covariance for parameter
201   // blocks 1 and 2. J_1 and J_2 are respectively the local to global
202   // jacobians for parameter blocks 1 and 2.
203   //
204   // See Result 5.11 on page 142 of Hartley & Zisserman (2nd Edition)
205   // for a proof.
206   //
207   // TODO(sameeragarwal): Add caching of local parameterization, so
208   // that they are computed just once per parameter block.
209   Matrix block1_jacobian(block1_size, block1_local_size);
210   if (local_param1 == NULL) {
211     block1_jacobian.setIdentity();
212   } else {
213     local_param1->ComputeJacobian(parameter_block1, block1_jacobian.data());
214   }
215
216   Matrix block2_jacobian(block2_size, block2_local_size);
217   // Fast path if the user is requesting a diagonal block.
218   if (parameter_block1 == parameter_block2) {
219     block2_jacobian = block1_jacobian;
220   } else {
221     if (local_param2 == NULL) {
222       block2_jacobian.setIdentity();
223     } else {
224       local_param2->ComputeJacobian(parameter_block2, block2_jacobian.data());
225     }
226   }
227
228   if (transpose) {
229     MatrixRef(covariance_block, block2_size, block1_size) =
230         block2_jacobian *
231         cov.block(0, offset, block1_local_size, block2_local_size).transpose() *
232         block1_jacobian.transpose();
233   } else {
234     MatrixRef(covariance_block, block1_size, block2_size) =
235         block1_jacobian *
236         cov.block(0, offset, block1_local_size, block2_local_size) *
237         block2_jacobian.transpose();
238   }
239
240   return true;
241 }
242
243 // Determine the sparsity pattern of the covariance matrix based on
244 // the block pairs requested by the user.
245 bool CovarianceImpl::ComputeCovarianceSparsity(
246     const CovarianceBlocks&  original_covariance_blocks,
247     ProblemImpl* problem) {
248   EventLogger event_logger("CovarianceImpl::ComputeCovarianceSparsity");
249
250   // Determine an ordering for the parameter block, by sorting the
251   // parameter blocks by their pointers.
252   vector<double*> all_parameter_blocks;
253   problem->GetParameterBlocks(&all_parameter_blocks);
254   const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map();
255   constant_parameter_blocks_.clear();
256   vector<double*>& active_parameter_blocks =
257       evaluate_options_.parameter_blocks;
258   active_parameter_blocks.clear();
259   for (int i = 0; i < all_parameter_blocks.size(); ++i) {
260     double* parameter_block = all_parameter_blocks[i];
261
262     ParameterBlock* block = FindOrDie(parameter_map, parameter_block);
263     if (block->IsConstant()) {
264       constant_parameter_blocks_.insert(parameter_block);
265     } else {
266       active_parameter_blocks.push_back(parameter_block);
267     }
268   }
269
270   std::sort(active_parameter_blocks.begin(), active_parameter_blocks.end());
271
272   // Compute the number of rows.  Map each parameter block to the
273   // first row corresponding to it in the covariance matrix using the
274   // ordering of parameter blocks just constructed.
275   int num_rows = 0;
276   parameter_block_to_row_index_.clear();
277   for (int i = 0; i < active_parameter_blocks.size(); ++i) {
278     double* parameter_block = active_parameter_blocks[i];
279     const int parameter_block_size =
280         problem->ParameterBlockLocalSize(parameter_block);
281     parameter_block_to_row_index_[parameter_block] = num_rows;
282     num_rows += parameter_block_size;
283   }
284
285   // Compute the number of non-zeros in the covariance matrix.  Along
286   // the way flip any covariance blocks which are in the lower
287   // triangular part of the matrix.
288   int num_nonzeros = 0;
289   CovarianceBlocks covariance_blocks;
290   for (int i = 0; i <  original_covariance_blocks.size(); ++i) {
291     const pair<const double*, const double*>& block_pair =
292         original_covariance_blocks[i];
293     if (constant_parameter_blocks_.count(block_pair.first) > 0 ||
294         constant_parameter_blocks_.count(block_pair.second) > 0) {
295       continue;
296     }
297
298     int index1 = FindOrDie(parameter_block_to_row_index_, block_pair.first);
299     int index2 = FindOrDie(parameter_block_to_row_index_, block_pair.second);
300     const int size1 = problem->ParameterBlockLocalSize(block_pair.first);
301     const int size2 = problem->ParameterBlockLocalSize(block_pair.second);
302     num_nonzeros += size1 * size2;
303
304     // Make sure we are constructing a block upper triangular matrix.
305     if (index1 > index2) {
306       covariance_blocks.push_back(make_pair(block_pair.second,
307                                             block_pair.first));
308     } else {
309       covariance_blocks.push_back(block_pair);
310     }
311   }
312
313   if (covariance_blocks.size() == 0) {
314     VLOG(2) << "No non-zero covariance blocks found";
315     covariance_matrix_.reset(NULL);
316     return true;
317   }
318
319   // Sort the block pairs. As a consequence we get the covariance
320   // blocks as they will occur in the CompressedRowSparseMatrix that
321   // will store the covariance.
322   sort(covariance_blocks.begin(), covariance_blocks.end());
323
324   // Fill the sparsity pattern of the covariance matrix.
325   covariance_matrix_.reset(
326       new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros));
327
328   int* rows = covariance_matrix_->mutable_rows();
329   int* cols = covariance_matrix_->mutable_cols();
330
331   // Iterate over parameter blocks and in turn over the rows of the
332   // covariance matrix. For each parameter block, look in the upper
333   // triangular part of the covariance matrix to see if there are any
334   // blocks requested by the user. If this is the case then fill out a
335   // set of compressed rows corresponding to this parameter block.
336   //
337   // The key thing that makes this loop work is the fact that the
338   // row/columns of the covariance matrix are ordered by the pointer
339   // values of the parameter blocks. Thus iterating over the keys of
340   // parameter_block_to_row_index_ corresponds to iterating over the
341   // rows of the covariance matrix in order.
342   int i = 0;  // index into covariance_blocks.
343   int cursor = 0;  // index into the covariance matrix.
344   for (map<const double*, int>::const_iterator it =
345            parameter_block_to_row_index_.begin();
346        it != parameter_block_to_row_index_.end();
347        ++it) {
348     const double* row_block =  it->first;
349     const int row_block_size = problem->ParameterBlockLocalSize(row_block);
350     int row_begin = it->second;
351
352     // Iterate over the covariance blocks contained in this row block
353     // and count the number of columns in this row block.
354     int num_col_blocks = 0;
355     int num_columns = 0;
356     for (int j = i; j < covariance_blocks.size(); ++j, ++num_col_blocks) {
357       const pair<const double*, const double*>& block_pair =
358           covariance_blocks[j];
359       if (block_pair.first != row_block) {
360         break;
361       }
362       num_columns += problem->ParameterBlockLocalSize(block_pair.second);
363     }
364
365     // Fill out all the compressed rows for this parameter block.
366     for (int r = 0; r < row_block_size; ++r) {
367       rows[row_begin + r] = cursor;
368       for (int c = 0; c < num_col_blocks; ++c) {
369         const double* col_block = covariance_blocks[i + c].second;
370         const int col_block_size = problem->ParameterBlockLocalSize(col_block);
371         int col_begin = FindOrDie(parameter_block_to_row_index_, col_block);
372         for (int k = 0; k < col_block_size; ++k) {
373           cols[cursor++] = col_begin++;
374         }
375       }
376     }
377
378     i+= num_col_blocks;
379   }
380
381   rows[num_rows] = cursor;
382   return true;
383 }
384
385 bool CovarianceImpl::ComputeCovarianceValues() {
386   switch (options_.algorithm_type) {
387     case DENSE_SVD:
388       return ComputeCovarianceValuesUsingDenseSVD();
389 #ifndef CERES_NO_SUITESPARSE
390     case SUITE_SPARSE_QR:
391       return ComputeCovarianceValuesUsingSuiteSparseQR();
392 #else
393       LOG(ERROR) << "SuiteSparse is required to use the "
394                  << "SUITE_SPARSE_QR algorithm.";
395       return false;
396 #endif
397     case EIGEN_SPARSE_QR:
398       return ComputeCovarianceValuesUsingEigenSparseQR();
399     default:
400       LOG(ERROR) << "Unsupported covariance estimation algorithm type: "
401                  << CovarianceAlgorithmTypeToString(options_.algorithm_type);
402       return false;
403   }
404   return false;
405 }
406
407 bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() {
408   EventLogger event_logger(
409       "CovarianceImpl::ComputeCovarianceValuesUsingSparseQR");
410
411 #ifndef CERES_NO_SUITESPARSE
412   if (covariance_matrix_.get() == NULL) {
413     // Nothing to do, all zeros covariance matrix.
414     return true;
415   }
416
417   CRSMatrix jacobian;
418   problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
419   event_logger.AddEvent("Evaluate");
420
421   // Construct a compressed column form of the Jacobian.
422   const int num_rows = jacobian.num_rows;
423   const int num_cols = jacobian.num_cols;
424   const int num_nonzeros = jacobian.values.size();
425
426   vector<SuiteSparse_long> transpose_rows(num_cols + 1, 0);
427   vector<SuiteSparse_long> transpose_cols(num_nonzeros, 0);
428   vector<double> transpose_values(num_nonzeros, 0);
429
430   for (int idx = 0; idx < num_nonzeros; ++idx) {
431     transpose_rows[jacobian.cols[idx] + 1] += 1;
432   }
433
434   for (int i = 1; i < transpose_rows.size(); ++i) {
435     transpose_rows[i] += transpose_rows[i - 1];
436   }
437
438   for (int r = 0; r < num_rows; ++r) {
439     for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
440       const int c = jacobian.cols[idx];
441       const int transpose_idx = transpose_rows[c];
442       transpose_cols[transpose_idx] = r;
443       transpose_values[transpose_idx] = jacobian.values[idx];
444       ++transpose_rows[c];
445     }
446   }
447
448   for (int i = transpose_rows.size() - 1; i > 0 ; --i) {
449     transpose_rows[i] = transpose_rows[i - 1];
450   }
451   transpose_rows[0] = 0;
452
453   cholmod_sparse cholmod_jacobian;
454   cholmod_jacobian.nrow = num_rows;
455   cholmod_jacobian.ncol = num_cols;
456   cholmod_jacobian.nzmax = num_nonzeros;
457   cholmod_jacobian.nz = NULL;
458   cholmod_jacobian.p = reinterpret_cast<void*>(&transpose_rows[0]);
459   cholmod_jacobian.i = reinterpret_cast<void*>(&transpose_cols[0]);
460   cholmod_jacobian.x = reinterpret_cast<void*>(&transpose_values[0]);
461   cholmod_jacobian.z = NULL;
462   cholmod_jacobian.stype = 0;  // Matrix is not symmetric.
463   cholmod_jacobian.itype = CHOLMOD_LONG;
464   cholmod_jacobian.xtype = CHOLMOD_REAL;
465   cholmod_jacobian.dtype = CHOLMOD_DOUBLE;
466   cholmod_jacobian.sorted = 1;
467   cholmod_jacobian.packed = 1;
468
469   cholmod_common cc;
470   cholmod_l_start(&cc);
471
472   cholmod_sparse* R = NULL;
473   SuiteSparse_long* permutation = NULL;
474
475   // Compute a Q-less QR factorization of the Jacobian. Since we are
476   // only interested in inverting J'J = R'R, we do not need Q. This
477   // saves memory and gives us R as a permuted compressed column
478   // sparse matrix.
479   //
480   // TODO(sameeragarwal): Currently the symbolic factorization and the
481   // numeric factorization is done at the same time, and this does not
482   // explicitly account for the block column and row structure in the
483   // matrix. When using AMD, we have observed in the past that
484   // computing the ordering with the block matrix is significantly
485   // more efficient, both in runtime as well as the quality of
486   // ordering computed. So, it maybe worth doing that analysis
487   // separately.
488   const SuiteSparse_long rank =
489       SuiteSparseQR<double>(SPQR_ORDERING_BESTAMD,
490                             SPQR_DEFAULT_TOL,
491                             cholmod_jacobian.ncol,
492                             &cholmod_jacobian,
493                             &R,
494                             &permutation,
495                             &cc);
496   event_logger.AddEvent("Numeric Factorization");
497   CHECK_NOTNULL(permutation);
498   CHECK_NOTNULL(R);
499
500   if (rank < cholmod_jacobian.ncol) {
501     LOG(ERROR) << "Jacobian matrix is rank deficient. "
502                << "Number of columns: " << cholmod_jacobian.ncol
503                << " rank: " << rank;
504     free(permutation);
505     cholmod_l_free_sparse(&R, &cc);
506     cholmod_l_finish(&cc);
507     return false;
508   }
509
510   vector<int> inverse_permutation(num_cols);
511   for (SuiteSparse_long i = 0; i < num_cols; ++i) {
512     inverse_permutation[permutation[i]] = i;
513   }
514
515   const int* rows = covariance_matrix_->rows();
516   const int* cols = covariance_matrix_->cols();
517   double* values = covariance_matrix_->mutable_values();
518
519   // The following loop exploits the fact that the i^th column of A^{-1}
520   // is given by the solution to the linear system
521   //
522   //  A x = e_i
523   //
524   // where e_i is a vector with e(i) = 1 and all other entries zero.
525   //
526   // Since the covariance matrix is symmetric, the i^th row and column
527   // are equal.
528   const int num_threads = options_.num_threads;
529   scoped_array<double> workspace(new double[num_threads * num_cols]);
530
531 #pragma omp parallel for num_threads(num_threads) schedule(dynamic)
532   for (int r = 0; r < num_cols; ++r) {
533     const int row_begin = rows[r];
534     const int row_end = rows[r + 1];
535     if (row_end == row_begin) {
536       continue;
537     }
538
539 #  ifdef CERES_USE_OPENMP
540     int thread_id = omp_get_thread_num();
541 #  else
542     int thread_id = 0;
543 #  endif
544
545     double* solution = workspace.get() + thread_id * num_cols;
546     SolveRTRWithSparseRHS<SuiteSparse_long>(
547         num_cols,
548         static_cast<SuiteSparse_long*>(R->i),
549         static_cast<SuiteSparse_long*>(R->p),
550         static_cast<double*>(R->x),
551         inverse_permutation[r],
552         solution);
553     for (int idx = row_begin; idx < row_end; ++idx) {
554      const int c = cols[idx];
555      values[idx] = solution[inverse_permutation[c]];
556     }
557   }
558
559   free(permutation);
560   cholmod_l_free_sparse(&R, &cc);
561   cholmod_l_finish(&cc);
562   event_logger.AddEvent("Inversion");
563   return true;
564
565 #else  // CERES_NO_SUITESPARSE
566
567   return false;
568
569 #endif  // CERES_NO_SUITESPARSE
570 }
571
572 bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() {
573   EventLogger event_logger(
574       "CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD");
575   if (covariance_matrix_.get() == NULL) {
576     // Nothing to do, all zeros covariance matrix.
577     return true;
578   }
579
580   CRSMatrix jacobian;
581   problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
582   event_logger.AddEvent("Evaluate");
583
584   Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols);
585   dense_jacobian.setZero();
586   for (int r = 0; r < jacobian.num_rows; ++r) {
587     for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
588       const int c = jacobian.cols[idx];
589       dense_jacobian(r, c) = jacobian.values[idx];
590     }
591   }
592   event_logger.AddEvent("ConvertToDenseMatrix");
593
594   Eigen::JacobiSVD<Matrix> svd(dense_jacobian,
595                                Eigen::ComputeThinU | Eigen::ComputeThinV);
596
597   event_logger.AddEvent("SingularValueDecomposition");
598
599   const Vector singular_values = svd.singularValues();
600   const int num_singular_values = singular_values.rows();
601   Vector inverse_squared_singular_values(num_singular_values);
602   inverse_squared_singular_values.setZero();
603
604   const double max_singular_value = singular_values[0];
605   const double min_singular_value_ratio =
606       sqrt(options_.min_reciprocal_condition_number);
607
608   const bool automatic_truncation = (options_.null_space_rank < 0);
609   const int max_rank = std::min(num_singular_values,
610                                 num_singular_values - options_.null_space_rank);
611
612   // Compute the squared inverse of the singular values. Truncate the
613   // computation based on min_singular_value_ratio and
614   // null_space_rank. When either of these two quantities are active,
615   // the resulting covariance matrix is a Moore-Penrose inverse
616   // instead of a regular inverse.
617   for (int i = 0; i < max_rank; ++i) {
618     const double singular_value_ratio = singular_values[i] / max_singular_value;
619     if (singular_value_ratio < min_singular_value_ratio) {
620       // Since the singular values are in decreasing order, if
621       // automatic truncation is enabled, then from this point on
622       // all values will fail the ratio test and there is nothing to
623       // do in this loop.
624       if (automatic_truncation) {
625         break;
626       } else {
627         LOG(ERROR) << "Cholesky factorization of J'J is not reliable. "
628                    << "Reciprocal condition number: "
629                    << singular_value_ratio * singular_value_ratio << " "
630                    << "min_reciprocal_condition_number: "
631                    << options_.min_reciprocal_condition_number;
632         return false;
633       }
634     }
635
636     inverse_squared_singular_values[i] =
637         1.0 / (singular_values[i] * singular_values[i]);
638   }
639
640   Matrix dense_covariance =
641       svd.matrixV() *
642       inverse_squared_singular_values.asDiagonal() *
643       svd.matrixV().transpose();
644   event_logger.AddEvent("PseudoInverse");
645
646   const int num_rows = covariance_matrix_->num_rows();
647   const int* rows = covariance_matrix_->rows();
648   const int* cols = covariance_matrix_->cols();
649   double* values = covariance_matrix_->mutable_values();
650
651   for (int r = 0; r < num_rows; ++r) {
652     for (int idx = rows[r]; idx < rows[r + 1]; ++idx) {
653       const int c = cols[idx];
654       values[idx] = dense_covariance(r, c);
655     }
656   }
657   event_logger.AddEvent("CopyToCovarianceMatrix");
658   return true;
659 }
660
661 bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() {
662   EventLogger event_logger(
663       "CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR");
664   if (covariance_matrix_.get() == NULL) {
665     // Nothing to do, all zeros covariance matrix.
666     return true;
667   }
668
669   CRSMatrix jacobian;
670   problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
671   event_logger.AddEvent("Evaluate");
672
673   typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;
674
675   // Convert the matrix to column major order as required by SparseQR.
676   EigenSparseMatrix sparse_jacobian =
677       Eigen::MappedSparseMatrix<double, Eigen::RowMajor>(
678           jacobian.num_rows, jacobian.num_cols,
679           static_cast<int>(jacobian.values.size()),
680           jacobian.rows.data(), jacobian.cols.data(), jacobian.values.data());
681   event_logger.AddEvent("ConvertToSparseMatrix");
682
683   Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int> >
684       qr_solver(sparse_jacobian);
685   event_logger.AddEvent("QRDecomposition");
686
687   if (qr_solver.info() != Eigen::Success) {
688     LOG(ERROR) << "Eigen::SparseQR decomposition failed.";
689     return false;
690   }
691
692   if (qr_solver.rank() < jacobian.num_cols) {
693     LOG(ERROR) << "Jacobian matrix is rank deficient. "
694                << "Number of columns: " << jacobian.num_cols
695                << " rank: " << qr_solver.rank();
696     return false;
697   }
698
699   const int* rows = covariance_matrix_->rows();
700   const int* cols = covariance_matrix_->cols();
701   double* values = covariance_matrix_->mutable_values();
702
703   // Compute the inverse column permutation used by QR factorization.
704   Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> inverse_permutation =
705       qr_solver.colsPermutation().inverse();
706
707   // The following loop exploits the fact that the i^th column of A^{-1}
708   // is given by the solution to the linear system
709   //
710   //  A x = e_i
711   //
712   // where e_i is a vector with e(i) = 1 and all other entries zero.
713   //
714   // Since the covariance matrix is symmetric, the i^th row and column
715   // are equal.
716   const int num_cols = jacobian.num_cols;
717   const int num_threads = options_.num_threads;
718   scoped_array<double> workspace(new double[num_threads * num_cols]);
719
720 #pragma omp parallel for num_threads(num_threads) schedule(dynamic)
721   for (int r = 0; r < num_cols; ++r) {
722     const int row_begin = rows[r];
723     const int row_end = rows[r + 1];
724     if (row_end == row_begin) {
725       continue;
726     }
727
728 #  ifdef CERES_USE_OPENMP
729     int thread_id = omp_get_thread_num();
730 #  else
731     int thread_id = 0;
732 #  endif
733
734     double* solution = workspace.get() + thread_id * num_cols;
735     SolveRTRWithSparseRHS<int>(
736         num_cols,
737         qr_solver.matrixR().innerIndexPtr(),
738         qr_solver.matrixR().outerIndexPtr(),
739         &qr_solver.matrixR().data().value(0),
740         inverse_permutation.indices().coeff(r),
741         solution);
742
743     // Assign the values of the computed covariance using the
744     // inverse permutation used in the QR factorization.
745     for (int idx = row_begin; idx < row_end; ++idx) {
746      const int c = cols[idx];
747      values[idx] = solution[inverse_permutation.indices().coeff(c)];
748     }
749   }
750
751   event_logger.AddEvent("Inverse");
752
753   return true;
754 }
755
756 }  // namespace internal
757 }  // namespace ceres