internal/ceres/compressed_col_sparse_matrix_utils.h

   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 #ifndef CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_
  32 #define CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_
  33
  34 #include <vector>
  35 #include "ceres/internal/port.h"
  36
  37 namespace ceres {
  38 namespace internal {
  39
  40 // Extract the block sparsity pattern of the scalar compressed columns
  41 // matrix and return it in compressed column form. The compressed
  42 // column form is stored in two vectors block_rows, and block_cols,
  43 // which correspond to the row and column arrays in a compressed
  44 // column sparse matrix.
  45 //
  46 // If c_ij is the block in the matrix A corresponding to row block i
  47 // and column block j, then it is expected that A contains at least
  48 // one non-zero entry corresponding to the top left entry of c_ij,
  49 // as that entry is used to detect the presence of a non-zero c_ij.
  50 void CompressedColumnScalarMatrixToBlockMatrix(
  51     const int* scalar_rows,
  52     const int* scalar_cols,
  53     const std::vector<int>& row_blocks,
  54     const std::vector<int>& col_blocks,
  55     std::vector<int>* block_rows,
  56     std::vector<int>* block_cols);
  57
  58 // Given a set of blocks and a permutation of these blocks, compute
  59 // the corresponding "scalar" ordering, where the scalar ordering of
  60 // size sum(blocks).
  61 void BlockOrderingToScalarOrdering(
  62     const std::vector<int>& blocks,
  63     const std::vector<int>& block_ordering,
  64     std::vector<int>* scalar_ordering);
  65
  66 // Solve the linear system
  67 //
  68 //   R * solution = rhs
  69 //
  70 // Where R is an upper triangular compressed column sparse matrix.
  71 template <typename IntegerType>
  72 void SolveUpperTriangularInPlace(IntegerType num_cols,
  73                                  const IntegerType* rows,
  74                                  const IntegerType* cols,
  75                                  const double* values,
  76                                  double* rhs_and_solution) {
  77   for (IntegerType c = num_cols - 1; c >= 0; --c) {
  78     rhs_and_solution[c] /= values[cols[c + 1] - 1];
  79     for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) {
  80       const IntegerType r = rows[idx];
  81       const double v = values[idx];
  82       rhs_and_solution[r] -= v * rhs_and_solution[c];
  83     }
  84   }
  85 }
  86
  87 // Solve the linear system
  88 //
  89 //   R' * solution = rhs
  90 //
  91 // Where R is an upper triangular compressed column sparse matrix.
  92 template <typename IntegerType>
  93 void SolveUpperTriangularTransposeInPlace(IntegerType num_cols,
  94                                           const IntegerType* rows,
  95                                           const IntegerType* cols,
  96                                           const double* values,
  97                                           double* rhs_and_solution) {
  98   for (IntegerType c = 0; c < num_cols; ++c) {
  99     for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) {
 100       const IntegerType r = rows[idx];
 101       const double v = values[idx];
 102       rhs_and_solution[c] -= v * rhs_and_solution[r];
 103     }
 104     rhs_and_solution[c] =  rhs_and_solution[c] / values[cols[c + 1] - 1];
 105   }
 106 }
 107
 108 // Given a upper triangular matrix R in compressed column form, solve
 109 // the linear system,
 110 //
 111 //  R'R x = b
 112 //
 113 // Where b is all zeros except for rhs_nonzero_index, where it is
 114 // equal to one.
 115 //
 116 // The function exploits this knowledge to reduce the number of
 117 // floating point operations.
 118 template <typename IntegerType>
 119 void SolveRTRWithSparseRHS(IntegerType num_cols,
 120                            const IntegerType* rows,
 121                            const IntegerType* cols,
 122                            const double* values,
 123                            const int rhs_nonzero_index,
 124                            double* solution) {
 125   std::fill(solution, solution + num_cols, 0.0);
 126   solution[rhs_nonzero_index] = 1.0 / values[cols[rhs_nonzero_index + 1] - 1];
 127
 128   for (IntegerType c = rhs_nonzero_index + 1; c < num_cols; ++c) {
 129     for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) {
 130       const IntegerType r = rows[idx];
 131       if (r < rhs_nonzero_index) continue;
 132       const double v = values[idx];
 133       solution[c] -= v * solution[r];
 134     }
 135     solution[c] =  solution[c] / values[cols[c + 1] - 1];
 136   }
 137
 138   SolveUpperTriangularInPlace(num_cols, rows, cols, values, solution);
 139 }
 140
 141 }  // namespace internal
 142 }  // namespace ceres
 143
 144 #endif  // CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_