Ceres: Update to the latest actual version
[blender.git] / extern / ceres / internal / ceres / schur_complement_solver.cc
1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 //   this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 //   this list of conditions and the following disclaimer in the documentation
12 //   and/or other materials provided with the distribution.
13 // * Neither the name of Google Inc. nor the names of its contributors may be
14 //   used to endorse or promote products derived from this software without
15 //   specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // Author: sameeragarwal@google.com (Sameer Agarwal)
30
31 #include "ceres/internal/port.h"
32
33 #include <algorithm>
34 #include <ctime>
35 #include <set>
36 #include <sstream>
37 #include <vector>
38
39 #include "ceres/block_random_access_dense_matrix.h"
40 #include "ceres/block_random_access_matrix.h"
41 #include "ceres/block_random_access_sparse_matrix.h"
42 #include "ceres/block_sparse_matrix.h"
43 #include "ceres/block_structure.h"
44 #include "ceres/conjugate_gradients_solver.h"
45 #include "ceres/cxsparse.h"
46 #include "ceres/detect_structure.h"
47 #include "ceres/internal/eigen.h"
48 #include "ceres/internal/scoped_ptr.h"
49 #include "ceres/lapack.h"
50 #include "ceres/linear_solver.h"
51 #include "ceres/schur_complement_solver.h"
52 #include "ceres/suitesparse.h"
53 #include "ceres/triplet_sparse_matrix.h"
54 #include "ceres/types.h"
55 #include "ceres/wall_time.h"
56 #include "Eigen/Dense"
57 #include "Eigen/SparseCore"
58
59 namespace ceres {
60 namespace internal {
61
62 using std::make_pair;
63 using std::pair;
64 using std::set;
65 using std::vector;
66
67 namespace {
68
69 class BlockRandomAccessSparseMatrixAdapter : public LinearOperator {
70  public:
71   explicit BlockRandomAccessSparseMatrixAdapter(
72       const BlockRandomAccessSparseMatrix& m)
73       : m_(m) {
74   }
75
76   virtual ~BlockRandomAccessSparseMatrixAdapter() {}
77
78   // y = y + Ax;
79   virtual void RightMultiply(const double* x, double* y) const {
80     m_.SymmetricRightMultiply(x, y);
81   }
82
83   // y = y + A'x;
84   virtual void LeftMultiply(const double* x, double* y) const {
85     m_.SymmetricRightMultiply(x, y);
86   }
87
88   virtual int num_rows() const { return m_.num_rows(); }
89   virtual int num_cols() const { return m_.num_rows(); }
90
91  private:
92   const BlockRandomAccessSparseMatrix& m_;
93 };
94
95 class BlockRandomAccessDiagonalMatrixAdapter : public LinearOperator {
96  public:
97   explicit BlockRandomAccessDiagonalMatrixAdapter(
98       const BlockRandomAccessDiagonalMatrix& m)
99       : m_(m) {
100   }
101
102   virtual ~BlockRandomAccessDiagonalMatrixAdapter() {}
103
104   // y = y + Ax;
105   virtual void RightMultiply(const double* x, double* y) const {
106     m_.RightMultiply(x, y);
107   }
108
109   // y = y + A'x;
110   virtual void LeftMultiply(const double* x, double* y) const {
111     m_.RightMultiply(x, y);
112   }
113
114   virtual int num_rows() const { return m_.num_rows(); }
115   virtual int num_cols() const { return m_.num_rows(); }
116
117  private:
118   const BlockRandomAccessDiagonalMatrix& m_;
119 };
120
121 } // namespace
122
123 LinearSolver::Summary SchurComplementSolver::SolveImpl(
124     BlockSparseMatrix* A,
125     const double* b,
126     const LinearSolver::PerSolveOptions& per_solve_options,
127     double* x) {
128   EventLogger event_logger("SchurComplementSolver::Solve");
129
130   if (eliminator_.get() == NULL) {
131     InitStorage(A->block_structure());
132     DetectStructure(*A->block_structure(),
133                     options_.elimination_groups[0],
134                     &options_.row_block_size,
135                     &options_.e_block_size,
136                     &options_.f_block_size);
137     eliminator_.reset(CHECK_NOTNULL(SchurEliminatorBase::Create(options_)));
138     eliminator_->Init(options_.elimination_groups[0], A->block_structure());
139   };
140   std::fill(x, x + A->num_cols(), 0.0);
141   event_logger.AddEvent("Setup");
142
143   eliminator_->Eliminate(A, b, per_solve_options.D, lhs_.get(), rhs_.get());
144   event_logger.AddEvent("Eliminate");
145
146   double* reduced_solution = x + A->num_cols() - lhs_->num_cols();
147   const LinearSolver::Summary summary =
148       SolveReducedLinearSystem(per_solve_options, reduced_solution);
149   event_logger.AddEvent("ReducedSolve");
150
151   if (summary.termination_type == LINEAR_SOLVER_SUCCESS) {
152     eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x);
153     event_logger.AddEvent("BackSubstitute");
154   }
155
156   return summary;
157 }
158
159 // Initialize a BlockRandomAccessDenseMatrix to store the Schur
160 // complement.
161 void DenseSchurComplementSolver::InitStorage(
162     const CompressedRowBlockStructure* bs) {
163   const int num_eliminate_blocks = options().elimination_groups[0];
164   const int num_col_blocks = bs->cols.size();
165
166   vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0);
167   for (int i = num_eliminate_blocks, j = 0;
168        i < num_col_blocks;
169        ++i, ++j) {
170     blocks[j] = bs->cols[i].size;
171   }
172
173   set_lhs(new BlockRandomAccessDenseMatrix(blocks));
174   set_rhs(new double[lhs()->num_rows()]);
175 }
176
177 // Solve the system Sx = r, assuming that the matrix S is stored in a
178 // BlockRandomAccessDenseMatrix. The linear system is solved using
179 // Eigen's Cholesky factorization.
180 LinearSolver::Summary
181 DenseSchurComplementSolver::SolveReducedLinearSystem(
182     const LinearSolver::PerSolveOptions& per_solve_options,
183     double* solution) {
184   LinearSolver::Summary summary;
185   summary.num_iterations = 0;
186   summary.termination_type = LINEAR_SOLVER_SUCCESS;
187   summary.message = "Success.";
188
189   const BlockRandomAccessDenseMatrix* m =
190       down_cast<const BlockRandomAccessDenseMatrix*>(lhs());
191   const int num_rows = m->num_rows();
192
193   // The case where there are no f blocks, and the system is block
194   // diagonal.
195   if (num_rows == 0) {
196     return summary;
197   }
198
199   summary.num_iterations = 1;
200
201   if (options().dense_linear_algebra_library_type == EIGEN) {
202     Eigen::LLT<Matrix, Eigen::Upper> llt =
203         ConstMatrixRef(m->values(), num_rows, num_rows)
204         .selfadjointView<Eigen::Upper>()
205         .llt();
206     if (llt.info() != Eigen::Success) {
207       summary.termination_type = LINEAR_SOLVER_FAILURE;
208       summary.message =
209           "Eigen failure. Unable to perform dense Cholesky factorization.";
210       return summary;
211     }
212
213     VectorRef(solution, num_rows) = llt.solve(ConstVectorRef(rhs(), num_rows));
214   } else {
215     VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows);
216     summary.termination_type =
217         LAPACK::SolveInPlaceUsingCholesky(num_rows,
218                                           m->values(),
219                                           solution,
220                                           &summary.message);
221   }
222
223   return summary;
224 }
225
226 SparseSchurComplementSolver::SparseSchurComplementSolver(
227     const LinearSolver::Options& options)
228     : SchurComplementSolver(options),
229       factor_(NULL),
230       cxsparse_factor_(NULL) {
231 }
232
233 SparseSchurComplementSolver::~SparseSchurComplementSolver() {
234   if (factor_ != NULL) {
235     ss_.Free(factor_);
236     factor_ = NULL;
237   }
238
239   if (cxsparse_factor_ != NULL) {
240     cxsparse_.Free(cxsparse_factor_);
241     cxsparse_factor_ = NULL;
242   }
243 }
244
245 // Determine the non-zero blocks in the Schur Complement matrix, and
246 // initialize a BlockRandomAccessSparseMatrix object.
247 void SparseSchurComplementSolver::InitStorage(
248     const CompressedRowBlockStructure* bs) {
249   const int num_eliminate_blocks = options().elimination_groups[0];
250   const int num_col_blocks = bs->cols.size();
251   const int num_row_blocks = bs->rows.size();
252
253   blocks_.resize(num_col_blocks - num_eliminate_blocks, 0);
254   for (int i = num_eliminate_blocks; i < num_col_blocks; ++i) {
255     blocks_[i - num_eliminate_blocks] = bs->cols[i].size;
256   }
257
258   set<pair<int, int> > block_pairs;
259   for (int i = 0; i < blocks_.size(); ++i) {
260     block_pairs.insert(make_pair(i, i));
261   }
262
263   int r = 0;
264   while (r < num_row_blocks) {
265     int e_block_id = bs->rows[r].cells.front().block_id;
266     if (e_block_id >= num_eliminate_blocks) {
267       break;
268     }
269     vector<int> f_blocks;
270
271     // Add to the chunk until the first block in the row is
272     // different than the one in the first row for the chunk.
273     for (; r < num_row_blocks; ++r) {
274       const CompressedRow& row = bs->rows[r];
275       if (row.cells.front().block_id != e_block_id) {
276         break;
277       }
278
279       // Iterate over the blocks in the row, ignoring the first
280       // block since it is the one to be eliminated.
281       for (int c = 1; c < row.cells.size(); ++c) {
282         const Cell& cell = row.cells[c];
283         f_blocks.push_back(cell.block_id - num_eliminate_blocks);
284       }
285     }
286
287     sort(f_blocks.begin(), f_blocks.end());
288     f_blocks.erase(unique(f_blocks.begin(), f_blocks.end()), f_blocks.end());
289     for (int i = 0; i < f_blocks.size(); ++i) {
290       for (int j = i + 1; j < f_blocks.size(); ++j) {
291         block_pairs.insert(make_pair(f_blocks[i], f_blocks[j]));
292       }
293     }
294   }
295
296   // Remaing rows do not contribute to the chunks and directly go
297   // into the schur complement via an outer product.
298   for (; r < num_row_blocks; ++r) {
299     const CompressedRow& row = bs->rows[r];
300     CHECK_GE(row.cells.front().block_id, num_eliminate_blocks);
301     for (int i = 0; i < row.cells.size(); ++i) {
302       int r_block1_id = row.cells[i].block_id - num_eliminate_blocks;
303       for (int j = 0; j < row.cells.size(); ++j) {
304         int r_block2_id = row.cells[j].block_id - num_eliminate_blocks;
305         if (r_block1_id <= r_block2_id) {
306           block_pairs.insert(make_pair(r_block1_id, r_block2_id));
307         }
308       }
309     }
310   }
311
312   set_lhs(new BlockRandomAccessSparseMatrix(blocks_, block_pairs));
313   set_rhs(new double[lhs()->num_rows()]);
314 }
315
316 LinearSolver::Summary
317 SparseSchurComplementSolver::SolveReducedLinearSystem(
318           const LinearSolver::PerSolveOptions& per_solve_options,
319           double* solution) {
320   if (options().type == ITERATIVE_SCHUR) {
321     CHECK(options().use_explicit_schur_complement);
322     return SolveReducedLinearSystemUsingConjugateGradients(per_solve_options,
323                                                            solution);
324   }
325
326   switch (options().sparse_linear_algebra_library_type) {
327     case SUITE_SPARSE:
328       return SolveReducedLinearSystemUsingSuiteSparse(per_solve_options,
329                                                       solution);
330     case CX_SPARSE:
331       return SolveReducedLinearSystemUsingCXSparse(per_solve_options,
332                                                    solution);
333     case EIGEN_SPARSE:
334       return SolveReducedLinearSystemUsingEigen(per_solve_options,
335                                                 solution);
336     default:
337       LOG(FATAL) << "Unknown sparse linear algebra library : "
338                  << options().sparse_linear_algebra_library_type;
339   }
340
341   return LinearSolver::Summary();
342 }
343
344 // Solve the system Sx = r, assuming that the matrix S is stored in a
345 // BlockRandomAccessSparseMatrix.  The linear system is solved using
346 // CHOLMOD's sparse cholesky factorization routines.
347 LinearSolver::Summary
348 SparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse(
349     const LinearSolver::PerSolveOptions& per_solve_options,
350     double* solution) {
351 #ifdef CERES_NO_SUITESPARSE
352
353   LinearSolver::Summary summary;
354   summary.num_iterations = 0;
355   summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
356   summary.message = "Ceres was not built with SuiteSparse support. "
357       "Therefore, SPARSE_SCHUR cannot be used with SUITE_SPARSE";
358   return summary;
359
360 #else
361
362   LinearSolver::Summary summary;
363   summary.num_iterations = 0;
364   summary.termination_type = LINEAR_SOLVER_SUCCESS;
365   summary.message = "Success.";
366
367   TripletSparseMatrix* tsm =
368       const_cast<TripletSparseMatrix*>(
369           down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix());
370   const int num_rows = tsm->num_rows();
371
372   // The case where there are no f blocks, and the system is block
373   // diagonal.
374   if (num_rows == 0) {
375     return summary;
376   }
377
378   summary.num_iterations = 1;
379   cholmod_sparse* cholmod_lhs = NULL;
380   if (options().use_postordering) {
381     // If we are going to do a full symbolic analysis of the schur
382     // complement matrix from scratch and not rely on the
383     // pre-ordering, then the fastest path in cholmod_factorize is the
384     // one corresponding to upper triangular matrices.
385
386     // Create a upper triangular symmetric matrix.
387     cholmod_lhs = ss_.CreateSparseMatrix(tsm);
388     cholmod_lhs->stype = 1;
389
390     if (factor_ == NULL) {
391       factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs,
392                                          blocks_,
393                                          blocks_,
394                                          &summary.message);
395     }
396   } else {
397     // If we are going to use the natural ordering (i.e. rely on the
398     // pre-ordering computed by solver_impl.cc), then the fastest
399     // path in cholmod_factorize is the one corresponding to lower
400     // triangular matrices.
401
402     // Create a upper triangular symmetric matrix.
403     cholmod_lhs = ss_.CreateSparseMatrixTranspose(tsm);
404     cholmod_lhs->stype = -1;
405
406     if (factor_ == NULL) {
407       factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(cholmod_lhs,
408                                                        &summary.message);
409     }
410   }
411
412   if (factor_ == NULL) {
413     ss_.Free(cholmod_lhs);
414     summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
415     // No need to set message as it has already been set by the
416     // symbolic analysis routines above.
417     return summary;
418   }
419
420   summary.termination_type =
421     ss_.Cholesky(cholmod_lhs, factor_, &summary.message);
422
423   ss_.Free(cholmod_lhs);
424
425   if (summary.termination_type != LINEAR_SOLVER_SUCCESS) {
426     // No need to set message as it has already been set by the
427     // numeric factorization routine above.
428     return summary;
429   }
430
431   cholmod_dense*  cholmod_rhs =
432       ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows);
433   cholmod_dense* cholmod_solution = ss_.Solve(factor_,
434                                               cholmod_rhs,
435                                               &summary.message);
436   ss_.Free(cholmod_rhs);
437
438   if (cholmod_solution == NULL) {
439     summary.message =
440         "SuiteSparse failure. Unable to perform triangular solve.";
441     summary.termination_type = LINEAR_SOLVER_FAILURE;
442     return summary;
443   }
444
445   VectorRef(solution, num_rows)
446       = VectorRef(static_cast<double*>(cholmod_solution->x), num_rows);
447   ss_.Free(cholmod_solution);
448   return summary;
449 #endif  // CERES_NO_SUITESPARSE
450 }
451
452 // Solve the system Sx = r, assuming that the matrix S is stored in a
453 // BlockRandomAccessSparseMatrix.  The linear system is solved using
454 // CXSparse's sparse cholesky factorization routines.
455 LinearSolver::Summary
456 SparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse(
457     const LinearSolver::PerSolveOptions& per_solve_options,
458     double* solution) {
459 #ifdef CERES_NO_CXSPARSE
460
461   LinearSolver::Summary summary;
462   summary.num_iterations = 0;
463   summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
464   summary.message = "Ceres was not built with CXSparse support. "
465       "Therefore, SPARSE_SCHUR cannot be used with CX_SPARSE";
466   return summary;
467
468 #else
469
470   LinearSolver::Summary summary;
471   summary.num_iterations = 0;
472   summary.termination_type = LINEAR_SOLVER_SUCCESS;
473   summary.message = "Success.";
474
475   // Extract the TripletSparseMatrix that is used for actually storing S.
476   TripletSparseMatrix* tsm =
477       const_cast<TripletSparseMatrix*>(
478           down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix());
479   const int num_rows = tsm->num_rows();
480
481   // The case where there are no f blocks, and the system is block
482   // diagonal.
483   if (num_rows == 0) {
484     return summary;
485   }
486
487   cs_di* lhs = CHECK_NOTNULL(cxsparse_.CreateSparseMatrix(tsm));
488   VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows);
489
490   // Compute symbolic factorization if not available.
491   if (cxsparse_factor_ == NULL) {
492     cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(lhs, blocks_, blocks_);
493   }
494
495   if (cxsparse_factor_ == NULL) {
496     summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
497     summary.message =
498         "CXSparse failure. Unable to find symbolic factorization.";
499   } else if (!cxsparse_.SolveCholesky(lhs, cxsparse_factor_, solution)) {
500     summary.termination_type = LINEAR_SOLVER_FAILURE;
501     summary.message = "CXSparse::SolveCholesky failed.";
502   }
503
504   cxsparse_.Free(lhs);
505   return summary;
506 #endif  // CERES_NO_CXPARSE
507 }
508
509 // Solve the system Sx = r, assuming that the matrix S is stored in a
510 // BlockRandomAccessSparseMatrix.  The linear system is solved using
511 // Eigen's sparse cholesky factorization routines.
512 LinearSolver::Summary
513 SparseSchurComplementSolver::SolveReducedLinearSystemUsingEigen(
514     const LinearSolver::PerSolveOptions& per_solve_options,
515     double* solution) {
516 #ifndef CERES_USE_EIGEN_SPARSE
517
518   LinearSolver::Summary summary;
519   summary.num_iterations = 0;
520   summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
521   summary.message =
522       "SPARSE_SCHUR cannot be used with EIGEN_SPARSE. "
523       "Ceres was not built with support for "
524       "Eigen's SimplicialLDLT decomposition. "
525       "This requires enabling building with -DEIGENSPARSE=ON.";
526   return summary;
527
528 #else
529   EventLogger event_logger("SchurComplementSolver::EigenSolve");
530   LinearSolver::Summary summary;
531   summary.num_iterations = 0;
532   summary.termination_type = LINEAR_SOLVER_SUCCESS;
533   summary.message = "Success.";
534
535   // Extract the TripletSparseMatrix that is used for actually storing S.
536   TripletSparseMatrix* tsm =
537       const_cast<TripletSparseMatrix*>(
538           down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix());
539   const int num_rows = tsm->num_rows();
540
541   // The case where there are no f blocks, and the system is block
542   // diagonal.
543   if (num_rows == 0) {
544     return summary;
545   }
546
547   // This is an upper triangular matrix.
548   CompressedRowSparseMatrix crsm(*tsm);
549   // Map this to a column major, lower triangular matrix.
550   Eigen::MappedSparseMatrix<double, Eigen::ColMajor> eigen_lhs(
551       crsm.num_rows(),
552       crsm.num_rows(),
553       crsm.num_nonzeros(),
554       crsm.mutable_rows(),
555       crsm.mutable_cols(),
556       crsm.mutable_values());
557   event_logger.AddEvent("ToCompressedRowSparseMatrix");
558
559   // Compute symbolic factorization if one does not exist.
560   if (simplicial_ldlt_.get() == NULL) {
561     simplicial_ldlt_.reset(new SimplicialLDLT);
562     // This ordering is quite bad. The scalar ordering produced by the
563     // AMD algorithm is quite bad and can be an order of magnitude
564     // worse than the one computed using the block version of the
565     // algorithm.
566     simplicial_ldlt_->analyzePattern(eigen_lhs);
567     if (VLOG_IS_ON(2)) {
568       std::stringstream ss;
569       simplicial_ldlt_->dumpMemory(ss);
570       VLOG(2) << "Symbolic Analysis\n"
571               << ss.str();
572     }
573     event_logger.AddEvent("Analysis");
574     if (simplicial_ldlt_->info() != Eigen::Success) {
575       summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
576       summary.message =
577           "Eigen failure. Unable to find symbolic factorization.";
578       return summary;
579     }
580   }
581
582   simplicial_ldlt_->factorize(eigen_lhs);
583   event_logger.AddEvent("Factorize");
584   if (simplicial_ldlt_->info() != Eigen::Success) {
585     summary.termination_type = LINEAR_SOLVER_FAILURE;
586     summary.message = "Eigen failure. Unable to find numeric factoriztion.";
587     return summary;
588   }
589
590   VectorRef(solution, num_rows) =
591       simplicial_ldlt_->solve(ConstVectorRef(rhs(), num_rows));
592   event_logger.AddEvent("Solve");
593   if (simplicial_ldlt_->info() != Eigen::Success) {
594     summary.termination_type = LINEAR_SOLVER_FAILURE;
595     summary.message = "Eigen failure. Unable to do triangular solve.";
596   }
597
598   return summary;
599 #endif  // CERES_USE_EIGEN_SPARSE
600 }
601
602 LinearSolver::Summary
603 SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients(
604     const LinearSolver::PerSolveOptions& per_solve_options,
605     double* solution) {
606   const int num_rows = lhs()->num_rows();
607   // The case where there are no f blocks, and the system is block
608   // diagonal.
609   if (num_rows == 0) {
610     LinearSolver::Summary summary;
611     summary.num_iterations = 0;
612     summary.termination_type = LINEAR_SOLVER_SUCCESS;
613     summary.message = "Success.";
614     return summary;
615   }
616
617   // Only SCHUR_JACOBI is supported over here right now.
618   CHECK_EQ(options().preconditioner_type, SCHUR_JACOBI);
619
620   if (preconditioner_.get() == NULL) {
621     preconditioner_.reset(new BlockRandomAccessDiagonalMatrix(blocks_));
622   }
623
624   BlockRandomAccessSparseMatrix* sc =
625       down_cast<BlockRandomAccessSparseMatrix*>(
626           const_cast<BlockRandomAccessMatrix*>(lhs()));
627
628   // Extract block diagonal from the Schur complement to construct the
629   // schur_jacobi preconditioner.
630   for (int i = 0; i  < blocks_.size(); ++i) {
631     const int block_size = blocks_[i];
632
633     int sc_r, sc_c, sc_row_stride, sc_col_stride;
634     CellInfo* sc_cell_info =
635         CHECK_NOTNULL(sc->GetCell(i, i,
636                                   &sc_r, &sc_c,
637                                   &sc_row_stride, &sc_col_stride));
638     MatrixRef sc_m(sc_cell_info->values, sc_row_stride, sc_col_stride);
639
640     int pre_r, pre_c, pre_row_stride, pre_col_stride;
641     CellInfo* pre_cell_info = CHECK_NOTNULL(
642         preconditioner_->GetCell(i, i,
643                                  &pre_r, &pre_c,
644                                  &pre_row_stride, &pre_col_stride));
645     MatrixRef pre_m(pre_cell_info->values, pre_row_stride, pre_col_stride);
646
647     pre_m.block(pre_r, pre_c, block_size, block_size) =
648         sc_m.block(sc_r, sc_c, block_size, block_size);
649   }
650   preconditioner_->Invert();
651
652   VectorRef(solution, num_rows).setZero();
653
654   scoped_ptr<LinearOperator> lhs_adapter(
655       new BlockRandomAccessSparseMatrixAdapter(*sc));
656   scoped_ptr<LinearOperator> preconditioner_adapter(
657       new BlockRandomAccessDiagonalMatrixAdapter(*preconditioner_));
658
659
660   LinearSolver::Options cg_options;
661   cg_options.min_num_iterations = options().min_num_iterations;
662   cg_options.max_num_iterations = options().max_num_iterations;
663   ConjugateGradientsSolver cg_solver(cg_options);
664
665   LinearSolver::PerSolveOptions cg_per_solve_options;
666   cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance;
667   cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance;
668   cg_per_solve_options.preconditioner = preconditioner_adapter.get();
669
670   return cg_solver.Solve(lhs_adapter.get(),
671                          rhs(),
672                          cg_per_solve_options,
673                          solution);
674 }
675
676 }  // namespace internal
677 }  // namespace ceres