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
45 namespace ceres {
46 namespace internal {
47
CXSparse()48 CXSparse::CXSparse() : scratch_(NULL), scratch_size_(0) {
49 }
50
~CXSparse()51 CXSparse::~CXSparse() {
52 if (scratch_size_ > 0) {
53 cs_di_free(scratch_);
54 }
55 }
56
57
SolveCholesky(cs_di * A,cs_dis * symbolic_factorization,double * b)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
AnalyzeCholesky(cs_di * A)96 cs_dis* CXSparse::AnalyzeCholesky(cs_di* A) {
97 // order = 1 for Cholesky factorization.
98 return cs_schol(1, A);
99 }
100
AnalyzeCholeskyWithNaturalOrdering(cs_di * A)101 cs_dis* CXSparse::AnalyzeCholeskyWithNaturalOrdering(cs_di* A) {
102 // order = 0 for Natural ordering.
103 return cs_schol(0, A);
104 }
105
BlockAnalyzeCholesky(cs_di * A,const vector<int> & row_blocks,const vector<int> & col_blocks)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
CreateSparseMatrixTransposeView(CompressedRowSparseMatrix * A)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
CreateSparseMatrix(TripletSparseMatrix * tsm)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
ApproximateMinimumDegreeOrdering(cs_di * A,int * ordering)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
TransposeMatrix(cs_di * A)198 cs_di* CXSparse::TransposeMatrix(cs_di* A) {
199 return cs_di_transpose(A, 1);
200 }
201
MatrixMatrixMultiply(cs_di * A,cs_di * B)202 cs_di* CXSparse::MatrixMatrixMultiply(cs_di* A, cs_di* B) {
203 return cs_di_multiply(A, B);
204 }
205
Free(cs_di * sparse_matrix)206 void CXSparse::Free(cs_di* sparse_matrix) {
207 cs_di_spfree(sparse_matrix);
208 }
209
Free(cs_dis * symbolic_factorization)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