diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index e4747ea..97d097c 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -234,6 +234,7 @@
   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 19b749b..ea49f07 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 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<int> 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 @@
 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<Cell>& 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<Cell>& 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<Eigen::Upper>()
-      //      .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<Eigen::Upper>()
-                 .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 3505a01..8579297 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 <vector>
+#include "ceres/block_random_access_diagonal_matrix.h"
+#include "ceres/internal/scoped_ptr.h"
 #include "ceres/preconditioner.h"
 
 namespace ceres {
@@ -39,7 +41,6 @@
 
 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 @@
   // 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<double*> blocks_;
-  std::vector<double> block_storage_;
-  int num_rows_;
-
-  // The block structure of the matrix this preconditioner is for (e.g. J).
-  const CompressedRowBlockStructure& block_structure_;
+  scoped_ptr<BlockRandomAccessDiagonalMatrix> 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 0000000..57c0909
--- /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 <vector>
+#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<LinearLeastSquaresProblem> problem(
+        CreateLinearLeastSquaresProblemFromId(problem_id));
+
+    CHECK_NOTNULL(problem.get());
+    A.reset(down_cast<BlockSparseMatrix*>(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<BlockRandomAccessDiagonalMatrix*>(&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<BlockSparseMatrix> A;
+  scoped_array<double> 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 d8bf4ef..b7ff331 100644
--- a/internal/ceres/block_random_access_diagonal_matrix.cc
+++ b/internal/ceres/block_random_access_diagonal_matrix.cc
@@ -34,16 +34,19 @@
 #include <set>
 #include <utility>
 #include <vector>
+#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<int>& blocks)
     : blocks_(blocks) {
@@ -51,9 +54,9 @@
   // rows/columns.
   int num_cols = 0;
   int num_nonzeros = 0;
-  vector<int> col_layout;
+  vector<int> 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 @@
   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::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<Eigen::Upper>()
+        .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 6b3cff2..ea99678 100644
--- a/internal/ceres/block_random_access_diagonal_matrix.h
+++ b/internal/ceres/block_random_access_diagonal_matrix.h
@@ -52,7 +52,7 @@
 class BlockRandomAccessDiagonalMatrix : public BlockRandomAccessMatrix {
  public:
   // blocks is an array of block sizes.
-  BlockRandomAccessDiagonalMatrix(const vector<int>& blocks);
+  explicit BlockRandomAccessDiagonalMatrix(const vector<int>& 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 @@
   // 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 e19268b..aa2d664 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<int> 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<int> 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<BlockRandomAccessDiagonalMatrix> 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 @@
   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 6dc9e89..cbdb708 100644
--- a/internal/ceres/schur_jacobi_preconditioner.cc
+++ b/internal/ceres/schur_jacobi_preconditioner.cc
@@ -32,7 +32,6 @@
 
 #include <utility>
 #include <vector>
-#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 @@
       << "Jacobian should have atleast 1 f_block for "
       << "SCHUR_JACOBI preconditioner.";
 
-  block_size_.resize(num_blocks);
+  vector<int> 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 @@
 
   // 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<BlockRandomAccessDiagonalMatrix*>(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<Eigen::Upper>()
-        .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 aecb015..8b528e2 100644
--- a/internal/ceres/schur_jacobi_preconditioner.h
+++ b/internal/ceres/schur_jacobi_preconditioner.h
@@ -94,11 +94,7 @@
   virtual bool UpdateImpl(const BlockSparseMatrix& A, const double* D);
 
   Preconditioner::Options options_;
-
-  // Sizes of the blocks in the schur complement.
-  vector<int> block_size_;
   scoped_ptr<SchurEliminatorBase> eliminator_;
-
   // Preconditioner matrix.
   scoped_ptr<BlockRandomAccessDiagonalMatrix> m_;
   CERES_DISALLOW_COPY_AND_ASSIGN(SchurJacobiPreconditioner);