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