1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2010, 2011, 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: sameeragarwal@google.com (Sameer Agarwal)
30
31#include "ceres/triplet_sparse_matrix.h"
32
33#include <algorithm>
34#include <cstddef>
35#include "ceres/internal/eigen.h"
36#include "ceres/internal/port.h"
37#include "ceres/internal/scoped_ptr.h"
38#include "ceres/types.h"
39#include "glog/logging.h"
40
41namespace ceres {
42namespace internal {
43
44TripletSparseMatrix::TripletSparseMatrix()
45    : num_rows_(0),
46      num_cols_(0),
47      max_num_nonzeros_(0),
48      num_nonzeros_(0),
49      rows_(NULL),
50      cols_(NULL),
51      values_(NULL) {}
52
53TripletSparseMatrix::~TripletSparseMatrix() {}
54
55TripletSparseMatrix::TripletSparseMatrix(int num_rows,
56                                         int num_cols,
57                                         int max_num_nonzeros)
58    : num_rows_(num_rows),
59      num_cols_(num_cols),
60      max_num_nonzeros_(max_num_nonzeros),
61      num_nonzeros_(0),
62      rows_(NULL),
63      cols_(NULL),
64      values_(NULL) {
65  // All the sizes should at least be zero
66  CHECK_GE(num_rows, 0);
67  CHECK_GE(num_cols, 0);
68  CHECK_GE(max_num_nonzeros, 0);
69  AllocateMemory();
70}
71
72TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig)
73    : SparseMatrix(),
74      num_rows_(orig.num_rows_),
75      num_cols_(orig.num_cols_),
76      max_num_nonzeros_(orig.max_num_nonzeros_),
77      num_nonzeros_(orig.num_nonzeros_),
78      rows_(NULL),
79      cols_(NULL),
80      values_(NULL) {
81  AllocateMemory();
82  CopyData(orig);
83}
84
85TripletSparseMatrix& TripletSparseMatrix::operator=(
86    const TripletSparseMatrix& rhs) {
87  num_rows_ = rhs.num_rows_;
88  num_cols_ = rhs.num_cols_;
89  num_nonzeros_ = rhs.num_nonzeros_;
90  max_num_nonzeros_ = rhs.max_num_nonzeros_;
91  AllocateMemory();
92  CopyData(rhs);
93  return *this;
94}
95
96bool TripletSparseMatrix::AllTripletsWithinBounds() const {
97  for (int i = 0; i < num_nonzeros_; ++i) {
98    if ((rows_[i] < 0) || (rows_[i] >= num_rows_) ||
99        (cols_[i] < 0) || (cols_[i] >= num_cols_))
100      return false;
101  }
102  return true;
103}
104
105void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) {
106  CHECK_LE(num_nonzeros_, new_max_num_nonzeros)
107      << "Reallocation will cause data loss";
108
109  // Nothing to do if we have enough space already.
110  if (new_max_num_nonzeros <= max_num_nonzeros_)
111    return;
112
113  int* new_rows = new int[new_max_num_nonzeros];
114  int* new_cols = new int[new_max_num_nonzeros];
115  double* new_values = new double[new_max_num_nonzeros];
116
117  for (int i = 0; i < num_nonzeros_; ++i) {
118    new_rows[i] = rows_[i];
119    new_cols[i] = cols_[i];
120    new_values[i] = values_[i];
121  }
122
123  rows_.reset(new_rows);
124  cols_.reset(new_cols);
125  values_.reset(new_values);
126
127  max_num_nonzeros_ = new_max_num_nonzeros;
128}
129
130void TripletSparseMatrix::SetZero() {
131  fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0);
132  num_nonzeros_ = 0;
133}
134
135void TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) {
136  CHECK_GE(num_nonzeros, 0);
137  CHECK_LE(num_nonzeros, max_num_nonzeros_);
138  num_nonzeros_ = num_nonzeros;
139};
140
141void TripletSparseMatrix::AllocateMemory() {
142  rows_.reset(new int[max_num_nonzeros_]);
143  cols_.reset(new int[max_num_nonzeros_]);
144  values_.reset(new double[max_num_nonzeros_]);
145}
146
147void TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) {
148  for (int i = 0; i < num_nonzeros_; ++i) {
149    rows_[i] = orig.rows_[i];
150    cols_[i] = orig.cols_[i];
151    values_[i] = orig.values_[i];
152  }
153}
154
155void TripletSparseMatrix::RightMultiply(const double* x,  double* y) const {
156  for (int i = 0; i < num_nonzeros_; ++i) {
157    y[rows_[i]] += values_[i]*x[cols_[i]];
158  }
159}
160
161void TripletSparseMatrix::LeftMultiply(const double* x, double* y) const {
162  for (int i = 0; i < num_nonzeros_; ++i) {
163    y[cols_[i]] += values_[i]*x[rows_[i]];
164  }
165}
166
167void TripletSparseMatrix::SquaredColumnNorm(double* x) const {
168  CHECK_NOTNULL(x);
169  VectorRef(x, num_cols_).setZero();
170  for (int i = 0; i < num_nonzeros_; ++i) {
171    x[cols_[i]] += values_[i] * values_[i];
172  }
173}
174
175void TripletSparseMatrix::ScaleColumns(const double* scale) {
176  CHECK_NOTNULL(scale);
177  for (int i = 0; i < num_nonzeros_; ++i) {
178    values_[i] = values_[i] * scale[cols_[i]];
179  }
180}
181
182void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const {
183  dense_matrix->resize(num_rows_, num_cols_);
184  dense_matrix->setZero();
185  Matrix& m = *dense_matrix;
186  for (int i = 0; i < num_nonzeros_; ++i) {
187    m(rows_[i], cols_[i]) += values_[i];
188  }
189}
190
191void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) {
192  CHECK_EQ(B.num_cols(), num_cols_);
193  Reserve(num_nonzeros_ + B.num_nonzeros_);
194  for (int i = 0; i < B.num_nonzeros_; ++i) {
195    rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_;
196    cols_.get()[num_nonzeros_] = B.cols()[i];
197    values_.get()[num_nonzeros_++] = B.values()[i];
198  }
199  num_rows_ = num_rows_ + B.num_rows();
200}
201
202void TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) {
203  CHECK_EQ(B.num_rows(), num_rows_);
204  Reserve(num_nonzeros_ + B.num_nonzeros_);
205  for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) {
206    rows_.get()[num_nonzeros_] = B.rows()[i];
207    cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_;
208    values_.get()[num_nonzeros_] = B.values()[i];
209  }
210  num_cols_ = num_cols_ + B.num_cols();
211}
212
213
214void TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) {
215  if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) {
216    num_rows_  = new_num_rows;
217    num_cols_ = new_num_cols;
218    return;
219  }
220
221  num_rows_ = new_num_rows;
222  num_cols_ = new_num_cols;
223
224  int* r_ptr = rows_.get();
225  int* c_ptr = cols_.get();
226  double* v_ptr = values_.get();
227
228  int dropped_terms = 0;
229  for (int i = 0; i < num_nonzeros_; ++i) {
230    if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) {
231      if (dropped_terms) {
232        r_ptr[i-dropped_terms] = r_ptr[i];
233        c_ptr[i-dropped_terms] = c_ptr[i];
234        v_ptr[i-dropped_terms] = v_ptr[i];
235      }
236    } else {
237      ++dropped_terms;
238    }
239  }
240  num_nonzeros_ -= dropped_terms;
241}
242
243TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix(
244    const double* values, int num_rows) {
245  TripletSparseMatrix* m =
246      new TripletSparseMatrix(num_rows, num_rows, num_rows);
247  for (int i = 0; i < num_rows; ++i) {
248    m->mutable_rows()[i] = i;
249    m->mutable_cols()[i] = i;
250    m->mutable_values()[i] = values[i];
251  }
252  m->set_num_nonzeros(num_rows);
253  return m;
254}
255
256void TripletSparseMatrix::ToTextFile(FILE* file) const {
257  CHECK_NOTNULL(file);
258  for (int i = 0; i < num_nonzeros_; ++i) {
259    fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]);
260  }
261}
262
263}  // namespace internal
264}  // namespace ceres
265