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