| // 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/inner_product_computer.h" |
| |
| #include <algorithm> |
| #include <memory> |
| |
| #include "ceres/small_blas.h" |
| |
| namespace ceres::internal { |
| |
| // Create the CompressedRowSparseMatrix matrix that will contain the |
| // inner product. |
| // |
| // storage_type controls whether the result matrix contains the upper |
| // or the lower triangular part of the product. |
| // |
| // num_nonzeros is the number of non-zeros in the result matrix. |
| std::unique_ptr<CompressedRowSparseMatrix> |
| InnerProductComputer::CreateResultMatrix( |
| const CompressedRowSparseMatrix::StorageType storage_type, |
| const int num_nonzeros) { |
| auto matrix = std::make_unique<CompressedRowSparseMatrix>( |
| m_.num_cols(), m_.num_cols(), num_nonzeros); |
| matrix->set_storage_type(storage_type); |
| const CompressedRowBlockStructure* bs = m_.block_structure(); |
| *matrix->mutable_row_blocks() = bs->cols; |
| *matrix->mutable_col_blocks() = bs->cols; |
| return matrix; |
| } |
| |
| // Given the set of product terms in the inner product, return the |
| // total number of non-zeros in the result and for each row block of |
| // the result matrix, compute the number of non-zeros in any one row |
| // of the row block. |
| int InnerProductComputer::ComputeNonzeros( |
| const std::vector<InnerProductComputer::ProductTerm>& product_terms, |
| std::vector<int>* row_nnz) { |
| const CompressedRowBlockStructure* bs = m_.block_structure(); |
| const std::vector<Block>& blocks = bs->cols; |
| |
| row_nnz->resize(blocks.size()); |
| std::fill(row_nnz->begin(), row_nnz->end(), 0); |
| |
| if (product_terms.empty()) { |
| return 0; |
| } |
| |
| // First product term. |
| (*row_nnz)[product_terms[0].row] = blocks[product_terms[0].col].size; |
| int num_nonzeros = |
| blocks[product_terms[0].row].size * blocks[product_terms[0].col].size; |
| |
| // Remaining product terms. |
| for (int i = 1; i < product_terms.size(); ++i) { |
| const ProductTerm& previous = product_terms[i - 1]; |
| const ProductTerm& current = product_terms[i]; |
| |
| // Each (row, col) block counts only once. |
| // This check depends on product sorted on (row, col). |
| if (current.row != previous.row || current.col != previous.col) { |
| (*row_nnz)[current.row] += blocks[current.col].size; |
| num_nonzeros += blocks[current.row].size * blocks[current.col].size; |
| } |
| } |
| |
| return num_nonzeros; |
| } |
| |
| InnerProductComputer::InnerProductComputer(const BlockSparseMatrix& m, |
| const int start_row_block, |
| const int end_row_block) |
| : m_(m), start_row_block_(start_row_block), end_row_block_(end_row_block) {} |
| |
| // Compute the sparsity structure of the product m.transpose() * m |
| // and create a CompressedRowSparseMatrix corresponding to it. |
| // |
| // Also compute the "program" vector, which for every term in the |
| // block outer product provides the information for the entry in the |
| // values array of the result matrix where it should be accumulated. |
| // |
| // Since the entries of the program are the same for rows with the |
| // same sparsity structure, the program only stores the result for one |
| // row per row block. The Compute function reuses this information for |
| // each row in the row block. |
| // |
| // product_storage_type controls the form of the output matrix. It |
| // can be LOWER_TRIANGULAR or UPPER_TRIANGULAR. |
| std::unique_ptr<InnerProductComputer> InnerProductComputer::Create( |
| const BlockSparseMatrix& m, |
| CompressedRowSparseMatrix::StorageType product_storage_type) { |
| return InnerProductComputer::Create( |
| m, 0, m.block_structure()->rows.size(), product_storage_type); |
| } |
| |
| std::unique_ptr<InnerProductComputer> InnerProductComputer::Create( |
| const BlockSparseMatrix& m, |
| const int start_row_block, |
| const int end_row_block, |
| CompressedRowSparseMatrix::StorageType product_storage_type) { |
| CHECK(product_storage_type == |
| CompressedRowSparseMatrix::StorageType::LOWER_TRIANGULAR || |
| product_storage_type == |
| CompressedRowSparseMatrix::StorageType::UPPER_TRIANGULAR); |
| CHECK_GT(m.num_nonzeros(), 0) |
| << "Congratulations, you found a bug in Ceres. Please report it."; |
| std::unique_ptr<InnerProductComputer> inner_product_computer( |
| new InnerProductComputer(m, start_row_block, end_row_block)); |
| inner_product_computer->Init(product_storage_type); |
| return inner_product_computer; |
| } |
| |
| void InnerProductComputer::Init( |
| const CompressedRowSparseMatrix::StorageType product_storage_type) { |
| std::vector<InnerProductComputer::ProductTerm> product_terms; |
| const CompressedRowBlockStructure* bs = m_.block_structure(); |
| |
| // Give input matrix m in Block Sparse format |
| // (row_block, col_block) |
| // represent each block multiplication |
| // (row_block, col_block1)' X (row_block, col_block2) |
| // by its product term: |
| // (col_block1, col_block2, index) |
| for (int row_block = start_row_block_; row_block < end_row_block_; |
| ++row_block) { |
| const CompressedRow& row = bs->rows[row_block]; |
| for (int c1 = 0; c1 < row.cells.size(); ++c1) { |
| const Cell& cell1 = row.cells[c1]; |
| int c2_begin, c2_end; |
| if (product_storage_type == |
| CompressedRowSparseMatrix::StorageType::LOWER_TRIANGULAR) { |
| c2_begin = 0; |
| c2_end = c1 + 1; |
| } else { |
| c2_begin = c1; |
| c2_end = row.cells.size(); |
| } |
| |
| for (int c2 = c2_begin; c2 < c2_end; ++c2) { |
| const Cell& cell2 = row.cells[c2]; |
| product_terms.emplace_back( |
| cell1.block_id, cell2.block_id, product_terms.size()); |
| } |
| } |
| } |
| |
| std::sort(product_terms.begin(), product_terms.end()); |
| ComputeOffsetsAndCreateResultMatrix(product_storage_type, product_terms); |
| } |
| |
| void InnerProductComputer::ComputeOffsetsAndCreateResultMatrix( |
| const CompressedRowSparseMatrix::StorageType product_storage_type, |
| const std::vector<InnerProductComputer::ProductTerm>& product_terms) { |
| const std::vector<Block>& col_blocks = m_.block_structure()->cols; |
| |
| std::vector<int> row_block_nnz; |
| const int num_nonzeros = ComputeNonzeros(product_terms, &row_block_nnz); |
| |
| result_ = CreateResultMatrix(product_storage_type, num_nonzeros); |
| |
| // Populate the row non-zero counts in the result matrix. |
| int* crsm_rows = result_->mutable_rows(); |
| crsm_rows[0] = 0; |
| for (int i = 0; i < col_blocks.size(); ++i) { |
| for (int j = 0; j < col_blocks[i].size; ++j, ++crsm_rows) { |
| *(crsm_rows + 1) = *crsm_rows + row_block_nnz[i]; |
| } |
| } |
| result_offsets_.resize(product_terms.size()); |
| if (num_nonzeros == 0) { |
| return; |
| } |
| |
| // The following macro FILL_CRSM_COL_BLOCK is key to understanding |
| // how this class works. |
| // |
| // It does two things. |
| // |
| // Sets the value for the current term in the result_offsets_ array |
| // and populates the cols array of the result matrix. |
| // |
| // row_block and col_block as the names imply, refer to the row and |
| // column blocks of the current term. |
| // |
| // row_nnz is the number of nonzeros in the result_matrix at the |
| // beginning of the first row of row_block. |
| // |
| // col_nnz is the number of nonzeros in the first row of the row |
| // block that occur before the current column block, i.e. this is |
| // sum of the sizes of all the column blocks in this row block that |
| // came before this column block. |
| // |
| // Given these two numbers and the total number of nonzeros in this |
| // row (nnz_in_row), we can now populate the cols array as follows: |
| // |
| // nnz + j * nnz_in_row is the beginning of the j^th row. |
| // |
| // nnz + j * nnz_in_row + col_nnz is the beginning of the column |
| // block in the j^th row. |
| // |
| // nnz + j * nnz_in_row + col_nnz + k is then the j^th row and the |
| // k^th column of the product block, whose value is |
| // |
| // col_blocks[col_block].position + k, which is the column number of |
| // the k^th column of the current column block. |
| #define FILL_CRSM_COL_BLOCK \ |
| const int row_block = current->row; \ |
| const int col_block = current->col; \ |
| const int nnz_in_row = row_block_nnz[row_block]; \ |
| int* crsm_cols = result_->mutable_cols(); \ |
| result_offsets_[current->index] = nnz + col_nnz; \ |
| for (int j = 0; j < col_blocks[row_block].size; ++j) { \ |
| for (int k = 0; k < col_blocks[col_block].size; ++k) { \ |
| crsm_cols[nnz + j * nnz_in_row + col_nnz + k] = \ |
| col_blocks[col_block].position + k; \ |
| } \ |
| } |
| |
| int col_nnz = 0; |
| int nnz = 0; |
| |
| // Process the first term. |
| const InnerProductComputer::ProductTerm* current = product_terms.data(); |
| FILL_CRSM_COL_BLOCK; |
| |
| // Process the rest of the terms. |
| for (int i = 1; i < product_terms.size(); ++i) { |
| current = &product_terms[i]; |
| const InnerProductComputer::ProductTerm* previous = &product_terms[i - 1]; |
| |
| // If the current term is the same as the previous term, then it |
| // stores its product at the same location as the previous term. |
| if (previous->row == current->row && previous->col == current->col) { |
| result_offsets_[current->index] = result_offsets_[previous->index]; |
| continue; |
| } |
| |
| if (previous->row == current->row) { |
| // if the current and previous terms are in the same row block, |
| // then they differ in the column block, in which case advance |
| // col_nnz by the column size of the previous term. |
| col_nnz += col_blocks[previous->col].size; |
| } else { |
| // If we have moved to a new row-block , then col_nnz is zero, |
| // and nnz is set to the beginning of the row block. |
| col_nnz = 0; |
| nnz += row_block_nnz[previous->row] * col_blocks[previous->row].size; |
| } |
| |
| FILL_CRSM_COL_BLOCK; |
| } |
| } |
| |
| // Use the results_offsets_ array to numerically compute the product |
| // m' * m and store it in result_. |
| // |
| // TODO(sameeragarwal): Multithreading support. |
| void InnerProductComputer::Compute() { |
| const double* m_values = m_.values(); |
| const CompressedRowBlockStructure* bs = m_.block_structure(); |
| |
| const CompressedRowSparseMatrix::StorageType storage_type = |
| result_->storage_type(); |
| result_->SetZero(); |
| double* values = result_->mutable_values(); |
| const int* rows = result_->rows(); |
| int cursor = 0; |
| |
| // Iterate row blocks. |
| for (int r = start_row_block_; r < end_row_block_; ++r) { |
| const CompressedRow& m_row = bs->rows[r]; |
| for (int c1 = 0; c1 < m_row.cells.size(); ++c1) { |
| const Cell& cell1 = m_row.cells[c1]; |
| const int c1_size = bs->cols[cell1.block_id].size; |
| const int row_nnz = rows[bs->cols[cell1.block_id].position + 1] - |
| rows[bs->cols[cell1.block_id].position]; |
| |
| int c2_begin, c2_end; |
| if (storage_type == |
| CompressedRowSparseMatrix::StorageType::LOWER_TRIANGULAR) { |
| c2_begin = 0; |
| c2_end = c1 + 1; |
| } else { |
| c2_begin = c1; |
| c2_end = m_row.cells.size(); |
| } |
| |
| for (int c2 = c2_begin; c2 < c2_end; ++c2, ++cursor) { |
| const Cell& cell2 = m_row.cells[c2]; |
| const int c2_size = bs->cols[cell2.block_id].size; |
| // clang-format off |
| MatrixTransposeMatrixMultiply<Eigen::Dynamic, Eigen::Dynamic, |
| Eigen::Dynamic, Eigen::Dynamic, 1>( |
| m_values + cell1.position, |
| m_row.block.size, c1_size, |
| m_values + cell2.position, |
| m_row.block.size, c2_size, |
| values + result_offsets_[cursor], |
| 0, 0, c1_size, row_nnz); |
| // clang-format on |
| } |
| } |
| } |
| |
| CHECK_EQ(cursor, result_offsets_.size()); |
| } |
| |
| } // namespace ceres::internal |