Explicitly compute number of non-zeros in row

Change-Id: Iaebaf6d23d33dbbbe5a7c7240319bf28cf2bdd3a

internal/ceres/block_sparse_matrix.cc

diff --git a/internal/ceres/block_sparse_matrix.cc b/internal/ceres/block_sparse_matrix.cc
index 6feedc8..b3d4efd 100644
--- a/internal/ceres/block_sparse_matrix.cc
+++ b/internal/ceres/block_sparse_matrix.cc
@@ -85,23 +85,18 @@
       continue;
     }
 
-    // Compute number of non-zeros per row from number of non-zeros in row-block
-    int row_block_nnz;
+    int row_nnz = 0;
     if constexpr (transpose) {
-      // Transposed block structure comes with nnz filled-in
-      row_block_nnz = row_block.nnz;
+      // Transposed block structure comes with nnz in row-block filled-in
+      row_nnz = row_block.nnz / row_block.block.size;
     } else {
-      // Non-transposed block structure has sequential layout, but nnz field of
-      // block structure is not filled. Difference between values position of
-      // the first and the last cells in row-block gives number of non-zero
-      // elements in row-block excluding the last cell.
-      const auto& front = row_block.cells.front();
-      const auto& back = row_block.cells.back();
-      row_block_nnz =
-          (back.position - front.position) +
-          block_structure->cols[back.block_id].size * row_block.block.size;
+      // Nnz field of non-transposed block structure is not filled and it can
+      // have non-sequential structure (consider the case of jacobian for
+      // Schur-complement solver: E and F blocks are stored separately).
+      for (auto& c : row_block.cells) {
+        row_nnz += cols[c.block_id].size;
+      }
     }
-    const int row_nnz = row_block_nnz / row_block.block.size;
 
     // Row-wise setup of matrix structure
     for (int row = 0; row < row_block.block.size; ++row) {

internal/ceres/block_sparse_matrix_test.cc

diff --git a/internal/ceres/block_sparse_matrix_test.cc b/internal/ceres/block_sparse_matrix_test.cc
index f749df3..daa2e2d 100644
--- a/internal/ceres/block_sparse_matrix_test.cc
+++ b/internal/ceres/block_sparse_matrix_test.cc
@@ -113,6 +113,39 @@
       values[i] = i + 1;
     }
     return m;
+  } else if (id == 2) {
+    // Create the following block sparse matrix:
+    // [ 1 2 0 | 6 7 0 ]
+    // [ 3 4 0 | 8 9 0 ]
+    // [ 0 0 5 | 0 0 10]
+    // With cells of the left submatrix preceding cells of the right submatrix
+    CompressedRowBlockStructure* bs = new CompressedRowBlockStructure;
+    bs->cols = {
+        // Block size 2, position 0.
+        Block(2, 0),
+        // Block size 1, position 2.
+        Block(1, 2),
+        // Block size 2, position 3.
+        Block(2, 3),
+        // Block size 1, position 5.
+        Block(1, 5),
+    };
+    bs->rows = {CompressedRow(2), CompressedRow(1)};
+    bs->rows[0].block = Block(2, 0);
+    bs->rows[0].cells = {Cell(0, 0), Cell(2, 5)};
+
+    bs->rows[1].block = Block(1, 2);
+    bs->rows[1].cells = {Cell(1, 4), Cell(3, 9)};
+    auto m = std::make_unique<BlockSparseMatrix>(bs);
+    EXPECT_NE(m, nullptr);
+    EXPECT_EQ(m->num_rows(), 3);
+    EXPECT_EQ(m->num_cols(), 6);
+    EXPECT_EQ(m->num_nonzeros(), 10);
+    double* values = m->mutable_values();
+    for (int i = 0; i < 10; ++i) {
+      values[i] = i + 1;
+    }
+    return m;
   }
   return nullptr;
 }
@@ -477,6 +510,17 @@
     m_expected << 1, 2, 0, 5, 6, 0, 3, 4, 0, 7, 8, 0, 0, 0, 9, 0, 0, 0;
     EXPECT_EQ(m_dense, m_expected);
   }
+
+  {
+    std::unique_ptr<BlockSparseMatrix> m = CreateTestMatrixFromId(2);
+    Matrix m_dense;
+    m->ToDenseMatrix(&m_dense);
+    EXPECT_EQ(m_dense.rows(), 3);
+    EXPECT_EQ(m_dense.cols(), 6);
+    Matrix m_expected(3, 6);
+    m_expected << 1, 2, 0, 6, 7, 0, 3, 4, 0, 8, 9, 0, 0, 0, 5, 0, 0, 10;
+    EXPECT_EQ(m_dense, m_expected);
+  }
 }
 
 TEST(BlockSparseMatrix, ToCRSMatrix) {
@@ -512,6 +556,22 @@
       EXPECT_EQ(m_crs->values()[i], values_expected[i]);
     }
   }
+  {
+    std::unique_ptr<BlockSparseMatrix> m = CreateTestMatrixFromId(2);
+    auto m_crs = m->ToCompressedRowSparseMatrix();
+    std::vector<int> rows_expected = {0, 4, 8, 10};
+    std::vector<int> cols_expected = {0, 1, 3, 4, 0, 1, 3, 4, 2, 5};
+    std::vector<double> values_expected = {1, 2, 6, 7, 3, 4, 8, 9, 5, 10};
+    for (int i = 0; i < rows_expected.size(); ++i) {
+      EXPECT_EQ(m_crs->rows()[i], rows_expected[i]);
+    }
+    for (int i = 0; i < cols_expected.size(); ++i) {
+      EXPECT_EQ(m_crs->cols()[i], cols_expected[i]);
+    }
+    for (int i = 0; i < values_expected.size(); ++i) {
+      EXPECT_EQ(m_crs->values()[i], values_expected[i]);
+    }
+  }
 }
 
 TEST(BlockSparseMatrix, ToCRSMatrixTranspose) {
@@ -551,6 +611,24 @@
       EXPECT_EQ(m_crs_transpose->values()[i], values_expected[i]);
     }
   }
+  {
+    std::unique_ptr<BlockSparseMatrix> m = CreateTestMatrixFromId(2);
+    auto m_crs_transpose = m->ToCompressedRowSparseMatrixTranspose();
+    std::vector<int> rows_expected = {0, 2, 4, 5, 7, 9, 10};
+    std::vector<int> cols_expected = {0, 1, 0, 1, 2, 0, 1, 0, 1, 2};
+    std::vector<double> values_expected = {1, 3, 2, 4, 5, 6, 8, 7, 9, 10};
+    EXPECT_EQ(m_crs_transpose->num_nonzeros(), cols_expected.size());
+    EXPECT_EQ(m_crs_transpose->num_rows(), rows_expected.size() - 1);
+    for (int i = 0; i < rows_expected.size(); ++i) {
+      EXPECT_EQ(m_crs_transpose->rows()[i], rows_expected[i]);
+    }
+    for (int i = 0; i < cols_expected.size(); ++i) {
+      EXPECT_EQ(m_crs_transpose->cols()[i], cols_expected[i]);
+    }
+    for (int i = 0; i < values_expected.size(); ++i) {
+      EXPECT_EQ(m_crs_transpose->values()[i], values_expected[i]);
+    }
+  }
 }
 
 TEST(BlockSparseMatrix, CreateTranspose) {