Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 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 | |
Carlos Hernandez | 79397c2 | 2014-08-07 17:51:38 -0700 | [diff] [blame] | 31 | // This include must come before any #ifndef check on Ceres compile options. |
| 32 | #include "ceres/internal/port.h" |
| 33 | |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 34 | #ifndef CERES_NO_CXSPARSE |
| 35 | |
| 36 | #include "ceres/cxsparse.h" |
| 37 | |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 38 | #include <vector> |
| 39 | #include "ceres/compressed_col_sparse_matrix_utils.h" |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 40 | #include "ceres/compressed_row_sparse_matrix.h" |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 41 | #include "ceres/internal/port.h" |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 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) { |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 53 | cs_di_free(scratch_); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 54 | } |
| 55 | } |
| 56 | |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 57 | |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 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) { |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 64 | cs_di_free(scratch_); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 65 | } |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 66 | scratch_ = |
| 67 | reinterpret_cast<CS_ENTRY*>(cs_di_malloc(A->n, sizeof(CS_ENTRY))); |
| 68 | scratch_size_ = A->n; |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 69 | } |
| 70 | |
| 71 | // Solve using Cholesky factorization |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 72 | csn* numeric_factorization = cs_di_chol(A, symbolic_factorization); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 73 | if (numeric_factorization == NULL) { |
| 74 | LOG(WARNING) << "Cholesky factorization failed."; |
| 75 | return false; |
| 76 | } |
| 77 | |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 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. |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 81 | // |
| 82 | // Set x = P * b. |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 83 | cs_di_ipvec(symbolic_factorization->pinv, b, scratch_, A->n); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 84 | // Set x = L \ x. |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 85 | cs_di_lsolve(numeric_factorization->L, scratch_); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 86 | // Set x = L' \ x. |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 87 | cs_di_ltsolve(numeric_factorization->L, scratch_); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 88 | // Set b = P' * x. |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 89 | cs_di_pvec(symbolic_factorization->pinv, scratch_, b, A->n); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 90 | |
| 91 | // Free Cholesky factorization. |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 92 | cs_di_nfree(numeric_factorization); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 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 | |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 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 | |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 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; |
Carlos Hernandez | 79397c2 | 2014-08-07 17:51:38 -0700 | [diff] [blame] | 181 | tsm_wrapper.nzmax = tsm->num_nonzeros(); |
| 182 | tsm_wrapper.nz = tsm->num_nonzeros(); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 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 | |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 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); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 196 | } |
| 197 | |
Sascha Haeberling | 1d2624a | 2013-07-23 19:00:21 -0700 | [diff] [blame] | 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); |
Angus Kong | 0ae28bd | 2013-02-13 14:56:04 -0800 | [diff] [blame] | 212 | } |
| 213 | |
| 214 | } // namespace internal |
| 215 | } // namespace ceres |
| 216 | |
| 217 | #endif // CERES_NO_CXSPARSE |