Change storage in BlockRandomAccessSparseMatrix

- TripletSparseMatrix in BlockRandomAccessSparseMatrix is replaced with
   BlockSparseMatrix
 - BlockSparseMatrix::ToCompressedRowSparseMatrix is performed in a
   direct sort-less way

Change-Id: Ib951fda1b9394050e2c47a9721172c5e3c674801

internal/ceres/block_random_access_sparse_matrix.cc

@@ -53,53 +53,46 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix(
   CHECK_LE(blocks.size(), std::numeric_limits<std::int32_t>::max());
 
   const int num_cols = NumScalarEntries(blocks);
+  const int num_blocks = blocks.size();
 
-  // Count the number of scalar non-zero entries and build the layout
-  // object for looking into the values array of the
-  // TripletSparseMatrix.
-  int num_nonzeros = 0;
-  for (const auto& block_pair : block_pairs) {
-    const int row_block_size = blocks_[block_pair.first].size;
-    const int col_block_size = blocks_[block_pair.second].size;
-    num_nonzeros += row_block_size * col_block_size;
+  std::vector<int> num_cells_at_row(num_blocks);
+  for (auto& p : block_pairs) {
+    ++num_cells_at_row[p.first];
   }
-
+  auto block_structure_ = new CompressedRowBlockStructure;
+  block_structure_->cols = blocks;
+  block_structure_->rows.resize(num_blocks);
+  auto p = block_pairs.begin();
+  int num_nonzeros = 0;
+  // Pairs of block indices are sorted lexicographically, thus pairs
+  // corresponding to a single row-block are stored in segments of index pairs
+  // with constant row-block index and increasing column-block index.
+  // CompressedRowBlockStructure is created by traversing block_pairs set.
+  for (int row_block_id = 0; row_block_id < num_blocks; ++row_block_id) {
+    auto& row = block_structure_->rows[row_block_id];
+    row.block = blocks[row_block_id];
+    row.cells.reserve(num_cells_at_row[row_block_id]);
+    const int row_block_size = blocks[row_block_id].size;
+    // Process all index pairs corresponding to the current row block. Because
+    // index pairs are sorted lexicographically, cells are being appended to the
+    // current row-block till the first change in row-block index
+    for (; p != block_pairs.end() && row_block_id == p->first; ++p) {
+      const int col_block_id = p->second;
+      row.cells.emplace_back(col_block_id, num_nonzeros);
+      num_nonzeros += row_block_size * blocks[col_block_id].size;
+    }
+  }
+  bsm_ = std::make_unique<BlockSparseMatrix>(block_structure_);
   VLOG(1) << "Matrix Size [" << num_cols << "," << num_cols << "] "
           << num_nonzeros;
-
-  tsm_ =
-      std::make_unique<TripletSparseMatrix>(num_cols, num_cols, num_nonzeros);
-  tsm_->set_num_nonzeros(num_nonzeros);
-  int* rows = tsm_->mutable_rows();
-  int* cols = tsm_->mutable_cols();
-  double* values = tsm_->mutable_values();
-
-  int pos = 0;
-  for (const auto& block_pair : block_pairs) {
-    const int row_block_size = blocks_[block_pair.first].size;
-    const int col_block_size = blocks_[block_pair.second].size;
-    cell_values_.emplace_back(block_pair, values + pos);
-    layout_[IntPairToInt64(block_pair.first, block_pair.second)] =
-        std::make_unique<CellInfo>(values + pos);
-    pos += row_block_size * col_block_size;
-  }
-
-  // Fill the sparsity pattern of the underlying matrix.
-  for (const auto& block_pair : block_pairs) {
-    const int row_block_id = block_pair.first;
-    const int col_block_id = block_pair.second;
-    const int row_block_size = blocks_[row_block_id].size;
-    const int col_block_size = blocks_[col_block_id].size;
-    int pos =
-        layout_[IntPairToInt64(row_block_id, col_block_id)]->values - values;
-    for (int r = 0; r < row_block_size; ++r) {
-      for (int c = 0; c < col_block_size; ++c, ++pos) {
-        rows[pos] = blocks_[row_block_id].position + r;
-        cols[pos] = blocks_[col_block_id].position + c;
-        values[pos] = 1.0;
-        DCHECK_LT(rows[pos], tsm_->num_rows());
-        DCHECK_LT(cols[pos], tsm_->num_rows());
-      }
+  double* values = bsm_->mutable_values();
+  for (int row_block_id = 0; row_block_id < num_blocks; ++row_block_id) {
+    const auto& cells = block_structure_->rows[row_block_id].cells;
+    for (auto& c : cells) {
+      const int col_block_id = c.block_id;
+      double* const data = values + c.position;
+      layout_[IntPairToInt64(row_block_id, col_block_id)] =
+          std::make_unique<CellInfo>(data);
     }
   }
 }
@@ -126,35 +119,41 @@ CellInfo* BlockRandomAccessSparseMatrix::GetCell(int row_block_id,
 // Assume that the user does not hold any locks on any cell blocks
 // when they are calling SetZero.
 void BlockRandomAccessSparseMatrix::SetZero() {
-  ParallelSetZero(
-      context_, num_threads_, tsm_->mutable_values(), tsm_->num_nonzeros());
+  bsm_->SetZero(context_, num_threads_);
 }
 
 void BlockRandomAccessSparseMatrix::SymmetricRightMultiplyAndAccumulate(
     const double* x, double* y) const {
-  for (const auto& cell_position_and_data : cell_values_) {
-    const int row = cell_position_and_data.first.first;
-    const int row_block_size = blocks_[row].size;
-    const int row_block_pos = blocks_[row].position;
+  const auto bs = bsm_->block_structure();
+  const auto values = bsm_->values();
+  const int num_blocks = blocks_.size();
 
-    const int col = cell_position_and_data.first.second;
-    const int col_block_size = blocks_[col].size;
-    const int col_block_pos = blocks_[col].position;
+  for (int row_block_id = 0; row_block_id < num_blocks; ++row_block_id) {
+    const auto& row_block = bs->rows[row_block_id];
+    const int row_block_size = row_block.block.size;
+    const int row_block_pos = row_block.block.position;
 
-    MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
-        cell_position_and_data.second,
-        row_block_size,
-        col_block_size,
-        x + col_block_pos,
-        y + row_block_pos);
+    for (auto& c : row_block.cells) {
+      const int col_block_id = c.block_id;
+      const int col_block_size = blocks_[col_block_id].size;
+      const int col_block_pos = blocks_[col_block_id].position;
 
-    // Since the matrix is symmetric, but only the upper triangular
-    // part is stored, if the block being accessed is not a diagonal
-    // block, then use the same block to do the corresponding lower
-    // triangular multiply also.
-    if (row != col) {
+      MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
+          values + c.position,
+          row_block_size,
+          col_block_size,
+          x + col_block_pos,
+          y + row_block_pos);
+      if (col_block_id == row_block_id) {
+        continue;
+      }
+
+      // Since the matrix is symmetric, but only the upper triangular
+      // part is stored, if the block being accessed is not a diagonal
+      // block, then use the same block to do the corresponding lower
+      // triangular multiply also
       MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
-          cell_position_and_data.second,
+          values + c.position,
           row_block_size,
           col_block_size,
           x + row_block_pos,