10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/compressed_row_sparse_matrix.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <algorithm> 3479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include <numeric> 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/crs_matrix.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/port.h" 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/triplet_sparse_matrix.h" 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace { 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Helper functor used by the constructor for reordering the contents 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// of a TripletSparseMatrix. This comparator assumes thay there are no 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// duplicates in the pair of arrays rows and cols, i.e., there is no 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// indices i and j (not equal to each other) s.t. 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// rows[i] == rows[j] && cols[i] == cols[j] 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// If this is the case, this functor will not be a StrictWeakOrdering. 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstruct RowColLessThan { 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong RowColLessThan(const int* rows, const int* cols) 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : rows(rows), cols(cols) { 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong bool operator()(const int x, const int y) const { 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (rows[x] == rows[y]) { 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return (cols[x] < cols[y]); 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return (rows[x] < rows[y]); 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int* rows; 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int* cols; 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// This constructor gives you a semi-initialized CompressedRowSparseMatrix. 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols, 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int max_num_nonzeros) { 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = num_rows; 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = num_cols; 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rows_.resize(num_rows + 1, 0); 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols_.resize(max_num_nonzeros, 0); 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values_.resize(max_num_nonzeros, 0.0); 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling VLOG(1) << "# of rows: " << num_rows_ 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " # of columns: " << num_cols_ 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " max_num_nonzeros: " << cols_.size() 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols_.size() * sizeof(int) + // NOLINT 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols_.size() * sizeof(double); // NOLINT 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCompressedRowSparseMatrix::CompressedRowSparseMatrix( 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const TripletSparseMatrix& m) { 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = m.num_rows(); 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = m.num_cols(); 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rows_.resize(num_rows_ + 1, 0); 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols_.resize(m.num_nonzeros(), 0); 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values_.resize(m.max_num_nonzeros(), 0.0); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // index is the list of indices into the TripletSparseMatrix m. 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> index(m.num_nonzeros(), 0); 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < m.num_nonzeros(); ++i) { 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong index[i] = i; 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Sort index such that the entries of m are ordered by row and ties 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // are broken by column. 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols())); 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling VLOG(1) << "# of rows: " << num_rows_ 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " # of columns: " << num_cols_ 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " max_num_nonzeros: " << cols_.size() 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << ". Allocating " 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << ((num_rows_ + 1) * sizeof(int) + // NOLINT 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols_.size() * sizeof(int) + // NOLINT 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols_.size() * sizeof(double)); // NOLINT 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Copy the contents of the cols and values array in the order given 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // by index and count the number of entries in each row. 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < m.num_nonzeros(); ++i) { 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int idx = index[i]; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++rows_[m.rows()[idx] + 1]; 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_[i] = m.cols()[idx]; 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_[i] = m.values()[idx]; 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Find the cumulative sum of the row counts. 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 1; i < num_rows_ + 1; ++i) { 12879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez rows_[i] += rows_[i - 1]; 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(num_nonzeros(), m.num_nonzeros()); 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows) { 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(diagonal); 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ = num_rows; 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_cols_ = num_rows; 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rows_.resize(num_rows + 1); 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols_.resize(num_rows); 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values_.resize(num_rows); 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_[0] = 0; 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < num_rows_; ++i) { 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cols_[i] = i; 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_[i] = diagonal[i]; 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_[i + 1] = i + 1; 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(num_nonzeros(), num_rows); 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCompressedRowSparseMatrix::~CompressedRowSparseMatrix() { 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::SetZero() { 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling fill(values_.begin(), values_.end(), 0); 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::RightMultiply(const double* x, 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* y) const { 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(x); 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(y); 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < num_rows_; ++r) { 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong y[r] += values_[idx] * x[cols_[idx]]; 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const { 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(x); 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(y); 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < num_rows_; ++r) { 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong y[cols_[idx]] += values_[idx] * x[r]; 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const { 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(x); 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fill(x, x + num_cols_, 0.0); 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int idx = 0; idx < rows_[num_rows_]; ++idx) { 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x[cols_[idx]] += values_[idx] * values_[idx]; 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::ScaleColumns(const double* scale) { 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(scale); 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int idx = 0; idx < rows_[num_rows_]; ++idx) { 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong values_[idx] *= scale[cols_[idx]]; 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(dense_matrix); 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dense_matrix->resize(num_rows_, num_cols_); 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong dense_matrix->setZero(); 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < num_rows_; ++r) { 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong (*dense_matrix)(r, cols_[idx]) = values_[idx]; 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::DeleteRows(int delta_rows) { 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(delta_rows, 0); 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_LE(delta_rows, num_rows_); 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_rows_ -= delta_rows; 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rows_.resize(num_rows_ + 1); 21979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 22079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Walk the list of row blocks until we reach the new number of rows 22179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // and the drop the rest of the row blocks. 22279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_row_blocks = 0; 22379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_rows = 0; 22479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez while (num_row_blocks < row_blocks_.size() && num_rows < num_rows_) { 22579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez num_rows += row_blocks_[num_row_blocks]; 22679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ++num_row_blocks; 22779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 22879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 22979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez row_blocks_.resize(num_row_blocks); 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(m.num_cols(), num_cols_); 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 23579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK(row_blocks_.size() == 0 || m.row_blocks().size() !=0) 23679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Cannot append a matrix with row blocks to one without and vice versa." 23779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "This matrix has : " << row_blocks_.size() << " row blocks." 23879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "The matrix being appended has: " << m.row_blocks().size() 23979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << " row blocks."; 24079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 2411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (cols_.size() < num_nonzeros() + m.num_nonzeros()) { 2421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols_.resize(num_nonzeros() + m.num_nonzeros()); 2431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values_.resize(num_nonzeros() + m.num_nonzeros()); 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Copy the contents of m into this matrix. 2471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling copy(m.cols(), m.cols() + m.num_nonzeros(), &cols_[num_nonzeros()]); 2481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling copy(m.values(), m.values() + m.num_nonzeros(), &values_[num_nonzeros()]); 2491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rows_.resize(num_rows_ + m.num_rows() + 1); 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // new_rows = [rows_, m.row() + rows_[num_rows_]] 2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling fill(rows_.begin() + num_rows_, 2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rows_.begin() + num_rows_ + m.num_rows() + 1, 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rows_[num_rows_]); 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < m.num_rows() + 1; ++r) { 2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rows_[num_rows_ + r] += m.rows()[r]; 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong num_rows_ += m.num_rows(); 26079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez row_blocks_.insert(row_blocks_.end(), m.row_blocks().begin(), m.row_blocks().end()); 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::ToTextFile(FILE* file) const { 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(file); 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < num_rows_; ++r) { 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { 2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling fprintf(file, 2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling "% 10d % 10d %17f\n", 2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling r, 2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cols_[idx], 2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values_[idx]); 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CompressedRowSparseMatrix::ToCRSMatrix(CRSMatrix* matrix) const { 2771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling matrix->num_rows = num_rows_; 2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling matrix->num_cols = num_cols_; 2791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling matrix->rows = rows_; 2801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling matrix->cols = cols_; 2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling matrix->values = values_; 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Trim. 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong matrix->rows.resize(matrix->num_rows + 1); 2851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling matrix->cols.resize(matrix->rows[matrix->num_rows]); 2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling matrix->values.resize(matrix->rows[matrix->num_rows]); 2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 28979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid CompressedRowSparseMatrix::SetMaxNumNonZeros(int num_nonzeros) { 29079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_GE(num_nonzeros, 0); 29179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 29279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cols_.resize(num_nonzeros); 29379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez values_.resize(num_nonzeros); 29479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 29579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 2961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid CompressedRowSparseMatrix::SolveLowerTriangularInPlace( 2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* solution) const { 2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int r = 0; r < num_rows_; ++r) { 2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int idx = rows_[r]; idx < rows_[r + 1] - 1; ++idx) { 3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling solution[r] -= values_[idx] * solution[cols_[idx]]; 3011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling solution[r] /= values_[rows_[r + 1] - 1]; 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid CompressedRowSparseMatrix::SolveLowerTriangularTransposeInPlace( 3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* solution) const { 3081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int r = num_rows_ - 1; r >= 0; --r) { 3091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling solution[r] /= values_[rows_[r + 1] - 1]; 3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int idx = rows_[r + 1] - 2; idx >= rows_[r]; --idx) { 3111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling solution[cols_[idx]] -= values_[idx] * solution[r]; 3121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingCompressedRowSparseMatrix* CompressedRowSparseMatrix::CreateBlockDiagonalMatrix( 3171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* diagonal, 3181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<int>& blocks) { 3191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_rows = 0; 3201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_nonzeros = 0; 3211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < blocks.size(); ++i) { 3221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_rows += blocks[i]; 3231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_nonzeros += blocks[i] * blocks[i]; 3241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CompressedRowSparseMatrix* matrix = 3271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new CompressedRowSparseMatrix(num_rows, num_rows, num_nonzeros); 3281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* rows = matrix->mutable_rows(); 3301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* cols = matrix->mutable_cols(); 3311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* values = matrix->mutable_values(); 3321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling fill(values, values + num_nonzeros, 0.0); 3331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int idx_cursor = 0; 3351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int col_cursor = 0; 3361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < blocks.size(); ++i) { 3371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int block_size = blocks[i]; 3381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int r = 0; r < block_size; ++r) { 3391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *(rows++) = idx_cursor; 3401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling values[idx_cursor + r] = diagonal[col_cursor + r]; 3411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int c = 0; c < block_size; ++c, ++idx_cursor) { 3421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *(cols++) = col_cursor + c; 3431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling col_cursor += block_size; 3461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *rows = idx_cursor; 3481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *matrix->mutable_row_blocks() = blocks; 3501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *matrix->mutable_col_blocks() = blocks; 3511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(idx_cursor, num_nonzeros); 3531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(col_cursor, num_rows); 3541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return matrix; 3551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingCompressedRowSparseMatrix* CompressedRowSparseMatrix::Transpose() const { 3581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CompressedRowSparseMatrix* transpose = 3591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new CompressedRowSparseMatrix(num_cols_, num_rows_, num_nonzeros()); 3601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* transpose_rows = transpose->mutable_rows(); 3621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* transpose_cols = transpose->mutable_cols(); 3631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* transpose_values = transpose->mutable_values(); 3641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int idx = 0; idx < num_nonzeros(); ++idx) { 3661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++transpose_rows[cols_[idx] + 1]; 3671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 1; i < transpose->num_rows() + 1; ++i) { 3701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling transpose_rows[i] += transpose_rows[i - 1]; 3711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int r = 0; r < num_rows(); ++r) { 3741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { 3751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int c = cols_[idx]; 3761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int transpose_idx = transpose_rows[c]++; 3771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling transpose_cols[transpose_idx] = r; 3781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling transpose_values[transpose_idx] = values_[idx]; 3791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = transpose->num_rows() - 1; i > 0 ; --i) { 3831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling transpose_rows[i] = transpose_rows[i - 1]; 3841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling transpose_rows[0] = 0; 3861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 38779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *(transpose->mutable_row_blocks()) = col_blocks_; 38879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *(transpose->mutable_col_blocks()) = row_blocks_; 38979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return transpose; 3911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 39379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandeznamespace { 39479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// A ProductTerm is a term in the outer product of a matrix with 39579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// itself. 39679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezstruct ProductTerm { 39779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ProductTerm(const int row, const int col, const int index) 39879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : row(row), col(col), index(index) { 39979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 40079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 40179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez bool operator<(const ProductTerm& right) const { 40279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (row == right.row) { 40379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (col == right.col) { 40479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return index < right.index; 40579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 40679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return col < right.col; 40779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 40879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return row < right.row; 40979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 41079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 41179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int row; 41279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int col; 41379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int index; 41479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 41579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 41679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezCompressedRowSparseMatrix* 41779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezCompressAndFillProgram(const int num_rows, 41879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_cols, 41979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<ProductTerm>& product, 42079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<int>* program) { 42179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_GT(product.size(), 0); 42279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 42379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Count the number of unique product term, which in turn is the 42479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // number of non-zeros in the outer product. 42579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_nonzeros = 1; 42679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 1; i < product.size(); ++i) { 42779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (product[i].row != product[i - 1].row || 42879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez product[i].col != product[i - 1].col) { 42979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ++num_nonzeros; 43079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 43179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 43279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 43379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CompressedRowSparseMatrix* matrix = 43479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez new CompressedRowSparseMatrix(num_rows, num_cols, num_nonzeros); 43579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 43679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int* crsm_rows = matrix->mutable_rows(); 43779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::fill(crsm_rows, crsm_rows + num_rows + 1, 0); 43879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int* crsm_cols = matrix->mutable_cols(); 43979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::fill(crsm_cols, crsm_cols + num_nonzeros, 0); 44079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 44179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(program)->clear(); 44279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez program->resize(product.size()); 44379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 44479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Iterate over the sorted product terms. This means each row is 44579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // filled one at a time, and we are able to assign a position in the 44679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // values array to each term. 44779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // 44879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // If terms repeat, i.e., they contribute to the same entry in the 44979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // result matrix), then they do not affect the sparsity structure of 45079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // the result matrix. 45179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int nnz = 0; 45279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm_cols[0] = product[0].col; 45379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm_rows[product[0].row + 1]++; 45479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*program)[product[0].index] = nnz; 45579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 1; i < product.size(); ++i) { 45679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ProductTerm& previous = product[i - 1]; 45779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const ProductTerm& current = product[i]; 45879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 45979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Sparsity structure is updated only if the term is not a repeat. 46079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (previous.row != current.row || previous.col != current.col) { 46179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm_cols[++nnz] = current.col; 46279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm_rows[current.row + 1]++; 46379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 46479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 46579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // All terms get assigned the position in the values array where 46679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // their value is accumulated. 46779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*program)[current.index] = nnz; 46879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 46979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 47079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 1; i < num_rows + 1; ++i) { 47179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm_rows[i] += crsm_rows[i - 1]; 47279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 47379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 47479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return matrix; 47579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 47679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 47779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} // namespace 47879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 47979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezCompressedRowSparseMatrix* 48079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezCompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( 48179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const CompressedRowSparseMatrix& m, 48279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<int>* program) { 48379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_NOTNULL(program)->clear(); 48479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_GT(m.num_nonzeros(), 0) << "Congratulations, " 48579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "you found a bug in Ceres. Please report it."; 48679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 48779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<ProductTerm> product; 48879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<int>& row_blocks = m.row_blocks(); 48979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int row_block_begin = 0; 49079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Iterate over row blocks 49179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int row_block = 0; row_block < row_blocks.size(); ++row_block) { 49279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int row_block_end = row_block_begin + row_blocks[row_block]; 49379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Compute the outer product terms for just one row per row block. 49479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int r = row_block_begin; 49579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Compute the lower triangular part of the product. 49679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int idx1 = m.rows()[r]; idx1 < m.rows()[r + 1]; ++idx1) { 49779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int idx2 = m.rows()[r]; idx2 <= idx1; ++idx2) { 49879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez product.push_back(ProductTerm(m.cols()[idx1], m.cols()[idx2], product.size())); 49979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 50079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 50179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez row_block_begin = row_block_end; 50279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 50379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_EQ(row_block_begin, m.num_rows()); 50479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez sort(product.begin(), product.end()); 50579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return CompressAndFillProgram(m.num_cols(), m.num_cols(), product, program); 50679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 50779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 50879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid CompressedRowSparseMatrix::ComputeOuterProduct( 50979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const CompressedRowSparseMatrix& m, 51079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<int>& program, 51179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CompressedRowSparseMatrix* result) { 51279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez result->SetZero(); 51379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* values = result->mutable_values(); 51479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<int>& row_blocks = m.row_blocks(); 51579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 51679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int cursor = 0; 51779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int row_block_begin = 0; 51879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double* m_values = m.values(); 51979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int* m_rows = m.rows(); 52079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Iterate over row blocks. 52179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int row_block = 0; row_block < row_blocks.size(); ++row_block) { 52279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int row_block_end = row_block_begin + row_blocks[row_block]; 52379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int saved_cursor = cursor; 52479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int r = row_block_begin; r < row_block_end; ++r) { 52579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Reuse the program segment for each row in this row block. 52679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cursor = saved_cursor; 52779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int row_begin = m_rows[r]; 52879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int row_end = m_rows[r + 1]; 52979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int idx1 = row_begin; idx1 < row_end; ++idx1) { 53079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double v1 = m_values[idx1]; 53179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int idx2 = row_begin; idx2 <= idx1; ++idx2, ++cursor) { 53279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez values[program[cursor]] += v1 * m_values[idx2]; 53379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 53479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 53579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 53679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez row_block_begin = row_block_end; 53779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 53879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 53979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_EQ(row_block_begin, m.num_rows()); 54079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_EQ(cursor, program.size()); 54179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 5421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 5440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 545