1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
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.
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.
29 // Author: sameeragarwal@google.com (Sameer Agarwal)
31 // TODO(sameeragarwal): row_block_counter can perhaps be replaced by
34 #ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_
35 #define CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_
37 // Eigen has an internal threshold switching between different matrix
38 // multiplication algorithms. In particular for matrices larger than
39 // EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD it uses a cache friendly
40 // matrix matrix product algorithm that has a higher setup cost. For
41 // matrix sizes close to this threshold, especially when the matrices
42 // are thin and long, the default choice may not be optimal. This is
43 // the case for us, as the default choice causes a 30% performance
44 // regression when we moved from Eigen2 to Eigen3.
46 #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10
48 // This include must come before any #ifndef check on Ceres compile options.
49 #include "ceres/internal/port.h"
51 #ifdef CERES_USE_OPENMP
57 #include "ceres/block_random_access_matrix.h"
58 #include "ceres/block_sparse_matrix.h"
59 #include "ceres/block_structure.h"
60 #include "ceres/internal/eigen.h"
61 #include "ceres/internal/fixed_array.h"
62 #include "ceres/internal/scoped_ptr.h"
63 #include "ceres/invert_psd_matrix.h"
64 #include "ceres/map_util.h"
65 #include "ceres/schur_eliminator.h"
66 #include "ceres/small_blas.h"
67 #include "ceres/stl_util.h"
68 #include "Eigen/Dense"
69 #include "glog/logging.h"
74 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
75 SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::~SchurEliminator() {
76 STLDeleteElements(&rhs_locks_);
79 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
80 void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::Init(
81 int num_eliminate_blocks,
82 bool assume_full_rank_ete,
83 const CompressedRowBlockStructure* bs) {
84 CHECK_GT(num_eliminate_blocks, 0)
85 << "SchurComplementSolver cannot be initialized with "
86 << "num_eliminate_blocks = 0.";
88 num_eliminate_blocks_ = num_eliminate_blocks;
89 assume_full_rank_ete_ = assume_full_rank_ete;
91 const int num_col_blocks = bs->cols.size();
92 const int num_row_blocks = bs->rows.size();
96 lhs_row_layout_.clear();
99 // Add a map object for each block in the reduced linear system
100 // and build the row/column block structure of the reduced linear
102 lhs_row_layout_.resize(num_col_blocks - num_eliminate_blocks_);
103 for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
104 lhs_row_layout_[i - num_eliminate_blocks_] = lhs_num_rows;
105 lhs_num_rows += bs->cols[i].size;
109 // Iterate over the row blocks of A, and detect the chunks. The
110 // matrix should already have been ordered so that all rows
111 // containing the same y block are vertically contiguous. Along
112 // the way also compute the amount of space each chunk will need
113 // to perform the elimination.
114 while (r < num_row_blocks) {
115 const int chunk_block_id = bs->rows[r].cells.front().block_id;
116 if (chunk_block_id >= num_eliminate_blocks_) {
120 chunks_.push_back(Chunk());
121 Chunk& chunk = chunks_.back();
125 const int e_block_size = bs->cols[chunk_block_id].size;
127 // Add to the chunk until the first block in the row is
128 // different than the one in the first row for the chunk.
129 while (r + chunk.size < num_row_blocks) {
130 const CompressedRow& row = bs->rows[r + chunk.size];
131 if (row.cells.front().block_id != chunk_block_id) {
135 // Iterate over the blocks in the row, ignoring the first
136 // block since it is the one to be eliminated.
137 for (int c = 1; c < row.cells.size(); ++c) {
138 const Cell& cell = row.cells[c];
139 if (InsertIfNotPresent(
140 &(chunk.buffer_layout), cell.block_id, buffer_size)) {
141 buffer_size += e_block_size * bs->cols[cell.block_id].size;
145 buffer_size_ = std::max(buffer_size, buffer_size_);
149 CHECK_GT(chunk.size, 0);
152 const Chunk& chunk = chunks_.back();
154 uneliminated_row_begins_ = chunk.start + chunk.size;
155 if (num_threads_ > 1) {
156 random_shuffle(chunks_.begin(), chunks_.end());
159 buffer_.reset(new double[buffer_size_ * num_threads_]);
161 // chunk_outer_product_buffer_ only needs to store e_block_size *
162 // f_block_size, which is always less than buffer_size_, so we just
163 // allocate buffer_size_ per thread.
164 chunk_outer_product_buffer_.reset(new double[buffer_size_ * num_threads_]);
166 STLDeleteElements(&rhs_locks_);
167 rhs_locks_.resize(num_col_blocks - num_eliminate_blocks_);
168 for (int i = 0; i < num_col_blocks - num_eliminate_blocks_; ++i) {
169 rhs_locks_[i] = new Mutex;
173 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
175 SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::
176 Eliminate(const BlockSparseMatrix* A,
179 BlockRandomAccessMatrix* lhs,
181 if (lhs->num_rows() > 0) {
183 VectorRef(rhs, lhs->num_rows()).setZero();
186 const CompressedRowBlockStructure* bs = A->block_structure();
187 const int num_col_blocks = bs->cols.size();
189 // Add the diagonal to the schur complement.
191 #pragma omp parallel for num_threads(num_threads_) schedule(dynamic)
192 for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) {
193 const int block_id = i - num_eliminate_blocks_;
194 int r, c, row_stride, col_stride;
195 CellInfo* cell_info = lhs->GetCell(block_id, block_id,
197 &row_stride, &col_stride);
198 if (cell_info != NULL) {
199 const int block_size = bs->cols[i].size;
200 typename EigenTypes<Eigen::Dynamic>::ConstVectorRef
201 diag(D + bs->cols[i].position, block_size);
203 CeresMutexLock l(&cell_info->m);
204 MatrixRef m(cell_info->values, row_stride, col_stride);
205 m.block(r, c, block_size, block_size).diagonal()
206 += diag.array().square().matrix();
211 // Eliminate y blocks one chunk at a time. For each chunk, compute
212 // the entries of the normal equations and the gradient vector block
213 // corresponding to the y block and then apply Gaussian elimination
214 // to them. The matrix ete stores the normal matrix corresponding to
215 // the block being eliminated and array buffer_ contains the
216 // non-zero blocks in the row corresponding to this y block in the
217 // normal equations. This computation is done in
218 // ChunkDiagonalBlockAndGradient. UpdateRhs then applies gaussian
219 // elimination to the rhs of the normal equations, updating the rhs
220 // of the reduced linear system by modifying rhs blocks for all the
221 // z blocks that share a row block/residual term with the y
222 // block. EliminateRowOuterProduct does the corresponding operation
223 // for the lhs of the reduced linear system.
224 #pragma omp parallel for num_threads(num_threads_) schedule(dynamic)
225 for (int i = 0; i < chunks_.size(); ++i) {
226 #ifdef CERES_USE_OPENMP
227 int thread_id = omp_get_thread_num();
231 double* buffer = buffer_.get() + thread_id * buffer_size_;
232 const Chunk& chunk = chunks_[i];
233 const int e_block_id = bs->rows[chunk.start].cells.front().block_id;
234 const int e_block_size = bs->cols[e_block_id].size;
236 VectorRef(buffer, buffer_size_).setZero();
238 typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix
239 ete(e_block_size, e_block_size);
242 const typename EigenTypes<kEBlockSize>::ConstVectorRef
243 diag(D + bs->cols[e_block_id].position, e_block_size);
244 ete = diag.array().square().matrix().asDiagonal();
249 FixedArray<double, 8> g(e_block_size);
250 typename EigenTypes<kEBlockSize>::VectorRef gref(g.get(), e_block_size);
253 // We are going to be computing
255 // S += F'F - F'E(E'E)^{-1}E'F
257 // for each Chunk. The computation is broken down into a number of
258 // function calls as below.
260 // Compute the outer product of the e_blocks with themselves (ete
261 // = E'E). Compute the product of the e_blocks with the
262 // corresonding f_blocks (buffer = E'F), the gradient of the terms
263 // in this chunk (g) and add the outer product of the f_blocks to
264 // Schur complement (S += F'F).
265 ChunkDiagonalBlockAndGradient(
266 chunk, A, b, chunk.start, &ete, g.get(), buffer, lhs);
268 // Normally one wouldn't compute the inverse explicitly, but
269 // e_block_size will typically be a small number like 3, in
270 // which case its much faster to compute the inverse once and
271 // use it to multiply other matrices/vectors instead of doing a
272 // Solve call over and over again.
273 typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix inverse_ete =
274 InvertPSDMatrix<kEBlockSize>(assume_full_rank_ete_, ete);
276 // For the current chunk compute and update the rhs of the reduced
279 // rhs = F'b - F'E(E'E)^(-1) E'b
281 FixedArray<double, 8> inverse_ete_g(e_block_size);
282 MatrixVectorMultiply<kEBlockSize, kEBlockSize, 0>(
287 inverse_ete_g.get());
289 UpdateRhs(chunk, A, b, chunk.start, inverse_ete_g.get(), rhs);
291 // S -= F'E(E'E)^{-1}E'F
292 ChunkOuterProduct(bs, inverse_ete, buffer, chunk.buffer_layout, lhs);
295 // For rows with no e_blocks, the schur complement update reduces to
297 NoEBlockRowsUpdate(A, b, uneliminated_row_begins_, lhs, rhs);
300 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
302 SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::
303 BackSubstitute(const BlockSparseMatrix* A,
308 const CompressedRowBlockStructure* bs = A->block_structure();
309 #pragma omp parallel for num_threads(num_threads_) schedule(dynamic)
310 for (int i = 0; i < chunks_.size(); ++i) {
311 const Chunk& chunk = chunks_[i];
312 const int e_block_id = bs->rows[chunk.start].cells.front().block_id;
313 const int e_block_size = bs->cols[e_block_id].size;
315 double* y_ptr = y + bs->cols[e_block_id].position;
316 typename EigenTypes<kEBlockSize>::VectorRef y_block(y_ptr, e_block_size);
318 typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix
319 ete(e_block_size, e_block_size);
321 const typename EigenTypes<kEBlockSize>::ConstVectorRef
322 diag(D + bs->cols[e_block_id].position, e_block_size);
323 ete = diag.array().square().matrix().asDiagonal();
328 const double* values = A->values();
329 for (int j = 0; j < chunk.size; ++j) {
330 const CompressedRow& row = bs->rows[chunk.start + j];
331 const Cell& e_cell = row.cells.front();
332 DCHECK_EQ(e_block_id, e_cell.block_id);
334 FixedArray<double, 8> sj(row.block.size);
336 typename EigenTypes<kRowBlockSize>::VectorRef(sj.get(), row.block.size) =
337 typename EigenTypes<kRowBlockSize>::ConstVectorRef
338 (b + bs->rows[chunk.start + j].block.position, row.block.size);
340 for (int c = 1; c < row.cells.size(); ++c) {
341 const int f_block_id = row.cells[c].block_id;
342 const int f_block_size = bs->cols[f_block_id].size;
343 const int r_block = f_block_id - num_eliminate_blocks_;
345 MatrixVectorMultiply<kRowBlockSize, kFBlockSize, -1>(
346 values + row.cells[c].position, row.block.size, f_block_size,
347 z + lhs_row_layout_[r_block],
351 MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>(
352 values + e_cell.position, row.block.size, e_block_size,
356 MatrixTransposeMatrixMultiply
357 <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>(
358 values + e_cell.position, row.block.size, e_block_size,
359 values + e_cell.position, row.block.size, e_block_size,
360 ete.data(), 0, 0, e_block_size, e_block_size);
363 y_block = InvertPSDMatrix<kEBlockSize>(assume_full_rank_ete_, ete)
368 // Update the rhs of the reduced linear system. Compute
370 // F'b - F'E(E'E)^(-1) E'b
371 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
373 SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::
374 UpdateRhs(const Chunk& chunk,
375 const BlockSparseMatrix* A,
377 int row_block_counter,
378 const double* inverse_ete_g,
380 const CompressedRowBlockStructure* bs = A->block_structure();
381 const int e_block_id = bs->rows[chunk.start].cells.front().block_id;
382 const int e_block_size = bs->cols[e_block_id].size;
384 int b_pos = bs->rows[row_block_counter].block.position;
385 const double* values = A->values();
386 for (int j = 0; j < chunk.size; ++j) {
387 const CompressedRow& row = bs->rows[row_block_counter + j];
388 const Cell& e_cell = row.cells.front();
390 typename EigenTypes<kRowBlockSize>::Vector sj =
391 typename EigenTypes<kRowBlockSize>::ConstVectorRef
392 (b + b_pos, row.block.size);
394 MatrixVectorMultiply<kRowBlockSize, kEBlockSize, -1>(
395 values + e_cell.position, row.block.size, e_block_size,
396 inverse_ete_g, sj.data());
398 for (int c = 1; c < row.cells.size(); ++c) {
399 const int block_id = row.cells[c].block_id;
400 const int block_size = bs->cols[block_id].size;
401 const int block = block_id - num_eliminate_blocks_;
402 CeresMutexLock l(rhs_locks_[block]);
403 MatrixTransposeVectorMultiply<kRowBlockSize, kFBlockSize, 1>(
404 values + row.cells[c].position,
405 row.block.size, block_size,
406 sj.data(), rhs + lhs_row_layout_[block]);
408 b_pos += row.block.size;
412 // Given a Chunk - set of rows with the same e_block, e.g. in the
413 // following Chunk with two rows.
416 // [ y11 0 0 0 | z11 0 0 0 z51]
417 // [ y12 0 0 0 | z12 z22 0 0 0]
419 // this function computes twp matrices. The diagonal block matrix
421 // ete = y11 * y11' + y12 * y12'
423 // and the off diagonal blocks in the Guass Newton Hessian.
425 // buffer = [y11'(z11 + z12), y12' * z22, y11' * z51]
427 // which are zero compressed versions of the block sparse matrices E'E
430 // and the gradient of the e_block, E'b.
431 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
433 SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::
434 ChunkDiagonalBlockAndGradient(
436 const BlockSparseMatrix* A,
438 int row_block_counter,
439 typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* ete,
442 BlockRandomAccessMatrix* lhs) {
443 const CompressedRowBlockStructure* bs = A->block_structure();
445 int b_pos = bs->rows[row_block_counter].block.position;
446 const int e_block_size = ete->rows();
448 // Iterate over the rows in this chunk, for each row, compute the
449 // contribution of its F blocks to the Schur complement, the
450 // contribution of its E block to the matrix EE' (ete), and the
451 // corresponding block in the gradient vector.
452 const double* values = A->values();
453 for (int j = 0; j < chunk.size; ++j) {
454 const CompressedRow& row = bs->rows[row_block_counter + j];
456 if (row.cells.size() > 1) {
457 EBlockRowOuterProduct(A, row_block_counter + j, lhs);
460 // Extract the e_block, ETE += E_i' E_i
461 const Cell& e_cell = row.cells.front();
462 MatrixTransposeMatrixMultiply
463 <kRowBlockSize, kEBlockSize, kRowBlockSize, kEBlockSize, 1>(
464 values + e_cell.position, row.block.size, e_block_size,
465 values + e_cell.position, row.block.size, e_block_size,
466 ete->data(), 0, 0, e_block_size, e_block_size);
469 MatrixTransposeVectorMultiply<kRowBlockSize, kEBlockSize, 1>(
470 values + e_cell.position, row.block.size, e_block_size,
475 // buffer = E'F. This computation is done by iterating over the
476 // f_blocks for each row in the chunk.
477 for (int c = 1; c < row.cells.size(); ++c) {
478 const int f_block_id = row.cells[c].block_id;
479 const int f_block_size = bs->cols[f_block_id].size;
481 buffer + FindOrDie(chunk.buffer_layout, f_block_id);
482 MatrixTransposeMatrixMultiply
483 <kRowBlockSize, kEBlockSize, kRowBlockSize, kFBlockSize, 1>(
484 values + e_cell.position, row.block.size, e_block_size,
485 values + row.cells[c].position, row.block.size, f_block_size,
486 buffer_ptr, 0, 0, e_block_size, f_block_size);
488 b_pos += row.block.size;
492 // Compute the outer product F'E(E'E)^{-1}E'F and subtract it from the
493 // Schur complement matrix, i.e
495 // S -= F'E(E'E)^{-1}E'F.
496 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
498 SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::
499 ChunkOuterProduct(const CompressedRowBlockStructure* bs,
500 const Matrix& inverse_ete,
501 const double* buffer,
502 const BufferLayoutType& buffer_layout,
503 BlockRandomAccessMatrix* lhs) {
504 // This is the most computationally expensive part of this
505 // code. Profiling experiments reveal that the bottleneck is not the
506 // computation of the right-hand matrix product, but memory
507 // references to the left hand side.
508 const int e_block_size = inverse_ete.rows();
509 BufferLayoutType::const_iterator it1 = buffer_layout.begin();
511 #ifdef CERES_USE_OPENMP
512 int thread_id = omp_get_thread_num();
516 double* b1_transpose_inverse_ete =
517 chunk_outer_product_buffer_.get() + thread_id * buffer_size_;
519 // S(i,j) -= bi' * ete^{-1} b_j
520 for (; it1 != buffer_layout.end(); ++it1) {
521 const int block1 = it1->first - num_eliminate_blocks_;
522 const int block1_size = bs->cols[it1->first].size;
523 MatrixTransposeMatrixMultiply
524 <kEBlockSize, kFBlockSize, kEBlockSize, kEBlockSize, 0>(
525 buffer + it1->second, e_block_size, block1_size,
526 inverse_ete.data(), e_block_size, e_block_size,
527 b1_transpose_inverse_ete, 0, 0, block1_size, e_block_size);
529 BufferLayoutType::const_iterator it2 = it1;
530 for (; it2 != buffer_layout.end(); ++it2) {
531 const int block2 = it2->first - num_eliminate_blocks_;
533 int r, c, row_stride, col_stride;
534 CellInfo* cell_info = lhs->GetCell(block1, block2,
536 &row_stride, &col_stride);
537 if (cell_info != NULL) {
538 const int block2_size = bs->cols[it2->first].size;
539 CeresMutexLock l(&cell_info->m);
541 <kFBlockSize, kEBlockSize, kEBlockSize, kFBlockSize, -1>(
542 b1_transpose_inverse_ete, block1_size, e_block_size,
543 buffer + it2->second, e_block_size, block2_size,
544 cell_info->values, r, c, row_stride, col_stride);
550 // For rows with no e_blocks, the schur complement update reduces to S
551 // += F'F. This function iterates over the rows of A with no e_block,
552 // and calls NoEBlockRowOuterProduct on each row.
553 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
555 SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::
556 NoEBlockRowsUpdate(const BlockSparseMatrix* A,
558 int row_block_counter,
559 BlockRandomAccessMatrix* lhs,
561 const CompressedRowBlockStructure* bs = A->block_structure();
562 const double* values = A->values();
563 for (; row_block_counter < bs->rows.size(); ++row_block_counter) {
564 const CompressedRow& row = bs->rows[row_block_counter];
565 for (int c = 0; c < row.cells.size(); ++c) {
566 const int block_id = row.cells[c].block_id;
567 const int block_size = bs->cols[block_id].size;
568 const int block = block_id - num_eliminate_blocks_;
569 MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
570 values + row.cells[c].position, row.block.size, block_size,
571 b + row.block.position,
572 rhs + lhs_row_layout_[block]);
574 NoEBlockRowOuterProduct(A, row_block_counter, lhs);
579 // A row r of A, which has no e_blocks gets added to the Schur
580 // Complement as S += r r'. This function is responsible for computing
581 // the contribution of a single row r to the Schur complement. It is
582 // very similar in structure to EBlockRowOuterProduct except for
583 // one difference. It does not use any of the template
584 // parameters. This is because the algorithm used for detecting the
585 // static structure of the matrix A only pays attention to rows with
586 // e_blocks. This is becase rows without e_blocks are rare and
587 // typically arise from regularization terms in the original
588 // optimization problem, and have a very different structure than the
589 // rows with e_blocks. Including them in the static structure
590 // detection will lead to most template parameters being set to
591 // dynamic. Since the number of rows without e_blocks is small, the
592 // lack of templating is not an issue.
593 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
595 SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::
596 NoEBlockRowOuterProduct(const BlockSparseMatrix* A,
598 BlockRandomAccessMatrix* lhs) {
599 const CompressedRowBlockStructure* bs = A->block_structure();
600 const CompressedRow& row = bs->rows[row_block_index];
601 const double* values = A->values();
602 for (int i = 0; i < row.cells.size(); ++i) {
603 const int block1 = row.cells[i].block_id - num_eliminate_blocks_;
604 DCHECK_GE(block1, 0);
606 const int block1_size = bs->cols[row.cells[i].block_id].size;
607 int r, c, row_stride, col_stride;
608 CellInfo* cell_info = lhs->GetCell(block1, block1,
610 &row_stride, &col_stride);
611 if (cell_info != NULL) {
612 CeresMutexLock l(&cell_info->m);
613 // This multiply currently ignores the fact that this is a
614 // symmetric outer product.
615 MatrixTransposeMatrixMultiply
616 <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>(
617 values + row.cells[i].position, row.block.size, block1_size,
618 values + row.cells[i].position, row.block.size, block1_size,
619 cell_info->values, r, c, row_stride, col_stride);
622 for (int j = i + 1; j < row.cells.size(); ++j) {
623 const int block2 = row.cells[j].block_id - num_eliminate_blocks_;
624 DCHECK_GE(block2, 0);
625 DCHECK_LT(block1, block2);
626 int r, c, row_stride, col_stride;
627 CellInfo* cell_info = lhs->GetCell(block1, block2,
629 &row_stride, &col_stride);
630 if (cell_info != NULL) {
631 const int block2_size = bs->cols[row.cells[j].block_id].size;
632 CeresMutexLock l(&cell_info->m);
633 MatrixTransposeMatrixMultiply
634 <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>(
635 values + row.cells[i].position, row.block.size, block1_size,
636 values + row.cells[j].position, row.block.size, block2_size,
637 cell_info->values, r, c, row_stride, col_stride);
643 // For a row with an e_block, compute the contribition S += F'F. This
644 // function has the same structure as NoEBlockRowOuterProduct, except
645 // that this function uses the template parameters.
646 template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
648 SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::
649 EBlockRowOuterProduct(const BlockSparseMatrix* A,
651 BlockRandomAccessMatrix* lhs) {
652 const CompressedRowBlockStructure* bs = A->block_structure();
653 const CompressedRow& row = bs->rows[row_block_index];
654 const double* values = A->values();
655 for (int i = 1; i < row.cells.size(); ++i) {
656 const int block1 = row.cells[i].block_id - num_eliminate_blocks_;
657 DCHECK_GE(block1, 0);
659 const int block1_size = bs->cols[row.cells[i].block_id].size;
660 int r, c, row_stride, col_stride;
661 CellInfo* cell_info = lhs->GetCell(block1, block1,
663 &row_stride, &col_stride);
664 if (cell_info != NULL) {
665 CeresMutexLock l(&cell_info->m);
666 // block += b1.transpose() * b1;
667 MatrixTransposeMatrixMultiply
668 <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>(
669 values + row.cells[i].position, row.block.size, block1_size,
670 values + row.cells[i].position, row.block.size, block1_size,
671 cell_info->values, r, c, row_stride, col_stride);
674 for (int j = i + 1; j < row.cells.size(); ++j) {
675 const int block2 = row.cells[j].block_id - num_eliminate_blocks_;
676 DCHECK_GE(block2, 0);
677 DCHECK_LT(block1, block2);
678 const int block2_size = bs->cols[row.cells[j].block_id].size;
679 int r, c, row_stride, col_stride;
680 CellInfo* cell_info = lhs->GetCell(block1, block2,
682 &row_stride, &col_stride);
683 if (cell_info != NULL) {
684 // block += b1.transpose() * b2;
685 CeresMutexLock l(&cell_info->m);
686 MatrixTransposeMatrixMultiply
687 <kRowBlockSize, kFBlockSize, kRowBlockSize, kFBlockSize, 1>(
688 values + row.cells[i].position, row.block.size, block1_size,
689 values + row.cells[j].position, row.block.size, block2_size,
690 cell_info->values, r, c, row_stride, col_stride);
696 } // namespace internal
699 #endif // CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_