internal/ceres/cxsparse.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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 <string>
  39 #include <vector>
  40
  41 #include "ceres/compressed_col_sparse_matrix_utils.h"
  42 #include "ceres/compressed_row_sparse_matrix.h"
  43 #include "ceres/triplet_sparse_matrix.h"
  44 #include "glog/logging.h"
  45
  46 namespace ceres {
  47 namespace internal {
  48
  49 using std::vector;
  50
  51 CXSparse::CXSparse() : scratch_(NULL), scratch_size_(0) {}
  52
  53 CXSparse::~CXSparse() {
  54   if (scratch_size_ > 0) {
  55     cs_di_free(scratch_);
  56   }
  57 }
  58
  59 csn* CXSparse::Cholesky(cs_di* A, cs_dis* symbolic_factor) {
  60   return cs_di_chol(A, symbolic_factor);
  61 }
  62
  63 void CXSparse::Solve(cs_dis* symbolic_factor, csn* numeric_factor, double* b) {
  64   // Make sure we have enough scratch space available.
  65   const int num_cols = numeric_factor->L->n;
  66   if (scratch_size_ < num_cols) {
  67     if (scratch_size_ > 0) {
  68       cs_di_free(scratch_);
  69     }
  70     scratch_ =
  71         reinterpret_cast<CS_ENTRY*>(cs_di_malloc(num_cols, sizeof(CS_ENTRY)));
  72     scratch_size_ = num_cols;
  73   }
  74
  75   // When the Cholesky factor succeeded, these methods are
  76   // guaranteed to succeeded as well. In the comments below, "x"
  77   // refers to the scratch space.
  78   //
  79   // Set x = P * b.
  80   CHECK(cs_di_ipvec(symbolic_factor->pinv, b, scratch_, num_cols));
  81   // Set x = L \ x.
  82   CHECK(cs_di_lsolve(numeric_factor->L, scratch_));
  83   // Set x = L' \ x.
  84   CHECK(cs_di_ltsolve(numeric_factor->L, scratch_));
  85   // Set b = P' * x.
  86   CHECK(cs_di_pvec(symbolic_factor->pinv, scratch_, b, num_cols));
  87 }
  88
  89 bool CXSparse::SolveCholesky(cs_di* lhs, double* rhs_and_solution) {
  90   return cs_cholsol(1, lhs, rhs_and_solution);
  91 }
  92
  93 cs_dis* CXSparse::AnalyzeCholesky(cs_di* A) {
  94   // order = 1 for Cholesky factor.
  95   return cs_schol(1, A);
  96 }
  97
  98 cs_dis* CXSparse::AnalyzeCholeskyWithNaturalOrdering(cs_di* A) {
  99   // order = 0 for Natural ordering.
 100   return cs_schol(0, A);
 101 }
 102
 103 cs_dis* CXSparse::BlockAnalyzeCholesky(cs_di* A,
 104                                        const vector<int>& row_blocks,
 105                                        const vector<int>& col_blocks) {
 106   const int num_row_blocks = row_blocks.size();
 107   const int num_col_blocks = col_blocks.size();
 108
 109   vector<int> block_rows;
 110   vector<int> block_cols;
 111   CompressedColumnScalarMatrixToBlockMatrix(
 112       A->i, A->p, row_blocks, col_blocks, &block_rows, &block_cols);
 113   cs_di block_matrix;
 114   block_matrix.m = num_row_blocks;
 115   block_matrix.n = num_col_blocks;
 116   block_matrix.nz = -1;
 117   block_matrix.nzmax = block_rows.size();
 118   block_matrix.p = &block_cols[0];
 119   block_matrix.i = &block_rows[0];
 120   block_matrix.x = NULL;
 121
 122   int* ordering = cs_amd(1, &block_matrix);
 123   vector<int> block_ordering(num_row_blocks, -1);
 124   std::copy(ordering, ordering + num_row_blocks, &block_ordering[0]);
 125   cs_free(ordering);
 126
 127   vector<int> scalar_ordering;
 128   BlockOrderingToScalarOrdering(row_blocks, block_ordering, &scalar_ordering);
 129
 130   cs_dis* symbolic_factor =
 131       reinterpret_cast<cs_dis*>(cs_calloc(1, sizeof(cs_dis)));
 132   symbolic_factor->pinv = cs_pinv(&scalar_ordering[0], A->n);
 133   cs* permuted_A = cs_symperm(A, symbolic_factor->pinv, 0);
 134
 135   symbolic_factor->parent = cs_etree(permuted_A, 0);
 136   int* postordering = cs_post(symbolic_factor->parent, A->n);
 137   int* column_counts =
 138       cs_counts(permuted_A, symbolic_factor->parent, postordering, 0);
 139   cs_free(postordering);
 140   cs_spfree(permuted_A);
 141
 142   symbolic_factor->cp = (int*)cs_malloc(A->n + 1, sizeof(int));
 143   symbolic_factor->lnz = cs_cumsum(symbolic_factor->cp, column_counts, A->n);
 144   symbolic_factor->unz = symbolic_factor->lnz;
 145
 146   cs_free(column_counts);
 147
 148   if (symbolic_factor->lnz < 0) {
 149     cs_sfree(symbolic_factor);
 150     symbolic_factor = NULL;
 151   }
 152
 153   return symbolic_factor;
 154 }
 155
 156 cs_di CXSparse::CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A) {
 157   cs_di At;
 158   At.m = A->num_cols();
 159   At.n = A->num_rows();
 160   At.nz = -1;
 161   At.nzmax = A->num_nonzeros();
 162   At.p = A->mutable_rows();
 163   At.i = A->mutable_cols();
 164   At.x = A->mutable_values();
 165   return At;
 166 }
 167
 168 cs_di* CXSparse::CreateSparseMatrix(TripletSparseMatrix* tsm) {
 169   cs_di_sparse tsm_wrapper;
 170   tsm_wrapper.nzmax = tsm->num_nonzeros();
 171   tsm_wrapper.nz = tsm->num_nonzeros();
 172   tsm_wrapper.m = tsm->num_rows();
 173   tsm_wrapper.n = tsm->num_cols();
 174   tsm_wrapper.p = tsm->mutable_cols();
 175   tsm_wrapper.i = tsm->mutable_rows();
 176   tsm_wrapper.x = tsm->mutable_values();
 177
 178   return cs_compress(&tsm_wrapper);
 179 }
 180
 181 void CXSparse::ApproximateMinimumDegreeOrdering(cs_di* A, int* ordering) {
 182   int* cs_ordering = cs_amd(1, A);
 183   std::copy(cs_ordering, cs_ordering + A->m, ordering);
 184   cs_free(cs_ordering);
 185 }
 186
 187 cs_di* CXSparse::TransposeMatrix(cs_di* A) { return cs_di_transpose(A, 1); }
 188
 189 cs_di* CXSparse::MatrixMatrixMultiply(cs_di* A, cs_di* B) {
 190   return cs_di_multiply(A, B);
 191 }
 192
 193 void CXSparse::Free(cs_di* sparse_matrix) { cs_di_spfree(sparse_matrix); }
 194
 195 void CXSparse::Free(cs_dis* symbolic_factor) { cs_di_sfree(symbolic_factor); }
 196
 197 void CXSparse::Free(csn* numeric_factor) { cs_di_nfree(numeric_factor); }
 198
 199 CXSparseCholesky* CXSparseCholesky::Create(const OrderingType ordering_type) {
 200   return new CXSparseCholesky(ordering_type);
 201 }
 202
 203 CompressedRowSparseMatrix::StorageType CXSparseCholesky::StorageType() const {
 204   return CompressedRowSparseMatrix::LOWER_TRIANGULAR;
 205 }
 206
 207 CXSparseCholesky::CXSparseCholesky(const OrderingType ordering_type)
 208     : ordering_type_(ordering_type),
 209       symbolic_factor_(NULL),
 210       numeric_factor_(NULL) {}
 211
 212 CXSparseCholesky::~CXSparseCholesky() {
 213   FreeSymbolicFactorization();
 214   FreeNumericFactorization();
 215 }
 216
 217 LinearSolverTerminationType CXSparseCholesky::Factorize(
 218     CompressedRowSparseMatrix* lhs, std::string* message) {
 219   CHECK_EQ(lhs->storage_type(), StorageType());
 220   if (lhs == NULL) {
 221     *message = "Failure: Input lhs is NULL.";
 222     return LINEAR_SOLVER_FATAL_ERROR;
 223   }
 224
 225   cs_di cs_lhs = cs_.CreateSparseMatrixTransposeView(lhs);
 226
 227   if (symbolic_factor_ == NULL) {
 228     if (ordering_type_ == NATURAL) {
 229       symbolic_factor_ = cs_.AnalyzeCholeskyWithNaturalOrdering(&cs_lhs);
 230     } else {
 231       if (!lhs->col_blocks().empty() && !(lhs->row_blocks().empty())) {
 232         symbolic_factor_ = cs_.BlockAnalyzeCholesky(
 233             &cs_lhs, lhs->col_blocks(), lhs->row_blocks());
 234       } else {
 235         symbolic_factor_ = cs_.AnalyzeCholesky(&cs_lhs);
 236       }
 237     }
 238
 239     if (symbolic_factor_ == NULL) {
 240       *message = "CXSparse Failure : Symbolic factorization failed.";
 241       return LINEAR_SOLVER_FATAL_ERROR;
 242     }
 243   }
 244
 245   FreeNumericFactorization();
 246   numeric_factor_ = cs_.Cholesky(&cs_lhs, symbolic_factor_);
 247   if (numeric_factor_ == NULL) {
 248     *message = "CXSparse Failure : Numeric factorization failed.";
 249     return LINEAR_SOLVER_FAILURE;
 250   }
 251
 252   return LINEAR_SOLVER_SUCCESS;
 253 }
 254
 255 LinearSolverTerminationType CXSparseCholesky::Solve(const double* rhs,
 256                                                     double* solution,
 257                                                     std::string* message) {
 258   CHECK(numeric_factor_ != NULL)
 259       << "Solve called without a call to Factorize first.";
 260   const int num_cols = numeric_factor_->L->n;
 261   memcpy(solution, rhs, num_cols * sizeof(*solution));
 262   cs_.Solve(symbolic_factor_, numeric_factor_, solution);
 263   return LINEAR_SOLVER_SUCCESS;
 264 }
 265
 266 void CXSparseCholesky::FreeSymbolicFactorization() {
 267   if (symbolic_factor_ != NULL) {
 268     cs_.Free(symbolic_factor_);
 269     symbolic_factor_ = NULL;
 270   }
 271 }
 272
 273 void CXSparseCholesky::FreeNumericFactorization() {
 274   if (numeric_factor_ != NULL) {
 275     cs_.Free(numeric_factor_);
 276     numeric_factor_ = NULL;
 277   }
 278 }
 279
 280 }  // namespace internal
 281 }  // namespace ceres
 282
 283 #endif  // CERES_NO_CXSPARSE