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/compressed_row_sparse_matrix.h" 32 33 #include <algorithm> 34 #include <vector> 35 #include "ceres/crs_matrix.h" 36 #include "ceres/internal/port.h" 37 #include "ceres/matrix_proto.h" 38 39 namespace ceres { 40 namespace internal { 41 namespace { 42 43 // Helper functor used by the constructor for reordering the contents 44 // of a TripletSparseMatrix. This comparator assumes thay there are no 45 // duplicates in the pair of arrays rows and cols, i.e., there is no 46 // indices i and j (not equal to each other) s.t. 47 // 48 // rows[i] == rows[j] && cols[i] == cols[j] 49 // 50 // If this is the case, this functor will not be a StrictWeakOrdering. 51 struct RowColLessThan { 52 RowColLessThan(const int* rows, const int* cols) 53 : rows(rows), cols(cols) { 54 } 55 56 bool operator()(const int x, const int y) const { 57 if (rows[x] == rows[y]) { 58 return (cols[x] < cols[y]); 59 } 60 return (rows[x] < rows[y]); 61 } 62 63 const int* rows; 64 const int* cols; 65 }; 66 67 } // namespace 68 69 // This constructor gives you a semi-initialized CompressedRowSparseMatrix. 70 CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, 71 int num_cols, 72 int max_num_nonzeros) { 73 num_rows_ = num_rows; 74 num_cols_ = num_cols; 75 max_num_nonzeros_ = max_num_nonzeros; 76 77 VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ 78 << " max_num_nonzeros: " << max_num_nonzeros_ 79 << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT 80 max_num_nonzeros_ * sizeof(int) + // NOLINT 81 max_num_nonzeros_ * sizeof(double); // NOLINT 82 83 rows_.reset(new int[num_rows_ + 1]); 84 cols_.reset(new int[max_num_nonzeros_]); 85 values_.reset(new double[max_num_nonzeros_]); 86 87 fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); 88 fill(cols_.get(), cols_.get() + max_num_nonzeros_, 0); 89 fill(values_.get(), values_.get() + max_num_nonzeros_, 0); 90 } 91 92 CompressedRowSparseMatrix::CompressedRowSparseMatrix( 93 const TripletSparseMatrix& m) { 94 num_rows_ = m.num_rows(); 95 num_cols_ = m.num_cols(); 96 max_num_nonzeros_ = m.max_num_nonzeros(); 97 98 // index is the list of indices into the TripletSparseMatrix m. 99 vector<int> index(m.num_nonzeros(), 0); 100 for (int i = 0; i < m.num_nonzeros(); ++i) { 101 index[i] = i; 102 } 103 104 // Sort index such that the entries of m are ordered by row and ties 105 // are broken by column. 106 sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols())); 107 108 VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ 109 << " max_num_nonzeros: " << max_num_nonzeros_ 110 << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT 111 max_num_nonzeros_ * sizeof(int) + // NOLINT 112 max_num_nonzeros_ * sizeof(double); // NOLINT 113 114 rows_.reset(new int[num_rows_ + 1]); 115 cols_.reset(new int[max_num_nonzeros_]); 116 values_.reset(new double[max_num_nonzeros_]); 117 118 // rows_ = 0 119 fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); 120 121 // Copy the contents of the cols and values array in the order given 122 // by index and count the number of entries in each row. 123 for (int i = 0; i < m.num_nonzeros(); ++i) { 124 const int idx = index[i]; 125 ++rows_[m.rows()[idx] + 1]; 126 cols_[i] = m.cols()[idx]; 127 values_[i] = m.values()[idx]; 128 } 129 130 // Find the cumulative sum of the row counts. 131 for (int i = 1; i < num_rows_ + 1; ++i) { 132 rows_[i] += rows_[i-1]; 133 } 134 135 CHECK_EQ(num_nonzeros(), m.num_nonzeros()); 136 } 137 138 #ifndef CERES_NO_PROTOCOL_BUFFERS 139 CompressedRowSparseMatrix::CompressedRowSparseMatrix( 140 const SparseMatrixProto& outer_proto) { 141 CHECK(outer_proto.has_compressed_row_matrix()); 142 143 const CompressedRowSparseMatrixProto& proto = 144 outer_proto.compressed_row_matrix(); 145 146 num_rows_ = proto.num_rows(); 147 num_cols_ = proto.num_cols(); 148 149 rows_.reset(new int[proto.rows_size()]); 150 cols_.reset(new int[proto.cols_size()]); 151 values_.reset(new double[proto.values_size()]); 152 153 for (int i = 0; i < proto.rows_size(); ++i) { 154 rows_[i] = proto.rows(i); 155 } 156 157 CHECK_EQ(proto.rows_size(), num_rows_ + 1); 158 CHECK_EQ(proto.cols_size(), proto.values_size()); 159 CHECK_EQ(proto.cols_size(), rows_[num_rows_]); 160 161 for (int i = 0; i < proto.cols_size(); ++i) { 162 cols_[i] = proto.cols(i); 163 values_[i] = proto.values(i); 164 } 165 166 max_num_nonzeros_ = proto.cols_size(); 167 } 168 #endif 169 170 CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, 171 int num_rows) { 172 CHECK_NOTNULL(diagonal); 173 174 num_rows_ = num_rows; 175 num_cols_ = num_rows; 176 max_num_nonzeros_ = num_rows; 177 178 rows_.reset(new int[num_rows_ + 1]); 179 cols_.reset(new int[num_rows_]); 180 values_.reset(new double[num_rows_]); 181 182 rows_[0] = 0; 183 for (int i = 0; i < num_rows_; ++i) { 184 cols_[i] = i; 185 values_[i] = diagonal[i]; 186 rows_[i + 1] = i + 1; 187 } 188 189 CHECK_EQ(num_nonzeros(), num_rows); 190 } 191 192 CompressedRowSparseMatrix::~CompressedRowSparseMatrix() { 193 } 194 195 void CompressedRowSparseMatrix::SetZero() { 196 fill(values_.get(), values_.get() + num_nonzeros(), 0.0); 197 } 198 199 void CompressedRowSparseMatrix::RightMultiply(const double* x, 200 double* y) const { 201 CHECK_NOTNULL(x); 202 CHECK_NOTNULL(y); 203 204 for (int r = 0; r < num_rows_; ++r) { 205 for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { 206 y[r] += values_[idx] * x[cols_[idx]]; 207 } 208 } 209 } 210 211 void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const { 212 CHECK_NOTNULL(x); 213 CHECK_NOTNULL(y); 214 215 for (int r = 0; r < num_rows_; ++r) { 216 for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { 217 y[cols_[idx]] += values_[idx] * x[r]; 218 } 219 } 220 } 221 222 void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const { 223 CHECK_NOTNULL(x); 224 225 fill(x, x + num_cols_, 0.0); 226 for (int idx = 0; idx < rows_[num_rows_]; ++idx) { 227 x[cols_[idx]] += values_[idx] * values_[idx]; 228 } 229 } 230 231 void CompressedRowSparseMatrix::ScaleColumns(const double* scale) { 232 CHECK_NOTNULL(scale); 233 234 for (int idx = 0; idx < rows_[num_rows_]; ++idx) { 235 values_[idx] *= scale[cols_[idx]]; 236 } 237 } 238 239 void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { 240 CHECK_NOTNULL(dense_matrix); 241 dense_matrix->resize(num_rows_, num_cols_); 242 dense_matrix->setZero(); 243 244 for (int r = 0; r < num_rows_; ++r) { 245 for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { 246 (*dense_matrix)(r, cols_[idx]) = values_[idx]; 247 } 248 } 249 } 250 251 #ifndef CERES_NO_PROTOCOL_BUFFERS 252 void CompressedRowSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { 253 CHECK_NOTNULL(outer_proto); 254 255 outer_proto->Clear(); 256 CompressedRowSparseMatrixProto* proto 257 = outer_proto->mutable_compressed_row_matrix(); 258 259 proto->set_num_rows(num_rows_); 260 proto->set_num_cols(num_cols_); 261 262 for (int r = 0; r < num_rows_ + 1; ++r) { 263 proto->add_rows(rows_[r]); 264 } 265 266 for (int idx = 0; idx < rows_[num_rows_]; ++idx) { 267 proto->add_cols(cols_[idx]); 268 proto->add_values(values_[idx]); 269 } 270 } 271 #endif 272 273 void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { 274 CHECK_GE(delta_rows, 0); 275 CHECK_LE(delta_rows, num_rows_); 276 277 int new_num_rows = num_rows_ - delta_rows; 278 279 num_rows_ = new_num_rows; 280 int* new_rows = new int[num_rows_ + 1]; 281 copy(rows_.get(), rows_.get() + num_rows_ + 1, new_rows); 282 rows_.reset(new_rows); 283 } 284 285 void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { 286 CHECK_EQ(m.num_cols(), num_cols_); 287 288 // Check if there is enough space. If not, then allocate new arrays 289 // to hold the combined matrix and copy the contents of this matrix 290 // into it. 291 if (max_num_nonzeros_ < num_nonzeros() + m.num_nonzeros()) { 292 int new_max_num_nonzeros = num_nonzeros() + m.num_nonzeros(); 293 294 VLOG(1) << "Reallocating " << sizeof(int) * new_max_num_nonzeros; // NOLINT 295 296 int* new_cols = new int[new_max_num_nonzeros]; 297 copy(cols_.get(), cols_.get() + max_num_nonzeros_, new_cols); 298 cols_.reset(new_cols); 299 300 double* new_values = new double[new_max_num_nonzeros]; 301 copy(values_.get(), values_.get() + max_num_nonzeros_, new_values); 302 values_.reset(new_values); 303 304 max_num_nonzeros_ = new_max_num_nonzeros; 305 } 306 307 // Copy the contents of m into this matrix. 308 copy(m.cols(), m.cols() + m.num_nonzeros(), cols_.get() + num_nonzeros()); 309 copy(m.values(), 310 m.values() + m.num_nonzeros(), 311 values_.get() + num_nonzeros()); 312 313 // Create the new rows array to hold the enlarged matrix. 314 int* new_rows = new int[num_rows_ + m.num_rows() + 1]; 315 // The first num_rows_ entries are the same 316 copy(rows_.get(), rows_.get() + num_rows_, new_rows); 317 318 // new_rows = [rows_, m.row() + rows_[num_rows_]] 319 fill(new_rows + num_rows_, 320 new_rows + num_rows_ + m.num_rows() + 1, 321 rows_[num_rows_]); 322 323 for (int r = 0; r < m.num_rows() + 1; ++r) { 324 new_rows[num_rows_ + r] += m.rows()[r]; 325 } 326 327 rows_.reset(new_rows); 328 num_rows_ += m.num_rows(); 329 } 330 331 void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { 332 CHECK_NOTNULL(file); 333 for (int r = 0; r < num_rows_; ++r) { 334 for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { 335 fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]); 336 } 337 } 338 } 339 340 void CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const { 341 matrix->num_rows = num_rows(); 342 matrix->num_cols = num_cols(); 343 344 matrix->rows.resize(matrix->num_rows + 1); 345 matrix->cols.resize(num_nonzeros()); 346 matrix->values.resize(num_nonzeros()); 347 348 copy(rows_.get(), rows_.get() + matrix->num_rows + 1, matrix->rows.begin()); 349 copy(cols_.get(), cols_.get() + num_nonzeros(), matrix->cols.begin()); 350 copy(values_.get(), values_.get() + num_nonzeros(), matrix->values.begin()); 351 } 352 353 } // namespace internal 354 } // namespace ceres 355