// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2015 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"

#ifdef CERES_USE_OPENMP

#include <omp.h>

#endif

#include <algorithm>

#include <cstdlib>

#include <numeric>

#include <sstream>

#include <utility>

#include <vector>

#include "Eigen/SparseCore"

#include "Eigen/SparseQR"

#include "Eigen/SVD"

#include "ceres/collections_port.h"

#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/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 {

namespace internal {

using std::make_pair;

using std::map;

using std::pair;

using std::sort;

using std::swap;

using std::vector;

typedef vector<pair<const double*, const double*> > CovarianceBlocks;

CovarianceImpl::CovarianceImpl(const Covariance::Options& options)

    : options_(options),

      is_computed_(false),

      is_valid_(false) {

#ifndef CERES_USE_OPENMP

  if (options_.num_threads > 1) {

    LOG(WARNING)

        << "OpenMP support is not compiled into this binary; "

        << "only options.num_threads = 1 is supported. Switching "

        << "to single threaded mode.";

    options_.num_threads = 1;

  }

#endif

  evaluate_options_.num_threads = options_.num_threads;

  evaluate_options_.apply_loss_function = options_.apply_loss_function;

}

CovarianceImpl::~CovarianceImpl() {

}

template <typename T> void CheckForDuplicates(vector<T> blocks) {

  sort(blocks.begin(), blocks.end());

  typename vector<T>::iterator it =

      std::adjacent_find(blocks.begin(), blocks.end());

  if (it != blocks.end()) {

    // In case there are duplicates, we search for their location.

    map<T, 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<pair<const double*, const double*> >(covariance_blocks);

  problem_ = problem;

  parameter_block_to_row_index_.clear();

  covariance_matrix_.reset(NULL);

  is_valid_ = (ComputeCovarianceSparsity(covariance_blocks, problem) &&

               ComputeCovarianceValues());

  is_computed_ = true;

  return is_valid_;

}

bool CovarianceImpl::Compute(const 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(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_local_size = block1->LocalSize();

    const int block2_local_size = block2->LocalSize();

    if (!lift_covariance_to_ambient_space) {

      MatrixRef(covariance_block, block1_local_size, block2_local_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) {

    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 correspnding 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 LocalParameterization* local_param1 = block1->local_parameterization();

  const LocalParameterization* local_param2 = block2->local_parameterization();

  const int block1_size = block1->Size();

  const int block1_local_size = block1->LocalSize();

  const int block2_size = block2->Size();

  const int block2_local_size = block2->LocalSize();

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

                     block1_size,

                     row_size);

  // Fast path when there are no local parameterizations or if the

  // user does not want it lifted to the ambient space.

  if ((local_param1 == NULL && local_param2 == NULL) ||

      !lift_covariance_to_ambient_space) {

    if (transpose) {

      MatrixRef(covariance_block, block2_local_size, block1_local_size) =

          cov.block(0, offset, block1_local_size,

                    block2_local_size).transpose();

    } else {

      MatrixRef(covariance_block, block1_local_size, block2_local_size) =

          cov.block(0, offset, block1_local_size, block2_local_size);

    }

    return true;

  }

  // If local parameterizations 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 of local parameterization, so

  // that they are computed just once per parameter block.

  Matrix block1_jacobian(block1_size, block1_local_size);

  if (local_param1 == NULL) {

    block1_jacobian.setIdentity();

  } else {

    local_param1->ComputeJacobian(parameter_block1, block1_jacobian.data());

  }

  Matrix block2_jacobian(block2_size, block2_local_size);

  // Fast path if the user is requesting a diagonal block.

  if (parameter_block1 == parameter_block2) {

    block2_jacobian = block1_jacobian;

  } else {

    if (local_param2 == NULL) {

      block2_jacobian.setIdentity();

    } else {

      local_param2->ComputeJacobian(parameter_block2, block2_jacobian.data());

    }

  }

  if (transpose) {

    MatrixRef(covariance_block, block2_size, block1_size) =

        block2_jacobian *

        cov.block(0, offset, block1_local_size, block2_local_size).transpose() *

        block1_jacobian.transpose();

  } else {

    MatrixRef(covariance_block, block1_size, block2_size) =

        block1_jacobian *

        cov.block(0, offset, block1_local_size, block2_local_size) *

        block2_jacobian.transpose();

  }

  return true;

}

bool CovarianceImpl::GetCovarianceMatrixInTangentOrAmbientSpace(

    const 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();

  vector<int> parameter_sizes;

  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->LocalSize());

    }

  }

  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;

  scoped_array<double> workspace(

      new double[num_threads * max_covariance_block_size *

                 max_covariance_block_size]);

  bool success = true;

// The collapse() directive is only supported in OpenMP 3.0 and higher. OpenMP

// 3.0 was released in May 2008 (hence the version number).

#if _OPENMP >= 200805

#  pragma omp parallel for num_threads(num_threads) schedule(dynamic) collapse(2)

#else

#  pragma omp parallel for num_threads(num_threads) schedule(dynamic)

#endif

  for (int i = 0; i < num_parameters; ++i) {

    for (int j = 0; j < num_parameters; ++j) {

      // The second loop can't start from j = i for compatibility with OpenMP

      // collapse command. The conditional serves as a workaround

      if (j >= i) {

        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];

#ifdef CERES_USE_OPENMP

        int thread_id = omp_get_thread_num();

#else

        int thread_id = 0;

#endif

        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.

  vector<double*> all_parameter_blocks;

  problem->GetParameterBlocks(&all_parameter_blocks);

  const ProblemImpl::ParameterMap& parameter_map = problem->parameter_map();

  HashSet<ParameterBlock*> parameter_blocks_in_use;

  vector<ResidualBlock*> residual_blocks;

  problem->GetResidualBlocks(&residual_blocks);

  for (int i = 0; i < residual_blocks.size(); ++i) {

    ResidualBlock* residual_block = residual_blocks[i];

    parameter_blocks_in_use.insert(residual_block->parameter_blocks(),

                                   residual_block->parameter_blocks() +

                                   residual_block->NumParameterBlocks());

  }

  constant_parameter_blocks_.clear();

  vector<double*>& active_parameter_blocks =

      evaluate_options_.parameter_blocks;

  active_parameter_blocks.clear();

  for (int i = 0; i < all_parameter_blocks.size(); ++i) {

    double* parameter_block = all_parameter_blocks[i];

    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 (int i = 0; i < active_parameter_blocks.size(); ++i) {

    double* parameter_block = active_parameter_blocks[i];

    const int parameter_block_size =

        problem->ParameterBlockLocalSize(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 (int i = 0; i <  original_covariance_blocks.size(); ++i) {

    const pair<const double*, const double*>& block_pair =

        original_covariance_blocks[i];

    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->ParameterBlockLocalSize(block_pair.first);

    const int size2 = problem->ParameterBlockLocalSize(block_pair.second);

    num_nonzeros += size1 * size2;

    // Make sure we are constructing a block upper triangular matrix.

    if (index1 > index2) {

      covariance_blocks.push_back(make_pair(block_pair.second,

                                            block_pair.first));

    } else {

      covariance_blocks.push_back(block_pair);

    }

  }

  if (covariance_blocks.size() == 0) {

    VLOG(2) << "No non-zero covariance blocks found";

    covariance_matrix_.reset(NULL);

    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.

  sort(covariance_blocks.begin(), covariance_blocks.end());

  // Fill the sparsity pattern of the covariance matrix.

  covariance_matrix_.reset(

      new 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 (map<const double*, int>::const_iterator it =

           parameter_block_to_row_index_.begin();

       it != parameter_block_to_row_index_.end();

       ++it) {

    const double* row_block =  it->first;

    const int row_block_size = problem->ParameterBlockLocalSize(row_block);

    int row_begin = it->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;

    int num_columns = 0;

    for (int j = i; j < covariance_blocks.size(); ++j, ++num_col_blocks) {

      const pair<const double*, const double*>& block_pair =

          covariance_blocks[j];

      if (block_pair.first != row_block) {

        break;

      }

      num_columns += problem->ParameterBlockLocalSize(block_pair.second);

    }

    // 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->ParameterBlockLocalSize(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_.get() == NULL) {

    // Nothing to do, all zeros covariance matrix.

    return true;

  }

  CRSMatrix jacobian;

  problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &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();

  vector<SuiteSparse_long> transpose_rows(num_cols + 1, 0);

  vector<SuiteSparse_long> transpose_cols(num_nonzeros, 0);

  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 = NULL;

  cholmod_jacobian.p = reinterpret_cast<void*>(&transpose_rows[0]);

  cholmod_jacobian.i = reinterpret_cast<void*>(&transpose_cols[0]);

  cholmod_jacobian.x = reinterpret_cast<void*>(&transpose_values[0]);

  cholmod_jacobian.z = NULL;

  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 = NULL;

  SuiteSparse_long* permutation = NULL;

  // 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,

                            SPQR_DEFAULT_TOL,

                            cholmod_jacobian.ncol,

                            &cholmod_jacobian,

                            &R,

                            &permutation,

                            &cc);

  event_logger.AddEvent("Numeric Factorization");

  CHECK_NOTNULL(permutation);

  CHECK_NOTNULL(R);

  if (rank < cholmod_jacobian.ncol) {

    LOG(ERROR) << "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;

  }

  vector<int> inverse_permutation(num_cols);

  for (SuiteSparse_long i = 0; i < num_cols; ++i) {

    inverse_permutation[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;

  scoped_array<double> workspace(new double[num_threads * num_cols]);

#pragma omp parallel for num_threads(num_threads) schedule(dynamic)

  for (int r = 0; r < num_cols; ++r) {

    const int row_begin = rows[r];

    const int row_end = rows[r + 1];

    if (row_end == row_begin) {

      continue;

    }

#  ifdef CERES_USE_OPENMP

    int thread_id = omp_get_thread_num();

#  else

    int thread_id = 0;

#  endif

    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_.get() == NULL) {

    // Nothing to do, all zeros covariance matrix.

    return true;

  }

  CRSMatrix jacobian;

  problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &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::JacobiSVD<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_.get() == NULL) {

    // Nothing to do, all zeros covariance matrix.

    return true;

  }

  CRSMatrix jacobian;

  problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);

  event_logger.AddEvent("Evaluate");

  typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;

  // Convert the matrix to column major order as required by SparseQR.

  EigenSparseMatrix sparse_jacobian =

      Eigen::MappedSparseMatrix<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_solver(sparse_jacobian);

  event_logger.AddEvent("QRDecomposition");

  if (qr_solver.info() != Eigen::Success) {

    LOG(ERROR) << "Eigen::SparseQR decomposition failed.";

    return false;

  }

  if (qr_solver.rank() < jacobian.num_cols) {

    LOG(ERROR) << "Jacobian matrix is rank deficient. "

               << "Number of columns: " << jacobian.num_cols

               << " rank: " << qr_solver.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_solver.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

  //