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