Replace Eigen block operations with small GEMM and GEMV loops.
1. Add Matrix-Matrix and Matrix-Vector multiply functions.
2. Replace Eigen usage in SchurEliminator with these custom
matrix operations.
3. Save on some memory allocations in ChunkOuterProduct.
4. Replace LDLT with LLT.
As a result on problem-16-22106-pre.txt, the linear solver time
goes down from 1.2s to 0.64s.
Change-Id: I2daa667960e0a1e8834489965a30be31f37fd87f
diff --git a/internal/ceres/schur_eliminator_impl.h b/internal/ceres/schur_eliminator_impl.h
index 339c44b..b46eab9 100644
--- a/internal/ceres/schur_eliminator_impl.h
+++ b/internal/ceres/schur_eliminator_impl.h
@@ -34,10 +34,6 @@
#ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_
#define CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_
-#ifdef CERES_USE_OPENMP
-#include <omp.h>
-#endif
-
// Eigen has an internal threshold switching between different matrix
// multiplication algorithms. In particular for matrices larger than
// EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD it uses a cache friendly
@@ -46,19 +42,25 @@
// are thin and long, the default choice may not be optimal. This is
// the case for us, as the default choice causes a 30% performance
// regression when we moved from Eigen2 to Eigen3.
+
#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10
+#ifdef CERES_USE_OPENMP
+#include <omp.h>
+#endif
+
#include <algorithm>
#include <map>
#include "Eigen/Dense"
+#include "ceres/blas.h"
#include "ceres/block_random_access_matrix.h"
#include "ceres/block_sparse_matrix.h"
#include "ceres/block_structure.h"
+#include "ceres/internal/eigen.h"
+#include "ceres/internal/scoped_ptr.h"
#include "ceres/map_util.h"
#include "ceres/schur_eliminator.h"
#include "ceres/stl_util.h"
-#include "ceres/internal/eigen.h"
-#include "ceres/internal/scoped_ptr.h"
#include "glog/logging.h"
namespace ceres {
@@ -149,13 +151,16 @@
buffer_.reset(new double[buffer_size_ * num_threads_]);
+ // chunk_outer_product_buffer_ only needs to store e_block_size *
+ // f_block_size, which is always less than buffer_size_, so we just
+ // allocate buffer_size_ per thread.
+ chunk_outer_product_buffer_.reset(new double[buffer_size_ * num_threads_]);
+
STLDeleteElements(&rhs_locks_);
rhs_locks_.resize(num_col_blocks - num_eliminate_blocks_);
for (int i = 0; i < num_col_blocks - num_eliminate_blocks_; ++i) {
rhs_locks_[i] = new Mutex;
}
-
- VLOG(1) << "Eliminator threads: " << num_threads_;
}
template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
@@ -261,7 +266,7 @@
typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix inverse_ete =
ete
.template selfadjointView<Eigen::Upper>()
- .ldlt()
+ .llt()
.solve(Matrix::Identity(e_block_size, e_block_size));
// For the current chunk compute and update the rhs of the reduced
@@ -294,8 +299,8 @@
const int e_block_id = bs->rows[chunk.start].cells.front().block_id;
const int e_block_size = bs->cols[e_block_id].size;
- typename EigenTypes<kEBlockSize>::VectorRef y_block(
- y + bs->cols[e_block_id].position, e_block_size);
+ double* y_ptr = y + bs->cols[e_block_id].position;
+ typename EigenTypes<kEBlockSize>::VectorRef y_block(y_ptr, e_block_size);
typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix
ete(e_block_size, e_block_size);
@@ -312,40 +317,35 @@
const double* row_values = A->RowBlockValues(chunk.start + j);
const Cell& e_cell = row.cells.front();
DCHECK_EQ(e_block_id, e_cell.block_id);
- const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef
- e_block(row_values + e_cell.position,
- row.block.size,
- e_block_size);
- typename EigenTypes<kRowBlockSize>::Vector
- sj =
+ typename EigenTypes<kRowBlockSize>::Vector sj =
typename EigenTypes<kRowBlockSize>::ConstVectorRef
- (b + bs->rows[chunk.start + j].block.position,
- row.block.size);
+ (b + bs->rows[chunk.start + j].block.position, row.block.size);
for (int c = 1; c < row.cells.size(); ++c) {
const int f_block_id = row.cells[c].block_id;
const int f_block_size = bs->cols[f_block_id].size;
- const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef
- f_block(row_values + row.cells[c].position,
- row.block.size, f_block_size);
const int r_block = f_block_id - num_eliminate_blocks_;
- sj -= f_block *
- typename EigenTypes<kFBlockSize>::ConstVectorRef
- (z + lhs_row_layout_[r_block], f_block_size);
+ MatrixVectorMultiply<kRowBlockSize, kFBlockSize, -1>(
+ row_values + row.cells[c].position, row.block.size, f_block_size,
+ z + lhs_row_layout_[r_block],
+ sj.data());
}
- y_block += e_block.transpose() * sj;
- ete.template selfadjointView<Eigen::Upper>()
- .rankUpdate(e_block.transpose(), 1.0);
+ MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>(
+ row_values + e_cell.position, row.block.size, e_block_size,
+ sj.data(),
+ y_ptr);
+
+ MatrixTransposeMatrixMultiply
+ <kRowBlockSize, kEBlockSize,kRowBlockSize, kEBlockSize, 1>(
+ row_values + e_cell.position, row.block.size, e_block_size,
+ row_values + e_cell.position, row.block.size, e_block_size,
+ ete.data(), 0, 0, e_block_size, e_block_size);
}
- y_block =
- ete
- .template selfadjointView<Eigen::Upper>()
- .ldlt()
- .solve(y_block);
+ ete.llt().solveInPlace(y_block);
}
}
@@ -382,15 +382,12 @@
for (int c = 1; c < row.cells.size(); ++c) {
const int block_id = row.cells[c].block_id;
const int block_size = bs->cols[block_id].size;
- const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef
- b(row_values + row.cells[c].position,
- row.block.size, block_size);
-
const int block = block_id - num_eliminate_blocks_;
CeresMutexLock l(rhs_locks_[block]);
- typename EigenTypes<kFBlockSize>::VectorRef
- (rhs + lhs_row_layout_[block], block_size).noalias()
- += b.transpose() * sj;
+ MatrixTransposeVectorMultiply<kRowBlockSize, kFBlockSize, 1>(
+ row_values + row.cells[c].position,
+ row.block.size, block_size,
+ sj.data(), rhs + lhs_row_layout_[block]);
}
b_pos += row.block.size;
}
@@ -446,34 +443,31 @@
// Extract the e_block, ETE += E_i' E_i
const Cell& e_cell = row.cells.front();
- const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef
- e_block(row_values + e_cell.position,
- row.block.size,
- e_block_size);
-
- ete->template selfadjointView<Eigen::Upper>()
- .rankUpdate(e_block.transpose(), 1.0);
+ MatrixTransposeMatrixMultiply
+ <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>(
+ row_values + e_cell.position, row.block.size, e_block_size,
+ row_values + e_cell.position, row.block.size, e_block_size,
+ ete->data(), 0, 0, e_block_size, e_block_size);
// g += E_i' b_i
- g->noalias() += e_block.transpose() *
- typename EigenTypes<kRowBlockSize>::ConstVectorRef
- (b + b_pos, row.block.size);
+ MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>(
+ row_values + e_cell.position, row.block.size, e_block_size,
+ b + b_pos,
+ g->data());
+
// buffer = E'F. This computation is done by iterating over the
// f_blocks for each row in the chunk.
for (int c = 1; c < row.cells.size(); ++c) {
const int f_block_id = row.cells[c].block_id;
const int f_block_size = bs->cols[f_block_id].size;
- const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef
- f_block(row_values + row.cells[c].position,
- row.block.size, f_block_size);
-
double* buffer_ptr =
buffer + FindOrDie(chunk.buffer_layout, f_block_id);
-
- typename EigenTypes<kEBlockSize, kFBlockSize>::MatrixRef
- (buffer_ptr, e_block_size, f_block_size).noalias()
- += e_block.transpose() * f_block;
+ MatrixTransposeMatrixMultiply
+ <kRowBlockSize, kEBlockSize, kRowBlockSize, kFBlockSize, 1>(
+ row_values + e_cell.position, row.block.size, e_block_size,
+ row_values + row.cells[c].position, row.block.size, f_block_size,
+ buffer_ptr, 0, 0, e_block_size, f_block_size);
}
b_pos += row.block.size;
}
@@ -497,15 +491,24 @@
// references to the left hand side.
const int e_block_size = inverse_ete.rows();
BufferLayoutType::const_iterator it1 = buffer_layout.begin();
+
+#ifdef CERES_USE_OPENMP
+ int thread_id = omp_get_thread_num();
+#else
+ int thread_id = 0;
+#endif
+ double* b1_transpose_inverse_ete =
+ chunk_outer_product_buffer_.get() + thread_id * buffer_size_;
+
// S(i,j) -= bi' * ete^{-1} b_j
for (; it1 != buffer_layout.end(); ++it1) {
const int block1 = it1->first - num_eliminate_blocks_;
const int block1_size = bs->cols[it1->first].size;
-
- const typename EigenTypes<kEBlockSize, kFBlockSize>::ConstMatrixRef
- b1(buffer + it1->second, e_block_size, block1_size);
- const typename EigenTypes<kFBlockSize, kEBlockSize>::Matrix
- b1_transpose_inverse_ete = b1.transpose() * inverse_ete;
+ MatrixTransposeMatrixMultiply
+ <kEBlockSize, kFBlockSize, kEBlockSize, kEBlockSize, 0>(
+ buffer + it1->second, e_block_size, block1_size,
+ inverse_ete.data(), e_block_size, e_block_size,
+ b1_transpose_inverse_ete, 0, 0, block1_size, e_block_size);
BufferLayoutType::const_iterator it2 = it1;
for (; it2 != buffer_layout.end(); ++it2) {
@@ -515,46 +518,15 @@
CellInfo* cell_info = lhs->GetCell(block1, block2,
&r, &c,
&row_stride, &col_stride);
- if (cell_info == NULL) {
- continue;
+ if (cell_info != NULL) {
+ const int block2_size = bs->cols[it2->first].size;
+ CeresMutexLock l(&cell_info->m);
+ MatrixMatrixMultiply
+ <kFBlockSize, kEBlockSize, kEBlockSize, kFBlockSize, -1>(
+ b1_transpose_inverse_ete, block1_size, e_block_size,
+ buffer + it2->second, e_block_size, block2_size,
+ cell_info->values, r, c, row_stride, col_stride);
}
-
- const int block2_size = bs->cols[it2->first].size;
- const typename EigenTypes<kEBlockSize, kFBlockSize>::ConstMatrixRef
- b2(buffer + it2->second, e_block_size, block2_size);
-
- CeresMutexLock l(&cell_info->m);
- MatrixRef m(cell_info->values, row_stride, col_stride);
-
- // We explicitly construct a block object here instead of using
- // m.block(), as m.block() variant of the constructor does not
- // allow mixing of template sizing and runtime sizing parameters
- // like the Matrix class does.
- Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize>
- block(m, r, c, block1_size, block2_size);
-#ifdef CERES_WORK_AROUND_ANDROID_NDK_COMPILER_BUG
- // Removing the ".noalias()" annotation on the following statement is
- // necessary to produce a correct build with the Android NDK, including
- // versions 6, 7, 8, and 8b, when built with STLPort and the
- // non-standalone toolchain (i.e. ndk-build). This appears to be a
- // compiler bug; if the workaround is not in place, the line
- //
- // block.noalias() -= b1_transpose_inverse_ete * b2;
- //
- // gets compiled to
- //
- // block.noalias() += b1_transpose_inverse_ete * b2;
- //
- // which breaks schur elimination. Introducing a temporary by removing the
- // .noalias() annotation causes the issue to disappear. Tracking this
- // issue down was tricky, since the test suite doesn't run when built with
- // the non-standalone toolchain.
- //
- // TODO(keir): Make a reproduction case for this and send it upstream.
- block -= b1_transpose_inverse_ete * b2;
-#else
- block.noalias() -= b1_transpose_inverse_ete * b2;
-#endif // CERES_WORK_AROUND_ANDROID_NDK_COMPILER_BUG
}
}
}
@@ -624,10 +596,13 @@
&row_stride, &col_stride);
if (cell_info != NULL) {
CeresMutexLock l(&cell_info->m);
- MatrixRef m(cell_info->values, row_stride, col_stride);
- m.block(r, c, block1_size, block1_size)
- .selfadjointView<Eigen::Upper>()
- .rankUpdate(b1.transpose(), 1.0);
+ // This multiply currently ignores the fact that this is a
+ // symmetric outer product.
+ MatrixTransposeMatrixMultiply
+ <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>(
+ row_values + row.cells[i].position, row.block.size, block1_size,
+ row_values + row.cells[i].position, row.block.size, block1_size,
+ cell_info->values, r, c, row_stride, col_stride);
}
for (int j = i + 1; j < row.cells.size(); ++j) {
@@ -638,17 +613,15 @@
CellInfo* cell_info = lhs->GetCell(block1, block2,
&r, &c,
&row_stride, &col_stride);
- if (cell_info == NULL) {
- continue;
+ if (cell_info != NULL) {
+ const int block2_size = bs->cols[row.cells[j].block_id].size;
+ CeresMutexLock l(&cell_info->m);
+ MatrixTransposeMatrixMultiply
+ <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>(
+ row_values + row.cells[i].position, row.block.size, block1_size,
+ row_values + row.cells[j].position, row.block.size, block2_size,
+ cell_info->values, r, c, row_stride, col_stride);
}
-
- const int block2_size = bs->cols[row.cells[j].block_id].size;
- CeresMutexLock l(&cell_info->m);
- MatrixRef m(cell_info->values, row_stride, col_stride);
- m.block(r, c, block1_size, block2_size).noalias() +=
- b1.transpose() * ConstMatrixRef(row_values + row.cells[j].position,
- row.block.size,
- block2_size);
}
}
}
@@ -670,25 +643,18 @@
DCHECK_GE(block1, 0);
const int block1_size = bs->cols[row.cells[i].block_id].size;
- const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef
- b1(row_values + row.cells[i].position,
- row.block.size, block1_size);
- {
- int r, c, row_stride, col_stride;
- CellInfo* cell_info = lhs->GetCell(block1, block1,
- &r, &c,
- &row_stride, &col_stride);
- if (cell_info == NULL) {
- continue;
- }
-
+ int r, c, row_stride, col_stride;
+ CellInfo* cell_info = lhs->GetCell(block1, block1,
+ &r, &c,
+ &row_stride, &col_stride);
+ if (cell_info != NULL) {
CeresMutexLock l(&cell_info->m);
- MatrixRef m(cell_info->values, row_stride, col_stride);
-
- Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize>
- block(m, r, c, block1_size, block1_size);
- block.template selfadjointView<Eigen::Upper>()
- .rankUpdate(b1.transpose(), 1.0);
+ // block += b1.transpose() * b1;
+ MatrixTransposeMatrixMultiply
+ <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>(
+ row_values + row.cells[i].position, row.block.size, block1_size,
+ row_values + row.cells[i].position, row.block.size, block1_size,
+ cell_info->values, r, c, row_stride, col_stride);
}
for (int j = i + 1; j < row.cells.size(); ++j) {
@@ -700,20 +666,14 @@
CellInfo* cell_info = lhs->GetCell(block1, block2,
&r, &c,
&row_stride, &col_stride);
- if (cell_info == NULL) {
- continue;
+ if (cell_info != NULL) {
+ // block += b1.transpose() * b2;
+ MatrixTransposeMatrixMultiply
+ <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>(
+ row_values + row.cells[i].position, row.block.size, block1_size,
+ row_values + row.cells[j].position, row.block.size, block2_size,
+ cell_info->values, r, c, row_stride, col_stride);
}
-
- const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef
- b2(row_values + row.cells[j].position,
- row.block.size,
- block2_size);
-
- CeresMutexLock l(&cell_info->m);
- MatrixRef m(cell_info->values, row_stride, col_stride);
- Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize>
- block(m, r, c, block1_size, block2_size);
- block.noalias() += b1.transpose() * b2;
}
}
}
