← Back to branch summary

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

« back to all changes in this revision

Viewing changes to internal/ceres/block_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/block_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/block_sparse_matrix.h"
 
32
 
 
33
#include <cstddef>
 
34
#include <algorithm>
 
35
#include <vector>
 
36
#include <glog/logging.h>
 
37
#include "ceres/block_structure.h"
 
38
#include "ceres/matrix_proto.h"
 
39
#include "ceres/triplet_sparse_matrix.h"
 
40
#include "ceres/internal/eigen.h"
 
41
 
 
42
namespace ceres {
 
43
namespace internal {
 
44
 
 
45
BlockSparseMatrix::~BlockSparseMatrix() {}
 
46
 
 
47
BlockSparseMatrix::BlockSparseMatrix(
 
48
    CompressedRowBlockStructure* block_structure)
 
49
    : num_rows_(0),
 
50
      num_cols_(0),
 
51
      num_nonzeros_(0),
 
52
      values_(NULL),
 
53
      block_structure_(block_structure) {
 
54
  CHECK_NOTNULL(block_structure_.get());
 
55
 
 
56
  // Count the number of columns in the matrix.
 
57
  for (int i = 0; i < block_structure_->cols.size(); ++i) {
 
58
    num_cols_ += block_structure_->cols[i].size;
 
59
  }
 
60
 
 
61
  // Count the number of non-zero entries and the number of rows in
 
62
  // the matrix.
 
63
  for (int i = 0; i < block_structure_->rows.size(); ++i) {
 
64
    int row_block_size = block_structure_->rows[i].block.size;
 
65
    num_rows_ += row_block_size;
 
66
 
 
67
    const vector<Cell>& cells = block_structure_->rows[i].cells;
 
68
    for (int j = 0; j < cells.size(); ++j) {
 
69
      int col_block_id = cells[j].block_id;
 
70
      int col_block_size = block_structure_->cols[col_block_id].size;
 
71
      num_nonzeros_ += col_block_size * row_block_size;
 
72
    }
 
73
  }
 
74
 
 
75
  CHECK_GE(num_rows_, 0);
 
76
  CHECK_GE(num_cols_, 0);
 
77
  CHECK_GE(num_nonzeros_, 0);
 
78
  VLOG(2) << "Allocating values array with "
 
79
          << num_nonzeros_ * sizeof(double) << " bytes.";  // NOLINT
 
80
  values_.reset(new double[num_nonzeros_]);
 
81
  CHECK_NOTNULL(values_.get());
 
82
}
 
83
 
 
84
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
 
85
BlockSparseMatrix::BlockSparseMatrix(const SparseMatrixProto& outer_proto) {
 
86
  CHECK(outer_proto.has_block_matrix());
 
87
 
 
88
  const BlockSparseMatrixProto& proto = outer_proto.block_matrix();
 
89
  CHECK(proto.has_num_rows());
 
90
  CHECK(proto.has_num_cols());
 
91
  CHECK_EQ(proto.num_nonzeros(), proto.values_size());
 
92
 
 
93
  num_rows_ = proto.num_rows();
 
94
  num_cols_ = proto.num_cols();
 
95
  num_nonzeros_ = proto.num_nonzeros();
 
96
 
 
97
  // Copy out the values into *this.
 
98
  values_.reset(new double[num_nonzeros_]);
 
99
  for (int i = 0; i < proto.num_nonzeros(); ++i) {
 
100
    values_[i] = proto.values(i);
 
101
  }
 
102
 
 
103
  // Create the block structure according to the proto.
 
104
  block_structure_.reset(new CompressedRowBlockStructure);
 
105
  ProtoToBlockStructure(proto.block_structure(), block_structure_.get());
 
106
}
 
107
#endif
 
108
 
 
109
void BlockSparseMatrix::SetZero() {
 
110
  fill(values_.get(), values_.get() + num_nonzeros_, 0.0);
 
111
}
 
112
 
 
113
void BlockSparseMatrix::RightMultiply(const double* x,  double* y) const {
 
114
  CHECK_NOTNULL(x);
 
115
  CHECK_NOTNULL(y);
 
116
 
 
117
  for (int i = 0; i < block_structure_->rows.size(); ++i) {
 
118
    int row_block_pos = block_structure_->rows[i].block.position;
 
119
    int row_block_size = block_structure_->rows[i].block.size;
 
120
    VectorRef yref(y + row_block_pos, row_block_size);
 
121
    const vector<Cell>& cells = block_structure_->rows[i].cells;
 
122
    for (int j = 0; j < cells.size(); ++j) {
 
123
      int col_block_id = cells[j].block_id;
 
124
      int col_block_size = block_structure_->cols[col_block_id].size;
 
125
      int col_block_pos = block_structure_->cols[col_block_id].position;
 
126
      ConstVectorRef xref(x + col_block_pos, col_block_size);
 
127
      MatrixRef m(values_.get() + cells[j].position,
 
128
                  row_block_size, col_block_size);
 
129
      yref += m.lazyProduct(xref);
 
130
    }
 
131
  }
 
132
}
 
133
 
 
134
void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const {
 
135
  CHECK_NOTNULL(x);
 
136
  CHECK_NOTNULL(y);
 
137
 
 
138
  for (int i = 0; i < block_structure_->rows.size(); ++i) {
 
139
    int row_block_pos = block_structure_->rows[i].block.position;
 
140
    int row_block_size = block_structure_->rows[i].block.size;
 
141
    const ConstVectorRef xref(x + row_block_pos, row_block_size);
 
142
    const vector<Cell>& cells = block_structure_->rows[i].cells;
 
143
    for (int j = 0; j < cells.size(); ++j) {
 
144
      int col_block_id = cells[j].block_id;
 
145
      int col_block_size = block_structure_->cols[col_block_id].size;
 
146
      int col_block_pos = block_structure_->cols[col_block_id].position;
 
147
      VectorRef yref(y + col_block_pos, col_block_size);
 
148
      MatrixRef m(values_.get() + cells[j].position,
 
149
                  row_block_size, col_block_size);
 
150
      yref += m.transpose().lazyProduct(xref);
 
151
    }
 
152
  }
 
153
}
 
154
 
 
155
void BlockSparseMatrix::SquaredColumnNorm(double* x) const {
 
156
  CHECK_NOTNULL(x);
 
157
  VectorRef(x, num_cols_).setZero();
 
158
  for (int i = 0; i < block_structure_->rows.size(); ++i) {
 
159
    int row_block_size = block_structure_->rows[i].block.size;
 
160
    const vector<Cell>& cells = block_structure_->rows[i].cells;
 
161
    for (int j = 0; j < cells.size(); ++j) {
 
162
      int col_block_id = cells[j].block_id;
 
163
      int col_block_size = block_structure_->cols[col_block_id].size;
 
164
      int col_block_pos = block_structure_->cols[col_block_id].position;
 
165
      const MatrixRef m(values_.get() + cells[j].position,
 
166
                        row_block_size, col_block_size);
 
167
      VectorRef(x + col_block_pos, col_block_size) += m.colwise().squaredNorm();
 
168
    }
 
169
  }
 
170
}
 
171
 
 
172
void BlockSparseMatrix::ScaleColumns(const double* scale) {
 
173
  CHECK_NOTNULL(scale);
 
174
 
 
175
  for (int i = 0; i < block_structure_->rows.size(); ++i) {
 
176
    int row_block_size = block_structure_->rows[i].block.size;
 
177
    const vector<Cell>& cells = block_structure_->rows[i].cells;
 
178
    for (int j = 0; j < cells.size(); ++j) {
 
179
      int col_block_id = cells[j].block_id;
 
180
      int col_block_size = block_structure_->cols[col_block_id].size;
 
181
      int col_block_pos = block_structure_->cols[col_block_id].position;
 
182
      MatrixRef m(values_.get() + cells[j].position,
 
183
                        row_block_size, col_block_size);
 
184
      m *= ConstVectorRef(scale + col_block_pos, col_block_size).asDiagonal();
 
185
    }
 
186
  }
 
187
}
 
188
 
 
189
void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const {
 
190
  CHECK_NOTNULL(dense_matrix);
 
191
 
 
192
  dense_matrix->resize(num_rows_, num_cols_);
 
193
  dense_matrix->setZero();
 
194
  Matrix& m = *dense_matrix;
 
195
 
 
196
  for (int i = 0; i < block_structure_->rows.size(); ++i) {
 
197
    int row_block_pos = block_structure_->rows[i].block.position;
 
198
    int row_block_size = block_structure_->rows[i].block.size;
 
199
    const vector<Cell>& cells = block_structure_->rows[i].cells;
 
200
    for (int j = 0; j < cells.size(); ++j) {
 
201
      int col_block_id = cells[j].block_id;
 
202
      int col_block_size = block_structure_->cols[col_block_id].size;
 
203
      int col_block_pos = block_structure_->cols[col_block_id].position;
 
204
      int jac_pos = cells[j].position;
 
205
      m.block(row_block_pos, col_block_pos, row_block_size, col_block_size)
 
206
          += MatrixRef(values_.get() + jac_pos, row_block_size, col_block_size);
 
207
    }
 
208
  }
 
209
}
 
210
 
 
211
void BlockSparseMatrix::ToTripletSparseMatrix(
 
212
    TripletSparseMatrix* matrix) const {
 
213
  CHECK_NOTNULL(matrix);
 
214
 
 
215
  matrix->Reserve(num_nonzeros_);
 
216
  matrix->Resize(num_rows_, num_cols_);
 
217
  matrix->SetZero();
 
218
 
 
219
  for (int i = 0; i < block_structure_->rows.size(); ++i) {
 
220
    int row_block_pos = block_structure_->rows[i].block.position;
 
221
    int row_block_size = block_structure_->rows[i].block.size;
 
222
    const vector<Cell>& cells = block_structure_->rows[i].cells;
 
223
    for (int j = 0; j < cells.size(); ++j) {
 
224
      int col_block_id = cells[j].block_id;
 
225
      int col_block_size = block_structure_->cols[col_block_id].size;
 
226
      int col_block_pos = block_structure_->cols[col_block_id].position;
 
227
      int jac_pos = cells[j].position;
 
228
       for (int r = 0; r < row_block_size; ++r) {
 
229
        for (int c = 0; c < col_block_size; ++c, ++jac_pos) {
 
230
          matrix->mutable_rows()[jac_pos] = row_block_pos + r;
 
231
          matrix->mutable_cols()[jac_pos] = col_block_pos + c;
 
232
          matrix->mutable_values()[jac_pos] = values_[jac_pos];
 
233
        }
 
234
      }
 
235
    }
 
236
  }
 
237
  matrix->set_num_nonzeros(num_nonzeros_);
 
238
}
 
239
 
 
240
// Return a pointer to the block structure. We continue to hold
 
241
// ownership of the object though.
 
242
const CompressedRowBlockStructure* BlockSparseMatrix::block_structure()
 
243
    const {
 
244
  return block_structure_.get();
 
245
}
 
246
 
 
247
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
 
248
void BlockSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const {
 
249
  outer_proto->Clear();
 
250
 
 
251
  BlockSparseMatrixProto* proto = outer_proto->mutable_block_matrix();
 
252
  proto->set_num_rows(num_rows_);
 
253
  proto->set_num_cols(num_cols_);
 
254
  proto->set_num_nonzeros(num_nonzeros_);
 
255
  for (int i = 0; i < num_nonzeros_; ++i) {
 
256
    proto->add_values(values_[i]);
 
257
  }
 
258
  BlockStructureToProto(*block_structure_, proto->mutable_block_structure());
 
259
}
 
260
#endif
 
261
 
 
262
void BlockSparseMatrix::ToTextFile(FILE* file) const {
 
263
  CHECK_NOTNULL(file);
 
264
  for (int i = 0; i < block_structure_->rows.size(); ++i) {
 
265
    const int row_block_pos = block_structure_->rows[i].block.position;
 
266
    const int row_block_size = block_structure_->rows[i].block.size;
 
267
    const vector<Cell>& cells = block_structure_->rows[i].cells;
 
268
    for (int j = 0; j < cells.size(); ++j) {
 
269
      const int col_block_id = cells[j].block_id;
 
270
      const int col_block_size = block_structure_->cols[col_block_id].size;
 
271
      const int col_block_pos = block_structure_->cols[col_block_id].position;
 
272
      int jac_pos = cells[j].position;
 
273
      for (int r = 0; r < row_block_size; ++r) {
 
274
        for (int c = 0; c < col_block_size; ++c) {
 
275
          fprintf(file, "% 10d % 10d %17f\n",
 
276
                  row_block_pos + r,
 
277
                  col_block_pos + c,
 
278
                  values_[jac_pos++]);
 
279
        }
 
280
      }
 
281
    }
 
282
  }
 
283
}
 
284
 
 
285
}  // namespace internal
 
286
}  // namespace ceres