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 #include <algorithm>
34 #include <cstdlib>
35 #include <memory>
36 #include <numeric>
37 #include <sstream>
38 #include <unordered_set>
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/parallel_for.h"
53 #include "ceres/parallel_utils.h"
54 #include "ceres/parameter_block.h"
55 #include "ceres/problem_impl.h"
56 #include "ceres/residual_block.h"
57 #include "ceres/suitesparse.h"
58 #include "ceres/wall_time.h"
59 #include "glog/logging.h"
60
61 namespace ceres {
62 namespace internal {
63
64 using std::make_pair;
65 using std::map;
66 using std::pair;
67 using std::sort;
68 using std::swap;
69 using std::vector;
70
71 typedef vector<pair<const double*, const double*>> CovarianceBlocks;
72
CovarianceImpl(const Covariance::Options & options)73 CovarianceImpl::CovarianceImpl(const Covariance::Options& options)
74 : options_(options),
75 is_computed_(false),
76 is_valid_(false) {
77 #ifdef CERES_NO_THREADS
78 if (options_.num_threads > 1) {
79 LOG(WARNING)
80 << "No threading support is compiled into this binary; "
81 << "only options.num_threads = 1 is supported. Switching "
82 << "to single threaded mode.";
83 options_.num_threads = 1;
84 }
85 #endif
86
87 evaluate_options_.num_threads = options_.num_threads;
88 evaluate_options_.apply_loss_function = options_.apply_loss_function;
89 }
90
~CovarianceImpl()91 CovarianceImpl::~CovarianceImpl() {
92 }
93
CheckForDuplicates(vector<T> blocks)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
Compute(const CovarianceBlocks & covariance_blocks,ProblemImpl * problem)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
Compute(const vector<const double * > & parameter_blocks,ProblemImpl * problem)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
GetCovarianceBlockInTangentOrAmbientSpace(const double * original_parameter_block1,const double * original_parameter_block2,bool lift_covariance_to_ambient_space,double * covariance_block) const149 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
GetCovarianceMatrixInTangentOrAmbientSpace(const vector<const double * > & parameters,bool lift_covariance_to_ambient_space,double * covariance_matrix) const300 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 std::unique_ptr<double[]> workspace(
337 new double[num_threads * max_covariance_block_size *
338 max_covariance_block_size]);
339
340 bool success = true;
341
342 // Technically the following code is a double nested loop where
343 // i = 1:n, j = i:n.
344 int iteration_count = (num_parameters * (num_parameters + 1)) / 2;
345 problem_->context()->EnsureMinimumThreads(num_threads);
346 ParallelFor(
347 problem_->context(),
348 0,
349 iteration_count,
350 num_threads,
351 [&](int thread_id, int k) {
352 int i, j;
353 LinearIndexToUpperTriangularIndex(k, num_parameters, &i, &j);
354
355 int covariance_row_idx = cum_parameter_size[i];
356 int covariance_col_idx = cum_parameter_size[j];
357 int size_i = parameter_sizes[i];
358 int size_j = parameter_sizes[j];
359 double* covariance_block =
360 workspace.get() + thread_id * max_covariance_block_size *
361 max_covariance_block_size;
362 if (!GetCovarianceBlockInTangentOrAmbientSpace(
363 parameters[i], parameters[j],
364 lift_covariance_to_ambient_space, covariance_block)) {
365 success = false;
366 }
367
368 covariance.block(covariance_row_idx, covariance_col_idx, size_i,
369 size_j) = MatrixRef(covariance_block, size_i, size_j);
370
371 if (i != j) {
372 covariance.block(covariance_col_idx, covariance_row_idx,
373 size_j, size_i) =
374 MatrixRef(covariance_block, size_i, size_j).transpose();
375 }
376 });
377 return success;
378 }
379
380 // Determine the sparsity pattern of the covariance matrix based on
381 // the block pairs requested by the user.
ComputeCovarianceSparsity(const CovarianceBlocks & original_covariance_blocks,ProblemImpl * problem)382 bool CovarianceImpl::ComputeCovarianceSparsity(
383 const CovarianceBlocks& original_covariance_blocks,
384 ProblemImpl* problem) {
385 EventLogger event_logger("CovarianceImpl::ComputeCovarianceSparsity");
386
387 // Determine an ordering for the parameter block, by sorting the
388 // parameter blocks by their pointers.
389 vector<double*> all_parameter_blocks;
390 problem->GetParameterBlocks(&all_parameter_blocks);
391 const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map();
392 std::unordered_set<ParameterBlock*> parameter_blocks_in_use;
393 vector<ResidualBlock*> residual_blocks;
394 problem->GetResidualBlocks(&residual_blocks);
395
396 for (int i = 0; i < residual_blocks.size(); ++i) {
397 ResidualBlock* residual_block = residual_blocks[i];
398 parameter_blocks_in_use.insert(residual_block->parameter_blocks(),
399 residual_block->parameter_blocks() +
400 residual_block->NumParameterBlocks());
401 }
402
403 constant_parameter_blocks_.clear();
404 vector<double*>& active_parameter_blocks =
405 evaluate_options_.parameter_blocks;
406 active_parameter_blocks.clear();
407 for (int i = 0; i < all_parameter_blocks.size(); ++i) {
408 double* parameter_block = all_parameter_blocks[i];
409 ParameterBlock* block = FindOrDie(parameter_map, parameter_block);
410 if (!block->IsConstant() && (parameter_blocks_in_use.count(block) > 0)) {
411 active_parameter_blocks.push_back(parameter_block);
412 } else {
413 constant_parameter_blocks_.insert(parameter_block);
414 }
415 }
416
417 std::sort(active_parameter_blocks.begin(), active_parameter_blocks.end());
418
419 // Compute the number of rows. Map each parameter block to the
420 // first row corresponding to it in the covariance matrix using the
421 // ordering of parameter blocks just constructed.
422 int num_rows = 0;
423 parameter_block_to_row_index_.clear();
424 for (int i = 0; i < active_parameter_blocks.size(); ++i) {
425 double* parameter_block = active_parameter_blocks[i];
426 const int parameter_block_size =
427 problem->ParameterBlockLocalSize(parameter_block);
428 parameter_block_to_row_index_[parameter_block] = num_rows;
429 num_rows += parameter_block_size;
430 }
431
432 // Compute the number of non-zeros in the covariance matrix. Along
433 // the way flip any covariance blocks which are in the lower
434 // triangular part of the matrix.
435 int num_nonzeros = 0;
436 CovarianceBlocks covariance_blocks;
437 for (int i = 0; i < original_covariance_blocks.size(); ++i) {
438 const pair<const double*, const double*>& block_pair =
439 original_covariance_blocks[i];
440 if (constant_parameter_blocks_.count(block_pair.first) > 0 ||
441 constant_parameter_blocks_.count(block_pair.second) > 0) {
442 continue;
443 }
444
445 int index1 = FindOrDie(parameter_block_to_row_index_, block_pair.first);
446 int index2 = FindOrDie(parameter_block_to_row_index_, block_pair.second);
447 const int size1 = problem->ParameterBlockLocalSize(block_pair.first);
448 const int size2 = problem->ParameterBlockLocalSize(block_pair.second);
449 num_nonzeros += size1 * size2;
450
451 // Make sure we are constructing a block upper triangular matrix.
452 if (index1 > index2) {
453 covariance_blocks.push_back(make_pair(block_pair.second,
454 block_pair.first));
455 } else {
456 covariance_blocks.push_back(block_pair);
457 }
458 }
459
460 if (covariance_blocks.size() == 0) {
461 VLOG(2) << "No non-zero covariance blocks found";
462 covariance_matrix_.reset(NULL);
463 return true;
464 }
465
466 // Sort the block pairs. As a consequence we get the covariance
467 // blocks as they will occur in the CompressedRowSparseMatrix that
468 // will store the covariance.
469 sort(covariance_blocks.begin(), covariance_blocks.end());
470
471 // Fill the sparsity pattern of the covariance matrix.
472 covariance_matrix_.reset(
473 new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros));
474
475 int* rows = covariance_matrix_->mutable_rows();
476 int* cols = covariance_matrix_->mutable_cols();
477
478 // Iterate over parameter blocks and in turn over the rows of the
479 // covariance matrix. For each parameter block, look in the upper
480 // triangular part of the covariance matrix to see if there are any
481 // blocks requested by the user. If this is the case then fill out a
482 // set of compressed rows corresponding to this parameter block.
483 //
484 // The key thing that makes this loop work is the fact that the
485 // row/columns of the covariance matrix are ordered by the pointer
486 // values of the parameter blocks. Thus iterating over the keys of
487 // parameter_block_to_row_index_ corresponds to iterating over the
488 // rows of the covariance matrix in order.
489 int i = 0; // index into covariance_blocks.
490 int cursor = 0; // index into the covariance matrix.
491 for (const auto& entry : parameter_block_to_row_index_) {
492 const double* row_block = entry.first;
493 const int row_block_size = problem->ParameterBlockLocalSize(row_block);
494 int row_begin = entry.second;
495
496 // Iterate over the covariance blocks contained in this row block
497 // and count the number of columns in this row block.
498 int num_col_blocks = 0;
499 int num_columns = 0;
500 for (int j = i; j < covariance_blocks.size(); ++j, ++num_col_blocks) {
501 const pair<const double*, const double*>& block_pair =
502 covariance_blocks[j];
503 if (block_pair.first != row_block) {
504 break;
505 }
506 num_columns += problem->ParameterBlockLocalSize(block_pair.second);
507 }
508
509 // Fill out all the compressed rows for this parameter block.
510 for (int r = 0; r < row_block_size; ++r) {
511 rows[row_begin + r] = cursor;
512 for (int c = 0; c < num_col_blocks; ++c) {
513 const double* col_block = covariance_blocks[i + c].second;
514 const int col_block_size = problem->ParameterBlockLocalSize(col_block);
515 int col_begin = FindOrDie(parameter_block_to_row_index_, col_block);
516 for (int k = 0; k < col_block_size; ++k) {
517 cols[cursor++] = col_begin++;
518 }
519 }
520 }
521
522 i+= num_col_blocks;
523 }
524
525 rows[num_rows] = cursor;
526 return true;
527 }
528
ComputeCovarianceValues()529 bool CovarianceImpl::ComputeCovarianceValues() {
530 if (options_.algorithm_type == DENSE_SVD) {
531 return ComputeCovarianceValuesUsingDenseSVD();
532 }
533
534 if (options_.algorithm_type == SPARSE_QR) {
535 if (options_.sparse_linear_algebra_library_type == EIGEN_SPARSE) {
536 return ComputeCovarianceValuesUsingEigenSparseQR();
537 }
538
539 if (options_.sparse_linear_algebra_library_type == SUITE_SPARSE) {
540 #if !defined(CERES_NO_SUITESPARSE)
541 return ComputeCovarianceValuesUsingSuiteSparseQR();
542 #else
543 LOG(ERROR) << "SuiteSparse is required to use the SPARSE_QR algorithm "
544 << "with "
545 << "Covariance::Options::sparse_linear_algebra_library_type "
546 << "= SUITE_SPARSE.";
547 return false;
548 #endif
549 }
550
551 LOG(ERROR) << "Unsupported "
552 << "Covariance::Options::sparse_linear_algebra_library_type "
553 << "= "
554 << SparseLinearAlgebraLibraryTypeToString(
555 options_.sparse_linear_algebra_library_type);
556 return false;
557 }
558
559 LOG(ERROR) << "Unsupported Covariance::Options::algorithm_type = "
560 << CovarianceAlgorithmTypeToString(options_.algorithm_type);
561 return false;
562 }
563
ComputeCovarianceValuesUsingSuiteSparseQR()564 bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() {
565 EventLogger event_logger(
566 "CovarianceImpl::ComputeCovarianceValuesUsingSparseQR");
567
568 #ifndef CERES_NO_SUITESPARSE
569 if (covariance_matrix_.get() == NULL) {
570 // Nothing to do, all zeros covariance matrix.
571 return true;
572 }
573
574 CRSMatrix jacobian;
575 problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
576 event_logger.AddEvent("Evaluate");
577
578 // Construct a compressed column form of the Jacobian.
579 const int num_rows = jacobian.num_rows;
580 const int num_cols = jacobian.num_cols;
581 const int num_nonzeros = jacobian.values.size();
582
583 vector<SuiteSparse_long> transpose_rows(num_cols + 1, 0);
584 vector<SuiteSparse_long> transpose_cols(num_nonzeros, 0);
585 vector<double> transpose_values(num_nonzeros, 0);
586
587 for (int idx = 0; idx < num_nonzeros; ++idx) {
588 transpose_rows[jacobian.cols[idx] + 1] += 1;
589 }
590
591 for (int i = 1; i < transpose_rows.size(); ++i) {
592 transpose_rows[i] += transpose_rows[i - 1];
593 }
594
595 for (int r = 0; r < num_rows; ++r) {
596 for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
597 const int c = jacobian.cols[idx];
598 const int transpose_idx = transpose_rows[c];
599 transpose_cols[transpose_idx] = r;
600 transpose_values[transpose_idx] = jacobian.values[idx];
601 ++transpose_rows[c];
602 }
603 }
604
605 for (int i = transpose_rows.size() - 1; i > 0 ; --i) {
606 transpose_rows[i] = transpose_rows[i - 1];
607 }
608 transpose_rows[0] = 0;
609
610 cholmod_sparse cholmod_jacobian;
611 cholmod_jacobian.nrow = num_rows;
612 cholmod_jacobian.ncol = num_cols;
613 cholmod_jacobian.nzmax = num_nonzeros;
614 cholmod_jacobian.nz = NULL;
615 cholmod_jacobian.p = reinterpret_cast<void*>(&transpose_rows[0]);
616 cholmod_jacobian.i = reinterpret_cast<void*>(&transpose_cols[0]);
617 cholmod_jacobian.x = reinterpret_cast<void*>(&transpose_values[0]);
618 cholmod_jacobian.z = NULL;
619 cholmod_jacobian.stype = 0; // Matrix is not symmetric.
620 cholmod_jacobian.itype = CHOLMOD_LONG;
621 cholmod_jacobian.xtype = CHOLMOD_REAL;
622 cholmod_jacobian.dtype = CHOLMOD_DOUBLE;
623 cholmod_jacobian.sorted = 1;
624 cholmod_jacobian.packed = 1;
625
626 cholmod_common cc;
627 cholmod_l_start(&cc);
628
629 cholmod_sparse* R = NULL;
630 SuiteSparse_long* permutation = NULL;
631
632 // Compute a Q-less QR factorization of the Jacobian. Since we are
633 // only interested in inverting J'J = R'R, we do not need Q. This
634 // saves memory and gives us R as a permuted compressed column
635 // sparse matrix.
636 //
637 // TODO(sameeragarwal): Currently the symbolic factorization and the
638 // numeric factorization is done at the same time, and this does not
639 // explicitly account for the block column and row structure in the
640 // matrix. When using AMD, we have observed in the past that
641 // computing the ordering with the block matrix is significantly
642 // more efficient, both in runtime as well as the quality of
643 // ordering computed. So, it maybe worth doing that analysis
644 // separately.
645 const SuiteSparse_long rank =
646 SuiteSparseQR<double>(SPQR_ORDERING_BESTAMD,
647 SPQR_DEFAULT_TOL,
648 cholmod_jacobian.ncol,
649 &cholmod_jacobian,
650 &R,
651 &permutation,
652 &cc);
653 event_logger.AddEvent("Numeric Factorization");
654 if (R == nullptr) {
655 LOG(ERROR) << "Something is wrong. SuiteSparseQR returned R = nullptr.";
656 free(permutation);
657 cholmod_l_finish(&cc);
658 return false;
659 }
660
661 if (rank < cholmod_jacobian.ncol) {
662 LOG(ERROR) << "Jacobian matrix is rank deficient. "
663 << "Number of columns: " << cholmod_jacobian.ncol
664 << " rank: " << rank;
665 free(permutation);
666 cholmod_l_free_sparse(&R, &cc);
667 cholmod_l_finish(&cc);
668 return false;
669 }
670
671 vector<int> inverse_permutation(num_cols);
672 if (permutation) {
673 for (SuiteSparse_long i = 0; i < num_cols; ++i) {
674 inverse_permutation[permutation[i]] = i;
675 }
676 } else {
677 for (SuiteSparse_long i = 0; i < num_cols; ++i) {
678 inverse_permutation[i] = i;
679 }
680 }
681
682 const int* rows = covariance_matrix_->rows();
683 const int* cols = covariance_matrix_->cols();
684 double* values = covariance_matrix_->mutable_values();
685
686 // The following loop exploits the fact that the i^th column of A^{-1}
687 // is given by the solution to the linear system
688 //
689 // A x = e_i
690 //
691 // where e_i is a vector with e(i) = 1 and all other entries zero.
692 //
693 // Since the covariance matrix is symmetric, the i^th row and column
694 // are equal.
695 const int num_threads = options_.num_threads;
696 std::unique_ptr<double[]> workspace(new double[num_threads * num_cols]);
697
698 problem_->context()->EnsureMinimumThreads(num_threads);
699 ParallelFor(
700 problem_->context(),
701 0,
702 num_cols,
703 num_threads,
704 [&](int thread_id, int r) {
705 const int row_begin = rows[r];
706 const int row_end = rows[r + 1];
707 if (row_end != row_begin) {
708 double* solution = workspace.get() + thread_id * num_cols;
709 SolveRTRWithSparseRHS<SuiteSparse_long>(
710 num_cols, static_cast<SuiteSparse_long*>(R->i),
711 static_cast<SuiteSparse_long*>(R->p), static_cast<double*>(R->x),
712 inverse_permutation[r], solution);
713 for (int idx = row_begin; idx < row_end; ++idx) {
714 const int c = cols[idx];
715 values[idx] = solution[inverse_permutation[c]];
716 }
717 }
718 });
719
720 free(permutation);
721 cholmod_l_free_sparse(&R, &cc);
722 cholmod_l_finish(&cc);
723 event_logger.AddEvent("Inversion");
724 return true;
725
726 #else // CERES_NO_SUITESPARSE
727
728 return false;
729
730 #endif // CERES_NO_SUITESPARSE
731 }
732
ComputeCovarianceValuesUsingDenseSVD()733 bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() {
734 EventLogger event_logger(
735 "CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD");
736 if (covariance_matrix_.get() == NULL) {
737 // Nothing to do, all zeros covariance matrix.
738 return true;
739 }
740
741 CRSMatrix jacobian;
742 problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
743 event_logger.AddEvent("Evaluate");
744
745 Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols);
746 dense_jacobian.setZero();
747 for (int r = 0; r < jacobian.num_rows; ++r) {
748 for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
749 const int c = jacobian.cols[idx];
750 dense_jacobian(r, c) = jacobian.values[idx];
751 }
752 }
753 event_logger.AddEvent("ConvertToDenseMatrix");
754
755 Eigen::BDCSVD<Matrix> svd(dense_jacobian,
756 Eigen::ComputeThinU | Eigen::ComputeThinV);
757
758 event_logger.AddEvent("SingularValueDecomposition");
759
760 const Vector singular_values = svd.singularValues();
761 const int num_singular_values = singular_values.rows();
762 Vector inverse_squared_singular_values(num_singular_values);
763 inverse_squared_singular_values.setZero();
764
765 const double max_singular_value = singular_values[0];
766 const double min_singular_value_ratio =
767 sqrt(options_.min_reciprocal_condition_number);
768
769 const bool automatic_truncation = (options_.null_space_rank < 0);
770 const int max_rank = std::min(num_singular_values,
771 num_singular_values - options_.null_space_rank);
772
773 // Compute the squared inverse of the singular values. Truncate the
774 // computation based on min_singular_value_ratio and
775 // null_space_rank. When either of these two quantities are active,
776 // the resulting covariance matrix is a Moore-Penrose inverse
777 // instead of a regular inverse.
778 for (int i = 0; i < max_rank; ++i) {
779 const double singular_value_ratio = singular_values[i] / max_singular_value;
780 if (singular_value_ratio < min_singular_value_ratio) {
781 // Since the singular values are in decreasing order, if
782 // automatic truncation is enabled, then from this point on
783 // all values will fail the ratio test and there is nothing to
784 // do in this loop.
785 if (automatic_truncation) {
786 break;
787 } else {
788 LOG(ERROR) << "Error: Covariance matrix is near rank deficient "
789 << "and the user did not specify a non-zero"
790 << "Covariance::Options::null_space_rank "
791 << "to enable the computation of a Pseudo-Inverse. "
792 << "Reciprocal condition number: "
793 << singular_value_ratio * singular_value_ratio << " "
794 << "min_reciprocal_condition_number: "
795 << options_.min_reciprocal_condition_number;
796 return false;
797 }
798 }
799
800 inverse_squared_singular_values[i] =
801 1.0 / (singular_values[i] * singular_values[i]);
802 }
803
804 Matrix dense_covariance =
805 svd.matrixV() *
806 inverse_squared_singular_values.asDiagonal() *
807 svd.matrixV().transpose();
808 event_logger.AddEvent("PseudoInverse");
809
810 const int num_rows = covariance_matrix_->num_rows();
811 const int* rows = covariance_matrix_->rows();
812 const int* cols = covariance_matrix_->cols();
813 double* values = covariance_matrix_->mutable_values();
814
815 for (int r = 0; r < num_rows; ++r) {
816 for (int idx = rows[r]; idx < rows[r + 1]; ++idx) {
817 const int c = cols[idx];
818 values[idx] = dense_covariance(r, c);
819 }
820 }
821 event_logger.AddEvent("CopyToCovarianceMatrix");
822 return true;
823 }
824
ComputeCovarianceValuesUsingEigenSparseQR()825 bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() {
826 EventLogger event_logger(
827 "CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR");
828 if (covariance_matrix_.get() == NULL) {
829 // Nothing to do, all zeros covariance matrix.
830 return true;
831 }
832
833 CRSMatrix jacobian;
834 problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
835 event_logger.AddEvent("Evaluate");
836
837 typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;
838
839 // Convert the matrix to column major order as required by SparseQR.
840 EigenSparseMatrix sparse_jacobian =
841 Eigen::MappedSparseMatrix<double, Eigen::RowMajor>(
842 jacobian.num_rows, jacobian.num_cols,
843 static_cast<int>(jacobian.values.size()),
844 jacobian.rows.data(), jacobian.cols.data(), jacobian.values.data());
845 event_logger.AddEvent("ConvertToSparseMatrix");
846
847 Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int>>
848 qr_solver(sparse_jacobian);
849 event_logger.AddEvent("QRDecomposition");
850
851 if (qr_solver.info() != Eigen::Success) {
852 LOG(ERROR) << "Eigen::SparseQR decomposition failed.";
853 return false;
854 }
855
856 if (qr_solver.rank() < jacobian.num_cols) {
857 LOG(ERROR) << "Jacobian matrix is rank deficient. "
858 << "Number of columns: " << jacobian.num_cols
859 << " rank: " << qr_solver.rank();
860 return false;
861 }
862
863 const int* rows = covariance_matrix_->rows();
864 const int* cols = covariance_matrix_->cols();
865 double* values = covariance_matrix_->mutable_values();
866
867 // Compute the inverse column permutation used by QR factorization.
868 Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> inverse_permutation =
869 qr_solver.colsPermutation().inverse();
870
871 // The following loop exploits the fact that the i^th column of A^{-1}
872 // is given by the solution to the linear system
873 //
874 // A x = e_i
875 //
876 // where e_i is a vector with e(i) = 1 and all other entries zero.
877 //
878 // Since the covariance matrix is symmetric, the i^th row and column
879 // are equal.
880 const int num_cols = jacobian.num_cols;
881 const int num_threads = options_.num_threads;
882 std::unique_ptr<double[]> workspace(new double[num_threads * num_cols]);
883
884 problem_->context()->EnsureMinimumThreads(num_threads);
885 ParallelFor(
886 problem_->context(),
887 0,
888 num_cols,
889 num_threads,
890 [&](int thread_id, int r) {
891 const int row_begin = rows[r];
892 const int row_end = rows[r + 1];
893 if (row_end != row_begin) {
894 double* solution = workspace.get() + thread_id * num_cols;
895 SolveRTRWithSparseRHS<int>(
896 num_cols,
897 qr_solver.matrixR().innerIndexPtr(),
898 qr_solver.matrixR().outerIndexPtr(),
899 &qr_solver.matrixR().data().value(0),
900 inverse_permutation.indices().coeff(r),
901 solution);
902
903 // Assign the values of the computed covariance using the
904 // inverse permutation used in the QR factorization.
905 for (int idx = row_begin; idx < row_end; ++idx) {
906 const int c = cols[idx];
907 values[idx] = solution[inverse_permutation.indices().coeff(c)];
908 }
909 }
910 });
911
912 event_logger.AddEvent("Inverse");
913
914 return true;
915 }
916
917 } // namespace internal
918 } // namespace ceres
919