| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289 | // Ceres Solver - A fast non-linear least squares minimizer// Copyright 2018 Google Inc. All rights reserved.// http://ceres-solver.org///// Redistribution and use in source and binary forms, with or without// modification, are permitted provided that the following conditions are met://// * Redistributions of source code must retain the above copyright notice,//   this list of conditions and the following disclaimer.// * Redistributions in binary form must reproduce the above copyright notice,//   this list of conditions and the following disclaimer in the documentation//   and/or other materials provided with the distribution.// * Neither the name of Google Inc. nor the names of its contributors may be//   used to endorse or promote products derived from this software without//   specific prior written permission.//// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE// POSSIBILITY OF SUCH DAMAGE.//// Author: alexs.mac@gmail.com (Alex Stewart)// This include must come before any #ifndef check on Ceres compile options.#include "ceres/internal/port.h"#ifndef CERES_NO_ACCELERATE_SPARSE#include "ceres/accelerate_sparse.h"#include <algorithm>#include <string>#include <vector>#include "ceres/compressed_col_sparse_matrix_utils.h"#include "ceres/compressed_row_sparse_matrix.h"#include "ceres/triplet_sparse_matrix.h"#include "glog/logging.h"#define CASESTR(x) case x: return #xnamespace ceres {namespace internal {namespace {const char* SparseStatusToString(SparseStatus_t status) {  switch (status) {    CASESTR(SparseStatusOK);    CASESTR(SparseFactorizationFailed);    CASESTR(SparseMatrixIsSingular);    CASESTR(SparseInternalError);    CASESTR(SparseParameterError);    CASESTR(SparseStatusReleased);    default:      return "UKNOWN";  }}}  // namespace.// Resizes workspace as required to contain at least required_size bytes// aligned to kAccelerateRequiredAlignment and returns a pointer to the// aligned start.void* ResizeForAccelerateAlignment(const size_t required_size,                                   std::vector<uint8_t> *workspace) {  // As per the Accelerate documentation, all workspace memory passed to the  // sparse solver functions must be 16-byte aligned.  constexpr int kAccelerateRequiredAlignment = 16;  // Although malloc() on macOS should always be 16-byte aligned, it is unclear  // if this holds for new(), or on other Apple OSs (phoneOS, watchOS etc).  // As such we assume it is not and use std::align() to create a (potentially  // offset) 16-byte aligned sub-buffer of the specified size within workspace.  workspace->resize(required_size + kAccelerateRequiredAlignment);  size_t size_from_aligned_start = workspace->size();  void* aligned_solve_workspace_start =      reinterpret_cast<void*>(workspace->data());  aligned_solve_workspace_start =      std::align(kAccelerateRequiredAlignment,                 required_size,                 aligned_solve_workspace_start,                 size_from_aligned_start);  CHECK(aligned_solve_workspace_start != nullptr)      << "required_size: " << required_size      << ", workspace size: " << workspace->size();  return aligned_solve_workspace_start;}template<typename Scalar>void AccelerateSparse<Scalar>::Solve(NumericFactorization* numeric_factor,                                     DenseVector* rhs_and_solution) {  // From SparseSolve() documentation in Solve.h  const int required_size =      numeric_factor->solveWorkspaceRequiredStatic +      numeric_factor->solveWorkspaceRequiredPerRHS;  SparseSolve(*numeric_factor, *rhs_and_solution,              ResizeForAccelerateAlignment(required_size, &solve_workspace_));}template<typename Scalar>typename AccelerateSparse<Scalar>::ASSparseMatrixAccelerateSparse<Scalar>::CreateSparseMatrixTransposeView(    CompressedRowSparseMatrix* A) {  // Accelerate uses CSC as its sparse storage format whereas Ceres uses CSR.  // As this method returns the transpose view we can flip rows/cols to map  // from CSR to CSC^T.  //  // Accelerate's columnStarts is a long*, not an int*.  These types might be  // different (e.g. ARM on iOS) so always make a copy.  column_starts_.resize(A->num_rows() +1); // +1 for final column length.  std::copy_n(A->rows(), column_starts_.size(), &column_starts_[0]);  ASSparseMatrix At;  At.structure.rowCount = A->num_cols();  At.structure.columnCount = A->num_rows();  At.structure.columnStarts = &column_starts_[0];  At.structure.rowIndices = A->mutable_cols();  At.structure.attributes.transpose = false;  At.structure.attributes.triangle = SparseUpperTriangle;  At.structure.attributes.kind = SparseSymmetric;  At.structure.attributes._reserved = 0;  At.structure.attributes._allocatedBySparse = 0;  At.structure.blockSize = 1;  if (std::is_same<Scalar, double>::value) {    At.data = reinterpret_cast<Scalar*>(A->mutable_values());  } else {    values_ =        ConstVectorRef(A->values(), A->num_nonzeros()).template cast<Scalar>();    At.data = values_.data();  }  return At;}template<typename Scalar>typename AccelerateSparse<Scalar>::SymbolicFactorizationAccelerateSparse<Scalar>::AnalyzeCholesky(ASSparseMatrix* A) {  return SparseFactor(SparseFactorizationCholesky, A->structure);}template<typename Scalar>typename AccelerateSparse<Scalar>::NumericFactorizationAccelerateSparse<Scalar>::Cholesky(ASSparseMatrix* A,                                   SymbolicFactorization* symbolic_factor) {  return SparseFactor(*symbolic_factor, *A);}template<typename Scalar>void AccelerateSparse<Scalar>::Cholesky(ASSparseMatrix* A,                                        NumericFactorization* numeric_factor) {  // From SparseRefactor() documentation in Solve.h  const int required_size = std::is_same<Scalar, double>::value      ? numeric_factor->symbolicFactorization.workspaceSize_Double      : numeric_factor->symbolicFactorization.workspaceSize_Float;  return SparseRefactor(*A, numeric_factor,                        ResizeForAccelerateAlignment(required_size,                                                     &factorization_workspace_));}// Instantiate only for the specific template types required/supported s/t the// definition can be in the .cc file.template class AccelerateSparse<double>;template class AccelerateSparse<float>;template<typename Scalar>std::unique_ptr<SparseCholesky>AppleAccelerateCholesky<Scalar>::Create(OrderingType ordering_type) {  return std::unique_ptr<SparseCholesky>(      new AppleAccelerateCholesky<Scalar>(ordering_type));}template<typename Scalar>AppleAccelerateCholesky<Scalar>::AppleAccelerateCholesky(    const OrderingType ordering_type)    : ordering_type_(ordering_type) {}template<typename Scalar>AppleAccelerateCholesky<Scalar>::~AppleAccelerateCholesky() {  FreeSymbolicFactorization();  FreeNumericFactorization();}template<typename Scalar>CompressedRowSparseMatrix::StorageTypeAppleAccelerateCholesky<Scalar>::StorageType() const {  return CompressedRowSparseMatrix::LOWER_TRIANGULAR;}template<typename Scalar>LinearSolverTerminationTypeAppleAccelerateCholesky<Scalar>::Factorize(CompressedRowSparseMatrix* lhs,                                           std::string* message) {  CHECK_EQ(lhs->storage_type(), StorageType());  if (lhs == NULL) {    *message = "Failure: Input lhs is NULL.";    return LINEAR_SOLVER_FATAL_ERROR;  }  typename SparseTypesTrait<Scalar>::SparseMatrix as_lhs =      as_.CreateSparseMatrixTransposeView(lhs);  if (!symbolic_factor_) {    symbolic_factor_.reset(        new typename SparseTypesTrait<Scalar>::SymbolicFactorization(            as_.AnalyzeCholesky(&as_lhs)));    if (symbolic_factor_->status != SparseStatusOK) {      *message = StringPrintf(          "Apple Accelerate Failure : Symbolic factorisation failed: %s",          SparseStatusToString(symbolic_factor_->status));      FreeSymbolicFactorization();      return LINEAR_SOLVER_FATAL_ERROR;    }  }  if (!numeric_factor_) {    numeric_factor_.reset(        new typename SparseTypesTrait<Scalar>::NumericFactorization(            as_.Cholesky(&as_lhs, symbolic_factor_.get())));  } else {    // Recycle memory from previous numeric factorization.    as_.Cholesky(&as_lhs, numeric_factor_.get());  }  if (numeric_factor_->status != SparseStatusOK) {    *message = StringPrintf(        "Apple Accelerate Failure : Numeric factorisation failed: %s",        SparseStatusToString(numeric_factor_->status));    FreeNumericFactorization();    return LINEAR_SOLVER_FAILURE;  }  return LINEAR_SOLVER_SUCCESS;}template<typename Scalar>LinearSolverTerminationTypeAppleAccelerateCholesky<Scalar>::Solve(const double* rhs,                                       double* solution,                                       std::string* message) {  CHECK_EQ(numeric_factor_->status, SparseStatusOK)      << "Solve called without a call to Factorize first ("      << SparseStatusToString(numeric_factor_->status) << ").";  const int num_cols = numeric_factor_->symbolicFactorization.columnCount;  typename SparseTypesTrait<Scalar>::DenseVector as_rhs_and_solution;  as_rhs_and_solution.count = num_cols;  if (std::is_same<Scalar, double>::value) {    as_rhs_and_solution.data = reinterpret_cast<Scalar*>(solution);    std::copy_n(rhs, num_cols, solution);  } else {    scalar_rhs_and_solution_ =        ConstVectorRef(rhs, num_cols).template cast<Scalar>();    as_rhs_and_solution.data = scalar_rhs_and_solution_.data();  }  as_.Solve(numeric_factor_.get(), &as_rhs_and_solution);  if (!std::is_same<Scalar, double>::value) {    VectorRef(solution, num_cols) =        scalar_rhs_and_solution_.template cast<double>();  }  return LINEAR_SOLVER_SUCCESS;}template<typename Scalar>void AppleAccelerateCholesky<Scalar>::FreeSymbolicFactorization() {  if (symbolic_factor_) {    SparseCleanup(*symbolic_factor_);    symbolic_factor_.reset();  }}template<typename Scalar>void AppleAccelerateCholesky<Scalar>::FreeNumericFactorization() {  if (numeric_factor_) {    SparseCleanup(*numeric_factor_);    numeric_factor_.reset();  }}// Instantiate only for the specific template types required/supported s/t the// definition can be in the .cc file.template class AppleAccelerateCholesky<double>;template class AppleAccelerateCholesky<float>;}}#endif  // CERES_NO_ACCELERATE_SPARSE
 |