internal/ceres/compressed_row_sparse_matrix.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2017 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/compressed_row_sparse_matrix.h"
  32
  33 #include <algorithm>
  34 #include <numeric>
  35 #include <vector>
  36 #include "ceres/crs_matrix.h"
  37 #include "ceres/internal/port.h"
  38 #include "ceres/random.h"
  39 #include "ceres/triplet_sparse_matrix.h"
  40 #include "glog/logging.h"
  41
  42 namespace ceres {
  43 namespace internal {
  44
  45 using std::vector;
  46
  47 namespace {
  48
  49 // Helper functor used by the constructor for reordering the contents
  50 // of a TripletSparseMatrix. This comparator assumes thay there are no
  51 // duplicates in the pair of arrays rows and cols, i.e., there is no
  52 // indices i and j (not equal to each other) s.t.
  53 //
  54 //  rows[i] == rows[j] && cols[i] == cols[j]
  55 //
  56 // If this is the case, this functor will not be a StrictWeakOrdering.
  57 struct RowColLessThan {
  58   RowColLessThan(const int* rows, const int* cols) : rows(rows), cols(cols) {}
  59
  60   bool operator()(const int x, const int y) const {
  61     if (rows[x] == rows[y]) {
  62       return (cols[x] < cols[y]);
  63     }
  64     return (rows[x] < rows[y]);
  65   }
  66
  67   const int* rows;
  68   const int* cols;
  69 };
  70
  71 void TransposeForCompressedRowSparseStructure(const int num_rows,
  72                                               const int num_cols,
  73                                               const int num_nonzeros,
  74                                               const int* rows,
  75                                               const int* cols,
  76                                               const double* values,
  77                                               int* transpose_rows,
  78                                               int* transpose_cols,
  79                                               double* transpose_values) {
  80   // Explicitly zero out transpose_rows.
  81   std::fill(transpose_rows, transpose_rows + num_cols + 1, 0);
  82
  83   // Count the number of entries in each column of the original matrix
  84   // and assign to transpose_rows[col + 1].
  85   for (int idx = 0; idx < num_nonzeros; ++idx) {
  86     ++transpose_rows[cols[idx] + 1];
  87   }
  88
  89   // Compute the starting position for each row in the transpose by
  90   // computing the cumulative sum of the entries of transpose_rows.
  91   for (int i = 1; i < num_cols + 1; ++i) {
  92     transpose_rows[i] += transpose_rows[i - 1];
  93   }
  94
  95   // Populate transpose_cols and (optionally) transpose_values by
  96   // walking the entries of the source matrices. For each entry that
  97   // is added, the value of transpose_row is incremented allowing us
  98   // to keep track of where the next entry for that row should go.
  99   //
 100   // As a result transpose_row is shifted to the left by one entry.
 101   for (int r = 0; r < num_rows; ++r) {
 102     for (int idx = rows[r]; idx < rows[r + 1]; ++idx) {
 103       const int c = cols[idx];
 104       const int transpose_idx = transpose_rows[c]++;
 105       transpose_cols[transpose_idx] = r;
 106       if (values != NULL && transpose_values != NULL) {
 107         transpose_values[transpose_idx] = values[idx];
 108       }
 109     }
 110   }
 111
 112   // This loop undoes the left shift to transpose_rows introduced by
 113   // the previous loop.
 114   for (int i = num_cols - 1; i > 0; --i) {
 115     transpose_rows[i] = transpose_rows[i - 1];
 116   }
 117   transpose_rows[0] = 0;
 118 }
 119
 120 void AddRandomBlock(const int num_rows,
 121                     const int num_cols,
 122                     const int row_block_begin,
 123                     const int col_block_begin,
 124                     std::vector<int>* rows,
 125                     std::vector<int>* cols,
 126                     std::vector<double>* values) {
 127   for (int r = 0; r < num_rows; ++r) {
 128     for (int c = 0; c < num_cols; ++c) {
 129       rows->push_back(row_block_begin + r);
 130       cols->push_back(col_block_begin + c);
 131       values->push_back(RandNormal());
 132     }
 133   }
 134 }
 135
 136 }  // namespace
 137
 138 // This constructor gives you a semi-initialized CompressedRowSparseMatrix.
 139 CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows,
 140                                                      int num_cols,
 141                                                      int max_num_nonzeros) {
 142   num_rows_ = num_rows;
 143   num_cols_ = num_cols;
 144   storage_type_ = UNSYMMETRIC;
 145   rows_.resize(num_rows + 1, 0);
 146   cols_.resize(max_num_nonzeros, 0);
 147   values_.resize(max_num_nonzeros, 0.0);
 148
 149   VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_
 150           << " max_num_nonzeros: " << cols_.size() << ". Allocating "
 151           << (num_rows_ + 1) * sizeof(int) +     // NOLINT
 152                  cols_.size() * sizeof(int) +    // NOLINT
 153                  cols_.size() * sizeof(double);  // NOLINT
 154 }
 155
 156 CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix(
 157     const TripletSparseMatrix& input) {
 158   return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, false);
 159 }
 160
 161 CompressedRowSparseMatrix*
 162 CompressedRowSparseMatrix::FromTripletSparseMatrixTransposed(
 163     const TripletSparseMatrix& input) {
 164   return CompressedRowSparseMatrix::FromTripletSparseMatrix(input, true);
 165 }
 166
 167 CompressedRowSparseMatrix* CompressedRowSparseMatrix::FromTripletSparseMatrix(
 168     const TripletSparseMatrix& input, bool transpose) {
 169   int num_rows = input.num_rows();
 170   int num_cols = input.num_cols();
 171   const int* rows = input.rows();
 172   const int* cols = input.cols();
 173   const double* values = input.values();
 174
 175   if (transpose) {
 176     std::swap(num_rows, num_cols);
 177     std::swap(rows, cols);
 178   }
 179
 180   // index is the list of indices into the TripletSparseMatrix input.
 181   vector<int> index(input.num_nonzeros(), 0);
 182   for (int i = 0; i < input.num_nonzeros(); ++i) {
 183     index[i] = i;
 184   }
 185
 186   // Sort index such that the entries of m are ordered by row and ties
 187   // are broken by column.
 188   std::sort(index.begin(), index.end(), RowColLessThan(rows, cols));
 189
 190   VLOG(1) << "# of rows: " << num_rows << " # of columns: " << num_cols
 191           << " num_nonzeros: " << input.num_nonzeros() << ". Allocating "
 192           << ((num_rows + 1) * sizeof(int) +           // NOLINT
 193               input.num_nonzeros() * sizeof(int) +     // NOLINT
 194               input.num_nonzeros() * sizeof(double));  // NOLINT
 195
 196   CompressedRowSparseMatrix* output =
 197       new CompressedRowSparseMatrix(num_rows, num_cols, input.num_nonzeros());
 198
 199   // Copy the contents of the cols and values array in the order given
 200   // by index and count the number of entries in each row.
 201   int* output_rows = output->mutable_rows();
 202   int* output_cols = output->mutable_cols();
 203   double* output_values = output->mutable_values();
 204
 205   output_rows[0] = 0;
 206   for (int i = 0; i < index.size(); ++i) {
 207     const int idx = index[i];
 208     ++output_rows[rows[idx] + 1];
 209     output_cols[i] = cols[idx];
 210     output_values[i] = values[idx];
 211   }
 212
 213   // Find the cumulative sum of the row counts.
 214   for (int i = 1; i < num_rows + 1; ++i) {
 215     output_rows[i] += output_rows[i - 1];
 216   }
 217
 218   CHECK_EQ(output->num_nonzeros(), input.num_nonzeros());
 219   return output;
 220 }
 221
 222 CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal,
 223                                                      int num_rows) {
 224   CHECK_NOTNULL(diagonal);
 225
 226   num_rows_ = num_rows;
 227   num_cols_ = num_rows;
 228   storage_type_ = UNSYMMETRIC;
 229   rows_.resize(num_rows + 1);
 230   cols_.resize(num_rows);
 231   values_.resize(num_rows);
 232
 233   rows_[0] = 0;
 234   for (int i = 0; i < num_rows_; ++i) {
 235     cols_[i] = i;
 236     values_[i] = diagonal[i];
 237     rows_[i + 1] = i + 1;
 238   }
 239
 240   CHECK_EQ(num_nonzeros(), num_rows);
 241 }
 242
 243 CompressedRowSparseMatrix::~CompressedRowSparseMatrix() {}
 244
 245 void CompressedRowSparseMatrix::SetZero() {
 246   std::fill(values_.begin(), values_.end(), 0);
 247 }
 248
 249 void CompressedRowSparseMatrix::RightMultiply(const double* x,
 250                                               double* y) const {
 251   CHECK_NOTNULL(x);
 252   CHECK_NOTNULL(y);
 253
 254   for (int r = 0; r < num_rows_; ++r) {
 255     for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {
 256       y[r] += values_[idx] * x[cols_[idx]];
 257     }
 258   }
 259 }
 260
 261 void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const {
 262   CHECK_NOTNULL(x);
 263   CHECK_NOTNULL(y);
 264
 265   for (int r = 0; r < num_rows_; ++r) {
 266     for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {
 267       y[cols_[idx]] += values_[idx] * x[r];
 268     }
 269   }
 270 }
 271
 272 void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const {
 273   CHECK_NOTNULL(x);
 274
 275   std::fill(x, x + num_cols_, 0.0);
 276   for (int idx = 0; idx < rows_[num_rows_]; ++idx) {
 277     x[cols_[idx]] += values_[idx] * values_[idx];
 278   }
 279 }
 280
 281 void CompressedRowSparseMatrix::ScaleColumns(const double* scale) {
 282   CHECK_NOTNULL(scale);
 283
 284   for (int idx = 0; idx < rows_[num_rows_]; ++idx) {
 285     values_[idx] *= scale[cols_[idx]];
 286   }
 287 }
 288
 289 void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const {
 290   CHECK_NOTNULL(dense_matrix);
 291   dense_matrix->resize(num_rows_, num_cols_);
 292   dense_matrix->setZero();
 293
 294   for (int r = 0; r < num_rows_; ++r) {
 295     for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {
 296       (*dense_matrix)(r, cols_[idx]) = values_[idx];
 297     }
 298   }
 299 }
 300
 301 void CompressedRowSparseMatrix::DeleteRows(int delta_rows) {
 302   CHECK_GE(delta_rows, 0);
 303   CHECK_LE(delta_rows, num_rows_);
 304
 305   num_rows_ -= delta_rows;
 306   rows_.resize(num_rows_ + 1);
 307
 308   // The rest of the code updates the block information. Immediately
 309   // return in case of no block information.
 310   if (row_blocks_.empty()) {
 311     return;
 312   }
 313
 314   // Walk the list of row blocks until we reach the new number of rows
 315   // and the drop the rest of the row blocks.
 316   int num_row_blocks = 0;
 317   int num_rows = 0;
 318   while (num_row_blocks < row_blocks_.size() && num_rows < num_rows_) {
 319     num_rows += row_blocks_[num_row_blocks];
 320     ++num_row_blocks;
 321   }
 322
 323   row_blocks_.resize(num_row_blocks);
 324 }
 325
 326 void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) {
 327   CHECK_EQ(m.num_cols(), num_cols_);
 328
 329   CHECK((row_blocks_.empty() && m.row_blocks().empty()) ||
 330         (!row_blocks_.empty() && !m.row_blocks().empty()))
 331       << "Cannot append a matrix with row blocks to one without and vice versa."
 332       << "This matrix has : " << row_blocks_.size() << " row blocks."
 333       << "The matrix being appended has: " << m.row_blocks().size()
 334       << " row blocks.";
 335
 336   if (m.num_rows() == 0) {
 337     return;
 338   }
 339
 340   if (cols_.size() < num_nonzeros() + m.num_nonzeros()) {
 341     cols_.resize(num_nonzeros() + m.num_nonzeros());
 342     values_.resize(num_nonzeros() + m.num_nonzeros());
 343   }
 344
 345   // Copy the contents of m into this matrix.
 346   DCHECK_LT(num_nonzeros(), cols_.size());
 347   if (m.num_nonzeros() > 0) {
 348     std::copy(m.cols(), m.cols() + m.num_nonzeros(), &cols_[num_nonzeros()]);
 349     std::copy(
 350         m.values(), m.values() + m.num_nonzeros(), &values_[num_nonzeros()]);
 351   }
 352
 353   rows_.resize(num_rows_ + m.num_rows() + 1);
 354   // new_rows = [rows_, m.row() + rows_[num_rows_]]
 355   std::fill(rows_.begin() + num_rows_,
 356             rows_.begin() + num_rows_ + m.num_rows() + 1,
 357             rows_[num_rows_]);
 358
 359   for (int r = 0; r < m.num_rows() + 1; ++r) {
 360     rows_[num_rows_ + r] += m.rows()[r];
 361   }
 362
 363   num_rows_ += m.num_rows();
 364
 365   // The rest of the code updates the block information. Immediately
 366   // return in case of no block information.
 367   if (row_blocks_.empty()) {
 368     return;
 369   }
 370
 371   row_blocks_.insert(
 372       row_blocks_.end(), m.row_blocks().begin(), m.row_blocks().end());
 373 }
 374
 375 void CompressedRowSparseMatrix::ToTextFile(FILE* file) const {
 376   CHECK_NOTNULL(file);
 377   for (int r = 0; r < num_rows_; ++r) {
 378     for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {
 379       fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]);
 380     }
 381   }
 382 }
 383
 384 void CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const {
 385   matrix->num_rows = num_rows_;
 386   matrix->num_cols = num_cols_;
 387   matrix->rows = rows_;
 388   matrix->cols = cols_;
 389   matrix->values = values_;
 390
 391   // Trim.
 392   matrix->rows.resize(matrix->num_rows + 1);
 393   matrix->cols.resize(matrix->rows[matrix->num_rows]);
 394   matrix->values.resize(matrix->rows[matrix->num_rows]);
 395 }
 396
 397 void CompressedRowSparseMatrix::SetMaxNumNonZeros(int num_nonzeros) {
 398   CHECK_GE(num_nonzeros, 0);
 399
 400   cols_.resize(num_nonzeros);
 401   values_.resize(num_nonzeros);
 402 }
 403
 404 CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateBlockDiagonalMatrix(
 405     const double* diagonal, const vector<int>& blocks) {
 406   int num_rows = 0;
 407   int num_nonzeros = 0;
 408   for (int i = 0; i < blocks.size(); ++i) {
 409     num_rows += blocks[i];
 410     num_nonzeros += blocks[i] * blocks[i];
 411   }
 412
 413   CompressedRowSparseMatrix* matrix =
 414       new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros);
 415
 416   int* rows = matrix->mutable_rows();
 417   int* cols = matrix->mutable_cols();
 418   double* values = matrix->mutable_values();
 419   std::fill(values, values + num_nonzeros, 0.0);
 420
 421   int idx_cursor = 0;
 422   int col_cursor = 0;
 423   for (int i = 0; i < blocks.size(); ++i) {
 424     const int block_size = blocks[i];
 425     for (int r = 0; r < block_size; ++r) {
 426       *(rows++) = idx_cursor;
 427       values[idx_cursor + r] = diagonal[col_cursor + r];
 428       for (int c = 0; c < block_size; ++c, ++idx_cursor) {
 429         *(cols++) = col_cursor + c;
 430       }
 431     }
 432     col_cursor += block_size;
 433   }
 434   *rows = idx_cursor;
 435
 436   *matrix->mutable_row_blocks() = blocks;
 437   *matrix->mutable_col_blocks() = blocks;
 438
 439   CHECK_EQ(idx_cursor, num_nonzeros);
 440   CHECK_EQ(col_cursor, num_rows);
 441   return matrix;
 442 }
 443
 444 CompressedRowSparseMatrix* CompressedRowSparseMatrix::Transpose() const {
 445   CompressedRowSparseMatrix* transpose =
 446       new CompressedRowSparseMatrix(num_cols_, num_rows_, num_nonzeros());
 447
 448   switch (storage_type_) {
 449     case UNSYMMETRIC:
 450       transpose->set_storage_type(UNSYMMETRIC);
 451       break;
 452     case LOWER_TRIANGULAR:
 453       transpose->set_storage_type(UPPER_TRIANGULAR);
 454       break;
 455     case UPPER_TRIANGULAR:
 456       transpose->set_storage_type(LOWER_TRIANGULAR);
 457       break;
 458     default:
 459       LOG(FATAL) << "Unknown storage type: " << storage_type_;
 460   };
 461
 462   TransposeForCompressedRowSparseStructure(num_rows(),
 463                                            num_cols(),
 464                                            num_nonzeros(),
 465                                            rows(),
 466                                            cols(),
 467                                            values(),
 468                                            transpose->mutable_rows(),
 469                                            transpose->mutable_cols(),
 470                                            transpose->mutable_values());
 471
 472   // The rest of the code updates the block information. Immediately
 473   // return in case of no block information.
 474   if (row_blocks_.empty()) {
 475     return transpose;
 476   }
 477
 478   *(transpose->mutable_row_blocks()) = col_blocks_;
 479   *(transpose->mutable_col_blocks()) = row_blocks_;
 480   return transpose;
 481 }
 482
 483 CompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateRandomMatrix(
 484     const CompressedRowSparseMatrix::RandomMatrixOptions& options) {
 485   CHECK_GT(options.num_row_blocks, 0);
 486   CHECK_GT(options.min_row_block_size, 0);
 487   CHECK_GT(options.max_row_block_size, 0);
 488   CHECK_LE(options.min_row_block_size, options.max_row_block_size);
 489   CHECK_GT(options.num_col_blocks, 0);
 490   CHECK_GT(options.min_col_block_size, 0);
 491   CHECK_GT(options.max_col_block_size, 0);
 492   CHECK_LE(options.min_col_block_size, options.max_col_block_size);
 493   CHECK_GT(options.block_density, 0.0);
 494   CHECK_LE(options.block_density, 1.0);
 495
 496   vector<int> row_blocks;
 497   vector<int> col_blocks;
 498
 499   // Generate the row block structure.
 500   for (int i = 0; i < options.num_row_blocks; ++i) {
 501     // Generate a random integer in [min_row_block_size, max_row_block_size]
 502     const int delta_block_size =
 503         Uniform(options.max_row_block_size - options.min_row_block_size);
 504     row_blocks.push_back(options.min_row_block_size + delta_block_size);
 505   }
 506
 507   // Generate the col block structure.
 508   for (int i = 0; i < options.num_col_blocks; ++i) {
 509     // Generate a random integer in [min_col_block_size, max_col_block_size]
 510     const int delta_block_size =
 511         Uniform(options.max_col_block_size - options.min_col_block_size);
 512     col_blocks.push_back(options.min_col_block_size + delta_block_size);
 513   }
 514
 515   vector<int> tsm_rows;
 516   vector<int> tsm_cols;
 517   vector<double> tsm_values;
 518
 519   // For ease of construction, we are going to generate the
 520   // CompressedRowSparseMatrix by generating it as a
 521   // TripletSparseMatrix and then converting it to a
 522   // CompressedRowSparseMatrix.
 523
 524   // It is possible that the random matrix is empty which is likely
 525   // not what the user wants, so do the matrix generation till we have
 526   // at least one non-zero entry.
 527   while (tsm_values.empty()) {
 528     tsm_rows.clear();
 529     tsm_cols.clear();
 530     tsm_values.clear();
 531
 532     int row_block_begin = 0;
 533     for (int r = 0; r < options.num_row_blocks; ++r) {
 534       int col_block_begin = 0;
 535       for (int c = 0; c < options.num_col_blocks; ++c) {
 536         // Randomly determine if this block is present or not.
 537         if (RandDouble() <= options.block_density) {
 538           AddRandomBlock(row_blocks[r],
 539                          col_blocks[c],
 540                          row_block_begin,
 541                          col_block_begin,
 542                          &tsm_rows,
 543                          &tsm_cols,
 544                          &tsm_values);
 545         }
 546         col_block_begin += col_blocks[c];
 547       }
 548       row_block_begin += row_blocks[r];
 549     }
 550   }
 551
 552   const int num_rows = std::accumulate(row_blocks.begin(), row_blocks.end(), 0);
 553   const int num_cols = std::accumulate(col_blocks.begin(), col_blocks.end(), 0);
 554   const bool kDoNotTranspose = false;
 555   CompressedRowSparseMatrix* matrix =
 556       CompressedRowSparseMatrix::FromTripletSparseMatrix(
 557           TripletSparseMatrix(
 558               num_rows, num_cols, tsm_rows, tsm_cols, tsm_values),
 559           kDoNotTranspose);
 560   (*matrix->mutable_row_blocks()) = row_blocks;
 561   (*matrix->mutable_col_blocks()) = col_blocks;
 562   matrix->set_storage_type(CompressedRowSparseMatrix::UNSYMMETRIC);
 563   return matrix;
 564 }
 565
 566 }  // namespace internal
 567 }  // namespace ceres