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
