10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/partitioned_matrix_view.h" 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <algorithm> 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cstring> 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_sparse_matrix.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_structure.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 41399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#include "ceres/small_blas.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongPartitionedMatrixView::PartitionedMatrixView( 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const BlockSparseMatrix& matrix, 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_col_blocks_a) 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : matrix_(matrix), 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_col_blocks_e_(num_col_blocks_a) { 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = matrix_.block_structure(); 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(bs); 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_col_blocks_f_ = bs->cols.size() - num_col_blocks_a; 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compute the number of row blocks in E. The number of row blocks 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // in E maybe less than the number of row blocks in the input matrix 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // as some of the row blocks at the bottom may not have any 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // e_blocks. For a definition of what an e_block is, please see 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // explicit_schur_complement_solver.h 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_row_blocks_e_ = 0; 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < bs->rows.size(); ++r) { 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<Cell>& cells = bs->rows[r].cells; 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cells[0].block_id < num_col_blocks_a) { 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++num_row_blocks_e_; 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compute the number of columns in E and F. 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_e_ = 0; 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_f_ = 0; 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = 0; c < bs->cols.size(); ++c) { 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Block& block = bs->cols[c]; 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (c < num_col_blocks_a) { 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_e_ += block.size; 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_f_ += block.size; 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(num_cols_e_ + num_cols_f_, matrix_.num_cols()); 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongPartitionedMatrixView::~PartitionedMatrixView() { 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The next four methods don't seem to be particularly cache 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// friendly. This is an artifact of how the BlockStructure of the 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// input matrix is constructed. These methods will benefit from 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// multithreading as well as improved data layout. 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const { 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = matrix_.block_structure(); 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over the first num_row_blocks_e_ row blocks, and multiply 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // by the first cell in each row block. 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* values = matrix_.values(); 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < num_row_blocks_e_; ++r) { 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Cell& cell = bs->rows[r].cells[0]; 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_pos = bs->rows[r].block.position; 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_size = bs->rows[r].block.size; 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_id = cell.block_id; 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_pos = bs->cols[col_block_id].position; 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_size = bs->cols[col_block_id].size; 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values + cell.position, row_block_size, col_block_size, 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x + col_block_pos, 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling y + row_block_pos); 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid PartitionedMatrixView::RightMultiplyF(const double* x, double* y) const { 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = matrix_.block_structure(); 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over row blocks, and if the row block is in E, then 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // multiply by all the cells except the first one which is of type 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // E. If the row block is not in E (i.e its in the bottom 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // num_row_blocks - num_row_blocks_e row blocks), then all the cells 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // are of type F and multiply by them all. 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* values = matrix_.values(); 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < bs->rows.size(); ++r) { 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_pos = bs->rows[r].block.position; 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_size = bs->rows[r].block.size; 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<Cell>& cells = bs->rows[r].cells; 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_id = cells[c].block_id; 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_pos = bs->cols[col_block_id].position; 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_size = bs->cols[col_block_id].size; 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values + cells[c].position, row_block_size, col_block_size, 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x + col_block_pos - num_cols_e(), 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling y + row_block_pos); 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid PartitionedMatrixView::LeftMultiplyE(const double* x, double* y) const { 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = matrix_.block_structure(); 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over the first num_row_blocks_e_ row blocks, and multiply 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // by the first cell in each row block. 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* values = matrix_.values(); 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < num_row_blocks_e_; ++r) { 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Cell& cell = bs->rows[r].cells[0]; 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_pos = bs->rows[r].block.position; 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_size = bs->rows[r].block.size; 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_id = cell.block_id; 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_pos = bs->cols[col_block_id].position; 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_size = bs->cols[col_block_id].size; 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values + cell.position, row_block_size, col_block_size, 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x + row_block_pos, 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling y + col_block_pos); 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid PartitionedMatrixView::LeftMultiplyF(const double* x, double* y) const { 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = matrix_.block_structure(); 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over row blocks, and if the row block is in E, then 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // multiply by all the cells except the first one which is of type 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // E. If the row block is not in E (i.e its in the bottom 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // num_row_blocks - num_row_blocks_e row blocks), then all the cells 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // are of type F and multiply by them all. 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* values = matrix_.values(); 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < bs->rows.size(); ++r) { 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_pos = bs->rows[r].block.position; 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_size = bs->rows[r].block.size; 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<Cell>& cells = bs->rows[r].cells; 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_id = cells[c].block_id; 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_pos = bs->cols[col_block_id].position; 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_size = bs->cols[col_block_id].size; 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values + cells[c].position, row_block_size, col_block_size, 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x + row_block_pos, 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling y + col_block_pos - num_cols_e()); 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Given a range of columns blocks of a matrix m, compute the block 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// structure of the block diagonal of the matrix m(:, 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// start_col_block:end_col_block)'m(:, start_col_block:end_col_block) 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and return a BlockSparseMatrix with the this block structure. The 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// caller owns the result. 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongBlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalMatrixLayout( 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int start_col_block, int end_col_block) const { 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = matrix_.block_structure(); 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRowBlockStructure* block_diagonal_structure = 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong new CompressedRowBlockStructure; 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int block_position = 0; 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int diagonal_cell_position = 0; 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over the column blocks, creating a new diagonal block for 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // each column block. 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = start_col_block; c < end_col_block; ++c) { 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Block& block = bs->cols[c]; 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_structure->cols.push_back(Block()); 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Block& diagonal_block = block_diagonal_structure->cols.back(); 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong diagonal_block.size = block.size; 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong diagonal_block.position = block_position; 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_structure->rows.push_back(CompressedRow()); 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRow& row = block_diagonal_structure->rows.back(); 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.block = diagonal_block; 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row.cells.push_back(Cell()); 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Cell& cell = row.cells.back(); 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cell.block_id = c - start_col_block; 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cell.position = diagonal_cell_position; 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_position += block.size; 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong diagonal_cell_position += block.size * block.size; 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Build a BlockSparseMatrix with the just computed block 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // structure. 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return new BlockSparseMatrix(block_diagonal_structure); 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongBlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalEtE() const { 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockSparseMatrix* block_diagonal = 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CreateBlockDiagonalMatrixLayout(0, num_col_blocks_e_); 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong UpdateBlockDiagonalEtE(block_diagonal); 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return block_diagonal; 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongBlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalFtF() const { 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockSparseMatrix* block_diagonal = 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CreateBlockDiagonalMatrixLayout( 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_col_blocks_e_, num_col_blocks_e_ + num_col_blocks_f_); 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong UpdateBlockDiagonalFtF(block_diagonal); 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return block_diagonal; 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Similar to the code in RightMultiplyE, except instead of the matrix 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// vector multiply its an outer product. 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// block_diagonal = block_diagonal(E'E) 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid PartitionedMatrixView::UpdateBlockDiagonalEtE( 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockSparseMatrix* block_diagonal) const { 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = matrix_.block_structure(); 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* block_diagonal_structure = 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal->block_structure(); 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal->SetZero(); 2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* values = matrix_.values(); 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < num_row_blocks_e_ ; ++r) { 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Cell& cell = bs->rows[r].cells[0]; 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_size = bs->rows[r].block.size; 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int block_id = cell.block_id; 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_size = bs->cols[block_id].size; 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int cell_position = 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_structure->rows[block_id].cells[0].position; 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixTransposeMatrixMultiply 2611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( 2621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values + cell.position, row_block_size, col_block_size, 2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values + cell.position, row_block_size, col_block_size, 2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_diagonal->mutable_values() + cell_position, 2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0, 0, col_block_size, col_block_size); 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Similar to the code in RightMultiplyF, except instead of the matrix 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// vector multiply its an outer product. 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// block_diagonal = block_diagonal(F'F) 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid PartitionedMatrixView::UpdateBlockDiagonalFtF( 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockSparseMatrix* block_diagonal) const { 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs = matrix_.block_structure(); 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* block_diagonal_structure = 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal->block_structure(); 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal->SetZero(); 2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* values = matrix_.values(); 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < bs->rows.size(); ++r) { 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_size = bs->rows[r].block.size; 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<Cell>& cells = bs->rows[r].cells; 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_id = cells[c].block_id; 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int col_block_size = bs->cols[col_block_id].size; 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int diagonal_block_id = col_block_id - num_col_blocks_e_; 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int cell_position = 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_structure->rows[diagonal_block_id].cells[0].position; 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MatrixTransposeMatrixMultiply 2931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( 2941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values + cells[c].position, row_block_size, col_block_size, 2951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values + cells[c].position, row_block_size, col_block_size, 2961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling block_diagonal->mutable_values() + cell_position, 2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 0, 0, col_block_size, col_block_size); 2980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 3030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 304