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 (at) 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 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 void BlockSparseMatrix::SetZero() { 85 fill(values_.get(), values_.get() + num_nonzeros_, 0.0); 86 } 87 88 void 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 108 void 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 128 void 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 145 void 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 162 void 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 184 void 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. 215 const CompressedRowBlockStructure* BlockSparseMatrix::block_structure() 216 const { 217 return block_structure_.get(); 218 } 219 220 void 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