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 #ifndef CERES_NO_CXSPARSE
32
33 #include "ceres/cxsparse.h"
34
35 #include "ceres/compressed_row_sparse_matrix.h"
36 #include "ceres/triplet_sparse_matrix.h"
37 #include "glog/logging.h"
38
39 namespace ceres {
40 namespace internal {
41
CXSparse()42 CXSparse::CXSparse() : scratch_(NULL), scratch_size_(0) {
43 }
44
~CXSparse()45 CXSparse::~CXSparse() {
46 if (scratch_size_ > 0) {
47 cs_free(scratch_);
48 }
49 }
50
SolveCholesky(cs_di * A,cs_dis * symbolic_factorization,double * b)51 bool CXSparse::SolveCholesky(cs_di* A,
52 cs_dis* symbolic_factorization,
53 double* b) {
54 // Make sure we have enough scratch space available.
55 if (scratch_size_ < A->n) {
56 if (scratch_size_ > 0) {
57 cs_free(scratch_);
58 }
59 scratch_ = reinterpret_cast<CS_ENTRY*>(cs_malloc(A->n, sizeof(CS_ENTRY)));
60 }
61
62 // Solve using Cholesky factorization
63 csn* numeric_factorization = cs_chol(A, symbolic_factorization);
64 if (numeric_factorization == NULL) {
65 LOG(WARNING) << "Cholesky factorization failed.";
66 return false;
67 }
68
69 // When the Cholesky factorization succeeded, these methods are guaranteed to
70 // succeeded as well. In the comments below, "x" refers to the scratch space.
71 //
72 // Set x = P * b.
73 cs_ipvec(symbolic_factorization->pinv, b, scratch_, A->n);
74
75 // Set x = L \ x.
76 cs_lsolve(numeric_factorization->L, scratch_);
77
78 // Set x = L' \ x.
79 cs_ltsolve(numeric_factorization->L, scratch_);
80
81 // Set b = P' * x.
82 cs_pvec(symbolic_factorization->pinv, scratch_, b, A->n);
83
84 // Free Cholesky factorization.
85 cs_nfree(numeric_factorization);
86 return true;
87 }
88
AnalyzeCholesky(cs_di * A)89 cs_dis* CXSparse::AnalyzeCholesky(cs_di* A) {
90 // order = 1 for Cholesky factorization.
91 return cs_schol(1, A);
92 }
93
CreateSparseMatrixTransposeView(CompressedRowSparseMatrix * A)94 cs_di CXSparse::CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A) {
95 cs_di At;
96 At.m = A->num_cols();
97 At.n = A->num_rows();
98 At.nz = -1;
99 At.nzmax = A->num_nonzeros();
100 At.p = A->mutable_rows();
101 At.i = A->mutable_cols();
102 At.x = A->mutable_values();
103 return At;
104 }
105
CreateSparseMatrix(TripletSparseMatrix * tsm)106 cs_di* CXSparse::CreateSparseMatrix(TripletSparseMatrix* tsm) {
107 cs_di_sparse tsm_wrapper;
108 tsm_wrapper.nzmax = tsm->num_nonzeros();;
109 tsm_wrapper.nz = tsm->num_nonzeros();;
110 tsm_wrapper.m = tsm->num_rows();
111 tsm_wrapper.n = tsm->num_cols();
112 tsm_wrapper.p = tsm->mutable_cols();
113 tsm_wrapper.i = tsm->mutable_rows();
114 tsm_wrapper.x = tsm->mutable_values();
115
116 return cs_compress(&tsm_wrapper);
117 }
118
Free(cs_di * factor)119 void CXSparse::Free(cs_di* factor) {
120 cs_free(factor);
121 }
122
Free(cs_dis * factor)123 void CXSparse::Free(cs_dis* factor) {
124 cs_sfree(factor);
125 }
126
127 } // namespace internal
128 } // namespace ceres
129
130 #endif // CERES_NO_CXSPARSE
131