1 files changed, 37 insertions, 4 deletions

diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc
index f789436364a..c43a9b78feb 100644
--- a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc
+++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc
@@ -54,9 +54,9 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix(
   // 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];
   }
 
@@ -88,6 +88,8 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix(
        ++it) {
     const int row_block_size = blocks_[it->first];
     const int col_block_size = blocks_[it->second];
+    cell_values_.push_back(make_pair(make_pair(it->first, it->second),
+                                     values + pos));
     layout_[IntPairToLong(it->first, it->second)] =
         new CellInfo(values + pos);
     pos += row_block_size * col_block_size;
@@ -105,8 +107,8 @@ BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix(
         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 +156,36 @@ void BlockRandomAccessSparseMatrix::SetZero() {
   }
 }
 
+void BlockRandomAccessSparseMatrix::SymmetricRightMultiply(const double* x,
+                                                           double* y) const {
+  vector< pair<pair<int, int>, double*> >::const_iterator it =
+      cell_values_.begin();
+  for (; it != cell_values_.end(); ++it) {
+    const int row = it->first.first;
+    const int row_block_size = blocks_[row];
+    const int row_block_pos = block_positions_[row];
+
+    const int col = it->first.second;
+    const int col_block_size = blocks_[col];
+    const int col_block_pos = block_positions_[col];
+
+    MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
+        it->second, 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, row_block_size, col_block_size,
+          x + row_block_pos,
+          y + col_block_pos);
+    }
+  }
+}
+
 }  // namespace internal
 }  // namespace ceres