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