Add BlockRandomAccessSparseMatrix::SymmetricRightMultiply. Change-Id: Ib06a22a209b4c985ba218162dfb6bf46bd93169e
diff --git a/internal/ceres/block_random_access_sparse_matrix.cc b/internal/ceres/block_random_access_sparse_matrix.cc index f789436..9da16a4 100644 --- a/internal/ceres/block_random_access_sparse_matrix.cc +++ b/internal/ceres/block_random_access_sparse_matrix.cc
@@ -54,9 +54,9 @@ // Build the row/column layout vector and count the number of scalar // rows/columns. int num_cols = 0; - vector<int> col_layout; + block_positions_.reserve(blocks_.size()); for (int i = 0; i < blocks_.size(); ++i) { - col_layout.push_back(num_cols); + block_positions_.push_back(num_cols); num_cols += blocks_[i]; } @@ -105,8 +105,8 @@ layout_[IntPairToLong(row_block_id, col_block_id)]->values - values; for (int r = 0; r < row_block_size; ++r) { for (int c = 0; c < col_block_size; ++c, ++pos) { - rows[pos] = col_layout[row_block_id] + r; - cols[pos] = col_layout[col_block_id] + c; + rows[pos] = block_positions_[row_block_id] + r; + cols[pos] = block_positions_[col_block_id] + c; values[pos] = 1.0; DCHECK_LT(rows[pos], tsm_->num_rows()); DCHECK_LT(cols[pos], tsm_->num_rows()); @@ -154,5 +154,37 @@ } } +void BlockRandomAccessSparseMatrix::SymmetricRightMultiply(const double* x, + double* y) const { + for (LayoutType::const_iterator it = layout_.begin(); + it != layout_.end(); + ++it) { + int row; + int col; + LongToIntPair(it->first, &row, &col); + + const int row_block_size = blocks_[row]; + const int row_block_pos = block_positions_[row]; + const int col_block_size = blocks_[col]; + const int col_block_pos = block_positions_[col]; + + MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + it->second->values, row_block_size, col_block_size, + x + col_block_pos, + y + row_block_pos); + + // Since the matrix is symmetric, but only the upper triangular + // part is stored, if the block being accessed is not a diagonal + // block, then use the same block to do the corresponding lower + // triangular multiply also. + if (row != col) { + MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( + it->second->values, row_block_size, col_block_size, + x + row_block_pos, + y + col_block_pos); + } + } +} + } // namespace internal } // namespace ceres
diff --git a/internal/ceres/block_random_access_sparse_matrix.h b/internal/ceres/block_random_access_sparse_matrix.h index 27b1029..a4f89d8 100644 --- a/internal/ceres/block_random_access_sparse_matrix.h +++ b/internal/ceres/block_random_access_sparse_matrix.h
@@ -43,6 +43,7 @@ #include "ceres/internal/port.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/types.h" +#include "ceres/small_blas.h" namespace ceres { namespace internal { @@ -75,6 +76,12 @@ // locked. virtual void SetZero(); + // Assume that the matrix is symmetric and only one half of the + // matrix is stored. + // + // y += S * x + void SymmetricRightMultiply(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(); } @@ -84,13 +91,20 @@ TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } private: - int64 IntPairToLong(int a, int b) { - return a * kMaxRowBlocks + b; + int64 IntPairToLong(int row, int col) const { + return row * kMaxRowBlocks + col; + } + + void LongToIntPair(int64 index, int* row, int* col) const { + *row = index / kMaxRowBlocks; + *col = index % kMaxRowBlocks; } const int64 kMaxRowBlocks; + // row/column block sizes. const vector<int> blocks_; + vector<int> block_positions_; // A mapping from <row_block_id, col_block_id> to the position in // the values array of tsm_ where the block is stored.
diff --git a/internal/ceres/block_random_access_sparse_matrix_test.cc b/internal/ceres/block_random_access_sparse_matrix_test.cc index b2396a1..10a4190 100644 --- a/internal/ceres/block_random_access_sparse_matrix_test.cc +++ b/internal/ceres/block_random_access_sparse_matrix_test.cc
@@ -56,7 +56,7 @@ block_pairs.insert(make_pair(1, 2)); num_nonzeros += blocks[1] * blocks[2]; - block_pairs.insert(make_pair(2, 0)); + block_pairs.insert(make_pair(0, 2)); num_nonzeros += blocks[2] * blocks[0]; BlockRandomAccessSparseMatrix m(blocks, block_pairs); @@ -97,26 +97,37 @@ double kTolerance = 1e-14; - // (0,0) + // (0, 0) EXPECT_NEAR((dense.block(0, 0, 3, 3) - Matrix::Ones(3, 3)).norm(), 0.0, kTolerance); - // (1,1) + // (1, 1) EXPECT_NEAR((dense.block(3, 3, 4, 4) - 2 * 2 * Matrix::Ones(4, 4)).norm(), 0.0, kTolerance); - // (1,2) + // (1, 2) EXPECT_NEAR((dense.block(3, 3 + 4, 4, 5) - 2 * 3 * Matrix::Ones(4, 5)).norm(), 0.0, kTolerance); - // (2,0) - EXPECT_NEAR((dense.block(3 + 4, 0, 5, 3) - 3 * 1 * Matrix::Ones(5, 3)).norm(), + // (0, 2) + EXPECT_NEAR((dense.block(0, 3 + 4, 3, 5) - 3 * 1 * Matrix::Ones(3, 5)).norm(), 0.0, kTolerance); // There is nothing else in the matrix besides these four blocks. EXPECT_NEAR(dense.norm(), sqrt(9. + 16. * 16. + 36. * 20. + 9. * 15.), kTolerance); + + Vector x = Vector::Ones(dense.rows()); + Vector actual_y = Vector::Zero(dense.rows()); + Vector expected_y = Vector::Zero(dense.rows()); + + expected_y += dense.selfadjointView<Eigen::Upper>() * x; + m.SymmetricRightMultiply(x.data(), actual_y.data()); + EXPECT_NEAR((expected_y - actual_y).norm(), 0.0, kTolerance) + << "actual: " << actual_y.transpose() << "\n" + << "expected: " << expected_y.transpose() + << "matrix: \n " << dense; } // IntPairToLong is private, thus this fixture is needed to access and @@ -131,19 +142,38 @@ m_.reset(new BlockRandomAccessSparseMatrix(blocks, block_pairs)); } - void CheckIntPair(int a, int b) { + void CheckIntPairToLong(int a, int b) { int64 value = m_->IntPairToLong(a, b); EXPECT_GT(value, 0) << "Overflow a = " << a << " b = " << b; EXPECT_GT(value, a) << "Overflow a = " << a << " b = " << b; EXPECT_GT(value, b) << "Overflow a = " << a << " b = " << b; } + void CheckLongToIntPair() { + uint64 max_rows = m_->kMaxRowBlocks; + for (int row = max_rows - 10; row < max_rows; ++row) { + for (int col = 0; col < 10; ++col) { + int row_computed; + int col_computed; + m_->LongToIntPair(m_->IntPairToLong(row, col), + &row_computed, + &col_computed); + EXPECT_EQ(row, row_computed); + EXPECT_EQ(col, col_computed); + } + } + } + private: scoped_ptr<BlockRandomAccessSparseMatrix> m_; }; TEST_F(BlockRandomAccessSparseMatrixTest, IntPairToLongOverflow) { - CheckIntPair(numeric_limits<int>::max(), numeric_limits<int>::max()); + CheckIntPairToLong(numeric_limits<int>::max(), numeric_limits<int>::max()); +} + +TEST_F(BlockRandomAccessSparseMatrixTest, LongToIntPair) { + CheckLongToIntPair(); } } // namespace internal