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   if (options_.algorithm_type == DENSE_SVD) {
 542     return ComputeCovarianceValuesUsingDenseSVD();
 543   }
 544
 545   if (options_.algorithm_type == SPARSE_QR) {
 546     if (options_.sparse_linear_algebra_library_type == EIGEN_SPARSE) {
 547       return ComputeCovarianceValuesUsingEigenSparseQR();
 548     }
 549
 550     if (options_.sparse_linear_algebra_library_type == SUITE_SPARSE) {
 551 #if !defined(CERES_NO_SUITESPARSE)
 552       return ComputeCovarianceValuesUsingSuiteSparseQR();
 553 #else
 554       LOG(ERROR) << "SuiteSparse is required to use the SPARSE_QR algorithm "
 555                  << "with "
 556                  << "Covariance::Options::sparse_linear_algebra_library_type "
 557                  << "= SUITE_SPARSE.";
 558       return false;
 559 #endif
 560     }
 561
 562     LOG(ERROR) << "Unsupported "
 563                << "Covariance::Options::sparse_linear_algebra_library_type "
 564                << "= "
 565                << SparseLinearAlgebraLibraryTypeToString(
 566                       options_.sparse_linear_algebra_library_type);
 567     return false;
 568   }
 569
 570   LOG(ERROR) << "Unsupported Covariance::Options::algorithm_type = "
 571              << CovarianceAlgorithmTypeToString(options_.algorithm_type);
 572   return false;
 573 }
 574
 575 bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() {
 576   EventLogger event_logger(
 577       "CovarianceImpl::ComputeCovarianceValuesUsingSparseQR");
 578
 579 #ifndef CERES_NO_SUITESPARSE
 580   if (covariance_matrix_.get() == NULL) {
 581     // Nothing to do, all zeros covariance matrix.
 582     return true;
 583   }
 584
 585   CRSMatrix jacobian;
 586   problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
 587   event_logger.AddEvent("Evaluate");
 588
 589   // Construct a compressed column form of the Jacobian.
 590   const int num_rows = jacobian.num_rows;
 591   const int num_cols = jacobian.num_cols;
 592   const int num_nonzeros = jacobian.values.size();
 593
 594   vector<SuiteSparse_long> transpose_rows(num_cols + 1, 0);
 595   vector<SuiteSparse_long> transpose_cols(num_nonzeros, 0);
 596   vector<double> transpose_values(num_nonzeros, 0);
 597
 598   for (int idx = 0; idx < num_nonzeros; ++idx) {
 599     transpose_rows[jacobian.cols[idx] + 1] += 1;
 600   }
 601
 602   for (int i = 1; i < transpose_rows.size(); ++i) {
 603     transpose_rows[i] += transpose_rows[i - 1];
 604   }
 605
 606   for (int r = 0; r < num_rows; ++r) {
 607     for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
 608       const int c = jacobian.cols[idx];
 609       const int transpose_idx = transpose_rows[c];
 610       transpose_cols[transpose_idx] = r;
 611       transpose_values[transpose_idx] = jacobian.values[idx];
 612       ++transpose_rows[c];
 613     }
 614   }
 615
 616   for (int i = transpose_rows.size() - 1; i > 0 ; --i) {
 617     transpose_rows[i] = transpose_rows[i - 1];
 618   }
 619   transpose_rows[0] = 0;
 620
 621   cholmod_sparse cholmod_jacobian;
 622   cholmod_jacobian.nrow = num_rows;
 623   cholmod_jacobian.ncol = num_cols;
 624   cholmod_jacobian.nzmax = num_nonzeros;
 625   cholmod_jacobian.nz = NULL;
 626   cholmod_jacobian.p = reinterpret_cast<void*>(&transpose_rows[0]);
 627   cholmod_jacobian.i = reinterpret_cast<void*>(&transpose_cols[0]);
 628   cholmod_jacobian.x = reinterpret_cast<void*>(&transpose_values[0]);
 629   cholmod_jacobian.z = NULL;
 630   cholmod_jacobian.stype = 0;  // Matrix is not symmetric.
 631   cholmod_jacobian.itype = CHOLMOD_LONG;
 632   cholmod_jacobian.xtype = CHOLMOD_REAL;
 633   cholmod_jacobian.dtype = CHOLMOD_DOUBLE;
 634   cholmod_jacobian.sorted = 1;
 635   cholmod_jacobian.packed = 1;
 636
 637   cholmod_common cc;
 638   cholmod_l_start(&cc);
 639
 640   cholmod_sparse* R = NULL;
 641   SuiteSparse_long* permutation = NULL;
 642
 643   // Compute a Q-less QR factorization of the Jacobian. Since we are
 644   // only interested in inverting J'J = R'R, we do not need Q. This
 645   // saves memory and gives us R as a permuted compressed column
 646   // sparse matrix.
 647   //
 648   // TODO(sameeragarwal): Currently the symbolic factorization and the
 649   // numeric factorization is done at the same time, and this does not
 650   // explicitly account for the block column and row structure in the
 651   // matrix. When using AMD, we have observed in the past that
 652   // computing the ordering with the block matrix is significantly
 653   // more efficient, both in runtime as well as the quality of
 654   // ordering computed. So, it maybe worth doing that analysis
 655   // separately.
 656   const SuiteSparse_long rank =
 657       SuiteSparseQR<double>(SPQR_ORDERING_BESTAMD,
 658                             SPQR_DEFAULT_TOL,
 659                             cholmod_jacobian.ncol,
 660                             &cholmod_jacobian,
 661                             &R,
 662                             &permutation,
 663                             &cc);
 664   event_logger.AddEvent("Numeric Factorization");
 665   CHECK_NOTNULL(permutation);
 666   CHECK_NOTNULL(R);
 667
 668   if (rank < cholmod_jacobian.ncol) {
 669     LOG(ERROR) << "Jacobian matrix is rank deficient. "
 670                << "Number of columns: " << cholmod_jacobian.ncol
 671                << " rank: " << rank;
 672     free(permutation);
 673     cholmod_l_free_sparse(&R, &cc);
 674     cholmod_l_finish(&cc);
 675     return false;
 676   }
 677
 678   vector<int> inverse_permutation(num_cols);
 679   for (SuiteSparse_long i = 0; i < num_cols; ++i) {
 680     inverse_permutation[permutation[i]] = i;
 681   }
 682
 683   const int* rows = covariance_matrix_->rows();
 684   const int* cols = covariance_matrix_->cols();
 685   double* values = covariance_matrix_->mutable_values();
 686
 687   // The following loop exploits the fact that the i^th column of A^{-1}
 688   // is given by the solution to the linear system
 689   //
 690   //  A x = e_i
 691   //
 692   // where e_i is a vector with e(i) = 1 and all other entries zero.
 693   //
 694   // Since the covariance matrix is symmetric, the i^th row and column
 695   // are equal.
 696   const int num_threads = options_.num_threads;
 697   scoped_array<double> workspace(new double[num_threads * num_cols]);
 698
 699 #pragma omp parallel for num_threads(num_threads) schedule(dynamic)
 700   for (int r = 0; r < num_cols; ++r) {
 701     const int row_begin = rows[r];
 702     const int row_end = rows[r + 1];
 703     if (row_end == row_begin) {
 704       continue;
 705     }
 706
 707 #  ifdef CERES_USE_OPENMP
 708     int thread_id = omp_get_thread_num();
 709 #  else
 710     int thread_id = 0;
 711 #  endif
 712
 713     double* solution = workspace.get() + thread_id * num_cols;
 714     SolveRTRWithSparseRHS<SuiteSparse_long>(
 715         num_cols,
 716         static_cast<SuiteSparse_long*>(R->i),
 717         static_cast<SuiteSparse_long*>(R->p),
 718         static_cast<double*>(R->x),
 719         inverse_permutation[r],
 720         solution);
 721     for (int idx = row_begin; idx < row_end; ++idx) {
 722      const int c = cols[idx];
 723      values[idx] = solution[inverse_permutation[c]];
 724     }
 725   }
 726
 727   free(permutation);
 728   cholmod_l_free_sparse(&R, &cc);
 729   cholmod_l_finish(&cc);
 730   event_logger.AddEvent("Inversion");
 731   return true;
 732
 733 #else  // CERES_NO_SUITESPARSE
 734
 735   return false;
 736
 737 #endif  // CERES_NO_SUITESPARSE
 738 }
 739
 740 bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() {
 741   EventLogger event_logger(
 742       "CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD");
 743   if (covariance_matrix_.get() == NULL) {
 744     // Nothing to do, all zeros covariance matrix.
 745     return true;
 746   }
 747
 748   CRSMatrix jacobian;
 749   problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
 750   event_logger.AddEvent("Evaluate");
 751
 752   Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols);
 753   dense_jacobian.setZero();
 754   for (int r = 0; r < jacobian.num_rows; ++r) {
 755     for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
 756       const int c = jacobian.cols[idx];
 757       dense_jacobian(r, c) = jacobian.values[idx];
 758     }
 759   }
 760   event_logger.AddEvent("ConvertToDenseMatrix");
 761
 762   Eigen::JacobiSVD<Matrix> svd(dense_jacobian,
 763                                Eigen::ComputeThinU | Eigen::ComputeThinV);
 764
 765   event_logger.AddEvent("SingularValueDecomposition");
 766
 767   const Vector singular_values = svd.singularValues();
 768   const int num_singular_values = singular_values.rows();
 769   Vector inverse_squared_singular_values(num_singular_values);
 770   inverse_squared_singular_values.setZero();
 771
 772   const double max_singular_value = singular_values[0];
 773   const double min_singular_value_ratio =
 774       sqrt(options_.min_reciprocal_condition_number);
 775
 776   const bool automatic_truncation = (options_.null_space_rank < 0);
 777   const int max_rank = std::min(num_singular_values,
 778                                 num_singular_values - options_.null_space_rank);
 779
 780   // Compute the squared inverse of the singular values. Truncate the
 781   // computation based on min_singular_value_ratio and
 782   // null_space_rank. When either of these two quantities are active,
 783   // the resulting covariance matrix is a Moore-Penrose inverse
 784   // instead of a regular inverse.
 785   for (int i = 0; i < max_rank; ++i) {
 786     const double singular_value_ratio = singular_values[i] / max_singular_value;
 787     if (singular_value_ratio < min_singular_value_ratio) {
 788       // Since the singular values are in decreasing order, if
 789       // automatic truncation is enabled, then from this point on
 790       // all values will fail the ratio test and there is nothing to
 791       // do in this loop.
 792       if (automatic_truncation) {
 793         break;
 794       } else {
 795         LOG(ERROR) << "Error: Covariance matrix is near rank deficient "
 796                    << "and the user did not specify a non-zero"
 797                    << "Covariance::Options::null_space_rank "
 798                    << "to enable the computation of a Pseudo-Inverse. "
 799                    << "Reciprocal condition number: "
 800                    << singular_value_ratio * singular_value_ratio << " "
 801                    << "min_reciprocal_condition_number: "
 802                    << options_.min_reciprocal_condition_number;
 803         return false;
 804       }
 805     }
 806
 807     inverse_squared_singular_values[i] =
 808         1.0 / (singular_values[i] * singular_values[i]);
 809   }
 810
 811   Matrix dense_covariance =
 812       svd.matrixV() *
 813       inverse_squared_singular_values.asDiagonal() *
 814       svd.matrixV().transpose();
 815   event_logger.AddEvent("PseudoInverse");
 816
 817   const int num_rows = covariance_matrix_->num_rows();
 818   const int* rows = covariance_matrix_->rows();
 819   const int* cols = covariance_matrix_->cols();
 820   double* values = covariance_matrix_->mutable_values();
 821
 822   for (int r = 0; r < num_rows; ++r) {
 823     for (int idx = rows[r]; idx < rows[r + 1]; ++idx) {
 824       const int c = cols[idx];
 825       values[idx] = dense_covariance(r, c);
 826     }
 827   }
 828   event_logger.AddEvent("CopyToCovarianceMatrix");
 829   return true;
 830 }
 831
 832 bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() {
 833   EventLogger event_logger(
 834       "CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR");
 835   if (covariance_matrix_.get() == NULL) {
 836     // Nothing to do, all zeros covariance matrix.
 837     return true;
 838   }
 839
 840   CRSMatrix jacobian;
 841   problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
 842   event_logger.AddEvent("Evaluate");
 843
 844   typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;
 845
 846   // Convert the matrix to column major order as required by SparseQR.
 847   EigenSparseMatrix sparse_jacobian =
 848       Eigen::MappedSparseMatrix<double, Eigen::RowMajor>(
 849           jacobian.num_rows, jacobian.num_cols,
 850           static_cast<int>(jacobian.values.size()),
 851           jacobian.rows.data(), jacobian.cols.data(), jacobian.values.data());
 852   event_logger.AddEvent("ConvertToSparseMatrix");
 853
 854   Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int> >
 855       qr_solver(sparse_jacobian);
 856   event_logger.AddEvent("QRDecomposition");
 857
 858   if (qr_solver.info() != Eigen::Success) {
 859     LOG(ERROR) << "Eigen::SparseQR decomposition failed.";
 860     return false;
 861   }
 862
 863   if (qr_solver.rank() < jacobian.num_cols) {
 864     LOG(ERROR) << "Jacobian matrix is rank deficient. "
 865                << "Number of columns: " << jacobian.num_cols
 866                << " rank: " << qr_solver.rank();
 867     return false;
 868   }
 869
 870   const int* rows = covariance_matrix_->rows();
 871   const int* cols = covariance_matrix_->cols();
 872   double* values = covariance_matrix_->mutable_values();
 873
 874   // Compute the inverse column permutation used by QR factorization.
 875   Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> inverse_permutation =
 876       qr_solver.colsPermutation().inverse();
 877
 878   // The following loop exploits the fact that the i^th column of A^{-1}
 879   // is given by the solution to the linear system
 880   //
 881   //  A x = e_i
 882   //
 883   // where e_i is a vector with e(i) = 1 and all other entries zero.
 884   //
 885   // Since the covariance matrix is symmetric, the i^th row and column
 886   // are equal.
 887   const int num_cols = jacobian.num_cols;
 888   const int num_threads = options_.num_threads;
 889   scoped_array<double> workspace(new double[num_threads * num_cols]);
 890
 891 #pragma omp parallel for num_threads(num_threads) schedule(dynamic)
 892   for (int r = 0; r < num_cols; ++r) {
 893     const int row_begin = rows[r];
 894     const int row_end = rows[r + 1];
 895     if (row_end == row_begin) {
 896       continue;
 897     }
 898
 899 #  ifdef CERES_USE_OPENMP
 900     int thread_id = omp_get_thread_num();
 901 #  else
 902     int thread_id = 0;
 903 #  endif
 904
 905     double* solution = workspace.get() + thread_id * num_cols;
 906     SolveRTRWithSparseRHS<int>(
 907         num_cols,
 908         qr_solver.matrixR().innerIndexPtr(),
 909         qr_solver.matrixR().outerIndexPtr(),
 910         &qr_solver.matrixR().data().value(0),
 911         inverse_permutation.indices().coeff(r),
 912         solution);
 913
 914     // Assign the values of the computed covariance using the
 915     // inverse permutation used in the QR factorization.
 916     for (int idx = row_begin; idx < row_end; ++idx) {
 917      const int c = cols[idx];
 918      values[idx] = solution[inverse_permutation.indices().coeff(c)];
 919     }
 920   }
 921
 922   event_logger.AddEvent("Inverse");
 923
 924   return true;
 925 }
 926
 927 }  // namespace internal
 928 }  // namespace ceres