internal/ceres/block_sparse_matrix.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/block_sparse_matrix.h"
  32
  33 #include <cstddef>
  34 #include <algorithm>
  35 #include <vector>
  36 #include "ceres/block_structure.h"
  37 #include "ceres/internal/eigen.h"
  38 #include "ceres/random.h"
  39 #include "ceres/small_blas.h"
  40 #include "ceres/triplet_sparse_matrix.h"
  41 #include "glog/logging.h"
  42
  43 namespace ceres {
  44 namespace internal {
  45
  46 using std::vector;
  47
  48 BlockSparseMatrix::~BlockSparseMatrix() {}
  49
  50 BlockSparseMatrix::BlockSparseMatrix(
  51     CompressedRowBlockStructure* block_structure)
  52     : num_rows_(0),
  53       num_cols_(0),
  54       num_nonzeros_(0),
  55       values_(NULL),
  56       block_structure_(block_structure) {
  57   CHECK_NOTNULL(block_structure_.get());
  58
  59   // Count the number of columns in the matrix.
  60   for (int i = 0; i < block_structure_->cols.size(); ++i) {
  61     num_cols_ += block_structure_->cols[i].size;
  62   }
  63
  64   // Count the number of non-zero entries and the number of rows in
  65   // the matrix.
  66   for (int i = 0; i < block_structure_->rows.size(); ++i) {
  67     int row_block_size = block_structure_->rows[i].block.size;
  68     num_rows_ += row_block_size;
  69
  70     const vector<Cell>& cells = block_structure_->rows[i].cells;
  71     for (int j = 0; j < cells.size(); ++j) {
  72       int col_block_id = cells[j].block_id;
  73       int col_block_size = block_structure_->cols[col_block_id].size;
  74       num_nonzeros_ += col_block_size * row_block_size;
  75     }
  76   }
  77
  78   CHECK_GE(num_rows_, 0);
  79   CHECK_GE(num_cols_, 0);
  80   CHECK_GE(num_nonzeros_, 0);
  81   VLOG(2) << "Allocating values array with "
  82           << num_nonzeros_ * sizeof(double) << " bytes.";  // NOLINT
  83   values_.reset(new double[num_nonzeros_]);
  84   max_num_nonzeros_ = num_nonzeros_;
  85   CHECK_NOTNULL(values_.get());
  86 }
  87
  88 void BlockSparseMatrix::SetZero() {
  89   std::fill(values_.get(), values_.get() + num_nonzeros_, 0.0);
  90 }
  91
  92 void BlockSparseMatrix::RightMultiply(const double* x,  double* y) const {
  93   CHECK_NOTNULL(x);
  94   CHECK_NOTNULL(y);
  95
  96   for (int i = 0; i < block_structure_->rows.size(); ++i) {
  97     int row_block_pos = block_structure_->rows[i].block.position;
  98     int row_block_size = block_structure_->rows[i].block.size;
  99     const vector<Cell>& cells = block_structure_->rows[i].cells;
 100     for (int j = 0; j < cells.size(); ++j) {
 101       int col_block_id = cells[j].block_id;
 102       int col_block_size = block_structure_->cols[col_block_id].size;
 103       int col_block_pos = block_structure_->cols[col_block_id].position;
 104       MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
 105           values_.get() + cells[j].position, row_block_size, col_block_size,
 106           x + col_block_pos,
 107           y + row_block_pos);
 108     }
 109   }
 110 }
 111
 112 void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const {
 113   CHECK_NOTNULL(x);
 114   CHECK_NOTNULL(y);
 115
 116   for (int i = 0; i < block_structure_->rows.size(); ++i) {
 117     int row_block_pos = block_structure_->rows[i].block.position;
 118     int row_block_size = block_structure_->rows[i].block.size;
 119     const vector<Cell>& cells = block_structure_->rows[i].cells;
 120     for (int j = 0; j < cells.size(); ++j) {
 121       int col_block_id = cells[j].block_id;
 122       int col_block_size = block_structure_->cols[col_block_id].size;
 123       int col_block_pos = block_structure_->cols[col_block_id].position;
 124       MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
 125           values_.get() + cells[j].position, row_block_size, col_block_size,
 126           x + row_block_pos,
 127           y + col_block_pos);
 128     }
 129   }
 130 }
 131
 132 void BlockSparseMatrix::SquaredColumnNorm(double* x) const {
 133   CHECK_NOTNULL(x);
 134   VectorRef(x, num_cols_).setZero();
 135   for (int i = 0; i < block_structure_->rows.size(); ++i) {
 136     int row_block_size = block_structure_->rows[i].block.size;
 137     const vector<Cell>& cells = block_structure_->rows[i].cells;
 138     for (int j = 0; j < cells.size(); ++j) {
 139       int col_block_id = cells[j].block_id;
 140       int col_block_size = block_structure_->cols[col_block_id].size;
 141       int col_block_pos = block_structure_->cols[col_block_id].position;
 142       const MatrixRef m(values_.get() + cells[j].position,
 143                         row_block_size, col_block_size);
 144       VectorRef(x + col_block_pos, col_block_size) += m.colwise().squaredNorm();
 145     }
 146   }
 147 }
 148
 149 void BlockSparseMatrix::ScaleColumns(const double* scale) {
 150   CHECK_NOTNULL(scale);
 151
 152   for (int i = 0; i < block_structure_->rows.size(); ++i) {
 153     int row_block_size = block_structure_->rows[i].block.size;
 154     const vector<Cell>& cells = block_structure_->rows[i].cells;
 155     for (int j = 0; j < cells.size(); ++j) {
 156       int col_block_id = cells[j].block_id;
 157       int col_block_size = block_structure_->cols[col_block_id].size;
 158       int col_block_pos = block_structure_->cols[col_block_id].position;
 159       MatrixRef m(values_.get() + cells[j].position,
 160                         row_block_size, col_block_size);
 161       m *= ConstVectorRef(scale + col_block_pos, col_block_size).asDiagonal();
 162     }
 163   }
 164 }
 165
 166 void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const {
 167   CHECK_NOTNULL(dense_matrix);
 168
 169   dense_matrix->resize(num_rows_, num_cols_);
 170   dense_matrix->setZero();
 171   Matrix& m = *dense_matrix;
 172
 173   for (int i = 0; i < block_structure_->rows.size(); ++i) {
 174     int row_block_pos = block_structure_->rows[i].block.position;
 175     int row_block_size = block_structure_->rows[i].block.size;
 176     const vector<Cell>& cells = block_structure_->rows[i].cells;
 177     for (int j = 0; j < cells.size(); ++j) {
 178       int col_block_id = cells[j].block_id;
 179       int col_block_size = block_structure_->cols[col_block_id].size;
 180       int col_block_pos = block_structure_->cols[col_block_id].position;
 181       int jac_pos = cells[j].position;
 182       m.block(row_block_pos, col_block_pos, row_block_size, col_block_size)
 183           += MatrixRef(values_.get() + jac_pos, row_block_size, col_block_size);
 184     }
 185   }
 186 }
 187
 188 void BlockSparseMatrix::ToTripletSparseMatrix(
 189     TripletSparseMatrix* matrix) const {
 190   CHECK_NOTNULL(matrix);
 191
 192   matrix->Reserve(num_nonzeros_);
 193   matrix->Resize(num_rows_, num_cols_);
 194   matrix->SetZero();
 195
 196   for (int i = 0; i < block_structure_->rows.size(); ++i) {
 197     int row_block_pos = block_structure_->rows[i].block.position;
 198     int row_block_size = block_structure_->rows[i].block.size;
 199     const vector<Cell>& cells = block_structure_->rows[i].cells;
 200     for (int j = 0; j < cells.size(); ++j) {
 201       int col_block_id = cells[j].block_id;
 202       int col_block_size = block_structure_->cols[col_block_id].size;
 203       int col_block_pos = block_structure_->cols[col_block_id].position;
 204       int jac_pos = cells[j].position;
 205        for (int r = 0; r < row_block_size; ++r) {
 206         for (int c = 0; c < col_block_size; ++c, ++jac_pos) {
 207           matrix->mutable_rows()[jac_pos] = row_block_pos + r;
 208           matrix->mutable_cols()[jac_pos] = col_block_pos + c;
 209           matrix->mutable_values()[jac_pos] = values_[jac_pos];
 210         }
 211       }
 212     }
 213   }
 214   matrix->set_num_nonzeros(num_nonzeros_);
 215 }
 216
 217 // Return a pointer to the block structure. We continue to hold
 218 // ownership of the object though.
 219 const CompressedRowBlockStructure* BlockSparseMatrix::block_structure()
 220     const {
 221   return block_structure_.get();
 222 }
 223
 224 void BlockSparseMatrix::ToTextFile(FILE* file) const {
 225   CHECK_NOTNULL(file);
 226   for (int i = 0; i < block_structure_->rows.size(); ++i) {
 227     const int row_block_pos = block_structure_->rows[i].block.position;
 228     const int row_block_size = block_structure_->rows[i].block.size;
 229     const vector<Cell>& cells = block_structure_->rows[i].cells;
 230     for (int j = 0; j < cells.size(); ++j) {
 231       const int col_block_id = cells[j].block_id;
 232       const int col_block_size = block_structure_->cols[col_block_id].size;
 233       const int col_block_pos = block_structure_->cols[col_block_id].position;
 234       int jac_pos = cells[j].position;
 235       for (int r = 0; r < row_block_size; ++r) {
 236         for (int c = 0; c < col_block_size; ++c) {
 237           fprintf(file, "% 10d % 10d %17f\n",
 238                   row_block_pos + r,
 239                   col_block_pos + c,
 240                   values_[jac_pos++]);
 241         }
 242       }
 243     }
 244   }
 245 }
 246
 247 BlockSparseMatrix* BlockSparseMatrix::CreateDiagonalMatrix(
 248     const double* diagonal, const std::vector<Block>& column_blocks) {
 249   // Create the block structure for the diagonal matrix.
 250   CompressedRowBlockStructure* bs = new CompressedRowBlockStructure();
 251   bs->cols = column_blocks;
 252   int position = 0;
 253   bs->rows.resize(column_blocks.size(), CompressedRow(1));
 254   for (int i = 0; i < column_blocks.size(); ++i) {
 255     CompressedRow& row = bs->rows[i];
 256     row.block = column_blocks[i];
 257     Cell& cell = row.cells[0];
 258     cell.block_id = i;
 259     cell.position = position;
 260     position += row.block.size * row.block.size;
 261   }
 262
 263   // Create the BlockSparseMatrix with the given block structure.
 264   BlockSparseMatrix* matrix = new BlockSparseMatrix(bs);
 265   matrix->SetZero();
 266
 267   // Fill the values array of the block sparse matrix.
 268   double* values = matrix->mutable_values();
 269   for (int i = 0; i < column_blocks.size(); ++i) {
 270     const int size = column_blocks[i].size;
 271     for (int j = 0; j < size; ++j) {
 272       // (j + 1) * size is compact way of accessing the (j,j) entry.
 273       values[j * (size + 1)] = diagonal[j];
 274     }
 275     diagonal += size;
 276     values += size * size;
 277   }
 278
 279   return matrix;
 280 }
 281
 282 void BlockSparseMatrix::AppendRows(const BlockSparseMatrix& m) {
 283   const int old_num_nonzeros = num_nonzeros_;
 284   const int old_num_row_blocks = block_structure_->rows.size();
 285   const CompressedRowBlockStructure* m_bs = m.block_structure();
 286   block_structure_->rows.resize(old_num_row_blocks + m_bs->rows.size());
 287
 288   for (int i = 0; i < m_bs->rows.size(); ++i) {
 289     const CompressedRow& m_row = m_bs->rows[i];
 290     CompressedRow& row = block_structure_->rows[old_num_row_blocks + i];
 291     row.block.size = m_row.block.size;
 292     row.block.position = num_rows_;
 293     num_rows_ += m_row.block.size;
 294     row.cells.resize(m_row.cells.size());
 295     for (int c = 0; c < m_row.cells.size(); ++c) {
 296       const int block_id = m_row.cells[c].block_id;
 297       row.cells[c].block_id = block_id;
 298       row.cells[c].position = num_nonzeros_;
 299       num_nonzeros_ += m_row.block.size * m_bs->cols[block_id].size;
 300     }
 301   }
 302
 303   if (num_nonzeros_ > max_num_nonzeros_) {
 304     double* new_values = new double[num_nonzeros_];
 305     std::copy(values_.get(), values_.get() + old_num_nonzeros, new_values);
 306     values_.reset(new_values);
 307     max_num_nonzeros_ = num_nonzeros_;
 308   }
 309
 310   std::copy(m.values(),
 311             m.values() + m.num_nonzeros(),
 312             values_.get() + old_num_nonzeros);
 313 }
 314
 315 void BlockSparseMatrix::DeleteRowBlocks(const int delta_row_blocks) {
 316   const int num_row_blocks = block_structure_->rows.size();
 317   int delta_num_nonzeros = 0;
 318   int delta_num_rows = 0;
 319   const std::vector<Block>& column_blocks = block_structure_->cols;
 320   for (int i = 0; i < delta_row_blocks; ++i) {
 321     const CompressedRow& row = block_structure_->rows[num_row_blocks - i - 1];
 322     delta_num_rows += row.block.size;
 323     for (int c = 0; c < row.cells.size(); ++c) {
 324       const Cell& cell = row.cells[c];
 325       delta_num_nonzeros += row.block.size * column_blocks[cell.block_id].size;
 326     }
 327   }
 328   num_nonzeros_ -= delta_num_nonzeros;
 329   num_rows_ -= delta_num_rows;
 330   block_structure_->rows.resize(num_row_blocks - delta_row_blocks);
 331 }
 332
 333 BlockSparseMatrix* BlockSparseMatrix::CreateRandomMatrix(
 334     const BlockSparseMatrix::RandomMatrixOptions& options) {
 335   CHECK_GT(options.num_row_blocks, 0);
 336   CHECK_GT(options.min_row_block_size, 0);
 337   CHECK_GT(options.max_row_block_size, 0);
 338   CHECK_LE(options.min_row_block_size, options.max_row_block_size);
 339   CHECK_GT(options.num_col_blocks, 0);
 340   CHECK_GT(options.min_col_block_size, 0);
 341   CHECK_GT(options.max_col_block_size, 0);
 342   CHECK_LE(options.min_col_block_size, options.max_col_block_size);
 343   CHECK_GT(options.block_density, 0.0);
 344   CHECK_LE(options.block_density, 1.0);
 345
 346   CompressedRowBlockStructure* bs = new CompressedRowBlockStructure();
 347     // Generate the col block structure.
 348   int col_block_position = 0;
 349   for (int i = 0; i < options.num_col_blocks; ++i) {
 350     // Generate a random integer in [min_col_block_size, max_col_block_size]
 351     const int delta_block_size =
 352         Uniform(options.max_col_block_size - options.min_col_block_size);
 353     const int col_block_size = options.min_col_block_size + delta_block_size;
 354     bs->cols.push_back(Block(col_block_size, col_block_position));
 355     col_block_position += col_block_size;
 356   }
 357
 358
 359   bool matrix_has_blocks = false;
 360   while (!matrix_has_blocks) {
 361     LOG(INFO) << "clearing";
 362     bs->rows.clear();
 363     int row_block_position = 0;
 364     int value_position = 0;
 365     for (int r = 0; r < options.num_row_blocks; ++r) {
 366
 367       const int delta_block_size =
 368           Uniform(options.max_row_block_size - options.min_row_block_size);
 369       const int row_block_size = options.min_row_block_size + delta_block_size;
 370       bs->rows.push_back(CompressedRow());
 371       CompressedRow& row = bs->rows.back();
 372       row.block.size = row_block_size;
 373       row.block.position = row_block_position;
 374       row_block_position += row_block_size;
 375       for (int c = 0; c < options.num_col_blocks; ++c) {
 376         if (RandDouble() > options.block_density) continue;
 377
 378         row.cells.push_back(Cell());
 379         Cell& cell = row.cells.back();
 380         cell.block_id = c;
 381         cell.position = value_position;
 382         value_position += row_block_size * bs->cols[c].size;
 383         matrix_has_blocks = true;
 384       }
 385     }
 386   }
 387
 388   BlockSparseMatrix* matrix = new BlockSparseMatrix(bs);
 389   double* values = matrix->mutable_values();
 390   for (int i = 0; i < matrix->num_nonzeros(); ++i) {
 391     values[i] = RandNormal();
 392   }
 393
 394   return matrix;
 395 }
 396
 397 }  // namespace internal
 398 }  // namespace ceres