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