| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309 | // Ceres Solver - A fast non-linear least squares minimizer// Copyright 2015 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: sameeragarwal@google.com (Sameer Agarwal)#include "ceres/triplet_sparse_matrix.h"#include <algorithm>#include <cstddef>#include "ceres/internal/eigen.h"#include "ceres/internal/port.h"#include "ceres/random.h"#include "ceres/types.h"#include "glog/logging.h"namespace ceres {namespace internal {TripletSparseMatrix::TripletSparseMatrix()    : num_rows_(0),      num_cols_(0),      max_num_nonzeros_(0),      num_nonzeros_(0) {}TripletSparseMatrix::~TripletSparseMatrix() {}TripletSparseMatrix::TripletSparseMatrix(int num_rows,                                         int num_cols,                                         int max_num_nonzeros)    : num_rows_(num_rows),      num_cols_(num_cols),      max_num_nonzeros_(max_num_nonzeros),      num_nonzeros_(0) {  // All the sizes should at least be zero  CHECK_GE(num_rows, 0);  CHECK_GE(num_cols, 0);  CHECK_GE(max_num_nonzeros, 0);  AllocateMemory();}TripletSparseMatrix::TripletSparseMatrix(const int num_rows,                                         const int num_cols,                                         const std::vector<int>& rows,                                         const std::vector<int>& cols,                                         const std::vector<double>& values)    : num_rows_(num_rows),      num_cols_(num_cols),      max_num_nonzeros_(values.size()),      num_nonzeros_(values.size()) {  // All the sizes should at least be zero  CHECK_GE(num_rows, 0);  CHECK_GE(num_cols, 0);  CHECK_EQ(rows.size(), cols.size());  CHECK_EQ(rows.size(), values.size());  AllocateMemory();  std::copy(rows.begin(), rows.end(), rows_.get());  std::copy(cols.begin(), cols.end(), cols_.get());  std::copy(values.begin(), values.end(), values_.get());}TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig)    : SparseMatrix(),      num_rows_(orig.num_rows_),      num_cols_(orig.num_cols_),      max_num_nonzeros_(orig.max_num_nonzeros_),      num_nonzeros_(orig.num_nonzeros_) {  AllocateMemory();  CopyData(orig);}TripletSparseMatrix& TripletSparseMatrix::operator=(    const TripletSparseMatrix& rhs) {  if (this == &rhs) {    return *this;  }  num_rows_ = rhs.num_rows_;  num_cols_ = rhs.num_cols_;  num_nonzeros_ = rhs.num_nonzeros_;  max_num_nonzeros_ = rhs.max_num_nonzeros_;  AllocateMemory();  CopyData(rhs);  return *this;}bool TripletSparseMatrix::AllTripletsWithinBounds() const {  for (int i = 0; i < num_nonzeros_; ++i) {    if ((rows_[i] < 0) || (rows_[i] >= num_rows_) ||        (cols_[i] < 0) || (cols_[i] >= num_cols_))      return false;  }  return true;}void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) {  CHECK_LE(num_nonzeros_, new_max_num_nonzeros)      << "Reallocation will cause data loss";  // Nothing to do if we have enough space already.  if (new_max_num_nonzeros <= max_num_nonzeros_)    return;  int* new_rows = new int[new_max_num_nonzeros];  int* new_cols = new int[new_max_num_nonzeros];  double* new_values = new double[new_max_num_nonzeros];  for (int i = 0; i < num_nonzeros_; ++i) {    new_rows[i] = rows_[i];    new_cols[i] = cols_[i];    new_values[i] = values_[i];  }  rows_.reset(new_rows);  cols_.reset(new_cols);  values_.reset(new_values);  max_num_nonzeros_ = new_max_num_nonzeros;}void TripletSparseMatrix::SetZero() {  std::fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0);  num_nonzeros_ = 0;}void TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) {  CHECK_GE(num_nonzeros, 0);  CHECK_LE(num_nonzeros, max_num_nonzeros_);  num_nonzeros_ = num_nonzeros;}void TripletSparseMatrix::AllocateMemory() {  rows_.reset(new int[max_num_nonzeros_]);  cols_.reset(new int[max_num_nonzeros_]);  values_.reset(new double[max_num_nonzeros_]);}void TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) {  for (int i = 0; i < num_nonzeros_; ++i) {    rows_[i] = orig.rows_[i];    cols_[i] = orig.cols_[i];    values_[i] = orig.values_[i];  }}void TripletSparseMatrix::RightMultiply(const double* x,  double* y) const {  for (int i = 0; i < num_nonzeros_; ++i) {    y[rows_[i]] += values_[i]*x[cols_[i]];  }}void TripletSparseMatrix::LeftMultiply(const double* x, double* y) const {  for (int i = 0; i < num_nonzeros_; ++i) {    y[cols_[i]] += values_[i]*x[rows_[i]];  }}void TripletSparseMatrix::SquaredColumnNorm(double* x) const {  CHECK(x != nullptr);  VectorRef(x, num_cols_).setZero();  for (int i = 0; i < num_nonzeros_; ++i) {    x[cols_[i]] += values_[i] * values_[i];  }}void TripletSparseMatrix::ScaleColumns(const double* scale) {  CHECK(scale != nullptr);  for (int i = 0; i < num_nonzeros_; ++i) {    values_[i] = values_[i] * scale[cols_[i]];  }}void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const {  dense_matrix->resize(num_rows_, num_cols_);  dense_matrix->setZero();  Matrix& m = *dense_matrix;  for (int i = 0; i < num_nonzeros_; ++i) {    m(rows_[i], cols_[i]) += values_[i];  }}void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) {  CHECK_EQ(B.num_cols(), num_cols_);  Reserve(num_nonzeros_ + B.num_nonzeros_);  for (int i = 0; i < B.num_nonzeros_; ++i) {    rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_;    cols_.get()[num_nonzeros_] = B.cols()[i];    values_.get()[num_nonzeros_++] = B.values()[i];  }  num_rows_ = num_rows_ + B.num_rows();}void TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) {  CHECK_EQ(B.num_rows(), num_rows_);  Reserve(num_nonzeros_ + B.num_nonzeros_);  for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) {    rows_.get()[num_nonzeros_] = B.rows()[i];    cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_;    values_.get()[num_nonzeros_] = B.values()[i];  }  num_cols_ = num_cols_ + B.num_cols();}void TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) {  if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) {    num_rows_  = new_num_rows;    num_cols_ = new_num_cols;    return;  }  num_rows_ = new_num_rows;  num_cols_ = new_num_cols;  int* r_ptr = rows_.get();  int* c_ptr = cols_.get();  double* v_ptr = values_.get();  int dropped_terms = 0;  for (int i = 0; i < num_nonzeros_; ++i) {    if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) {      if (dropped_terms) {        r_ptr[i-dropped_terms] = r_ptr[i];        c_ptr[i-dropped_terms] = c_ptr[i];        v_ptr[i-dropped_terms] = v_ptr[i];      }    } else {      ++dropped_terms;    }  }  num_nonzeros_ -= dropped_terms;}TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix(    const double* values, int num_rows) {  TripletSparseMatrix* m =      new TripletSparseMatrix(num_rows, num_rows, num_rows);  for (int i = 0; i < num_rows; ++i) {    m->mutable_rows()[i] = i;    m->mutable_cols()[i] = i;    m->mutable_values()[i] = values[i];  }  m->set_num_nonzeros(num_rows);  return m;}void TripletSparseMatrix::ToTextFile(FILE* file) const {  CHECK(file != nullptr);  for (int i = 0; i < num_nonzeros_; ++i) {    fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]);  }}TripletSparseMatrix* TripletSparseMatrix::CreateRandomMatrix(    const TripletSparseMatrix::RandomMatrixOptions& options) {  CHECK_GT(options.num_rows, 0);  CHECK_GT(options.num_cols, 0);  CHECK_GT(options.density, 0.0);  CHECK_LE(options.density, 1.0);  std::vector<int> rows;  std::vector<int> cols;  std::vector<double> values;  while (rows.empty()) {    rows.clear();    cols.clear();    values.clear();    for (int r = 0; r < options.num_rows; ++r) {      for (int c = 0; c < options.num_cols; ++c) {        if (RandDouble() <= options.density) {          rows.push_back(r);          cols.push_back(c);          values.push_back(RandNormal());        }      }    }  }  return new TripletSparseMatrix(      options.num_rows, options.num_cols, rows, cols, values);}}  // namespace internal}  // namespace ceres
 |