Fix a bug in the Schur eliminator The schur eliminator treats rows with e blocks and row with no e blocks separately. The template specialization logic only applies to the rows with e blocks. So, in cases where the rows with e-blocks have a fixed size f-block but the rows without e-blocks have f-blocks of varying sizes, DetectStructure will return a static f-block size, but we need to be careful that we do not blindly use that static f-block size everywhere. This patch fixes a bug where such care was not being taken, where it was assumed that the static f-block size could be assumed for all f-block sizes. A new test is added, which triggers an exception in debug mode. In release mode this error does not present itself, due to a peculiarity of the way Eigen works. Thanks to Werner Trobin for reporting this bug. Change-Id: I8ae7aabf8eed8c3f9cf74b6c74d632ba44f82581
diff --git a/internal/ceres/detect_structure.cc b/internal/ceres/detect_structure.cc index 6bb7bb4..a87f6ba6 100644 --- a/internal/ceres/detect_structure.cc +++ b/internal/ceres/detect_structure.cc
@@ -54,7 +54,6 @@ if (row.cells.front().block_id >= num_eliminate_blocks) { break; } - const int e_block_id = row.cells.front().block_id; if (*row_block_size == 0) { *row_block_size = row.block.size; @@ -66,7 +65,7 @@ *row_block_size = Eigen::Dynamic; } - + const int e_block_id = row.cells.front().block_id; if (*e_block_size == 0) { *e_block_size = bs.cols[e_block_id].size; } else if (*e_block_size != Eigen::Dynamic &&
diff --git a/internal/ceres/detect_structure.h b/internal/ceres/detect_structure.h index fb8bbb4..602581c 100644 --- a/internal/ceres/detect_structure.h +++ b/internal/ceres/detect_structure.h
@@ -51,6 +51,10 @@ // For more details about e_blocks and f_blocks, see // schur_eliminator.h. This information is used to initialized an // appropriate template specialization of SchurEliminator. +// +// Note: The structure of rows without any e-blocks has no effect on +// the values returned by this function. It is entirely possible that +// the f_block_size and row_blocks_size is not constant in such rows. void DetectStructure(const CompressedRowBlockStructure& bs, const int num_eliminate_blocks, int* row_block_size,
diff --git a/internal/ceres/linear_least_squares_problems.cc b/internal/ceres/linear_least_squares_problems.cc index 15da333..0a69375 100644 --- a/internal/ceres/linear_least_squares_problems.cc +++ b/internal/ceres/linear_least_squares_problems.cc
@@ -58,6 +58,8 @@ return LinearLeastSquaresProblem2(); case 3: return LinearLeastSquaresProblem3(); + case 4: + return LinearLeastSquaresProblem4(); default: LOG(FATAL) << "Unknown problem id requested " << id; } @@ -505,6 +507,106 @@ return problem; } +/* + A = [1 2 0 0 0 1 1 + 1 4 0 0 0 5 6 + 0 0 9 0 0 3 1] + + b = [0 + 1 + 2] +*/ +// BlockSparseMatrix version +// +// This problem has the unique property that it has two different +// sized f-blocks, but only one of them occurs in the rows involving +// the one e-block. So performing Schur elimination on this problem +// tests the Schur Eliminator's ability to handle non-e-block rows +// correctly when their structure does not conform to the static +// structure determined by DetectStructure. +// +// NOTE: This problem is too small and rank deficient to be solved without +// the diagonal regularization. +LinearLeastSquaresProblem* LinearLeastSquaresProblem4() { + int num_rows = 3; + int num_cols = 7; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 1; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + // Column block structure + bs->cols.push_back(Block()); + bs->cols.back().size = 2; + bs->cols.back().position = 0; + + bs->cols.push_back(Block()); + bs->cols.back().size = 3; + bs->cols.back().position = 2; + + bs->cols.push_back(Block()); + bs->cols.back().size = 2; + bs->cols.back().position = 5; + + int nnz = 0; + + // Row 1 & 2 + { + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 2; + row.block.position = 0; + + row.cells.push_back(Cell(0, nnz)); + values[nnz++] = 1; + values[nnz++] = 2; + values[nnz++] = 1; + values[nnz++] = 4; + + row.cells.push_back(Cell(2, nnz)); + values[nnz++] = 1; + values[nnz++] = 1; + values[nnz++] = 5; + values[nnz++] = 6; + } + + // Row 3 + { + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + + row.cells.push_back(Cell(1, nnz)); + values[nnz++] = 9; + values[nnz++] = 0; + values[nnz++] = 0; + + row.cells.push_back(Cell(2, nnz)); + values[nnz++] = 3; + values[nnz++] = 1; + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = (i + 1) * 100; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + return problem; +} + namespace { bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A, const double* D,
diff --git a/internal/ceres/linear_least_squares_problems.h b/internal/ceres/linear_least_squares_problems.h index 1dccc90..384efb5 100644 --- a/internal/ceres/linear_least_squares_problems.h +++ b/internal/ceres/linear_least_squares_problems.h
@@ -68,6 +68,7 @@ LinearLeastSquaresProblem* LinearLeastSquaresProblem1(); LinearLeastSquaresProblem* LinearLeastSquaresProblem2(); LinearLeastSquaresProblem* LinearLeastSquaresProblem3(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem4(); // Write the linear least squares problem to disk. The exact format // depends on dump_format_type.
diff --git a/internal/ceres/schur_complement_solver_test.cc b/internal/ceres/schur_complement_solver_test.cc index 4473613..cf9b09b 100644 --- a/internal/ceres/schur_complement_solver_test.cc +++ b/internal/ceres/schur_complement_solver_test.cc
@@ -32,6 +32,7 @@ #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" #include "ceres/casts.h" +#include "ceres/detect_structure.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/linear_least_squares_problems.h" #include "ceres/linear_solver.h" @@ -59,9 +60,9 @@ num_rows = A->num_rows(); num_eliminate_blocks = problem->num_eliminate_blocks; - x.reset(new double[num_cols]); - sol.reset(new double[num_cols]); - sol_d.reset(new double[num_cols]); + x.resize(num_cols); + sol.resize(num_cols); + sol_d.resize(num_cols); LinearSolver::Options options; options.type = DENSE_QR; @@ -75,12 +76,12 @@ // Gold standard solutions using dense QR factorization. DenseSparseMatrix dense_A(triplet_A); - qr->Solve(&dense_A, b.get(), LinearSolver::PerSolveOptions(), sol.get()); + qr->Solve(&dense_A, b.get(), LinearSolver::PerSolveOptions(), sol.data()); // Gold standard solution with appended diagonal. LinearSolver::PerSolveOptions per_solve_options; per_solve_options.D = D.get(); - qr->Solve(&dense_A, b.get(), per_solve_options, sol_d.get()); + qr->Solve(&dense_A, b.get(), per_solve_options, sol_d.data()); } void ComputeAndCompareSolutions( @@ -101,6 +102,11 @@ options.sparse_linear_algebra_library_type = sparse_linear_algebra_library_type; options.use_postordering = use_postordering; + DetectStructure(*A->block_structure(), + num_eliminate_blocks, + &options.row_block_size, + &options.e_block_size, + &options.f_block_size); scoped_ptr<LinearSolver> solver(LinearSolver::Create(options)); @@ -110,16 +116,17 @@ per_solve_options.D = D.get(); } - summary = solver->Solve(A.get(), b.get(), per_solve_options, x.get()); + summary = solver->Solve(A.get(), b.get(), per_solve_options, x.data()); + EXPECT_EQ(summary.termination_type, LINEAR_SOLVER_SUCCESS); if (regularization) { - for (int i = 0; i < num_cols; ++i) { - ASSERT_NEAR(sol_d.get()[i], x[i], 1e-10); - } + ASSERT_NEAR((sol_d - x).norm() / num_cols, 0, 1e-10) + << "Expected solution: " << sol_d.transpose() + << " Actual solution: " << x.transpose(); } else { - for (int i = 0; i < num_cols; ++i) { - ASSERT_NEAR(sol.get()[i], x[i], 1e-10); - } + ASSERT_NEAR((sol - x).norm() / num_cols, 0, 1e-10) + << "Expected solution: " << sol.transpose() + << " Actual solution: " << x.transpose(); } } @@ -129,10 +136,10 @@ scoped_ptr<BlockSparseMatrix> A; scoped_array<double> b; - scoped_array<double> x; scoped_array<double> D; - scoped_array<double> sol; - scoped_array<double> sol_d; + Vector x; + Vector sol; + Vector sol_d; }; TEST_F(SchurComplementSolverTest, EigenBasedDenseSchurWithSmallProblem) { @@ -145,6 +152,10 @@ ComputeAndCompareSolutions(3, true, DENSE_SCHUR, EIGEN, SUITE_SPARSE, true); } +TEST_F(SchurComplementSolverTest, EigenBasedDenseSchurWithVaryingFBlockSize) { + ComputeAndCompareSolutions(4, true, DENSE_SCHUR, EIGEN, SUITE_SPARSE, true); +} + #ifndef CERES_NO_LAPACK TEST_F(SchurComplementSolverTest, LAPACKBasedDenseSchurWithSmallProblem) { ComputeAndCompareSolutions(2, false, DENSE_SCHUR, LAPACK, SUITE_SPARSE, true);
diff --git a/internal/ceres/schur_eliminator_impl.h b/internal/ceres/schur_eliminator_impl.h index 7e06806..f253588 100644 --- a/internal/ceres/schur_eliminator_impl.h +++ b/internal/ceres/schur_eliminator_impl.h
@@ -194,7 +194,7 @@ &row_stride, &col_stride); if (cell_info != NULL) { const int block_size = bs->cols[i].size; - typename EigenTypes<kFBlockSize>::ConstVectorRef + typename EigenTypes<Eigen::Dynamic>::ConstVectorRef diag(D + bs->cols[i].position, block_size); CeresMutexLock l(&cell_info->m); @@ -205,20 +205,19 @@ } } - // Eliminate y blocks one chunk at a time. For each chunk,x3 - // compute the entries of the normal equations and the gradient - // vector block corresponding to the y block and then apply - // Gaussian elimination to them. The matrix ete stores the normal - // matrix corresponding to the block being eliminated and array - // buffer_ contains the non-zero blocks in the row corresponding - // to this y block in the normal equations. This computation is - // done in ChunkDiagonalBlockAndGradient. UpdateRhs then applies - // gaussian elimination to the rhs of the normal equations, - // updating the rhs of the reduced linear system by modifying rhs - // blocks for all the z blocks that share a row block/residual - // term with the y block. EliminateRowOuterProduct does the - // corresponding operation for the lhs of the reduced linear - // system. + // Eliminate y blocks one chunk at a time. For each chunk, compute + // the entries of the normal equations and the gradient vector block + // corresponding to the y block and then apply Gaussian elimination + // to them. The matrix ete stores the normal matrix corresponding to + // the block being eliminated and array buffer_ contains the + // non-zero blocks in the row corresponding to this y block in the + // normal equations. This computation is done in + // ChunkDiagonalBlockAndGradient. UpdateRhs then applies gaussian + // elimination to the rhs of the normal equations, updating the rhs + // of the reduced linear system by modifying rhs blocks for all the + // z blocks that share a row block/residual term with the y + // block. EliminateRowOuterProduct does the corresponding operation + // for the lhs of the reduced linear system. #pragma omp parallel for num_threads(num_threads_) schedule(dynamic) for (int i = 0; i < chunks_.size(); ++i) { #ifdef CERES_USE_OPENMP @@ -594,8 +593,8 @@ void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: NoEBlockRowOuterProduct(const BlockSparseMatrix* A, - int row_block_index, - BlockRandomAccessMatrix* lhs) { + int row_block_index, + BlockRandomAccessMatrix* lhs) { const CompressedRowBlockStructure* bs = A->block_structure(); const CompressedRow& row = bs->rows[row_block_index]; const double* values = A->values();
diff --git a/internal/ceres/schur_eliminator_test.cc b/internal/ceres/schur_eliminator_test.cc index 40bc904..85ecd8f 100644 --- a/internal/ceres/schur_eliminator_test.cc +++ b/internal/ceres/schur_eliminator_test.cc
@@ -193,7 +193,7 @@ Vector sol_expected; }; -TEST_F(SchurEliminatorTest, ScalarProblem) { +TEST_F(SchurEliminatorTest, ScalarProblemNoRegularization) { SetUpFromId(2); Vector zero(A->num_cols()); zero.setZero(); @@ -201,11 +201,26 @@ ComputeReferenceSolution(VectorRef(zero.data(), A->num_cols())); EliminateSolveAndCompare(VectorRef(zero.data(), A->num_cols()), true, 1e-14); EliminateSolveAndCompare(VectorRef(zero.data(), A->num_cols()), false, 1e-14); +} +TEST_F(SchurEliminatorTest, ScalarProblemWithRegularization) { + SetUpFromId(2); ComputeReferenceSolution(VectorRef(D.get(), A->num_cols())); EliminateSolveAndCompare(VectorRef(D.get(), A->num_cols()), true, 1e-14); EliminateSolveAndCompare(VectorRef(D.get(), A->num_cols()), false, 1e-14); } +TEST_F(SchurEliminatorTest, VaryingFBlockSizeWithStaticStructure) { + SetUpFromId(4); + ComputeReferenceSolution(VectorRef(D.get(), A->num_cols())); + EliminateSolveAndCompare(VectorRef(D.get(), A->num_cols()), true, 1e-14); +} + +TEST_F(SchurEliminatorTest, VaryingFBlockSizeWithoutStaticStructure) { + SetUpFromId(4); + ComputeReferenceSolution(VectorRef(D.get(), A->num_cols())); + EliminateSolveAndCompare(VectorRef(D.get(), A->num_cols()), false, 1e-14); +} + } // namespace internal } // namespace ceres