1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2013 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 #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(const int* scalar_rows, 51 const int* scalar_cols, 52 const vector<int>& row_blocks, 53 const vector<int>& col_blocks, 54 vector<int>* block_rows, 55 vector<int>* block_cols); 56 57 // Given a set of blocks and a permutation of these blocks, compute 58 // the corresponding "scalar" ordering, where the scalar ordering of 59 // size sum(blocks). 60 void BlockOrderingToScalarOrdering(const vector<int>& blocks, 61 const vector<int>& block_ordering, 62 vector<int>* scalar_ordering); 63 64 // Solve the linear system 65 // 66 // R * solution = rhs 67 // 68 // Where R is an upper triangular compressed column sparse matrix. 69 template <typename IntegerType> 70 void SolveUpperTriangularInPlace(IntegerType num_cols, 71 const IntegerType* rows, 72 const IntegerType* cols, 73 const double* values, 74 double* rhs_and_solution) { 75 for (IntegerType c = num_cols - 1; c >= 0; --c) { 76 rhs_and_solution[c] /= values[cols[c + 1] - 1]; 77 for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { 78 const IntegerType r = rows[idx]; 79 const double v = values[idx]; 80 rhs_and_solution[r] -= v * rhs_and_solution[c]; 81 } 82 } 83 } 84 85 // Solve the linear system 86 // 87 // R' * solution = rhs 88 // 89 // Where R is an upper triangular compressed column sparse matrix. 90 template <typename IntegerType> 91 void SolveUpperTriangularTransposeInPlace(IntegerType num_cols, 92 const IntegerType* rows, 93 const IntegerType* cols, 94 const double* values, 95 double* rhs_and_solution) { 96 for (IntegerType c = 0; c < num_cols; ++c) { 97 for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { 98 const IntegerType r = rows[idx]; 99 const double v = values[idx]; 100 rhs_and_solution[c] -= v * rhs_and_solution[r]; 101 } 102 rhs_and_solution[c] = rhs_and_solution[c] / values[cols[c + 1] - 1]; 103 } 104 } 105 106 // Given a upper triangular matrix R in compressed column form, solve 107 // the linear system, 108 // 109 // R'R x = b 110 // 111 // Where b is all zeros except for rhs_nonzero_index, where it is 112 // equal to one. 113 // 114 // The function exploits this knowledge to reduce the number of 115 // floating point operations. 116 template <typename IntegerType> 117 void SolveRTRWithSparseRHS(IntegerType num_cols, 118 const IntegerType* rows, 119 const IntegerType* cols, 120 const double* values, 121 const int rhs_nonzero_index, 122 double* solution) { 123 fill(solution, solution + num_cols, 0.0); 124 solution[rhs_nonzero_index] = 1.0 / values[cols[rhs_nonzero_index + 1] - 1]; 125 126 for (IntegerType c = rhs_nonzero_index + 1; c < num_cols; ++c) { 127 for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { 128 const IntegerType r = rows[idx]; 129 if (r < rhs_nonzero_index) continue; 130 const double v = values[idx]; 131 solution[c] -= v * solution[r]; 132 } 133 solution[c] = solution[c] / values[cols[c + 1] - 1]; 134 } 135 136 SolveUpperTriangularInPlace(num_cols, rows, cols, values, solution); 137 } 138 139 } // namespace internal 140 } // namespace ceres 141 142 #endif // CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ 143
