From 2b16b0080b6e673eaaf9ed478c9e971d9fcd65de Mon Sep 17 00:00:00 2001 From: Sameer Agarwal Date: Fri, 20 Dec 2013 15:22:26 -0800 Subject: [PATCH] CompressedRowSparseMatrix::AppendRows and DeleteRows bugfix. CompressedRowSparseMatrix can store the row and column block structure but the AppendRows and DeleteRows methods did not pay attention to them. This meant that it was possible to get to a CompressedRowSparseMatrix whose block structure did not match the contents of the matrix. This change fixes this problem. Change-Id: I1b3c807fc03d8c049ee20511e2bc62806d211b81 --- .../ceres/compressed_row_sparse_matrix.cc | 19 +++++++ .../compressed_row_sparse_matrix_test.cc | 56 +++++++++++++++++++ .../ceres/sparse_normal_cholesky_solver.cc | 22 ++++++-- 3 files changed, 93 insertions(+), 4 deletions(-) diff --git a/internal/ceres/compressed_row_sparse_matrix.cc b/internal/ceres/compressed_row_sparse_matrix.cc index e200c9285..439270ed3 100644 --- a/internal/ceres/compressed_row_sparse_matrix.cc +++ b/internal/ceres/compressed_row_sparse_matrix.cc @@ -31,6 +31,7 @@ #include "ceres/compressed_row_sparse_matrix.h" #include +#include #include #include "ceres/crs_matrix.h" #include "ceres/internal/port.h" @@ -215,11 +216,28 @@ void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { num_rows_ -= delta_rows; rows_.resize(num_rows_ + 1); + + // Walk the list of row blocks untill we reach the new number of + // rows and then drop the rest of the row blocks. + int num_row_blocks = 0; + int num_rows = 0; + while (num_row_blocks < row_blocks_.size() && num_rows < num_rows_) { + num_rows += row_blocks_[num_row_blocks]; + ++num_row_blocks; + } + + row_blocks_.resize(num_row_blocks); } void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { CHECK_EQ(m.num_cols(), num_cols_); + CHECK(row_blocks_.size() == 0 || m.row_blocks().size() !=0) + << "Cannot append a matrix with row blocks to one without and vice versa." + << "This matrix has : " << row_blocks_.size() << " row blocks." + << "The matrix being appended has: " << m.row_blocks().size() + << " row blocks."; + if (cols_.size() < num_nonzeros() + m.num_nonzeros()) { cols_.resize(num_nonzeros() + m.num_nonzeros()); values_.resize(num_nonzeros() + m.num_nonzeros()); @@ -239,6 +257,7 @@ void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { } num_rows_ += m.num_rows(); + row_blocks_.insert(row_blocks_.end(), m.row_blocks().begin(), m.row_blocks().end()); } void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { diff --git a/internal/ceres/compressed_row_sparse_matrix_test.cc b/internal/ceres/compressed_row_sparse_matrix_test.cc index 02109cc2e..4d20083a3 100644 --- a/internal/ceres/compressed_row_sparse_matrix_test.cc +++ b/internal/ceres/compressed_row_sparse_matrix_test.cc @@ -76,6 +76,14 @@ class CompressedRowSparseMatrixTest : public ::testing::Test { num_rows = tsm->num_rows(); num_cols = tsm->num_cols(); + + vector* row_blocks = crsm->mutable_row_blocks(); + row_blocks->resize(num_rows); + std::fill(row_blocks->begin(), row_blocks->end(), 1); + + vector* col_blocks = crsm->mutable_col_blocks(); + col_blocks->resize(num_cols); + std::fill(col_blocks->begin(), col_blocks->end(), 1); } int num_rows; @@ -126,6 +134,9 @@ TEST_F(CompressedRowSparseMatrixTest, Scale) { } TEST_F(CompressedRowSparseMatrixTest, DeleteRows) { + // Clear the row and column blocks as these are purely scalar tests. + crsm->mutable_row_blocks()->clear(); + crsm->mutable_col_blocks()->clear(); for (int i = 0; i < num_rows; ++i) { tsm->Resize(num_rows - i, num_cols); crsm->DeleteRows(crsm->num_rows() - tsm->num_rows()); @@ -134,6 +145,10 @@ TEST_F(CompressedRowSparseMatrixTest, DeleteRows) { } TEST_F(CompressedRowSparseMatrixTest, AppendRows) { + // Clear the row and column blocks as these are purely scalar tests. + crsm->mutable_row_blocks()->clear(); + crsm->mutable_col_blocks()->clear(); + for (int i = 0; i < num_rows; ++i) { TripletSparseMatrix tsm_appendage(*tsm); tsm_appendage.Resize(i, num_cols); @@ -146,6 +161,47 @@ TEST_F(CompressedRowSparseMatrixTest, AppendRows) { } } +TEST_F(CompressedRowSparseMatrixTest, AppendAndDeleteBlockDiagonalMatrix) { + int num_diagonal_rows = crsm->num_cols(); + + scoped_array diagonal(new double[num_diagonal_rows]); + for (int i = 0; i < num_diagonal_rows; ++i) { + diagonal[i] =i; + } + + vector row_and_column_blocks; + row_and_column_blocks.push_back(1); + row_and_column_blocks.push_back(2); + row_and_column_blocks.push_back(2); + + const vector pre_row_blocks = crsm->row_blocks(); + const vector pre_col_blocks = crsm->col_blocks(); + + scoped_ptr appendage( + CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( + diagonal.get(), row_and_column_blocks)); + LOG(INFO) << appendage->row_blocks().size(); + + crsm->AppendRows(*appendage); + + const vector post_row_blocks = crsm->row_blocks(); + const vector post_col_blocks = crsm->col_blocks(); + + vector expected_row_blocks = pre_row_blocks; + expected_row_blocks.insert(expected_row_blocks.end(), + row_and_column_blocks.begin(), + row_and_column_blocks.end()); + + vector expected_col_blocks = pre_col_blocks; + + EXPECT_EQ(expected_row_blocks, crsm->row_blocks()); + EXPECT_EQ(expected_col_blocks, crsm->col_blocks()); + + crsm->DeleteRows(num_diagonal_rows); + EXPECT_EQ(crsm->row_blocks(), pre_row_blocks); + EXPECT_EQ(crsm->col_blocks(), pre_col_blocks); +} + TEST_F(CompressedRowSparseMatrixTest, ToDenseMatrix) { Matrix tsm_dense; Matrix crsm_dense; diff --git a/internal/ceres/sparse_normal_cholesky_solver.cc b/internal/ceres/sparse_normal_cholesky_solver.cc index 1ead8f70a..b2dba440e 100644 --- a/internal/ceres/sparse_normal_cholesky_solver.cc +++ b/internal/ceres/sparse_normal_cholesky_solver.cc @@ -112,8 +112,15 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( if (per_solve_options.D != NULL) { // Temporarily append a diagonal block to the A matrix, but undo // it before returning the matrix to the user. - CompressedRowSparseMatrix D(per_solve_options.D, num_cols); - A->AppendRows(D); + scoped_ptr regularizer; + if (A->col_blocks().size() > 0) { + regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( + per_solve_options.D, A->col_blocks())); + } else { + regularizer.reset(new CompressedRowSparseMatrix( + per_solve_options.D, num_cols)); + } + A->AppendRows(*regularizer); } VectorRef(x, num_cols).setZero(); @@ -196,8 +203,15 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( if (per_solve_options.D != NULL) { // Temporarily append a diagonal block to the A matrix, but undo // it before returning the matrix to the user. - CompressedRowSparseMatrix D(per_solve_options.D, num_cols); - A->AppendRows(D); + scoped_ptr regularizer; + if (A->col_blocks().size() > 0) { + regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( + per_solve_options.D, A->col_blocks())); + } else { + regularizer.reset(new CompressedRowSparseMatrix( + per_solve_options.D, num_cols)); + } + A->AppendRows(*regularizer); } VectorRef(x, num_cols).setZero();