← Back to branch summary

~ubuntu-branches/ubuntu/raring/ceres-solver/raring

« back to all changes in this revision

Viewing changes to internal/ceres/compressed_row_sparse_matrix.cc

  • Committer: Package Import Robot
  • Author(s): Koichi Akabe
  • Date: 2012-06-04 07:15:43 UTC
  • Revision ID: package-import@ubuntu.com-20120604071543-zx6uthupvmtqn3k2
Tags: upstream-1.1.1
Import upstream version 1.1.1

Show diffs side-by-side

added added

removed removed

Lines of Context:
internal/ceres/compressed_row_sparse_matrix.cc
 
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/compressed_row_sparse_matrix.h"
 
32
 
 
33
#include <algorithm>
 
34
#include <vector>
 
35
#include "ceres/matrix_proto.h"
 
36
#include "ceres/internal/port.h"
 
37
 
 
38
namespace ceres {
 
39
namespace internal {
 
40
namespace {
 
41
 
 
42
// Helper functor used by the constructor for reordering the contents
 
43
// of a TripletSparseMatrix. This comparator assumes thay there are no
 
44
// duplicates in the pair of arrays rows and cols, i.e., there is no
 
45
// indices i and j (not equal to each other) s.t.
 
46
//
 
47
//  rows[i] == rows[j] && cols[i] == cols[j]
 
48
//
 
49
// If this is the case, this functor will not be a StrictWeakOrdering.
 
50
struct RowColLessThan {
 
51
  RowColLessThan(const int* rows, const int* cols)
 
52
      : rows(rows), cols(cols) {
 
53
  }
 
54
 
 
55
  bool operator()(const int x, const int y) const {
 
56
    if (rows[x] == rows[y]) {
 
57
      return (cols[x] < cols[y]);
 
58
    }
 
59
    return (rows[x] < rows[y]);
 
60
  }
 
61
 
 
62
  const int* rows;
 
63
  const int* cols;
 
64
};
 
65
 
 
66
}  // namespace
 
67
 
 
68
// This constructor gives you a semi-initialized CompressedRowSparseMatrix.
 
69
CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows,
 
70
                                                     int num_cols,
 
71
                                                     int max_num_nonzeros) {
 
72
  num_rows_ = num_rows;
 
73
  num_cols_ = num_cols;
 
74
  max_num_nonzeros_ = max_num_nonzeros;
 
75
 
 
76
  VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_
 
77
          << " max_num_nonzeros: " << max_num_nonzeros_
 
78
          << ". Allocating " << (num_rows_ + 1) * sizeof(int) +  // NOLINT
 
79
      max_num_nonzeros_ * sizeof(int) +  // NOLINT
 
80
      max_num_nonzeros_ * sizeof(double);  // NOLINT
 
81
 
 
82
  rows_.reset(new int[num_rows_ + 1]);
 
83
  cols_.reset(new int[max_num_nonzeros_]);
 
84
  values_.reset(new double[max_num_nonzeros_]);
 
85
 
 
86
  fill(rows_.get(), rows_.get() + num_rows_ + 1, 0);
 
87
  fill(cols_.get(), cols_.get() + max_num_nonzeros_, 0);
 
88
  fill(values_.get(), values_.get() + max_num_nonzeros_, 0);
 
89
}
 
90
 
 
91
CompressedRowSparseMatrix::CompressedRowSparseMatrix(
 
92
    const TripletSparseMatrix& m) {
 
93
  num_rows_ = m.num_rows();
 
94
  num_cols_ = m.num_cols();
 
95
  max_num_nonzeros_ = m.max_num_nonzeros();
 
96
 
 
97
  // index is the list of indices into the TripletSparseMatrix m.
 
98
  vector<int> index(m.num_nonzeros(), 0);
 
99
  for (int i = 0; i < m.num_nonzeros(); ++i) {
 
100
    index[i] = i;
 
101
  }
 
102
 
 
103
  // Sort index such that the entries of m are ordered by row and ties
 
104
  // are broken by column.
 
105
  sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols()));
 
106
 
 
107
  VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_
 
108
          << " max_num_nonzeros: " << max_num_nonzeros_
 
109
          << ". Allocating " << (num_rows_ + 1) * sizeof(int) +  // NOLINT
 
110
      max_num_nonzeros_ * sizeof(int) +  // NOLINT
 
111
      max_num_nonzeros_ * sizeof(double);  // NOLINT
 
112
 
 
113
  rows_.reset(new int[num_rows_ + 1]);
 
114
  cols_.reset(new int[max_num_nonzeros_]);
 
115
  values_.reset(new double[max_num_nonzeros_]);
 
116
 
 
117
  // rows_ = 0
 
118
  fill(rows_.get(), rows_.get() + num_rows_ + 1, 0);
 
119
 
 
120
  // Copy the contents of the cols and values array in the order given
 
121
  // by index and count the number of entries in each row.
 
122
  for (int i = 0; i < m.num_nonzeros(); ++i) {
 
123
    const int idx = index[i];
 
124
    ++rows_[m.rows()[idx] + 1];
 
125
    cols_[i] = m.cols()[idx];
 
126
    values_[i] = m.values()[idx];
 
127
  }
 
128
 
 
129
  // Find the cumulative sum of the row counts.
 
130
  for (int i = 1; i < num_rows_ + 1; ++i) {
 
131
    rows_[i] += rows_[i-1];
 
132
  }
 
133
 
 
134
  CHECK_EQ(num_nonzeros(), m.num_nonzeros());
 
135
}
 
136
 
 
137
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
 
138
CompressedRowSparseMatrix::CompressedRowSparseMatrix(
 
139
    const SparseMatrixProto& outer_proto) {
 
140
  CHECK(outer_proto.has_compressed_row_matrix());
 
141
 
 
142
  const CompressedRowSparseMatrixProto& proto =
 
143
      outer_proto.compressed_row_matrix();
 
144
 
 
145
  num_rows_ = proto.num_rows();
 
146
  num_cols_ = proto.num_cols();
 
147
 
 
148
  rows_.reset(new int[proto.rows_size()]);
 
149
  cols_.reset(new int[proto.cols_size()]);
 
150
  values_.reset(new double[proto.values_size()]);
 
151
 
 
152
  for (int i = 0; i < proto.rows_size(); ++i) {
 
153
    rows_[i] = proto.rows(i);
 
154
  }
 
155
 
 
156
  CHECK_EQ(proto.rows_size(), num_rows_ + 1);
 
157
  CHECK_EQ(proto.cols_size(), proto.values_size());
 
158
  CHECK_EQ(proto.cols_size(), rows_[num_rows_]);
 
159
 
 
160
  for (int i = 0; i < proto.cols_size(); ++i) {
 
161
    cols_[i] = proto.cols(i);
 
162
    values_[i] = proto.values(i);
 
163
  }
 
164
 
 
165
  max_num_nonzeros_ = proto.cols_size();
 
166
}
 
167
#endif
 
168
 
 
169
CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal,
 
170
                                                     int num_rows) {
 
171
  CHECK_NOTNULL(diagonal);
 
172
 
 
173
  num_rows_ = num_rows;
 
174
  num_cols_ = num_rows;
 
175
  max_num_nonzeros_ = num_rows;
 
176
 
 
177
  rows_.reset(new int[num_rows_ + 1]);
 
178
  cols_.reset(new int[num_rows_]);
 
179
  values_.reset(new double[num_rows_]);
 
180
 
 
181
  rows_[0] = 0;
 
182
  for (int i = 0; i < num_rows_; ++i) {
 
183
    cols_[i] = i;
 
184
    values_[i] = diagonal[i];
 
185
    rows_[i + 1] = i + 1;
 
186
  }
 
187
 
 
188
  CHECK_EQ(num_nonzeros(), num_rows);
 
189
}
 
190
 
 
191
CompressedRowSparseMatrix::~CompressedRowSparseMatrix() {
 
192
}
 
193
 
 
194
void CompressedRowSparseMatrix::SetZero() {
 
195
  fill(values_.get(), values_.get() + num_nonzeros(), 0.0);
 
196
}
 
197
 
 
198
void CompressedRowSparseMatrix::RightMultiply(const double* x,
 
199
                                              double* y) const {
 
200
  CHECK_NOTNULL(x);
 
201
  CHECK_NOTNULL(y);
 
202
 
 
203
  for (int r = 0; r < num_rows_; ++r) {
 
204
    for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {
 
205
      y[r] += values_[idx] * x[cols_[idx]];
 
206
    }
 
207
  }
 
208
}
 
209
 
 
210
void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const {
 
211
  CHECK_NOTNULL(x);
 
212
  CHECK_NOTNULL(y);
 
213
 
 
214
  for (int r = 0; r < num_rows_; ++r) {
 
215
    for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {
 
216
      y[cols_[idx]] += values_[idx] * x[r];
 
217
    }
 
218
  }
 
219
}
 
220
 
 
221
void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const {
 
222
  CHECK_NOTNULL(x);
 
223
 
 
224
  fill(x, x + num_cols_, 0.0);
 
225
  for (int idx = 0; idx < rows_[num_rows_]; ++idx) {
 
226
    x[cols_[idx]] += values_[idx] * values_[idx];
 
227
  }
 
228
}
 
229
 
 
230
void CompressedRowSparseMatrix::ScaleColumns(const double* scale) {
 
231
  CHECK_NOTNULL(scale);
 
232
 
 
233
  for (int idx = 0; idx < rows_[num_rows_]; ++idx) {
 
234
    values_[idx] *= scale[cols_[idx]];
 
235
  }
 
236
}
 
237
 
 
238
void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const {
 
239
  CHECK_NOTNULL(dense_matrix);
 
240
  dense_matrix->resize(num_rows_, num_cols_);
 
241
  dense_matrix->setZero();
 
242
 
 
243
  for (int r = 0; r < num_rows_; ++r) {
 
244
    for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {
 
245
      (*dense_matrix)(r, cols_[idx]) = values_[idx];
 
246
    }
 
247
  }
 
248
}
 
249
 
 
250
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
 
251
void CompressedRowSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const {
 
252
  CHECK_NOTNULL(outer_proto);
 
253
 
 
254
  outer_proto->Clear();
 
255
  CompressedRowSparseMatrixProto* proto
 
256
      = outer_proto->mutable_compressed_row_matrix();
 
257
 
 
258
  proto->set_num_rows(num_rows_);
 
259
  proto->set_num_cols(num_cols_);
 
260
 
 
261
  for (int r = 0; r < num_rows_ + 1; ++r) {
 
262
    proto->add_rows(rows_[r]);
 
263
  }
 
264
 
 
265
  for (int idx = 0; idx < rows_[num_rows_]; ++idx) {
 
266
    proto->add_cols(cols_[idx]);
 
267
    proto->add_values(values_[idx]);
 
268
  }
 
269
}
 
270
#endif
 
271
 
 
272
void CompressedRowSparseMatrix::DeleteRows(int delta_rows) {
 
273
  CHECK_GE(delta_rows, 0);
 
274
  CHECK_LE(delta_rows, num_rows_);
 
275
 
 
276
  int new_num_rows = num_rows_ - delta_rows;
 
277
 
 
278
  num_rows_ = new_num_rows;
 
279
  int* new_rows = new int[num_rows_ + 1];
 
280
  copy(rows_.get(), rows_.get() + num_rows_ + 1, new_rows);
 
281
  rows_.reset(new_rows);
 
282
}
 
283
 
 
284
void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) {
 
285
  CHECK_EQ(m.num_cols(), num_cols_);
 
286
 
 
287
  // Check if there is enough space. If not, then allocate new arrays
 
288
  // to hold the combined matrix and copy the contents of this matrix
 
289
  // into it.
 
290
  if (max_num_nonzeros_ < num_nonzeros() + m.num_nonzeros()) {
 
291
    int new_max_num_nonzeros =  num_nonzeros() + m.num_nonzeros();
 
292
 
 
293
    VLOG(1) << "Reallocating " << sizeof(int) * new_max_num_nonzeros;  // NOLINT
 
294
 
 
295
    int* new_cols = new int[new_max_num_nonzeros];
 
296
    copy(cols_.get(), cols_.get() + max_num_nonzeros_, new_cols);
 
297
    cols_.reset(new_cols);
 
298
 
 
299
    double* new_values = new double[new_max_num_nonzeros];
 
300
    copy(values_.get(), values_.get() + max_num_nonzeros_, new_values);
 
301
    values_.reset(new_values);
 
302
 
 
303
    max_num_nonzeros_ = new_max_num_nonzeros;
 
304
  }
 
305
 
 
306
  // Copy the contents of m into this matrix.
 
307
  copy(m.cols(), m.cols() + m.num_nonzeros(), cols_.get() + num_nonzeros());
 
308
  copy(m.values(),
 
309
       m.values() + m.num_nonzeros(),
 
310
       values_.get() + num_nonzeros());
 
311
 
 
312
  // Create the new rows array to hold the enlarged matrix.
 
313
  int* new_rows = new int[num_rows_ + m.num_rows() + 1];
 
314
  // The first num_rows_ entries are the same
 
315
  copy(rows_.get(), rows_.get() + num_rows_, new_rows);
 
316
 
 
317
  // new_rows = [rows_, m.row() + rows_[num_rows_]]
 
318
  fill(new_rows + num_rows_,
 
319
       new_rows + num_rows_ + m.num_rows() + 1,
 
320
       rows_[num_rows_]);
 
321
 
 
322
  for (int r = 0; r < m.num_rows() + 1; ++r) {
 
323
    new_rows[num_rows_ + r] += m.rows()[r];
 
324
  }
 
325
 
 
326
  rows_.reset(new_rows);
 
327
  num_rows_ += m.num_rows();
 
328
}
 
329
 
 
330
void CompressedRowSparseMatrix::ToTextFile(FILE* file) const {
 
331
  CHECK_NOTNULL(file);
 
332
  for (int r = 0; r < num_rows_; ++r) {
 
333
    for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {
 
334
      fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]);
 
335
    }
 
336
  }
 
337
}
 
338
 
 
339
}  // namespace internal
 
340
}  // namespace ceres