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();