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/small_blas.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
84void BlockSparseMatrix::SetZero() {
85  fill(values_.get(), values_.get() + num_nonzeros_, 0.0);
86}
87
88void BlockSparseMatrix::RightMultiply(const double* x,  double* y) const {
89  CHECK_NOTNULL(x);
90  CHECK_NOTNULL(y);
91
92  for (int i = 0; i < block_structure_->rows.size(); ++i) {
93    int row_block_pos = block_structure_->rows[i].block.position;
94    int row_block_size = block_structure_->rows[i].block.size;
95    const vector<Cell>& cells = block_structure_->rows[i].cells;
96    for (int j = 0; j < cells.size(); ++j) {
97      int col_block_id = cells[j].block_id;
98      int col_block_size = block_structure_->cols[col_block_id].size;
99      int col_block_pos = block_structure_->cols[col_block_id].position;
100      MatrixVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
101          values_.get() + cells[j].position, row_block_size, col_block_size,
102          x + col_block_pos,
103          y + row_block_pos);
104    }
105  }
106}
107
108void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const {
109  CHECK_NOTNULL(x);
110  CHECK_NOTNULL(y);
111
112  for (int i = 0; i < block_structure_->rows.size(); ++i) {
113    int row_block_pos = block_structure_->rows[i].block.position;
114    int row_block_size = block_structure_->rows[i].block.size;
115    const vector<Cell>& cells = block_structure_->rows[i].cells;
116    for (int j = 0; j < cells.size(); ++j) {
117      int col_block_id = cells[j].block_id;
118      int col_block_size = block_structure_->cols[col_block_id].size;
119      int col_block_pos = block_structure_->cols[col_block_id].position;
120      MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>(
121          values_.get() + cells[j].position, row_block_size, col_block_size,
122          x + row_block_pos,
123          y + col_block_pos);
124    }
125  }
126}
127
128void BlockSparseMatrix::SquaredColumnNorm(double* x) const {
129  CHECK_NOTNULL(x);
130  VectorRef(x, num_cols_).setZero();
131  for (int i = 0; i < block_structure_->rows.size(); ++i) {
132    int row_block_size = block_structure_->rows[i].block.size;
133    const vector<Cell>& cells = block_structure_->rows[i].cells;
134    for (int j = 0; j < cells.size(); ++j) {
135      int col_block_id = cells[j].block_id;
136      int col_block_size = block_structure_->cols[col_block_id].size;
137      int col_block_pos = block_structure_->cols[col_block_id].position;
138      const MatrixRef m(values_.get() + cells[j].position,
139                        row_block_size, col_block_size);
140      VectorRef(x + col_block_pos, col_block_size) += m.colwise().squaredNorm();
141    }
142  }
143}
144
145void BlockSparseMatrix::ScaleColumns(const double* scale) {
146  CHECK_NOTNULL(scale);
147
148  for (int i = 0; i < block_structure_->rows.size(); ++i) {
149    int row_block_size = block_structure_->rows[i].block.size;
150    const vector<Cell>& cells = block_structure_->rows[i].cells;
151    for (int j = 0; j < cells.size(); ++j) {
152      int col_block_id = cells[j].block_id;
153      int col_block_size = block_structure_->cols[col_block_id].size;
154      int col_block_pos = block_structure_->cols[col_block_id].position;
155      MatrixRef m(values_.get() + cells[j].position,
156                        row_block_size, col_block_size);
157      m *= ConstVectorRef(scale + col_block_pos, col_block_size).asDiagonal();
158    }
159  }
160}
161
162void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const {
163  CHECK_NOTNULL(dense_matrix);
164
165  dense_matrix->resize(num_rows_, num_cols_);
166  dense_matrix->setZero();
167  Matrix& m = *dense_matrix;
168
169  for (int i = 0; i < block_structure_->rows.size(); ++i) {
170    int row_block_pos = block_structure_->rows[i].block.position;
171    int row_block_size = block_structure_->rows[i].block.size;
172    const vector<Cell>& cells = block_structure_->rows[i].cells;
173    for (int j = 0; j < cells.size(); ++j) {
174      int col_block_id = cells[j].block_id;
175      int col_block_size = block_structure_->cols[col_block_id].size;
176      int col_block_pos = block_structure_->cols[col_block_id].position;
177      int jac_pos = cells[j].position;
178      m.block(row_block_pos, col_block_pos, row_block_size, col_block_size)
179          += MatrixRef(values_.get() + jac_pos, row_block_size, col_block_size);
180    }
181  }
182}
183
184void BlockSparseMatrix::ToTripletSparseMatrix(
185    TripletSparseMatrix* matrix) const {
186  CHECK_NOTNULL(matrix);
187
188  matrix->Reserve(num_nonzeros_);
189  matrix->Resize(num_rows_, num_cols_);
190  matrix->SetZero();
191
192  for (int i = 0; i < block_structure_->rows.size(); ++i) {
193    int row_block_pos = block_structure_->rows[i].block.position;
194    int row_block_size = block_structure_->rows[i].block.size;
195    const vector<Cell>& cells = block_structure_->rows[i].cells;
196    for (int j = 0; j < cells.size(); ++j) {
197      int col_block_id = cells[j].block_id;
198      int col_block_size = block_structure_->cols[col_block_id].size;
199      int col_block_pos = block_structure_->cols[col_block_id].position;
200      int jac_pos = cells[j].position;
201       for (int r = 0; r < row_block_size; ++r) {
202        for (int c = 0; c < col_block_size; ++c, ++jac_pos) {
203          matrix->mutable_rows()[jac_pos] = row_block_pos + r;
204          matrix->mutable_cols()[jac_pos] = col_block_pos + c;
205          matrix->mutable_values()[jac_pos] = values_[jac_pos];
206        }
207      }
208    }
209  }
210  matrix->set_num_nonzeros(num_nonzeros_);
211}
212
213// Return a pointer to the block structure. We continue to hold
214// ownership of the object though.
215const CompressedRowBlockStructure* BlockSparseMatrix::block_structure()
216    const {
217  return block_structure_.get();
218}
219
220void BlockSparseMatrix::ToTextFile(FILE* file) const {
221  CHECK_NOTNULL(file);
222  for (int i = 0; i < block_structure_->rows.size(); ++i) {
223    const int row_block_pos = block_structure_->rows[i].block.position;
224    const int row_block_size = block_structure_->rows[i].block.size;
225    const vector<Cell>& cells = block_structure_->rows[i].cells;
226    for (int j = 0; j < cells.size(); ++j) {
227      const int col_block_id = cells[j].block_id;
228      const int col_block_size = block_structure_->cols[col_block_id].size;
229      const int col_block_pos = block_structure_->cols[col_block_id].position;
230      int jac_pos = cells[j].position;
231      for (int r = 0; r < row_block_size; ++r) {
232        for (int c = 0; c < col_block_size; ++c) {
233          fprintf(file, "% 10d % 10d %17f\n",
234                  row_block_pos + r,
235                  col_block_pos + c,
236                  values_[jac_pos++]);
237        }
238      }
239    }
240  }
241}
242
243}  // namespace internal
244}  // namespace ceres
245