Fix a reallocation bug in CreateJacobianBlockSparsityTranspose.

CreateJacobianBlockSparsityTranspose starts with a conservative
estimate of the size of the block sparsity pattern of the Jacobian.
When the Jacobian has more non-zeros than that, the TripletSparseMatrix
being used to store the sparsity has a Reallocate method which
allows one to resize the matrix and IF num_nonzeros is set, then the
existing values in the array are also copied into the newly allocated
memory.

Unfortunately the pattern we follow in ceres code is to call
set_num_nonzeros after one is done populating the sparsity pattern
of a matrix. This does not mix well with Reallocate and results
in the matrix having uninitialized memory.

This patch fixes this problem and adds a test that verifies the fix.

Thanks to Yuliy Schwartzburg for reporting this bug and providing
code to reproduce it.

Change-Id: I58583714ffaebd880d85af16e3685b2d6ee053e8

internal/ceres/solver_impl.cc

@@ -1418,6 +1418,7 @@ TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose(
 
       // Re-size the matrix if needed.
       if (num_nonzeros >= tsm->max_num_nonzeros()) {
+        tsm->set_num_nonzeros(num_nonzeros);
         tsm->Reserve(2 * num_nonzeros);
         rows = tsm->mutable_rows();
         cols = tsm->mutable_cols();

internal/ceres/solver_impl_test.cc

@@ -924,5 +924,72 @@ TEST(SolverImpl, CreateJacobianBlockSparsityTranspose) {
   EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
 }
 
+template <int kNumResiduals, int kNumParameterBlocks>
+class NumParameterBlocksCostFunction : public CostFunction {
+ public:
+  NumParameterBlocksCostFunction() {
+    set_num_residuals(kNumResiduals);
+    for (int i = 0; i < kNumParameterBlocks; ++i) {
+      mutable_parameter_block_sizes()->push_back(1);
+    }
+  }
+
+  virtual ~NumParameterBlocksCostFunction() {
+  }
+
+  virtual bool Evaluate(double const* const* parameters,
+                        double* residuals,
+                        double** jacobians) const {
+    return true;
+  }
+};
+
+TEST(SolverImpl, ReallocationInCreateJacobianBlockSparsityTranspose) {
+  // CreateJacobianBlockSparsityTranspose starts with a conservative
+  // estimate of the size of the sparsity pattern. This test ensures
+  // that when those estimates are violated, the reallocation/resizing
+  // logic works correctly.
+
+  ProblemImpl problem;
+  double x[20];
+
+  vector<double*> parameter_blocks;
+  for (int i = 0; i < 20; ++i) {
+    problem.AddParameterBlock(x + i, 1);
+    parameter_blocks.push_back(x + i);
+  }
+
+  problem.AddResidualBlock(new NumParameterBlocksCostFunction<1, 20>(),
+                           NULL,
+                           parameter_blocks);
+
+  TripletSparseMatrix expected_block_sparse_jacobian(20, 1, 20);
+  {
+    int* rows = expected_block_sparse_jacobian.mutable_rows();
+    int* cols = expected_block_sparse_jacobian.mutable_cols();
+    for (int i = 0; i < 20; ++i) {
+      rows[i] = i;
+      cols[i] = 0;
+    }
+
+    double* values = expected_block_sparse_jacobian.mutable_values();
+    fill(values, values + 20, 1.0);
+    expected_block_sparse_jacobian.set_num_nonzeros(20);
+  }
+
+  Program* program = problem.mutable_program();
+  program->SetParameterOffsetsAndIndex();
+
+  scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
+      SolverImpl::CreateJacobianBlockSparsityTranspose(program));
+
+  Matrix expected_dense_jacobian;
+  expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
+
+  Matrix actual_dense_jacobian;
+  actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
+  EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
+}
+
 }  // namespace internal
 }  // namespace ceres