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 "ceres/block_structure.h"
37#include "ceres/internal/eigen.h"
38#include "ceres/matrix_proto.h"
39#include "ceres/triplet_sparse_matrix.h"
40#include "glog/logging.h"
41
42namespace ceres {
43namespace internal {
44
45BlockSparseMatrix::~BlockSparseMatrix() {}
46
47BlockSparseMatrix::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_NO_PROTOCOL_BUFFERS
85BlockSparseMatrix::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
109void BlockSparseMatrix::SetZero() {
110  fill(values_.get(), values_.get() + num_nonzeros_, 0.0);
111}
112
113void 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
134void 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
155void 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
172void 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
189void 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
211void 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.
242const CompressedRowBlockStructure* BlockSparseMatrix::block_structure()
243    const {
244  return block_structure_.get();
245}
246
247#ifndef CERES_NO_PROTOCOL_BUFFERS
248void 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
262void 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
287