| // 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) { |
| // clang-format off |
| if ((rows_[i] < 0) || (rows_[i] >= num_rows_) || |
| (cols_[i] < 0) || (cols_[i] >= num_cols_)) |
| return false; |
| // clang-format on |
| } |
| 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 |