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