1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 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: strandmark (at) google.com (Petter Strandmark) 30 31 // This include must come before any #ifndef check on Ceres compile options. 32 #include "ceres/internal/port.h" 33 34 #ifndef CERES_NO_CXSPARSE 35 36 #include "ceres/cxsparse.h" 37 38 #include <vector> 39 #include "ceres/compressed_col_sparse_matrix_utils.h" 40 #include "ceres/compressed_row_sparse_matrix.h" 41 #include "ceres/internal/port.h" 42 #include "ceres/triplet_sparse_matrix.h" 43 #include "glog/logging.h" 44 45 namespace ceres { 46 namespace internal { 47 48 CXSparse::CXSparse() : scratch_(NULL), scratch_size_(0) { 49 } 50 51 CXSparse::~CXSparse() { 52 if (scratch_size_ > 0) { 53 cs_di_free(scratch_); 54 } 55 } 56 57 58 bool CXSparse::SolveCholesky(cs_di* A, 59 cs_dis* symbolic_factorization, 60 double* b) { 61 // Make sure we have enough scratch space available. 62 if (scratch_size_ < A->n) { 63 if (scratch_size_ > 0) { 64 cs_di_free(scratch_); 65 } 66 scratch_ = 67 reinterpret_cast<CS_ENTRY*>(cs_di_malloc(A->n, sizeof(CS_ENTRY))); 68 scratch_size_ = A->n; 69 } 70 71 // Solve using Cholesky factorization 72 csn* numeric_factorization = cs_di_chol(A, symbolic_factorization); 73 if (numeric_factorization == NULL) { 74 LOG(WARNING) << "Cholesky factorization failed."; 75 return false; 76 } 77 78 // When the Cholesky factorization succeeded, these methods are 79 // guaranteed to succeeded as well. In the comments below, "x" 80 // refers to the scratch space. 81 // 82 // Set x = P * b. 83 cs_di_ipvec(symbolic_factorization->pinv, b, scratch_, A->n); 84 // Set x = L \ x. 85 cs_di_lsolve(numeric_factorization->L, scratch_); 86 // Set x = L' \ x. 87 cs_di_ltsolve(numeric_factorization->L, scratch_); 88 // Set b = P' * x. 89 cs_di_pvec(symbolic_factorization->pinv, scratch_, b, A->n); 90 91 // Free Cholesky factorization. 92 cs_di_nfree(numeric_factorization); 93 return true; 94 } 95 96 cs_dis* CXSparse::AnalyzeCholesky(cs_di* A) { 97 // order = 1 for Cholesky factorization. 98 return cs_schol(1, A); 99 } 100 101 cs_dis* CXSparse::AnalyzeCholeskyWithNaturalOrdering(cs_di* A) { 102 // order = 0 for Natural ordering. 103 return cs_schol(0, A); 104 } 105 106 cs_dis* CXSparse::BlockAnalyzeCholesky(cs_di* A, 107 const vector<int>& row_blocks, 108 const vector<int>& col_blocks) { 109 const int num_row_blocks = row_blocks.size(); 110 const int num_col_blocks = col_blocks.size(); 111 112 vector<int> block_rows; 113 vector<int> block_cols; 114 CompressedColumnScalarMatrixToBlockMatrix(A->i, 115 A->p, 116 row_blocks, 117 col_blocks, 118 &block_rows, 119 &block_cols); 120 cs_di block_matrix; 121 block_matrix.m = num_row_blocks; 122 block_matrix.n = num_col_blocks; 123 block_matrix.nz = -1; 124 block_matrix.nzmax = block_rows.size(); 125 block_matrix.p = &block_cols[0]; 126 block_matrix.i = &block_rows[0]; 127 block_matrix.x = NULL; 128 129 int* ordering = cs_amd(1, &block_matrix); 130 vector<int> block_ordering(num_row_blocks, -1); 131 copy(ordering, ordering + num_row_blocks, &block_ordering[0]); 132 cs_free(ordering); 133 134 vector<int> scalar_ordering; 135 BlockOrderingToScalarOrdering(row_blocks, block_ordering, &scalar_ordering); 136 137 cs_dis* symbolic_factorization = 138 reinterpret_cast<cs_dis*>(cs_calloc(1, sizeof(cs_dis))); 139 symbolic_factorization->pinv = cs_pinv(&scalar_ordering[0], A->n); 140 cs* permuted_A = cs_symperm(A, symbolic_factorization->pinv, 0); 141 142 symbolic_factorization->parent = cs_etree(permuted_A, 0); 143 int* postordering = cs_post(symbolic_factorization->parent, A->n); 144 int* column_counts = cs_counts(permuted_A, 145 symbolic_factorization->parent, 146 postordering, 147 0); 148 cs_free(postordering); 149 cs_spfree(permuted_A); 150 151 symbolic_factorization->cp = (int*) cs_malloc(A->n+1, sizeof(int)); 152 symbolic_factorization->lnz = cs_cumsum(symbolic_factorization->cp, 153 column_counts, 154 A->n); 155 symbolic_factorization->unz = symbolic_factorization->lnz; 156 157 cs_free(column_counts); 158 159 if (symbolic_factorization->lnz < 0) { 160 cs_sfree(symbolic_factorization); 161 symbolic_factorization = NULL; 162 } 163 164 return symbolic_factorization; 165 } 166 167 cs_di CXSparse::CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A) { 168 cs_di At; 169 At.m = A->num_cols(); 170 At.n = A->num_rows(); 171 At.nz = -1; 172 At.nzmax = A->num_nonzeros(); 173 At.p = A->mutable_rows(); 174 At.i = A->mutable_cols(); 175 At.x = A->mutable_values(); 176 return At; 177 } 178 179 cs_di* CXSparse::CreateSparseMatrix(TripletSparseMatrix* tsm) { 180 cs_di_sparse tsm_wrapper; 181 tsm_wrapper.nzmax = tsm->num_nonzeros(); 182 tsm_wrapper.nz = tsm->num_nonzeros(); 183 tsm_wrapper.m = tsm->num_rows(); 184 tsm_wrapper.n = tsm->num_cols(); 185 tsm_wrapper.p = tsm->mutable_cols(); 186 tsm_wrapper.i = tsm->mutable_rows(); 187 tsm_wrapper.x = tsm->mutable_values(); 188 189 return cs_compress(&tsm_wrapper); 190 } 191 192 void CXSparse::ApproximateMinimumDegreeOrdering(cs_di* A, int* ordering) { 193 int* cs_ordering = cs_amd(1, A); 194 copy(cs_ordering, cs_ordering + A->m, ordering); 195 cs_free(cs_ordering); 196 } 197 198 cs_di* CXSparse::TransposeMatrix(cs_di* A) { 199 return cs_di_transpose(A, 1); 200 } 201 202 cs_di* CXSparse::MatrixMatrixMultiply(cs_di* A, cs_di* B) { 203 return cs_di_multiply(A, B); 204 } 205 206 void CXSparse::Free(cs_di* sparse_matrix) { 207 cs_di_spfree(sparse_matrix); 208 } 209 210 void CXSparse::Free(cs_dis* symbolic_factorization) { 211 cs_di_sfree(symbolic_factorization); 212 } 213 214 } // namespace internal 215 } // namespace ceres 216 217 #endif // CERES_NO_CXSPARSE 218
