From 4ad91490827f2ebebcc70d17e63ef653bf06fd0d Mon Sep 17 00:00:00 2001 From: Sameer Agarwal Date: Wed, 24 Sep 2014 23:54:18 -0700 Subject: [PATCH] Simplify the Block Jacobi and Schur Jacobi preconditioners. 1. Extend the implementation of BlockRandomAccessDiagonalMatrix by adding Invert and RightMultiply methods. 2. Simplify the implementation of the Schur Jacobi preconditioner using these new methods. 3. Replace the custom storage used inside Block Jacobi preconditioner with BlockRandomAccessDiagonalMatrix and simplify its implementation too. Change-Id: I9d4888b35f0f228c08244abbdda5298b3ce9c466 --- internal/ceres/CMakeLists.txt | 1 + internal/ceres/block_jacobi_preconditioner.cc | 107 +++++--------- internal/ceres/block_jacobi_preconditioner.h | 17 +-- .../ceres/block_jacobi_preconditioner_test.cc | 105 ++++++++++++++ .../block_random_access_diagonal_matrix.cc | 40 +++++- .../block_random_access_diagonal_matrix.h | 9 +- ...lock_random_access_diagonal_matrix_test.cc | 134 ++++++++++++------ internal/ceres/schur_jacobi_preconditioner.cc | 30 +--- internal/ceres/schur_jacobi_preconditioner.h | 4 - 9 files changed, 289 insertions(+), 158 deletions(-) create mode 100644 internal/ceres/block_jacobi_preconditioner_test.cc diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index e4747ea54..97d097ca1 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt @@ -234,6 +234,7 @@ IF (BUILD_TESTING AND GFLAGS) CERES_TEST(autodiff) CERES_TEST(autodiff_cost_function) CERES_TEST(autodiff_local_parameterization) + CERES_TEST(block_jacobi_preconditioner) CERES_TEST(block_random_access_dense_matrix) CERES_TEST(block_random_access_diagonal_matrix) CERES_TEST(block_random_access_sparse_matrix) diff --git a/internal/ceres/block_jacobi_preconditioner.cc b/internal/ceres/block_jacobi_preconditioner.cc index 19b749bfc..ea49f077e 100644 --- a/internal/ceres/block_jacobi_preconditioner.cc +++ b/internal/ceres/block_jacobi_preconditioner.cc @@ -30,9 +30,9 @@ #include "ceres/block_jacobi_preconditioner.h" -#include "Eigen/Cholesky" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" +#include "ceres/block_random_access_diagonal_matrix.h" #include "ceres/casts.h" #include "ceres/integral_types.h" #include "ceres/internal/eigen.h" @@ -41,27 +41,14 @@ namespace ceres { namespace internal { BlockJacobiPreconditioner::BlockJacobiPreconditioner( - const BlockSparseMatrix& A) - : num_rows_(A.num_rows()), - block_structure_(*A.block_structure()) { - // Calculate the amount of storage needed. - int storage_needed = 0; - for (int c = 0; c < block_structure_.cols.size(); ++c) { - int size = block_structure_.cols[c].size; - storage_needed += size * size; + const BlockSparseMatrix& A) { + const CompressedRowBlockStructure* bs = A.block_structure(); + vector blocks(bs->cols.size()); + for (int i = 0; i < blocks.size(); ++i) { + blocks[i] = bs->cols[i].size; } - // Size the offsets and storage. - blocks_.resize(block_structure_.cols.size()); - block_storage_.resize(storage_needed); - - // Put pointers to the storage in the offsets. - double* block_cursor = &block_storage_[0]; - for (int c = 0; c < block_structure_.cols.size(); ++c) { - int size = block_structure_.cols[c].size; - blocks_[c] = block_cursor; - block_cursor += size * size; - } + m_.reset(new BlockRandomAccessDiagonalMatrix(blocks)); } BlockJacobiPreconditioner::~BlockJacobiPreconditioner() {} @@ -69,70 +56,54 @@ BlockJacobiPreconditioner::~BlockJacobiPreconditioner() {} bool BlockJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, const double* D) { const CompressedRowBlockStructure* bs = A.block_structure(); - - // Compute the diagonal blocks by block inner products. - std::fill(block_storage_.begin(), block_storage_.end(), 0.0); const double* values = A.values(); - for (int r = 0; r < bs->rows.size(); ++r) { - const int row_block_size = bs->rows[r].block.size; - const vector& cells = bs->rows[r].cells; - for (int c = 0; c < cells.size(); ++c) { - const int col_block_size = bs->cols[cells[c].block_id].size; - ConstMatrixRef m(values + cells[c].position, + m_->SetZero(); + for (int i = 0; i < bs->rows.size(); ++i) { + const int row_block_size = bs->rows[i].block.size; + const vector& cells = bs->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + const int block_id = cells[j].block_id; + const int col_block_size = bs->cols[block_id].size; + + int r, c, row_stride, col_stride; + CellInfo* cell_info = m_->GetCell(block_id, block_id, + &r, &c, + &row_stride, &col_stride); + MatrixRef m(cell_info->values, row_stride, col_stride); + ConstMatrixRef b(values + cells[j].position, row_block_size, col_block_size); - - MatrixRef(blocks_[cells[c].block_id], - col_block_size, - col_block_size).noalias() += m.transpose() * m; - - // TODO(keir): Figure out when the below expression is actually faster - // than doing the full rank update. The issue is that for smaller sizes, - // the rankUpdate() function is slower than the full product done above. - // - // On the typical bundling problems, the above product is ~5% faster. - // - // MatrixRef(blocks_[cells[c].block_id], - // col_block_size, - // col_block_size) - // .selfadjointView() - // .rankUpdate(m); - // + m.block(r, c, col_block_size, col_block_size) += b.transpose() * b; } } - // Add the diagonal and invert each block. - for (int c = 0; c < bs->cols.size(); ++c) { - const int size = block_structure_.cols[c].size; - const int position = block_structure_.cols[c].position; - MatrixRef block(blocks_[c], size, size); - - if (D != NULL) { - block.diagonal() += - ConstVectorRef(D + position, size).array().square().matrix(); + if (D != NULL) { + // Add the diagonal. + int position = 0; + for (int i = 0; i < bs->cols.size(); ++i) { + const int block_size = bs->cols[i].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = m_->GetCell(i, i, + &r, &c, + &row_stride, &col_stride); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size, block_size).diagonal() += + ConstVectorRef(D + position, block_size).array().square().matrix(); + position += block_size; } - - block = block.selfadjointView() - .llt() - .solve(Matrix::Identity(size, size)); } + + m_->Invert(); return true; } void BlockJacobiPreconditioner::RightMultiply(const double* x, double* y) const { - for (int c = 0; c < block_structure_.cols.size(); ++c) { - const int size = block_structure_.cols[c].size; - const int position = block_structure_.cols[c].position; - ConstMatrixRef D(blocks_[c], size, size); - ConstVectorRef x_block(x + position, size); - VectorRef y_block(y + position, size); - y_block += D * x_block; - } + m_->RightMultiply(x, y); } void BlockJacobiPreconditioner::LeftMultiply(const double* x, double* y) const { - RightMultiply(x, y); + m_->RightMultiply(x, y); } } // namespace internal diff --git a/internal/ceres/block_jacobi_preconditioner.h b/internal/ceres/block_jacobi_preconditioner.h index 3505a0124..857929709 100644 --- a/internal/ceres/block_jacobi_preconditioner.h +++ b/internal/ceres/block_jacobi_preconditioner.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2012 Google Inc. All rights reserved. +// Copyright 2014 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -32,6 +32,8 @@ #define CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ #include +#include "ceres/block_random_access_diagonal_matrix.h" +#include "ceres/internal/scoped_ptr.h" #include "ceres/preconditioner.h" namespace ceres { @@ -39,7 +41,6 @@ namespace internal { class BlockSparseMatrix; struct CompressedRowBlockStructure; -class LinearOperator; // A block Jacobi preconditioner. This is intended for use with // conjugate gradients, or other iterative symmetric solvers. To use @@ -60,18 +61,14 @@ class BlockJacobiPreconditioner : public BlockSparseMatrixPreconditioner { // Preconditioner interface virtual void RightMultiply(const double* x, double* y) const; virtual void LeftMultiply(const double* x, double* y) const; - virtual int num_rows() const { return num_rows_; } - virtual int num_cols() const { return num_rows_; } + virtual int num_rows() const { return m_->num_rows(); } + virtual int num_cols() const { return m_->num_rows(); } + const BlockRandomAccessDiagonalMatrix& matrix() const { return *m_; } private: virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); - std::vector blocks_; - std::vector block_storage_; - int num_rows_; - - // The block structure of the matrix this preconditioner is for (e.g. J). - const CompressedRowBlockStructure& block_structure_; + scoped_ptr m_; }; } // namespace internal diff --git a/internal/ceres/block_jacobi_preconditioner_test.cc b/internal/ceres/block_jacobi_preconditioner_test.cc new file mode 100644 index 000000000..57c0909bb --- /dev/null +++ b/internal/ceres/block_jacobi_preconditioner_test.cc @@ -0,0 +1,105 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_jacobi_preconditioner.h" + +#include +#include "ceres/block_random_access_diagonal_matrix.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/internal/scoped_ptr.h" +#include "gtest/gtest.h" +#include "Eigen/Dense" + +namespace ceres { +namespace internal { + + +class BlockJacobiPreconditionerTest : public ::testing::Test { + protected: + void SetUpFromProblemId(int problem_id) { + scoped_ptr problem( + CreateLinearLeastSquaresProblemFromId(problem_id)); + + CHECK_NOTNULL(problem.get()); + A.reset(down_cast(problem->A.release())); + D.reset(problem->D.release()); + + Matrix dense_a; + A->ToDenseMatrix(&dense_a); + dense_ata = dense_a.transpose() * dense_a; + dense_ata += VectorRef(D.get(), A->num_cols()) + .array().square().matrix().asDiagonal(); + } + + void VerifyDiagonalBlocks(const int problem_id) { + SetUpFromProblemId(problem_id); + + BlockJacobiPreconditioner pre(*A); + pre.Update(*A, D.get()); + BlockRandomAccessDiagonalMatrix* m = + const_cast(&pre.matrix()); + EXPECT_EQ(m->num_rows(), A->num_cols()); + EXPECT_EQ(m->num_cols(), A->num_cols()); + + const CompressedRowBlockStructure* bs = A->block_structure(); + for (int i = 0; i < bs->cols.size(); ++i) { + const int block_size = bs->cols[i].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = m->GetCell(i, i, + &r, &c, + &row_stride, &col_stride); + MatrixRef m(cell_info->values, row_stride, col_stride); + Matrix actual_block_inverse = m.block(r, c, block_size, block_size); + Matrix expected_block = dense_ata.block(bs->cols[i].position, + bs->cols[i].position, + block_size, + block_size); + const double residual = (actual_block_inverse * expected_block - + Matrix::Identity(block_size, block_size)).norm(); + EXPECT_NEAR(residual, 0.0, 1e-12) << "Block: " << i; + } + } + + scoped_ptr A; + scoped_array D; + Matrix dense_ata; +}; + +TEST_F(BlockJacobiPreconditionerTest, SmallProblem) { + VerifyDiagonalBlocks(2); +} + +TEST_F(BlockJacobiPreconditionerTest, LargeProblem) { + VerifyDiagonalBlocks(3); +} + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/block_random_access_diagonal_matrix.cc b/internal/ceres/block_random_access_diagonal_matrix.cc index d8bf4ef0c..b7ff33184 100644 --- a/internal/ceres/block_random_access_diagonal_matrix.cc +++ b/internal/ceres/block_random_access_diagonal_matrix.cc @@ -34,16 +34,19 @@ #include #include #include +#include "Eigen/Dense" #include "ceres/internal/port.h" #include "ceres/internal/scoped_ptr.h" +#include "ceres/stl_util.h" #include "ceres/triplet_sparse_matrix.h" #include "ceres/types.h" -#include "ceres/stl_util.h" #include "glog/logging.h" namespace ceres { namespace internal { +// TODO(sameeragarwal): Drop the dependence on TripletSparseMatrix. + BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix( const vector& blocks) : blocks_(blocks) { @@ -51,9 +54,9 @@ BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix( // rows/columns. int num_cols = 0; int num_nonzeros = 0; - vector col_layout; + vector block_positions; for (int i = 0; i < blocks_.size(); ++i) { - col_layout.push_back(num_cols); + block_positions.push_back(num_cols); num_cols += blocks_[i]; num_nonzeros += blocks_[i] * blocks_[i]; } @@ -72,7 +75,7 @@ BlockRandomAccessDiagonalMatrix::BlockRandomAccessDiagonalMatrix( for (int i = 0; i < blocks_.size(); ++i) { const int block_size = blocks_[i]; layout_.push_back(new CellInfo(values + pos)); - const int block_begin = col_layout[i]; + const int block_begin = block_positions[i]; for (int r = 0; r < block_size; ++r) { for (int c = 0; c < block_size; ++c, ++pos) { rows[pos] = block_begin + r; @@ -116,5 +119,34 @@ void BlockRandomAccessDiagonalMatrix::SetZero() { } } +void BlockRandomAccessDiagonalMatrix::Invert() { + double* values = tsm_->mutable_values(); + for (int i = 0; i < blocks_.size(); ++i) { + const int block_size = blocks_[i]; + MatrixRef block(values, block_size, block_size); + block = + block + .selfadjointView() + .llt() + .solve(Matrix::Identity(block_size, block_size)); + values += block_size * block_size; + } +} + +void BlockRandomAccessDiagonalMatrix::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + const double* values = tsm_->values(); + for (int i = 0; i < blocks_.size(); ++i) { + const int block_size = blocks_[i]; + ConstMatrixRef block(values, block_size, block_size); + VectorRef(y, block_size).noalias() += block * ConstVectorRef(x, block_size); + x += block_size; + y += block_size; + values += block_size * block_size; + } +} + } // namespace internal } // namespace ceres diff --git a/internal/ceres/block_random_access_diagonal_matrix.h b/internal/ceres/block_random_access_diagonal_matrix.h index 6b3cff233..ea9967817 100644 --- a/internal/ceres/block_random_access_diagonal_matrix.h +++ b/internal/ceres/block_random_access_diagonal_matrix.h @@ -52,7 +52,7 @@ namespace internal { class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix { public: // blocks is an array of block sizes. - BlockRandomAccessDiagonalMatrix(const vector& blocks); + explicit BlockRandomAccessDiagonalMatrix(const vector& blocks); // The destructor is not thread safe. It assumes that no one is // modifying any cells when the matrix is being destroyed. @@ -70,11 +70,16 @@ class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix { // locked. virtual void SetZero(); + // Invert the matrix assuming that each block is positive definite. + void Invert(); + + // y += S * x + void RightMultiply(const double* x, double* y) const; + // Since the matrix is square, num_rows() == num_cols(). virtual int num_rows() const { return tsm_->num_rows(); } virtual int num_cols() const { return tsm_->num_cols(); } - // Access to the underlying matrix object. const TripletSparseMatrix* matrix() const { return tsm_.get(); } TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } diff --git a/internal/ceres/block_random_access_diagonal_matrix_test.cc b/internal/ceres/block_random_access_diagonal_matrix_test.cc index e19268bef..aa2d664e6 100644 --- a/internal/ceres/block_random_access_diagonal_matrix_test.cc +++ b/internal/ceres/block_random_access_diagonal_matrix_test.cc @@ -35,58 +35,70 @@ #include "ceres/internal/eigen.h" #include "glog/logging.h" #include "gtest/gtest.h" +#include "Eigen/Cholesky" namespace ceres { namespace internal { -TEST(BlockRandomAccessDiagonalMatrix, GetCell) { - vector blocks; - blocks.push_back(3); - blocks.push_back(4); - blocks.push_back(5); - const int num_rows = 3 + 4 + 5; - const int num_nonzeros = 3 * 3 + 4 * 4 + 5 * 5; +class BlockRandomAccessDiagonalMatrixTest : public ::testing::Test { + public: + void SetUp() { + vector blocks; + blocks.push_back(3); + blocks.push_back(4); + blocks.push_back(5); + const int num_rows = 3 + 4 + 5; + num_nonzeros_ = 3 * 3 + 4 * 4 + 5 * 5; - BlockRandomAccessDiagonalMatrix m(blocks); - EXPECT_EQ(m.num_rows(), num_rows); - EXPECT_EQ(m.num_cols(), num_rows); + m_.reset(new BlockRandomAccessDiagonalMatrix(blocks)); - for (int i = 0; i < blocks.size(); ++i) { - const int row_block_id = i; - int col_block_id; - int row; - int col; - int row_stride; - int col_stride; + EXPECT_EQ(m_->num_rows(), num_rows); + EXPECT_EQ(m_->num_cols(), num_rows); - for (int j = 0; j < blocks.size(); ++j) { - col_block_id = j; - CellInfo* cell = m.GetCell(row_block_id, col_block_id, - &row, &col, - &row_stride, &col_stride); - // Off diagonal entries are not present. - if (i != j) { - EXPECT_TRUE(cell == NULL); - continue; + for (int i = 0; i < blocks.size(); ++i) { + const int row_block_id = i; + int col_block_id; + int row; + int col; + int row_stride; + int col_stride; + + for (int j = 0; j < blocks.size(); ++j) { + col_block_id = j; + CellInfo* cell = m_->GetCell(row_block_id, col_block_id, + &row, &col, + &row_stride, &col_stride); + // Off diagonal entries are not present. + if (i != j) { + EXPECT_TRUE(cell == NULL); + continue; + } + + EXPECT_TRUE(cell != NULL); + EXPECT_EQ(row, 0); + EXPECT_EQ(col, 0); + EXPECT_EQ(row_stride, blocks[row_block_id]); + EXPECT_EQ(col_stride, blocks[col_block_id]); + + // Write into the block + MatrixRef(cell->values, row_stride, col_stride).block( + row, col, blocks[row_block_id], blocks[col_block_id]) = + (row_block_id + 1) * (col_block_id +1) * + Matrix::Ones(blocks[row_block_id], blocks[col_block_id]) + + Matrix::Identity(blocks[row_block_id], blocks[row_block_id]); } - - EXPECT_TRUE(cell != NULL); - EXPECT_EQ(row, 0); - EXPECT_EQ(col, 0); - EXPECT_EQ(row_stride, blocks[row_block_id]); - EXPECT_EQ(col_stride, blocks[col_block_id]); - - // Write into the block - MatrixRef(cell->values, row_stride, col_stride).block( - row, col, blocks[row_block_id], blocks[col_block_id]) = - (row_block_id + 1) * (col_block_id +1) * - Matrix::Ones(blocks[row_block_id], blocks[col_block_id]); } } - const TripletSparseMatrix* tsm = m.matrix(); - EXPECT_EQ(tsm->num_nonzeros(), num_nonzeros); - EXPECT_EQ(tsm->max_num_nonzeros(), num_nonzeros); + protected: + int num_nonzeros_; + scoped_ptr m_; +}; + +TEST_F(BlockRandomAccessDiagonalMatrixTest, MatrixContents) { + const TripletSparseMatrix* tsm = m_->matrix(); + EXPECT_EQ(tsm->num_nonzeros(), num_nonzeros_); + EXPECT_EQ(tsm->max_num_nonzeros(), num_nonzeros_); Matrix dense; tsm->ToDenseMatrix(&dense); @@ -94,22 +106,54 @@ TEST(BlockRandomAccessDiagonalMatrix, GetCell) { double kTolerance = 1e-14; // (0,0) - EXPECT_NEAR((dense.block(0, 0, 3, 3) - Matrix::Ones(3, 3)).norm(), + EXPECT_NEAR((dense.block(0, 0, 3, 3) - + (Matrix::Ones(3, 3) + Matrix::Identity(3, 3))).norm(), 0.0, kTolerance); // (1,1) - EXPECT_NEAR((dense.block(3, 3, 4, 4) - 2 * 2 * Matrix::Ones(4, 4)).norm(), + EXPECT_NEAR((dense.block(3, 3, 4, 4) - + (2 * 2 * Matrix::Ones(4, 4) + Matrix::Identity(4, 4))).norm(), 0.0, kTolerance); // (1,1) - EXPECT_NEAR((dense.block(7, 7, 5, 5) - 3 * 3 * Matrix::Ones(5, 5)).norm(), + EXPECT_NEAR((dense.block(7, 7, 5, 5) - + (3 * 3 * Matrix::Ones(5, 5) + Matrix::Identity(5, 5))).norm(), 0.0, kTolerance); // There is nothing else in the matrix besides these four blocks. - EXPECT_NEAR(dense.norm(), sqrt(9.0 + 16. * 16. + 81.0 * 25.), kTolerance); + EXPECT_NEAR(dense.norm(), + sqrt(6 * 1.0 + 3 * 4.0 + + 12 * 16.0 + 4 * 25.0 + + 20 * 81.0 + 5 * 100.0), kTolerance); +} + +TEST_F(BlockRandomAccessDiagonalMatrixTest, RightMultiply) { + double kTolerance = 1e-14; + const TripletSparseMatrix* tsm = m_->matrix(); + Matrix dense; + tsm->ToDenseMatrix(&dense); + Vector x = Vector::Random(dense.rows()); + Vector expected_y = dense * x; + Vector actual_y = Vector::Zero(dense.rows()); + m_->RightMultiply(x.data(), actual_y.data()); + EXPECT_NEAR((expected_y - actual_y).norm(), 0, kTolerance); +} + +TEST_F(BlockRandomAccessDiagonalMatrixTest, Invert) { + double kTolerance = 1e-14; + const TripletSparseMatrix* tsm = m_->matrix(); + Matrix dense; + tsm->ToDenseMatrix(&dense); + Matrix expected_inverse = + dense.llt().solve(Matrix::Identity(dense.rows(), dense.rows())); + + m_->Invert(); + tsm->ToDenseMatrix(&dense); + + EXPECT_NEAR((expected_inverse - dense).norm(), 0.0, kTolerance); } } // namespace internal diff --git a/internal/ceres/schur_jacobi_preconditioner.cc b/internal/ceres/schur_jacobi_preconditioner.cc index 6dc9e89d3..cbdb70861 100644 --- a/internal/ceres/schur_jacobi_preconditioner.cc +++ b/internal/ceres/schur_jacobi_preconditioner.cc @@ -32,7 +32,6 @@ #include #include -#include "Eigen/Dense" #include "ceres/block_random_access_diagonal_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/collections_port.h" @@ -55,12 +54,12 @@ SchurJacobiPreconditioner::SchurJacobiPreconditioner( << "Jacobian should have atleast 1 f_block for " << "SCHUR_JACOBI preconditioner."; - block_size_.resize(num_blocks); + vector blocks(num_blocks); for (int i = 0; i < num_blocks; ++i) { - block_size_[i] = bs.cols[i + options_.elimination_groups[0]].size; + blocks[i] = bs.cols[i + options_.elimination_groups[0]].size; } - m_.reset(new BlockRandomAccessDiagonalMatrix(block_size_)); + m_.reset(new BlockRandomAccessDiagonalMatrix(blocks)); InitEliminator(bs); } @@ -99,32 +98,13 @@ bool SchurJacobiPreconditioner::UpdateImpl(const BlockSparseMatrix& A, // Compute a subset of the entries of the Schur complement. eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); + m_->Invert(); return true; } void SchurJacobiPreconditioner::RightMultiply(const double* x, double* y) const { - CHECK_NOTNULL(x); - CHECK_NOTNULL(y); - - const double* lhs_values = - down_cast(m_.get())->matrix()->values(); - - // This loop can be easily multi-threaded with OpenMP if need be. - for (int i = 0; i < block_size_.size(); ++i) { - const int block_size = block_size_[i]; - ConstMatrixRef block(lhs_values, block_size, block_size); - - VectorRef(y, block_size) = - block - .selfadjointView() - .llt() - .solve(ConstVectorRef(x, block_size)); - - x += block_size; - y += block_size; - lhs_values += block_size * block_size; - } + m_->RightMultiply(x, y); } int SchurJacobiPreconditioner::num_rows() const { diff --git a/internal/ceres/schur_jacobi_preconditioner.h b/internal/ceres/schur_jacobi_preconditioner.h index aecb01510..8b528e250 100644 --- a/internal/ceres/schur_jacobi_preconditioner.h +++ b/internal/ceres/schur_jacobi_preconditioner.h @@ -94,11 +94,7 @@ class SchurJacobiPreconditioner : public BlockSparseMatrixPreconditioner { virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D); Preconditioner::Options options_; - - // Sizes of the blocks in the schur complement. - vector block_size_; scoped_ptr eliminator_; - // Preconditioner matrix. scoped_ptr m_; CERES_DISALLOW_COPY_AND_ASSIGN(SchurJacobiPreconditioner);