env/ceres-solver/internal/ceres/covariance_impl.cc

// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2022 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)

#include "ceres/covariance_impl.h"

#include <algorithm>
#include <cstdlib>
#include <memory>
#include <numeric>
#include <sstream>
#include <unordered_set>
#include <utility>
#include <vector>

#include "Eigen/SVD"
#include "Eigen/SparseCore"
#include "Eigen/SparseQR"
#include "ceres/compressed_col_sparse_matrix_utils.h"
#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/covariance.h"
#include "ceres/crs_matrix.h"
#include "ceres/internal/eigen.h"
#include "ceres/map_util.h"
#include "ceres/parallel_for.h"
#include "ceres/parallel_utils.h"
#include "ceres/parameter_block.h"
#include "ceres/problem_impl.h"
#include "ceres/residual_block.h"
#include "ceres/suitesparse.h"
#include "ceres/wall_time.h"
#include "glog/logging.h"

namespace ceres::internal {

using CovarianceBlocks = std::vector<std::pair<const double*, const double*>>;

CovarianceImpl::CovarianceImpl(const Covariance::Options& options)
    : options_(options), is_computed_(false), is_valid_(false) {
  evaluate_options_.num_threads = options_.num_threads;
  evaluate_options_.apply_loss_function = options_.apply_loss_function;
}

CovarianceImpl::~CovarianceImpl() = default;

template <typename T>
void CheckForDuplicates(std::vector<T> blocks) {
  std::sort(blocks.begin(), blocks.end());
  auto it = std::adjacent_find(blocks.begin(), blocks.end());
  if (it != blocks.end()) {
    // In case there are duplicates, we search for their location.
    std::map<T, std::vector<int>> blocks_map;
    for (int i = 0; i < blocks.size(); ++i) {
      blocks_map[blocks[i]].push_back(i);
    }

    std::ostringstream duplicates;
    while (it != blocks.end()) {
      duplicates << "(";
      for (int i = 0; i < blocks_map[*it].size() - 1; ++i) {
        duplicates << blocks_map[*it][i] << ", ";
      }
      duplicates << blocks_map[*it].back() << ")";
      it = std::adjacent_find(it + 1, blocks.end());
      if (it < blocks.end()) {
        duplicates << " and ";
      }
    }

    LOG(FATAL) << "Covariance::Compute called with duplicate blocks at "
               << "indices " << duplicates.str();
  }
}

bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks,
                             ProblemImpl* problem) {
  CheckForDuplicates<std::pair<const double*, const double*>>(
      covariance_blocks);
  problem_ = problem;
  parameter_block_to_row_index_.clear();
  covariance_matrix_ = nullptr;
  is_valid_ = (ComputeCovarianceSparsity(covariance_blocks, problem) &&
               ComputeCovarianceValues());
  is_computed_ = true;
  return is_valid_;
}

bool CovarianceImpl::Compute(const std::vector<const double*>& parameter_blocks,
                             ProblemImpl* problem) {
  CheckForDuplicates<const double*>(parameter_blocks);
  CovarianceBlocks covariance_blocks;
  for (int i = 0; i < parameter_blocks.size(); ++i) {
    for (int j = i; j < parameter_blocks.size(); ++j) {
      covariance_blocks.push_back(
          std::make_pair(parameter_blocks[i], parameter_blocks[j]));
    }
  }

  return Compute(covariance_blocks, problem);
}

bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace(
    const double* original_parameter_block1,
    const double* original_parameter_block2,
    bool lift_covariance_to_ambient_space,
    double* covariance_block) const {
  CHECK(is_computed_)
      << "Covariance::GetCovarianceBlock called before Covariance::Compute";
  CHECK(is_valid_)
      << "Covariance::GetCovarianceBlock called when Covariance::Compute "
      << "returned false.";

  // If either of the two parameter blocks is constant, then the
  // covariance block is also zero.
  if (constant_parameter_blocks_.count(original_parameter_block1) > 0 ||
      constant_parameter_blocks_.count(original_parameter_block2) > 0) {
    const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map();
    ParameterBlock* block1 = FindOrDie(
        parameter_map, const_cast<double*>(original_parameter_block1));

    ParameterBlock* block2 = FindOrDie(
        parameter_map, const_cast<double*>(original_parameter_block2));

    const int block1_size = block1->Size();
    const int block2_size = block2->Size();
    const int block1_tangent_size = block1->TangentSize();
    const int block2_tangent_size = block2->TangentSize();
    if (!lift_covariance_to_ambient_space) {
      MatrixRef(covariance_block, block1_tangent_size, block2_tangent_size)
          .setZero();
    } else {
      MatrixRef(covariance_block, block1_size, block2_size).setZero();
    }
    return true;
  }

  const double* parameter_block1 = original_parameter_block1;
  const double* parameter_block2 = original_parameter_block2;
  const bool transpose = parameter_block1 > parameter_block2;
  if (transpose) {
    std::swap(parameter_block1, parameter_block2);
  }

  // Find where in the covariance matrix the block is located.
  const int row_begin =
      FindOrDie(parameter_block_to_row_index_, parameter_block1);
  const int col_begin =
      FindOrDie(parameter_block_to_row_index_, parameter_block2);
  const int* rows = covariance_matrix_->rows();
  const int* cols = covariance_matrix_->cols();
  const int row_size = rows[row_begin + 1] - rows[row_begin];
  const int* cols_begin = cols + rows[row_begin];

  // The only part that requires work is walking the compressed column
  // vector to determine where the set of columns corresponding to the
  // covariance block begin.
  int offset = 0;
  while (cols_begin[offset] != col_begin && offset < row_size) {
    ++offset;
  }

  if (offset == row_size) {
    LOG(ERROR) << "Unable to find covariance block for "
               << original_parameter_block1 << " " << original_parameter_block2;
    return false;
  }

  const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map();
  ParameterBlock* block1 =
      FindOrDie(parameter_map, const_cast<double*>(parameter_block1));
  ParameterBlock* block2 =
      FindOrDie(parameter_map, const_cast<double*>(parameter_block2));
  const Manifold* manifold1 = block1->manifold();
  const Manifold* manifold2 = block2->manifold();
  const int block1_size = block1->Size();
  const int block1_tangent_size = block1->TangentSize();
  const int block2_size = block2->Size();
  const int block2_tangent_size = block2->TangentSize();

  ConstMatrixRef cov(covariance_matrix_->values() + rows[row_begin],
                     block1_tangent_size,
                     row_size);

  // Fast path when there are no manifolds or if the user does not want it
  // lifted to the ambient space.
  if ((manifold1 == nullptr && manifold2 == nullptr) ||
      !lift_covariance_to_ambient_space) {
    if (transpose) {
      MatrixRef(covariance_block, block2_tangent_size, block1_tangent_size) =
          cov.block(0, offset, block1_tangent_size, block2_tangent_size)
              .transpose();
    } else {
      MatrixRef(covariance_block, block1_tangent_size, block2_tangent_size) =
          cov.block(0, offset, block1_tangent_size, block2_tangent_size);
    }
    return true;
  }

  // If manifolds are used then the covariance that has been computed is in the
  // tangent space and it needs to be lifted back to the ambient space.
  //
  // This is given by the formula
  //
  //  C'_12 = J_1 C_12 J_2'
  //
  // Where C_12 is the local tangent space covariance for parameter
  // blocks 1 and 2. J_1 and J_2 are respectively the local to global
  // jacobians for parameter blocks 1 and 2.
  //
  // See Result 5.11 on page 142 of Hartley & Zisserman (2nd Edition)
  // for a proof.
  //
  // TODO(sameeragarwal): Add caching the manifold plus_jacobian, so that they
  // are computed just once per parameter block.
  Matrix block1_jacobian(block1_size, block1_tangent_size);
  if (manifold1 == nullptr) {
    block1_jacobian.setIdentity();
  } else {
    manifold1->PlusJacobian(parameter_block1, block1_jacobian.data());
  }

  Matrix block2_jacobian(block2_size, block2_tangent_size);
  // Fast path if the user is requesting a diagonal block.
  if (parameter_block1 == parameter_block2) {
    block2_jacobian = block1_jacobian;
  } else {
    if (manifold2 == nullptr) {
      block2_jacobian.setIdentity();
    } else {
      manifold2->PlusJacobian(parameter_block2, block2_jacobian.data());
    }
  }

  if (transpose) {
    MatrixRef(covariance_block, block2_size, block1_size) =
        block2_jacobian *
        cov.block(0, offset, block1_tangent_size, block2_tangent_size)
            .transpose() *
        block1_jacobian.transpose();
  } else {
    MatrixRef(covariance_block, block1_size, block2_size) =
        block1_jacobian *
        cov.block(0, offset, block1_tangent_size, block2_tangent_size) *
        block2_jacobian.transpose();
  }

  return true;
}

bool CovarianceImpl::GetCovarianceMatrixInTangentOrAmbientSpace(
    const std::vector<const double*>& parameters,
    bool lift_covariance_to_ambient_space,
    double* covariance_matrix) const {
  CHECK(is_computed_)
      << "Covariance::GetCovarianceMatrix called before Covariance::Compute";
  CHECK(is_valid_)
      << "Covariance::GetCovarianceMatrix called when Covariance::Compute "
      << "returned false.";

  const ProblemImpl::ParameterMap& parameter_map = problem_->parameter_map();
  // For OpenMP compatibility we need to define these vectors in advance
  const int num_parameters = parameters.size();
  std::vector<int> parameter_sizes;
  std::vector<int> cum_parameter_size;
  parameter_sizes.reserve(num_parameters);
  cum_parameter_size.resize(num_parameters + 1);
  cum_parameter_size[0] = 0;
  for (int i = 0; i < num_parameters; ++i) {
    ParameterBlock* block =
        FindOrDie(parameter_map, const_cast<double*>(parameters[i]));
    if (lift_covariance_to_ambient_space) {
      parameter_sizes.push_back(block->Size());
    } else {
      parameter_sizes.push_back(block->TangentSize());
    }
  }
  std::partial_sum(parameter_sizes.begin(),
                   parameter_sizes.end(),
                   cum_parameter_size.begin() + 1);
  const int max_covariance_block_size =
      *std::max_element(parameter_sizes.begin(), parameter_sizes.end());
  const int covariance_size = cum_parameter_size.back();

  // Assemble the blocks in the covariance matrix.
  MatrixRef covariance(covariance_matrix, covariance_size, covariance_size);
  const int num_threads = options_.num_threads;
  auto workspace = std::make_unique<double[]>(
      num_threads * max_covariance_block_size * max_covariance_block_size);

  bool success = true;

  // Technically the following code is a double nested loop where
  // i = 1:n, j = i:n.
  int iteration_count = (num_parameters * (num_parameters + 1)) / 2;
  problem_->context()->EnsureMinimumThreads(num_threads);
  ParallelFor(problem_->context(),
              0,
              iteration_count,
              num_threads,
              [&](int thread_id, int k) {
                int i, j;
                LinearIndexToUpperTriangularIndex(k, num_parameters, &i, &j);

                int covariance_row_idx = cum_parameter_size[i];
                int covariance_col_idx = cum_parameter_size[j];
                int size_i = parameter_sizes[i];
                int size_j = parameter_sizes[j];
                double* covariance_block =
                    workspace.get() + thread_id * max_covariance_block_size *
                                          max_covariance_block_size;
                if (!GetCovarianceBlockInTangentOrAmbientSpace(
                        parameters[i],
                        parameters[j],
                        lift_covariance_to_ambient_space,
                        covariance_block)) {
                  success = false;
                }

                covariance.block(
                    covariance_row_idx, covariance_col_idx, size_i, size_j) =
                    MatrixRef(covariance_block, size_i, size_j);

                if (i != j) {
                  covariance.block(
                      covariance_col_idx, covariance_row_idx, size_j, size_i) =
                      MatrixRef(covariance_block, size_i, size_j).transpose();
                }
              });
  return success;
}

// Determine the sparsity pattern of the covariance matrix based on
// the block pairs requested by the user.
bool CovarianceImpl::ComputeCovarianceSparsity(
    const CovarianceBlocks& original_covariance_blocks, ProblemImpl* problem) {
  EventLogger event_logger("CovarianceImpl::ComputeCovarianceSparsity");

  // Determine an ordering for the parameter block, by sorting the
  // parameter blocks by their pointers.
  std::vector<double*> all_parameter_blocks;
  problem->GetParameterBlocks(&all_parameter_blocks);
  const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map();
  std::unordered_set<ParameterBlock*> parameter_blocks_in_use;
  std::vector<ResidualBlock*> residual_blocks;
  problem->GetResidualBlocks(&residual_blocks);

  for (auto* residual_block : residual_blocks) {
    parameter_blocks_in_use.insert(residual_block->parameter_blocks(),
                                   residual_block->parameter_blocks() +
                                       residual_block->NumParameterBlocks());
  }

  constant_parameter_blocks_.clear();
  std::vector<double*>& active_parameter_blocks =
      evaluate_options_.parameter_blocks;
  active_parameter_blocks.clear();
  for (auto* parameter_block : all_parameter_blocks) {
    ParameterBlock* block = FindOrDie(parameter_map, parameter_block);
    if (!block->IsConstant() && (parameter_blocks_in_use.count(block) > 0)) {
      active_parameter_blocks.push_back(parameter_block);
    } else {
      constant_parameter_blocks_.insert(parameter_block);
    }
  }

  std::sort(active_parameter_blocks.begin(), active_parameter_blocks.end());

  // Compute the number of rows.  Map each parameter block to the
  // first row corresponding to it in the covariance matrix using the
  // ordering of parameter blocks just constructed.
  int num_rows = 0;
  parameter_block_to_row_index_.clear();
  for (auto* parameter_block : active_parameter_blocks) {
    const int parameter_block_size =
        problem->ParameterBlockTangentSize(parameter_block);
    parameter_block_to_row_index_[parameter_block] = num_rows;
    num_rows += parameter_block_size;
  }

  // Compute the number of non-zeros in the covariance matrix.  Along
  // the way flip any covariance blocks which are in the lower
  // triangular part of the matrix.
  int num_nonzeros = 0;
  CovarianceBlocks covariance_blocks;
  for (const auto& block_pair : original_covariance_blocks) {
    if (constant_parameter_blocks_.count(block_pair.first) > 0 ||
        constant_parameter_blocks_.count(block_pair.second) > 0) {
      continue;
    }

    int index1 = FindOrDie(parameter_block_to_row_index_, block_pair.first);
    int index2 = FindOrDie(parameter_block_to_row_index_, block_pair.second);
    const int size1 = problem->ParameterBlockTangentSize(block_pair.first);
    const int size2 = problem->ParameterBlockTangentSize(block_pair.second);
    num_nonzeros += size1 * size2;

    // Make sure we are constructing a block upper triangular matrix.
    if (index1 > index2) {
      covariance_blocks.push_back(
          std::make_pair(block_pair.second, block_pair.first));
    } else {
      covariance_blocks.push_back(block_pair);
    }
  }

  if (covariance_blocks.empty()) {
    VLOG(2) << "No non-zero covariance blocks found";
    covariance_matrix_ = nullptr;
    return true;
  }

  // Sort the block pairs. As a consequence we get the covariance
  // blocks as they will occur in the CompressedRowSparseMatrix that
  // will store the covariance.
  std::sort(covariance_blocks.begin(), covariance_blocks.end());

  // Fill the sparsity pattern of the covariance matrix.
  covariance_matrix_ = std::make_unique<CompressedRowSparseMatrix>(
      num_rows, num_rows, num_nonzeros);

  int* rows = covariance_matrix_->mutable_rows();
  int* cols = covariance_matrix_->mutable_cols();

  // Iterate over parameter blocks and in turn over the rows of the
  // covariance matrix. For each parameter block, look in the upper
  // triangular part of the covariance matrix to see if there are any
  // blocks requested by the user. If this is the case then fill out a
  // set of compressed rows corresponding to this parameter block.
  //
  // The key thing that makes this loop work is the fact that the
  // row/columns of the covariance matrix are ordered by the pointer
  // values of the parameter blocks. Thus iterating over the keys of
  // parameter_block_to_row_index_ corresponds to iterating over the
  // rows of the covariance matrix in order.
  int i = 0;       // index into covariance_blocks.
  int cursor = 0;  // index into the covariance matrix.
  for (const auto& entry : parameter_block_to_row_index_) {
    const double* row_block = entry.first;
    const int row_block_size = problem->ParameterBlockTangentSize(row_block);
    int row_begin = entry.second;

    // Iterate over the covariance blocks contained in this row block
    // and count the number of columns in this row block.
    int num_col_blocks = 0;
    for (int j = i; j < covariance_blocks.size(); ++j, ++num_col_blocks) {
      const std::pair<const double*, const double*>& block_pair =
          covariance_blocks[j];
      if (block_pair.first != row_block) {
        break;
      }
    }

    // Fill out all the compressed rows for this parameter block.
    for (int r = 0; r < row_block_size; ++r) {
      rows[row_begin + r] = cursor;
      for (int c = 0; c < num_col_blocks; ++c) {
        const double* col_block = covariance_blocks[i + c].second;
        const int col_block_size =
            problem->ParameterBlockTangentSize(col_block);
        int col_begin = FindOrDie(parameter_block_to_row_index_, col_block);
        for (int k = 0; k < col_block_size; ++k) {
          cols[cursor++] = col_begin++;
        }
      }
    }

    i += num_col_blocks;
  }

  rows[num_rows] = cursor;
  return true;
}

bool CovarianceImpl::ComputeCovarianceValues() {
  if (options_.algorithm_type == DENSE_SVD) {
    return ComputeCovarianceValuesUsingDenseSVD();
  }

  if (options_.algorithm_type == SPARSE_QR) {
    if (options_.sparse_linear_algebra_library_type == EIGEN_SPARSE) {
      return ComputeCovarianceValuesUsingEigenSparseQR();
    }

    if (options_.sparse_linear_algebra_library_type == SUITE_SPARSE) {
#if !defined(CERES_NO_SUITESPARSE)
      return ComputeCovarianceValuesUsingSuiteSparseQR();
#else
      LOG(ERROR) << "SuiteSparse is required to use the SPARSE_QR algorithm "
                 << "with "
                 << "Covariance::Options::sparse_linear_algebra_library_type "
                 << "= SUITE_SPARSE.";
      return false;
#endif
    }

    LOG(ERROR) << "Unsupported "
               << "Covariance::Options::sparse_linear_algebra_library_type "
               << "= "
               << SparseLinearAlgebraLibraryTypeToString(
                      options_.sparse_linear_algebra_library_type);
    return false;
  }

  LOG(ERROR) << "Unsupported Covariance::Options::algorithm_type = "
             << CovarianceAlgorithmTypeToString(options_.algorithm_type);
  return false;
}

bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparseQR() {
  EventLogger event_logger(
      "CovarianceImpl::ComputeCovarianceValuesUsingSparseQR");

#ifndef CERES_NO_SUITESPARSE
  if (covariance_matrix_ == nullptr) {
    // Nothing to do, all zeros covariance matrix.
    return true;
  }

  CRSMatrix jacobian;
  problem_->Evaluate(evaluate_options_, nullptr, nullptr, nullptr, &jacobian);
  event_logger.AddEvent("Evaluate");

  // Construct a compressed column form of the Jacobian.
  const int num_rows = jacobian.num_rows;
  const int num_cols = jacobian.num_cols;
  const int num_nonzeros = jacobian.values.size();

  std::vector<SuiteSparse_long> transpose_rows(num_cols + 1, 0);
  std::vector<SuiteSparse_long> transpose_cols(num_nonzeros, 0);
  std::vector<double> transpose_values(num_nonzeros, 0);

  for (int idx = 0; idx < num_nonzeros; ++idx) {
    transpose_rows[jacobian.cols[idx] + 1] += 1;
  }

  for (int i = 1; i < transpose_rows.size(); ++i) {
    transpose_rows[i] += transpose_rows[i - 1];
  }

  for (int r = 0; r < num_rows; ++r) {
    for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
      const int c = jacobian.cols[idx];
      const int transpose_idx = transpose_rows[c];
      transpose_cols[transpose_idx] = r;
      transpose_values[transpose_idx] = jacobian.values[idx];
      ++transpose_rows[c];
    }
  }

  for (int i = transpose_rows.size() - 1; i > 0; --i) {
    transpose_rows[i] = transpose_rows[i - 1];
  }
  transpose_rows[0] = 0;

  cholmod_sparse cholmod_jacobian;
  cholmod_jacobian.nrow = num_rows;
  cholmod_jacobian.ncol = num_cols;
  cholmod_jacobian.nzmax = num_nonzeros;
  cholmod_jacobian.nz = nullptr;
  cholmod_jacobian.p = reinterpret_cast<void*>(transpose_rows.data());
  cholmod_jacobian.i = reinterpret_cast<void*>(transpose_cols.data());
  cholmod_jacobian.x = reinterpret_cast<void*>(transpose_values.data());
  cholmod_jacobian.z = nullptr;
  cholmod_jacobian.stype = 0;  // Matrix is not symmetric.
  cholmod_jacobian.itype = CHOLMOD_LONG;
  cholmod_jacobian.xtype = CHOLMOD_REAL;
  cholmod_jacobian.dtype = CHOLMOD_DOUBLE;
  cholmod_jacobian.sorted = 1;
  cholmod_jacobian.packed = 1;

  cholmod_common cc;
  cholmod_l_start(&cc);

  cholmod_sparse* R = nullptr;
  SuiteSparse_long* permutation = nullptr;

  // Compute a Q-less QR factorization of the Jacobian. Since we are
  // only interested in inverting J'J = R'R, we do not need Q. This
  // saves memory and gives us R as a permuted compressed column
  // sparse matrix.
  //
  // TODO(sameeragarwal): Currently the symbolic factorization and the
  // numeric factorization is done at the same time, and this does not
  // explicitly account for the block column and row structure in the
  // matrix. When using AMD, we have observed in the past that
  // computing the ordering with the block matrix is significantly
  // more efficient, both in runtime as well as the quality of
  // ordering computed. So, it maybe worth doing that analysis
  // separately.
  const SuiteSparse_long rank = SuiteSparseQR<double>(
      SPQR_ORDERING_BESTAMD,
      options_.column_pivot_threshold < 0 ? SPQR_DEFAULT_TOL
                                          : options_.column_pivot_threshold,
      cholmod_jacobian.ncol,
      &cholmod_jacobian,
      &R,
      &permutation,
      &cc);
  event_logger.AddEvent("Numeric Factorization");
  if (R == nullptr) {
    LOG(ERROR) << "Something is wrong. SuiteSparseQR returned R = nullptr.";
    free(permutation);
    cholmod_l_finish(&cc);
    return false;
  }

  if (rank < cholmod_jacobian.ncol) {
    LOG(WARNING) << "Jacobian matrix is rank deficient. "
                 << "Number of columns: " << cholmod_jacobian.ncol
                 << " rank: " << rank;
    free(permutation);
    cholmod_l_free_sparse(&R, &cc);
    cholmod_l_finish(&cc);
    return false;
  }

  std::vector<int> inverse_permutation(num_cols);
  if (permutation) {
    for (SuiteSparse_long i = 0; i < num_cols; ++i) {
      inverse_permutation[permutation[i]] = i;
    }
  } else {
    for (SuiteSparse_long i = 0; i < num_cols; ++i) {
      inverse_permutation[i] = i;
    }
  }

  const int* rows = covariance_matrix_->rows();
  const int* cols = covariance_matrix_->cols();
  double* values = covariance_matrix_->mutable_values();

  // The following loop exploits the fact that the i^th column of A^{-1}
  // is given by the solution to the linear system
  //
  //  A x = e_i
  //
  // where e_i is a vector with e(i) = 1 and all other entries zero.
  //
  // Since the covariance matrix is symmetric, the i^th row and column
  // are equal.
  const int num_threads = options_.num_threads;
  auto workspace = std::make_unique<double[]>(num_threads * num_cols);

  problem_->context()->EnsureMinimumThreads(num_threads);
  ParallelFor(
      problem_->context(), 0, num_cols, num_threads, [&](int thread_id, int r) {
        const int row_begin = rows[r];
        const int row_end = rows[r + 1];
        if (row_end != row_begin) {
          double* solution = workspace.get() + thread_id * num_cols;
          SolveRTRWithSparseRHS<SuiteSparse_long>(
              num_cols,
              static_cast<SuiteSparse_long*>(R->i),
              static_cast<SuiteSparse_long*>(R->p),
              static_cast<double*>(R->x),
              inverse_permutation[r],
              solution);
          for (int idx = row_begin; idx < row_end; ++idx) {
            const int c = cols[idx];
            values[idx] = solution[inverse_permutation[c]];
          }
        }
      });

  free(permutation);
  cholmod_l_free_sparse(&R, &cc);
  cholmod_l_finish(&cc);
  event_logger.AddEvent("Inversion");
  return true;

#else  // CERES_NO_SUITESPARSE

  return false;

#endif  // CERES_NO_SUITESPARSE
}

bool CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD() {
  EventLogger event_logger(
      "CovarianceImpl::ComputeCovarianceValuesUsingDenseSVD");
  if (covariance_matrix_ == nullptr) {
    // Nothing to do, all zeros covariance matrix.
    return true;
  }

  CRSMatrix jacobian;
  problem_->Evaluate(evaluate_options_, nullptr, nullptr, nullptr, &jacobian);
  event_logger.AddEvent("Evaluate");

  Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols);
  dense_jacobian.setZero();
  for (int r = 0; r < jacobian.num_rows; ++r) {
    for (int idx = jacobian.rows[r]; idx < jacobian.rows[r + 1]; ++idx) {
      const int c = jacobian.cols[idx];
      dense_jacobian(r, c) = jacobian.values[idx];
    }
  }
  event_logger.AddEvent("ConvertToDenseMatrix");

  Eigen::BDCSVD<Matrix> svd(dense_jacobian,
                            Eigen::ComputeThinU | Eigen::ComputeThinV);

  event_logger.AddEvent("SingularValueDecomposition");

  const Vector singular_values = svd.singularValues();
  const int num_singular_values = singular_values.rows();
  Vector inverse_squared_singular_values(num_singular_values);
  inverse_squared_singular_values.setZero();

  const double max_singular_value = singular_values[0];
  const double min_singular_value_ratio =
      sqrt(options_.min_reciprocal_condition_number);

  const bool automatic_truncation = (options_.null_space_rank < 0);
  const int max_rank = std::min(num_singular_values,
                                num_singular_values - options_.null_space_rank);

  // Compute the squared inverse of the singular values. Truncate the
  // computation based on min_singular_value_ratio and
  // null_space_rank. When either of these two quantities are active,
  // the resulting covariance matrix is a Moore-Penrose inverse
  // instead of a regular inverse.
  for (int i = 0; i < max_rank; ++i) {
    const double singular_value_ratio = singular_values[i] / max_singular_value;
    if (singular_value_ratio < min_singular_value_ratio) {
      // Since the singular values are in decreasing order, if
      // automatic truncation is enabled, then from this point on
      // all values will fail the ratio test and there is nothing to
      // do in this loop.
      if (automatic_truncation) {
        break;
      } else {
        LOG(ERROR) << "Error: Covariance matrix is near rank deficient "
                   << "and the user did not specify a non-zero"
                   << "Covariance::Options::null_space_rank "
                   << "to enable the computation of a Pseudo-Inverse. "
                   << "Reciprocal condition number: "
                   << singular_value_ratio * singular_value_ratio << " "
                   << "min_reciprocal_condition_number: "
                   << options_.min_reciprocal_condition_number;
        return false;
      }
    }

    inverse_squared_singular_values[i] =
        1.0 / (singular_values[i] * singular_values[i]);
  }

  Matrix dense_covariance = svd.matrixV() *
                            inverse_squared_singular_values.asDiagonal() *
                            svd.matrixV().transpose();
  event_logger.AddEvent("PseudoInverse");

  const int num_rows = covariance_matrix_->num_rows();
  const int* rows = covariance_matrix_->rows();
  const int* cols = covariance_matrix_->cols();
  double* values = covariance_matrix_->mutable_values();

  for (int r = 0; r < num_rows; ++r) {
    for (int idx = rows[r]; idx < rows[r + 1]; ++idx) {
      const int c = cols[idx];
      values[idx] = dense_covariance(r, c);
    }
  }
  event_logger.AddEvent("CopyToCovarianceMatrix");
  return true;
}

bool CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR() {
  EventLogger event_logger(
      "CovarianceImpl::ComputeCovarianceValuesUsingEigenSparseQR");
  if (covariance_matrix_ == nullptr) {
    // Nothing to do, all zeros covariance matrix.
    return true;
  }

  CRSMatrix jacobian;
  problem_->Evaluate(evaluate_options_, nullptr, nullptr, nullptr, &jacobian);
  event_logger.AddEvent("Evaluate");

  using EigenSparseMatrix = Eigen::SparseMatrix<double, Eigen::ColMajor>;

  // Convert the matrix to column major order as required by SparseQR.
  EigenSparseMatrix sparse_jacobian =
      Eigen::Map<Eigen::SparseMatrix<double, Eigen::RowMajor>>(
          jacobian.num_rows,
          jacobian.num_cols,
          static_cast<int>(jacobian.values.size()),
          jacobian.rows.data(),
          jacobian.cols.data(),
          jacobian.values.data());
  event_logger.AddEvent("ConvertToSparseMatrix");

  Eigen::SparseQR<EigenSparseMatrix, Eigen::COLAMDOrdering<int>> qr;
  if (options_.column_pivot_threshold > 0) {
    qr.setPivotThreshold(options_.column_pivot_threshold);
  }

  qr.compute(sparse_jacobian);
  event_logger.AddEvent("QRDecomposition");

  if (qr.info() != Eigen::Success) {
    LOG(ERROR) << "Eigen::SparseQR decomposition failed.";
    return false;
  }

  if (qr.rank() < jacobian.num_cols) {
    LOG(ERROR) << "Jacobian matrix is rank deficient. "
               << "Number of columns: " << jacobian.num_cols
               << " rank: " << qr.rank();
    return false;
  }

  const int* rows = covariance_matrix_->rows();
  const int* cols = covariance_matrix_->cols();
  double* values = covariance_matrix_->mutable_values();

  // Compute the inverse column permutation used by QR factorization.
  Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> inverse_permutation =
      qr.colsPermutation().inverse();

  // The following loop exploits the fact that the i^th column of A^{-1}
  // is given by the solution to the linear system
  //
  //  A x = e_i
  //
  // where e_i is a vector with e(i) = 1 and all other entries zero.
  //
  // Since the covariance matrix is symmetric, the i^th row and column
  // are equal.
  const int num_cols = jacobian.num_cols;
  const int num_threads = options_.num_threads;
  auto workspace = std::make_unique<double[]>(num_threads * num_cols);

  problem_->context()->EnsureMinimumThreads(num_threads);
  ParallelFor(
      problem_->context(), 0, num_cols, num_threads, [&](int thread_id, int r) {
        const int row_begin = rows[r];
        const int row_end = rows[r + 1];
        if (row_end != row_begin) {
          double* solution = workspace.get() + thread_id * num_cols;
          SolveRTRWithSparseRHS<int>(num_cols,
                                     qr.matrixR().innerIndexPtr(),
                                     qr.matrixR().outerIndexPtr(),
                                     &qr.matrixR().data().value(0),
                                     inverse_permutation.indices().coeff(r),
                                     solution);

          // Assign the values of the computed covariance using the
          // inverse permutation used in the QR factorization.
          for (int idx = row_begin; idx < row_end; ++idx) {
            const int c = cols[idx];
            values[idx] = solution[inverse_permutation.indices().coeff(c)];
          }
        }
      });

  event_logger.AddEvent("Inverse");

  return true;
}

}  // namespace ceres::internal