11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2013 Google Inc. All rights reserved. 31d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// http://code.google.com/p/ceres-solver/ 41d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 51d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Redistribution and use in source and binary forms, with or without 61d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// modification, are permitted provided that the following conditions are met: 71d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 81d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions of source code must retain the above copyright notice, 91d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer. 101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions in binary form must reproduce the above copyright notice, 111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer in the documentation 121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and/or other materials provided with the distribution. 131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Neither the name of Google Inc. nor the names of its contributors may be 141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// used to endorse or promote products derived from this software without 151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// specific prior written permission. 161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// POSSIBILITY OF SUCH DAMAGE. 281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Author: sameeragarwal@google.com (Sameer Agarwal) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#ifndef CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <vector> 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/port.h" 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Extract the block sparsity pattern of the scalar compressed columns 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// matrix and return it in compressed column form. The compressed 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// column form is stored in two vectors block_rows, and block_cols, 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// which correspond to the row and column arrays in a compressed 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// column sparse matrix. 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// If c_ij is the block in the matrix A corresponding to row block i 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and column block j, then it is expected that A contains at least 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// one non-zero entry corresponding to the top left entry of c_ij, 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// as that entry is used to detect the presence of a non-zero c_ij. 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid CompressedColumnScalarMatrixToBlockMatrix(const int* scalar_rows, 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int* scalar_cols, 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<int>& row_blocks, 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<int>& col_blocks, 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int>* block_rows, 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int>* block_cols); 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Given a set of blocks and a permutation of these blocks, compute 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the corresponding "scalar" ordering, where the scalar ordering of 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// size sum(blocks). 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid BlockOrderingToScalarOrdering(const vector<int>& blocks, 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const vector<int>& block_ordering, 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling vector<int>* scalar_ordering); 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 64399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Solve the linear system 65399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 66399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// R * solution = rhs 67399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 68399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Where R is an upper triangular compressed column sparse matrix. 69399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingertemplate <typename IntegerType> 70399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingervoid SolveUpperTriangularInPlace(IntegerType num_cols, 71399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const IntegerType* rows, 72399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const IntegerType* cols, 73399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const double* values, 74399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* rhs_and_solution) { 75399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger for (IntegerType c = num_cols - 1; c >= 0; --c) { 76399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger rhs_and_solution[c] /= values[cols[c + 1] - 1]; 77399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { 78399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const IntegerType r = rows[idx]; 79399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const double v = values[idx]; 80399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger rhs_and_solution[r] -= v * rhs_and_solution[c]; 81399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger } 82399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger } 83399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger} 84399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 85399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Solve the linear system 86399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 87399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// R' * solution = rhs 88399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 89399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Where R is an upper triangular compressed column sparse matrix. 90399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingertemplate <typename IntegerType> 91399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingervoid SolveUpperTriangularTransposeInPlace(IntegerType num_cols, 92399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const IntegerType* rows, 93399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const IntegerType* cols, 94399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const double* values, 95399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* rhs_and_solution) { 96399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger for (IntegerType c = 0; c < num_cols; ++c) { 97399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { 98399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const IntegerType r = rows[idx]; 99399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const double v = values[idx]; 100399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger rhs_and_solution[c] -= v * rhs_and_solution[r]; 101399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger } 102399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger rhs_and_solution[c] = rhs_and_solution[c] / values[cols[c + 1] - 1]; 103399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger } 104399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger} 105399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 106399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Given a upper triangular matrix R in compressed column form, solve 107399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// the linear system, 108399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 109399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// R'R x = b 110399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 111399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Where b is all zeros except for rhs_nonzero_index, where it is 112399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// equal to one. 113399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 114399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// The function exploits this knowledge to reduce the number of 115399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// floating point operations. 116399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingertemplate <typename IntegerType> 117399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingervoid SolveRTRWithSparseRHS(IntegerType num_cols, 118399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const IntegerType* rows, 119399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const IntegerType* cols, 120399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const double* values, 121399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const int rhs_nonzero_index, 122399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* solution) { 123399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger fill(solution, solution + num_cols, 0.0); 124399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger solution[rhs_nonzero_index] = 1.0 / values[cols[rhs_nonzero_index + 1] - 1]; 125399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 126399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger for (IntegerType c = rhs_nonzero_index + 1; c < num_cols; ++c) { 127399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger for (IntegerType idx = cols[c]; idx < cols[c + 1] - 1; ++idx) { 128399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const IntegerType r = rows[idx]; 129399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger if (r < rhs_nonzero_index) continue; 130399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger const double v = values[idx]; 131399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger solution[c] -= v * solution[r]; 132399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger } 133399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger solution[c] = solution[c] / values[cols[c + 1] - 1]; 134399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger } 135399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 136399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger SolveUpperTriangularInPlace(num_cols, rows, cols, values, solution); 137399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger} 138399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#endif // CERES_INTERNAL_COMPRESSED_COL_SPARSE_MATRIX_UTILS_H_ 143