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