1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3// http://code.google.com/p/ceres-solver/ 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#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10 32 33#include "ceres/partitioned_matrix_view.h" 34 35#include <algorithm> 36#include <cstring> 37#include <vector> 38#include "ceres/block_sparse_matrix.h" 39#include "ceres/block_structure.h" 40#include "ceres/internal/eigen.h" 41#include "ceres/small_blas.h" 42#include "glog/logging.h" 43 44namespace ceres { 45namespace internal { 46 47PartitionedMatrixView::PartitionedMatrixView( 48 const BlockSparseMatrix& matrix, 49 int num_col_blocks_a) 50 : matrix_(matrix), 51 num_col_blocks_e_(num_col_blocks_a) { 52 const CompressedRowBlockStructure* bs = matrix_.block_structure(); 53 CHECK_NOTNULL(bs); 54 55 num_col_blocks_f_ = bs->cols.size() - num_col_blocks_a; 56 57 // Compute the number of row blocks in E. The number of row blocks 58 // in E maybe less than the number of row blocks in the input matrix 59 // as some of the row blocks at the bottom may not have any 60 // e_blocks. For a definition of what an e_block is, please see 61 // explicit_schur_complement_solver.h 62 num_row_blocks_e_ = 0; 63 for (int r = 0; r < bs->rows.size(); ++r) { 64 const vector<Cell>& cells = bs->rows[r].cells; 65 if (cells[0].block_id < num_col_blocks_a) { 66 ++num_row_blocks_e_; 67 } 68 } 69 70 // Compute the number of columns in E and F. 71 num_cols_e_ = 0; 72 num_cols_f_ = 0; 73 74 for (int c = 0; c < bs->cols.size(); ++c) { 75 const Block& block = bs->cols[c]; 76 if (c < num_col_blocks_a) { 77 num_cols_e_ += block.size; 78 } else { 79 num_cols_f_ += block.size; 80 } 81 } 82 83 CHECK_EQ(num_cols_e_ + num_cols_f_, matrix_.num_cols()); 84} 85 86PartitionedMatrixView::~PartitionedMatrixView() { 87} 88 89// The next four methods don't seem to be particularly cache 90// friendly. This is an artifact of how the BlockStructure of the 91// input matrix is constructed. These methods will benefit from 92// multithreading as well as improved data layout. 93 94void PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const { 95 const CompressedRowBlockStructure* bs = matrix_.block_structure(); 96 97 // Iterate over the first num_row_blocks_e_ row blocks, and multiply 98 // by the first cell in each row block. 99 const double* values = matrix_.values(); 100 for (int r = 0; r < num_row_blocks_e_; ++r) { 101 const Cell& cell = bs->rows[r].cells[0]; 102 const int row_block_pos = bs->rows[r].block.position; 103 const int row_block_size = bs->rows[r].block.size; 104 const int col_block_id = cell.block_id; 105 const int col_block_pos = bs->cols[col_block_id].position; 106 const int col_block_size = bs->cols[col_block_id].size; 107 MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( 108 values + cell.position, row_block_size, col_block_size, 109 x + col_block_pos, 110 y + row_block_pos); 111 } 112} 113 114void PartitionedMatrixView::RightMultiplyF(const double* x, double* y) const { 115 const CompressedRowBlockStructure* bs = matrix_.block_structure(); 116 117 // Iterate over row blocks, and if the row block is in E, then 118 // multiply by all the cells except the first one which is of type 119 // E. If the row block is not in E (i.e its in the bottom 120 // num_row_blocks - num_row_blocks_e row blocks), then all the cells 121 // are of type F and multiply by them all. 122 const double* values = matrix_.values(); 123 for (int r = 0; r < bs->rows.size(); ++r) { 124 const int row_block_pos = bs->rows[r].block.position; 125 const int row_block_size = bs->rows[r].block.size; 126 const vector<Cell>& cells = bs->rows[r].cells; 127 for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { 128 const int col_block_id = cells[c].block_id; 129 const int col_block_pos = bs->cols[col_block_id].position; 130 const int col_block_size = bs->cols[col_block_id].size; 131 MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( 132 values + cells[c].position, row_block_size, col_block_size, 133 x + col_block_pos - num_cols_e(), 134 y + row_block_pos); 135 } 136 } 137} 138 139void PartitionedMatrixView::LeftMultiplyE(const double* x, double* y) const { 140 const CompressedRowBlockStructure* bs = matrix_.block_structure(); 141 142 // Iterate over the first num_row_blocks_e_ row blocks, and multiply 143 // by the first cell in each row block. 144 const double* values = matrix_.values(); 145 for (int r = 0; r < num_row_blocks_e_; ++r) { 146 const Cell& cell = bs->rows[r].cells[0]; 147 const int row_block_pos = bs->rows[r].block.position; 148 const int row_block_size = bs->rows[r].block.size; 149 const int col_block_id = cell.block_id; 150 const int col_block_pos = bs->cols[col_block_id].position; 151 const int col_block_size = bs->cols[col_block_id].size; 152 MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( 153 values + cell.position, row_block_size, col_block_size, 154 x + row_block_pos, 155 y + col_block_pos); 156 } 157} 158 159void PartitionedMatrixView::LeftMultiplyF(const double* x, double* y) const { 160 const CompressedRowBlockStructure* bs = matrix_.block_structure(); 161 162 // Iterate over row blocks, and if the row block is in E, then 163 // multiply by all the cells except the first one which is of type 164 // E. If the row block is not in E (i.e its in the bottom 165 // num_row_blocks - num_row_blocks_e row blocks), then all the cells 166 // are of type F and multiply by them all. 167 const double* values = matrix_.values(); 168 for (int r = 0; r < bs->rows.size(); ++r) { 169 const int row_block_pos = bs->rows[r].block.position; 170 const int row_block_size = bs->rows[r].block.size; 171 const vector<Cell>& cells = bs->rows[r].cells; 172 for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { 173 const int col_block_id = cells[c].block_id; 174 const int col_block_pos = bs->cols[col_block_id].position; 175 const int col_block_size = bs->cols[col_block_id].size; 176 MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( 177 values + cells[c].position, row_block_size, col_block_size, 178 x + row_block_pos, 179 y + col_block_pos - num_cols_e()); 180 } 181 } 182} 183 184// Given a range of columns blocks of a matrix m, compute the block 185// structure of the block diagonal of the matrix m(:, 186// start_col_block:end_col_block)'m(:, start_col_block:end_col_block) 187// and return a BlockSparseMatrix with the this block structure. The 188// caller owns the result. 189BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalMatrixLayout( 190 int start_col_block, int end_col_block) const { 191 const CompressedRowBlockStructure* bs = matrix_.block_structure(); 192 CompressedRowBlockStructure* block_diagonal_structure = 193 new CompressedRowBlockStructure; 194 195 int block_position = 0; 196 int diagonal_cell_position = 0; 197 198 // Iterate over the column blocks, creating a new diagonal block for 199 // each column block. 200 for (int c = start_col_block; c < end_col_block; ++c) { 201 const Block& block = bs->cols[c]; 202 block_diagonal_structure->cols.push_back(Block()); 203 Block& diagonal_block = block_diagonal_structure->cols.back(); 204 diagonal_block.size = block.size; 205 diagonal_block.position = block_position; 206 207 block_diagonal_structure->rows.push_back(CompressedRow()); 208 CompressedRow& row = block_diagonal_structure->rows.back(); 209 row.block = diagonal_block; 210 211 row.cells.push_back(Cell()); 212 Cell& cell = row.cells.back(); 213 cell.block_id = c - start_col_block; 214 cell.position = diagonal_cell_position; 215 216 block_position += block.size; 217 diagonal_cell_position += block.size * block.size; 218 } 219 220 // Build a BlockSparseMatrix with the just computed block 221 // structure. 222 return new BlockSparseMatrix(block_diagonal_structure); 223} 224 225BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalEtE() const { 226 BlockSparseMatrix* block_diagonal = 227 CreateBlockDiagonalMatrixLayout(0, num_col_blocks_e_); 228 UpdateBlockDiagonalEtE(block_diagonal); 229 return block_diagonal; 230} 231 232BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalFtF() const { 233 BlockSparseMatrix* block_diagonal = 234 CreateBlockDiagonalMatrixLayout( 235 num_col_blocks_e_, num_col_blocks_e_ + num_col_blocks_f_); 236 UpdateBlockDiagonalFtF(block_diagonal); 237 return block_diagonal; 238} 239 240// Similar to the code in RightMultiplyE, except instead of the matrix 241// vector multiply its an outer product. 242// 243// block_diagonal = block_diagonal(E'E) 244void PartitionedMatrixView::UpdateBlockDiagonalEtE( 245 BlockSparseMatrix* block_diagonal) const { 246 const CompressedRowBlockStructure* bs = matrix_.block_structure(); 247 const CompressedRowBlockStructure* block_diagonal_structure = 248 block_diagonal->block_structure(); 249 250 block_diagonal->SetZero(); 251 const double* values = matrix_.values(); 252 for (int r = 0; r < num_row_blocks_e_ ; ++r) { 253 const Cell& cell = bs->rows[r].cells[0]; 254 const int row_block_size = bs->rows[r].block.size; 255 const int block_id = cell.block_id; 256 const int col_block_size = bs->cols[block_id].size; 257 const int cell_position = 258 block_diagonal_structure->rows[block_id].cells[0].position; 259 260 MatrixTransposeMatrixMultiply 261 <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( 262 values + cell.position, row_block_size, col_block_size, 263 values + cell.position, row_block_size, col_block_size, 264 block_diagonal->mutable_values() + cell_position, 265 0, 0, col_block_size, col_block_size); 266 } 267} 268 269// Similar to the code in RightMultiplyF, except instead of the matrix 270// vector multiply its an outer product. 271// 272// block_diagonal = block_diagonal(F'F) 273// 274void PartitionedMatrixView::UpdateBlockDiagonalFtF( 275 BlockSparseMatrix* block_diagonal) const { 276 const CompressedRowBlockStructure* bs = matrix_.block_structure(); 277 const CompressedRowBlockStructure* block_diagonal_structure = 278 block_diagonal->block_structure(); 279 280 block_diagonal->SetZero(); 281 const double* values = matrix_.values(); 282 for (int r = 0; r < bs->rows.size(); ++r) { 283 const int row_block_size = bs->rows[r].block.size; 284 const vector<Cell>& cells = bs->rows[r].cells; 285 for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { 286 const int col_block_id = cells[c].block_id; 287 const int col_block_size = bs->cols[col_block_id].size; 288 const int diagonal_block_id = col_block_id - num_col_blocks_e_; 289 const int cell_position = 290 block_diagonal_structure->rows[diagonal_block_id].cells[0].position; 291 292 MatrixTransposeMatrixMultiply 293 <Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic, 1>( 294 values + cells[c].position, row_block_size, col_block_size, 295 values + cells[c].position, row_block_size, col_block_size, 296 block_diagonal->mutable_values() + cell_position, 297 0, 0, col_block_size, col_block_size); 298 } 299 } 300} 301 302} // namespace internal 303} // namespace ceres 304