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
