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#ifndef CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/macros.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/port.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/sparse_matrix.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstruct CRSMatrix; 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass TripletSparseMatrix; 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass CompressedRowSparseMatrix : public SparseMatrix { 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Build a matrix with the same content as the TripletSparseMatrix 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // m. TripletSparseMatrix objects are easier to construct 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // incrementally, so we use them to initialize SparseMatrix 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // objects. 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // We assume that m does not have any repeated entries. 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong explicit CompressedRowSparseMatrix(const TripletSparseMatrix& m); 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Use this constructor only if you know what you are doing. This 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // creates a "blank" matrix with the appropriate amount of memory 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // allocated. However, the object itself is in an inconsistent state 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // as the rows and cols matrices do not match the values of 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // num_rows, num_cols and max_num_nonzeros. 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The use case for this constructor is that when the user knows the 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // size of the matrix to begin with and wants to update the layout 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // manually, instead of going via the indirect route of first 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // constructing a TripletSparseMatrix, which leads to more than 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // double the peak memory usage. 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRowSparseMatrix(int num_rows, 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols, 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int max_num_nonzeros); 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Build a square sparse diagonal matrix with num_rows rows and 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // columns. The diagonal m(i,i) = diagonal(i); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRowSparseMatrix(const double* diagonal, int num_rows); 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual ~CompressedRowSparseMatrix(); 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // SparseMatrix interface. 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void SetZero(); 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void RightMultiply(const double* x, double* y) const; 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void LeftMultiply(const double* x, double* y) const; 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void SquaredColumnNorm(double* x) const; 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void ScaleColumns(const double* scale); 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void ToDenseMatrix(Matrix* dense_matrix) const; 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual void ToTextFile(FILE* file) const; 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual int num_rows() const { return num_rows_; } 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual int num_cols() const { return num_cols_; } 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong virtual int num_nonzeros() const { return rows_[num_rows_]; } 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual const double* values() const { return &values_[0]; } 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual double* mutable_values() { return &values_[0]; } 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Delete the bottom delta_rows. 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // num_rows -= delta_rows 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void DeleteRows(int delta_rows); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Append the contents of m to the bottom of this matrix. m must 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // have the same number of columns as this matrix. 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void AppendRows(const CompressedRowSparseMatrix& m); 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong void ToCRSMatrix(CRSMatrix* matrix) const; 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Low level access methods that expose the structure of the matrix. 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* cols() const { return &cols_[0]; } 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* mutable_cols() { return &cols_[0]; } 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* rows() const { return &rows_[0]; } 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* mutable_rows() { return &rows_[0]; } 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<int>& row_blocks() const { return row_blocks_; } 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int>* mutable_row_blocks() { return &row_blocks_; } 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<int>& col_blocks() const { return col_blocks_; } 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int>* mutable_col_blocks() { return &col_blocks_; } 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 11879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Destructive array resizing method. 11979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez void SetMaxNumNonZeros(int num_nonzeros); 12079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Non-destructive array resizing method. 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void set_num_rows(const int num_rows) { num_rows_ = num_rows; } 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void set_num_cols(const int num_cols) { num_cols_ = num_cols; } 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void SolveLowerTriangularInPlace(double* solution) const; 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling void SolveLowerTriangularTransposeInPlace(double* solution) const; 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CompressedRowSparseMatrix* Transpose() const; 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling static CompressedRowSparseMatrix* CreateBlockDiagonalMatrix( 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* diagonal, 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<int>& blocks); 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 13479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Compute the sparsity structure of the product m.transpose() * m 13579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // and create a CompressedRowSparseMatrix corresponding to it. 13679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // 13779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Also compute a "program" vector, which for every term in the 13879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // outer product points to the entry in the values array of the 13979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // result matrix where it should be accumulated. 14079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // 14179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // This program is used by the ComputeOuterProduct function below to 14279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // compute the outer product. 14379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // 14479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Since the entries of the program are the same for rows with the 14579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // same sparsity structure, the program only stores the result for 14679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // one row per row block. The ComputeOuterProduct function reuses 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // this information for each row in the row block. 14879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez static CompressedRowSparseMatrix* CreateOuterProductMatrixAndProgram( 14979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const CompressedRowSparseMatrix& m, 15079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez vector<int>* program); 15179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 15279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Compute the values array for the expression m.transpose() * m, 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // where the matrix used to store the result and a program have been 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // created using the CreateOuterProductMatrixAndProgram function 15579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // above. 15679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez static void ComputeOuterProduct(const CompressedRowSparseMatrix& m, 15779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<int>& program, 15879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CompressedRowSparseMatrix* result); 15979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling private: 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_rows_; 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int num_cols_; 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int> rows_; 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int> cols_; 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<double> values_; 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If the matrix has an underlying block structure, then it can also 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // carry with it row and column block sizes. This is auxilliary and 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // optional information for use by algorithms operating on the 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // matrix. The class itself does not make use of this information in 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // any way. 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> row_blocks_; 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> col_blocks_; 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CERES_DISALLOW_COPY_AND_ASSIGN(CompressedRowSparseMatrix); 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ 182