Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
new file mode 100644
index 0000000..d24f5c9
--- /dev/null
+++ b/internal/ceres/covariance_impl.cc
@@ -0,0 +1,913 @@
+// 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"
+
+#include <algorithm>
+#include <cstdlib>
+#include <memory>
+#include <numeric>
+#include <sstream>
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+#include "Eigen/SparseCore"
+#include "Eigen/SparseQR"
+#include "Eigen/SVD"
+
+#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 {
+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) {
+#ifdef CERES_NO_THREADS
+ if (options_.num_threads > 1) {
+ LOG(WARNING)
+ << "No threading support is 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;
+ std::unique_ptr<double[]> workspace(
+ new 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.
+ 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;
+ 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 (const auto& entry : parameter_block_to_row_index_) {
+ const double* row_block = entry.first;
+ const int row_block_size = problem->ParameterBlockLocalSize(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;
+ 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(R != nullptr);
+
+ 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);
+ 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;
+ std::unique_ptr<double[]> workspace(new 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_.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
+ //
+ // 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;
+ std::unique_ptr<double[]> workspace(new 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_solver.matrixR().innerIndexPtr(),
+ qr_solver.matrixR().outerIndexPtr(),
+ &qr_solver.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 internal
+} // namespace ceres