← Back to branch summary

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

« back to all changes in this revision

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