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