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@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 45namespace ceres { 46namespace internal { 47 48CXSparse::CXSparse() : scratch_(NULL), scratch_size_(0) { 49} 50 51CXSparse::~CXSparse() { 52 if (scratch_size_ > 0) { 53 cs_di_free(scratch_); 54 } 55} 56 57 58bool 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 96cs_dis* CXSparse::AnalyzeCholesky(cs_di* A) { 97 // order = 1 for Cholesky factorization. 98 return cs_schol(1, A); 99} 100 101cs_dis* CXSparse::AnalyzeCholeskyWithNaturalOrdering(cs_di* A) { 102 // order = 0 for Natural ordering. 103 return cs_schol(0, A); 104} 105 106cs_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 167cs_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 179cs_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 192void 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 198cs_di* CXSparse::TransposeMatrix(cs_di* A) { 199 return cs_di_transpose(A, 1); 200} 201 202cs_di* CXSparse::MatrixMatrixMultiply(cs_di* A, cs_di* B) { 203 return cs_di_multiply(A, B); 204} 205 206void CXSparse::Free(cs_di* sparse_matrix) { 207 cs_di_spfree(sparse_matrix); 208} 209 210void 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