| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2017 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/compressed_row_sparse_matrix.h" |
| |
| #include <algorithm> |
| #include <numeric> |
| #include <vector> |
| #include "ceres/crs_matrix.h" |
| #include "ceres/internal/port.h" |
| #include "ceres/random.h" |
| #include "ceres/triplet_sparse_matrix.h" |
| #include "glog/logging.h" |
| |
| namespace ceres { |
| namespace internal { |
| |
| using std::vector; |
| |
| namespace { |
| |
| // Helper functor used by the constructor for reordering the contents |
| // of a TripletSparseMatrix. This comparator assumes thay there are no |
| // duplicates in the pair of arrays rows and cols, i.e., there is no |
| // indices i and j (not equal to each other) s.t. |
| // |
| // rows[i] == rows[j] && cols[i] == cols[j] |
| // |
| // If this is the case, this functor will not be a StrictWeakOrdering. |
| struct RowColLessThan { |
| RowColLessThan(const int* rows, const int* cols) : rows(rows), cols(cols) {} |
| |
| bool operator()(const int x, const int y) const { |
| if (rows[x] == rows[y]) { |
| return (cols[x] < cols[y]); |
| } |
| return (rows[x] < rows[y]); |
| } |
| |
| const int* rows; |
| const int* cols; |
| }; |
| |
| void TransposeForCompressedRowSparseStructure(const int num_rows, |
| const int num_cols, |
| const int num_nonzeros, |
| const int* rows, |
| const int* cols, |
| const double* values, |
| int* transpose_rows, |
| int* transpose_cols, |
| double* transpose_values) { |
| // Explicitly zero out transpose_rows. |
| std::fill(transpose_rows, transpose_rows + num_cols + 1, 0); |
| |
| // Count the number of entries in each column of the original matrix |
| // and assign to transpose_rows[col + 1]. |
| for (int idx = 0; idx < num_nonzeros; ++idx) { |
| ++transpose_rows[cols[idx] + 1]; |
| } |
| |
| // Compute the starting position for each row in the transpose by |
| // computing the cumulative sum of the entries of transpose_rows. |
| for (int i = 1; i < num_cols + 1; ++i) { |
| transpose_rows[i] += transpose_rows[i - 1]; |
| } |
| |
| // Populate transpose_cols and (optionally) transpose_values by |
| // walking the entries of the source matrices. For each entry that |
| // is added, the value of transpose_row is incremented allowing us |
| // to keep track of where the next entry for that row should go. |
| // |
| // As a result transpose_row is shifted to the left by one entry. |
| for (int r = 0; r < num_rows; ++r) { |
| for (int idx = rows[r]; idx < rows[r + 1]; ++idx) { |
| const int c = cols[idx]; |
| const int transpose_idx = transpose_rows[c]++; |
| transpose_cols[transpose_idx] = r; |
| if (values != NULL && transpose_values != NULL) { |
| transpose_values[transpose_idx] = values[idx]; |
| } |
| } |
| } |
| |
| // This loop undoes the left shift to transpose_rows introduced by |
| // the previous loop. |
| for (int i = num_cols - 1; i > 0; --i) { |
| transpose_rows[i] = transpose_rows[i - 1]; |
| } |
| transpose_rows[0] = 0; |
| } |
| |
| void AddRandomBlock(const int num_rows, |
| const int num_cols, |
| const int row_block_begin, |
| const int col_block_begin, |
| std::vector<int>* rows, |
| std::vector<int>* cols, |
| std::vector<double>* values) { |
| for (int r = 0; r < num_rows; ++r) { |
| for (int c = 0; c < num_cols; ++c) { |
| rows->push_back(row_block_begin + r); |
| cols->push_back(col_block_begin + c); |
| values->push_back(RandNormal()); |
| } |
| } |
| } |
| |
| } // namespace |
| |
| // This constructor gives you a semi-initialized CompressedRowSparseMatrix. |
| CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, |
| int num_cols, |
| int max_num_nonzeros) { |
| num_rows_ = num_rows; |
| num_cols_ = num_cols; |
| storage_type_ = UNSYMMETRIC; |
| rows_.resize(num_rows + 1, 0); |
| cols_.resize(max_num_nonzeros, 0); |
| values_.resize(max_num_nonzeros, 0.0); |
| |
| VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ |
| << " max_num_nonzeros: " << cols_.size() << ". Allocating " |
| << (num_rows_ + 1) * sizeof(int) + // NOLINT |
| cols_.size() * sizeof(int) + // NOLINT |
| cols_.size() * sizeof(double); // NOLINT |
| } |
| |
| CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix( |
| const TripletSparseMatrix& input) { |
| return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, false); |
| } |
| |
| CompressedRowSparseMatrix* |
| CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed( |
| const TripletSparseMatrix& input) { |
| return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, true); |
| } |
| |
| CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix( |
| const TripletSparseMatrix& input, bool transpose) { |
| int num_rows = input.num_rows(); |
| int num_cols = input.num_cols(); |
| const int* rows = input.rows(); |
| const int* cols = input.cols(); |
| const double* values = input.values(); |
| |
| if (transpose) { |
| std::swap(num_rows, num_cols); |
| std::swap(rows, cols); |
| } |
| |
| // index is the list of indices into the TripletSparseMatrix input. |
| vector<int> index(input.num_nonzeros(), 0); |
| for (int i = 0; i < input.num_nonzeros(); ++i) { |
| index[i] = i; |
| } |
| |
| // Sort index such that the entries of m are ordered by row and ties |
| // are broken by column. |
| std::sort(index.begin(), index.end(), RowColLessThan(rows, cols)); |
| |
| VLOG(1) << "# of rows: " << num_rows << " # of columns: " << num_cols |
| << " num_nonzeros: " << input.num_nonzeros() << ". Allocating " |
| << ((num_rows + 1) * sizeof(int) + // NOLINT |
| input.num_nonzeros() * sizeof(int) + // NOLINT |
| input.num_nonzeros() * sizeof(double)); // NOLINT |
| |
| CompressedRowSparseMatrix* output = |
| new CompressedRowSparseMatrix(num_rows, num_cols, input.num_nonzeros()); |
| |
| // Copy the contents of the cols and values array in the order given |
| // by index and count the number of entries in each row. |
| int* output_rows = output->mutable_rows(); |
| int* output_cols = output->mutable_cols(); |
| double* output_values = output->mutable_values(); |
| |
| output_rows[0] = 0; |
| for (int i = 0; i < index.size(); ++i) { |
| const int idx = index[i]; |
| ++output_rows[rows[idx] + 1]; |
| output_cols[i] = cols[idx]; |
| output_values[i] = values[idx]; |
| } |
| |
| // Find the cumulative sum of the row counts. |
| for (int i = 1; i < num_rows + 1; ++i) { |
| output_rows[i] += output_rows[i - 1]; |
| } |
| |
| CHECK_EQ(output->num_nonzeros(), input.num_nonzeros()); |
| return output; |
| } |
| |
| CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, |
| int num_rows) { |
| CHECK_NOTNULL(diagonal); |
| |
| num_rows_ = num_rows; |
| num_cols_ = num_rows; |
| storage_type_ = UNSYMMETRIC; |
| rows_.resize(num_rows + 1); |
| cols_.resize(num_rows); |
| values_.resize(num_rows); |
| |
| rows_[0] = 0; |
| for (int i = 0; i < num_rows_; ++i) { |
| cols_[i] = i; |
| values_[i] = diagonal[i]; |
| rows_[i + 1] = i + 1; |
| } |
| |
| CHECK_EQ(num_nonzeros(), num_rows); |
| } |
| |
| CompressedRowSparseMatrix::~CompressedRowSparseMatrix() {} |
| |
| void CompressedRowSparseMatrix::SetZero() { |
| std::fill(values_.begin(), values_.end(), 0); |
| } |
| |
| void CompressedRowSparseMatrix::RightMultiply(const double* x, |
| double* y) const { |
| CHECK_NOTNULL(x); |
| CHECK_NOTNULL(y); |
| |
| for (int r = 0; r < num_rows_; ++r) { |
| for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { |
| y[r] += values_[idx] * x[cols_[idx]]; |
| } |
| } |
| } |
| |
| void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const { |
| CHECK_NOTNULL(x); |
| CHECK_NOTNULL(y); |
| |
| for (int r = 0; r < num_rows_; ++r) { |
| for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { |
| y[cols_[idx]] += values_[idx] * x[r]; |
| } |
| } |
| } |
| |
| void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const { |
| CHECK_NOTNULL(x); |
| |
| std::fill(x, x + num_cols_, 0.0); |
| for (int idx = 0; idx < rows_[num_rows_]; ++idx) { |
| x[cols_[idx]] += values_[idx] * values_[idx]; |
| } |
| } |
| |
| void CompressedRowSparseMatrix::ScaleColumns(const double* scale) { |
| CHECK_NOTNULL(scale); |
| |
| for (int idx = 0; idx < rows_[num_rows_]; ++idx) { |
| values_[idx] *= scale[cols_[idx]]; |
| } |
| } |
| |
| void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { |
| CHECK_NOTNULL(dense_matrix); |
| dense_matrix->resize(num_rows_, num_cols_); |
| dense_matrix->setZero(); |
| |
| for (int r = 0; r < num_rows_; ++r) { |
| for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { |
| (*dense_matrix)(r, cols_[idx]) = values_[idx]; |
| } |
| } |
| } |
| |
| void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { |
| CHECK_GE(delta_rows, 0); |
| CHECK_LE(delta_rows, num_rows_); |
| |
| num_rows_ -= delta_rows; |
| rows_.resize(num_rows_ + 1); |
| |
| // The rest of the code updates the block information. Immediately |
| // return in case of no block information. |
| if (row_blocks_.empty()) { |
| return; |
| } |
| |
| // Walk the list of row blocks until we reach the new number of rows |
| // and the drop the rest of the row blocks. |
| int num_row_blocks = 0; |
| int num_rows = 0; |
| while (num_row_blocks < row_blocks_.size() && num_rows < num_rows_) { |
| num_rows += row_blocks_[num_row_blocks]; |
| ++num_row_blocks; |
| } |
| |
| row_blocks_.resize(num_row_blocks); |
| } |
| |
| void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { |
| CHECK_EQ(m.num_cols(), num_cols_); |
| |
| CHECK((row_blocks_.empty() && m.row_blocks().empty()) || |
| (!row_blocks_.empty() && !m.row_blocks().empty())) |
| << "Cannot append a matrix with row blocks to one without and vice versa." |
| << "This matrix has : " << row_blocks_.size() << " row blocks." |
| << "The matrix being appended has: " << m.row_blocks().size() |
| << " row blocks."; |
| |
| if (m.num_rows() == 0) { |
| return; |
| } |
| |
| if (cols_.size() < num_nonzeros() + m.num_nonzeros()) { |
| cols_.resize(num_nonzeros() + m.num_nonzeros()); |
| values_.resize(num_nonzeros() + m.num_nonzeros()); |
| } |
| |
| // Copy the contents of m into this matrix. |
| DCHECK_LT(num_nonzeros(), cols_.size()); |
| if (m.num_nonzeros() > 0) { |
| std::copy(m.cols(), m.cols() + m.num_nonzeros(), &cols_[num_nonzeros()]); |
| std::copy( |
| m.values(), m.values() + m.num_nonzeros(), &values_[num_nonzeros()]); |
| } |
| |
| rows_.resize(num_rows_ + m.num_rows() + 1); |
| // new_rows = [rows_, m.row() + rows_[num_rows_]] |
| std::fill(rows_.begin() + num_rows_, |
| rows_.begin() + num_rows_ + m.num_rows() + 1, |
| rows_[num_rows_]); |
| |
| for (int r = 0; r < m.num_rows() + 1; ++r) { |
| rows_[num_rows_ + r] += m.rows()[r]; |
| } |
| |
| num_rows_ += m.num_rows(); |
| |
| // The rest of the code updates the block information. Immediately |
| // return in case of no block information. |
| if (row_blocks_.empty()) { |
| return; |
| } |
| |
| row_blocks_.insert( |
| row_blocks_.end(), m.row_blocks().begin(), m.row_blocks().end()); |
| } |
| |
| void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { |
| CHECK_NOTNULL(file); |
| for (int r = 0; r < num_rows_; ++r) { |
| for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { |
| fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]); |
| } |
| } |
| } |
| |
| void CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const { |
| matrix->num_rows = num_rows_; |
| matrix->num_cols = num_cols_; |
| matrix->rows = rows_; |
| matrix->cols = cols_; |
| matrix->values = values_; |
| |
| // Trim. |
| matrix->rows.resize(matrix->num_rows + 1); |
| matrix->cols.resize(matrix->rows[matrix->num_rows]); |
| matrix->values.resize(matrix->rows[matrix->num_rows]); |
| } |
| |
| void CompressedRowSparseMatrix::SetMaxNumNonZeros(int num_nonzeros) { |
| CHECK_GE(num_nonzeros, 0); |
| |
| cols_.resize(num_nonzeros); |
| values_.resize(num_nonzeros); |
| } |
| |
| CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( |
| const double* diagonal, const vector<int>& blocks) { |
| int num_rows = 0; |
| int num_nonzeros = 0; |
| for (int i = 0; i < blocks.size(); ++i) { |
| num_rows += blocks[i]; |
| num_nonzeros += blocks[i] * blocks[i]; |
| } |
| |
| CompressedRowSparseMatrix* matrix = |
| new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros); |
| |
| int* rows = matrix->mutable_rows(); |
| int* cols = matrix->mutable_cols(); |
| double* values = matrix->mutable_values(); |
| std::fill(values, values + num_nonzeros, 0.0); |
| |
| int idx_cursor = 0; |
| int col_cursor = 0; |
| for (int i = 0; i < blocks.size(); ++i) { |
| const int block_size = blocks[i]; |
| for (int r = 0; r < block_size; ++r) { |
| *(rows++) = idx_cursor; |
| values[idx_cursor + r] = diagonal[col_cursor + r]; |
| for (int c = 0; c < block_size; ++c, ++idx_cursor) { |
| *(cols++) = col_cursor + c; |
| } |
| } |
| col_cursor += block_size; |
| } |
| *rows = idx_cursor; |
| |
| *matrix->mutable_row_blocks() = blocks; |
| *matrix->mutable_col_blocks() = blocks; |
| |
| CHECK_EQ(idx_cursor, num_nonzeros); |
| CHECK_EQ(col_cursor, num_rows); |
| return matrix; |
| } |
| |
| CompressedRowSparseMatrix* CompressedRowSparseMatrix::Transpose() const { |
| CompressedRowSparseMatrix* transpose = |
| new CompressedRowSparseMatrix(num_cols_, num_rows_, num_nonzeros()); |
| |
| switch (storage_type_) { |
| case UNSYMMETRIC: |
| transpose->set_storage_type(UNSYMMETRIC); |
| break; |
| case LOWER_TRIANGULAR: |
| transpose->set_storage_type(UPPER_TRIANGULAR); |
| break; |
| case UPPER_TRIANGULAR: |
| transpose->set_storage_type(LOWER_TRIANGULAR); |
| break; |
| default: |
| LOG(FATAL) << "Unknown storage type: " << storage_type_; |
| }; |
| |
| TransposeForCompressedRowSparseStructure(num_rows(), |
| num_cols(), |
| num_nonzeros(), |
| rows(), |
| cols(), |
| values(), |
| transpose->mutable_rows(), |
| transpose->mutable_cols(), |
| transpose->mutable_values()); |
| |
| // The rest of the code updates the block information. Immediately |
| // return in case of no block information. |
| if (row_blocks_.empty()) { |
| return transpose; |
| } |
| |
| *(transpose->mutable_row_blocks()) = col_blocks_; |
| *(transpose->mutable_col_blocks()) = row_blocks_; |
| return transpose; |
| } |
| |
| CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateRandomMatrix( |
| const CompressedRowSparseMatrix::RandomMatrixOptions& options) { |
| CHECK_GT(options.num_row_blocks, 0); |
| CHECK_GT(options.min_row_block_size, 0); |
| CHECK_GT(options.max_row_block_size, 0); |
| CHECK_LE(options.min_row_block_size, options.max_row_block_size); |
| CHECK_GT(options.num_col_blocks, 0); |
| CHECK_GT(options.min_col_block_size, 0); |
| CHECK_GT(options.max_col_block_size, 0); |
| CHECK_LE(options.min_col_block_size, options.max_col_block_size); |
| CHECK_GT(options.block_density, 0.0); |
| CHECK_LE(options.block_density, 1.0); |
| |
| vector<int> row_blocks; |
| vector<int> col_blocks; |
| |
| // Generate the row block structure. |
| for (int i = 0; i < options.num_row_blocks; ++i) { |
| // Generate a random integer in [min_row_block_size, max_row_block_size] |
| const int delta_block_size = |
| Uniform(options.max_row_block_size - options.min_row_block_size); |
| row_blocks.push_back(options.min_row_block_size + delta_block_size); |
| } |
| |
| // Generate the col block structure. |
| for (int i = 0; i < options.num_col_blocks; ++i) { |
| // Generate a random integer in [min_col_block_size, max_col_block_size] |
| const int delta_block_size = |
| Uniform(options.max_col_block_size - options.min_col_block_size); |
| col_blocks.push_back(options.min_col_block_size + delta_block_size); |
| } |
| |
| vector<int> tsm_rows; |
| vector<int> tsm_cols; |
| vector<double> tsm_values; |
| |
| // For ease of construction, we are going to generate the |
| // CompressedRowSparseMatrix by generating it as a |
| // TripletSparseMatrix and then converting it to a |
| // CompressedRowSparseMatrix. |
| |
| // It is possible that the random matrix is empty which is likely |
| // not what the user wants, so do the matrix generation till we have |
| // at least one non-zero entry. |
| while (tsm_values.empty()) { |
| tsm_rows.clear(); |
| tsm_cols.clear(); |
| tsm_values.clear(); |
| |
| int row_block_begin = 0; |
| for (int r = 0; r < options.num_row_blocks; ++r) { |
| int col_block_begin = 0; |
| for (int c = 0; c < options.num_col_blocks; ++c) { |
| // Randomly determine if this block is present or not. |
| if (RandDouble() <= options.block_density) { |
| AddRandomBlock(row_blocks[r], |
| col_blocks[c], |
| row_block_begin, |
| col_block_begin, |
| &tsm_rows, |
| &tsm_cols, |
| &tsm_values); |
| } |
| col_block_begin += col_blocks[c]; |
| } |
| row_block_begin += row_blocks[r]; |
| } |
| } |
| |
| const int num_rows = std::accumulate(row_blocks.begin(), row_blocks.end(), 0); |
| const int num_cols = std::accumulate(col_blocks.begin(), col_blocks.end(), 0); |
| const bool kDoNotTranspose = false; |
| CompressedRowSparseMatrix* matrix = |
| CompressedRowSparseMatrix::FromTripletSparseMatrix( |
| TripletSparseMatrix( |
| num_rows, num_cols, tsm_rows, tsm_cols, tsm_values), |
| kDoNotTranspose); |
| (*matrix->mutable_row_blocks()) = row_blocks; |
| (*matrix->mutable_col_blocks()) = col_blocks; |
| matrix->set_storage_type(CompressedRowSparseMatrix::UNSYMMETRIC); |
| return matrix; |
| } |
| |
| } // namespace internal |
| } // namespace ceres |