1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 3 // All rights reserved. 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are 7 // met: 8 // 9 // * Redistributions of source code must retain the above copyright notice, 10 // this list of conditions and the following disclaimer. 11 // * Redistributions in binary form must reproduce the above copyright 12 // notice, this list of conditions and the following disclaimer in the 13 // documentation and/or other materials provided with the distribution. 14 // 15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 27 #ifndef G2O_LINEAR_SOLVER_H 28 #define G2O_LINEAR_SOLVER_H 29 30 #include <functional> 31 32 #include "g2o/core/marginal_covariance_cholesky.h" 33 #include "sparse_block_matrix.h" 34 #include "sparse_block_matrix_ccs.h" 35 36 namespace g2o { 37 38 /** 39 * \brief basic solver for Ax = b 40 * 41 * basic solver for Ax = b which has to reimplemented for different linear algebra libraries. 42 * A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit. 43 */ 44 template <typename MatrixType> 45 class LinearSolver { 46 public: LinearSolver()47 LinearSolver() : _writeDebug(true){}; ~LinearSolver()48 virtual ~LinearSolver() {} 49 50 /** 51 * init for operating on matrices with a different non-zero pattern like before 52 */ 53 virtual bool init() = 0; 54 55 /** 56 * Assumes that A is the same matrix for several calls. 57 * Among other assumptions, the non-zero pattern does not change! 58 * If the matrix changes call init() before. 59 * solve system Ax = b, x and b have to allocated beforehand!! 60 */ 61 virtual bool solve(const SparseBlockMatrix<MatrixType>& A, number_t* x, number_t* b) = 0; 62 63 /** 64 * Inverts the diagonal blocks of A 65 * @returns false if not defined. 66 */ solveBlocks(number_t ** & blocks,const SparseBlockMatrix<MatrixType> & A)67 virtual bool solveBlocks(number_t**& blocks, const SparseBlockMatrix<MatrixType>& A) { 68 (void)blocks; 69 (void)A; 70 return false; 71 } 72 73 /** 74 * Inverts the a block pattern of A in spinv 75 * @returns false if not defined. 76 */ solvePattern(SparseBlockMatrix<MatrixX> & spinv,const std::vector<std::pair<int,int>> & blockIndices,const SparseBlockMatrix<MatrixType> & A)77 virtual bool solvePattern(SparseBlockMatrix<MatrixX>& spinv, 78 const std::vector<std::pair<int, int> >& blockIndices, 79 const SparseBlockMatrix<MatrixType>& A) { 80 (void)spinv; 81 (void)blockIndices; 82 (void)A; 83 return false; 84 } 85 86 //! write a debug dump of the system matrix if it is not PSD in solve writeDebug()87 bool writeDebug() const { return _writeDebug; } setWriteDebug(bool b)88 void setWriteDebug(bool b) { _writeDebug = b; } 89 90 //! allocate block memory structure allocateBlocks(const SparseBlockMatrix<MatrixType> & A,number_t ** & blocks)91 static void allocateBlocks(const SparseBlockMatrix<MatrixType>& A, number_t**& blocks) { 92 blocks = new number_t*[A.rows()]; 93 number_t** block = blocks; 94 for (size_t i = 0; i < A.rowBlockIndices().size(); ++i) { 95 int dim = A.rowsOfBlock(i) * A.colsOfBlock(i); 96 *block = new number_t[dim]; 97 block++; 98 } 99 } 100 101 //! de-allocate the block structure deallocateBlocks(const SparseBlockMatrix<MatrixType> & A,number_t ** & blocks)102 static void deallocateBlocks(const SparseBlockMatrix<MatrixType>& A, number_t**& blocks) { 103 for (size_t i = 0; i < A.rowBlockIndices().size(); ++i) { 104 delete[] blocks[i]; 105 } 106 delete[] blocks; 107 blocks = nullptr; 108 } 109 110 /** 111 * Convert a block permutation matrix to a scalar permutation 112 */ 113 template <typename BlockDerived, typename ScalarDerived> blockToScalarPermutation(const SparseBlockMatrix<MatrixType> & A,const Eigen::MatrixBase<BlockDerived> & p,const Eigen::MatrixBase<ScalarDerived> & scalar)114 static void blockToScalarPermutation( 115 const SparseBlockMatrix<MatrixType>& A, const Eigen::MatrixBase<BlockDerived>& p, 116 const Eigen::MatrixBase<ScalarDerived>& scalar /* output */) { 117 int n = A.cols(); 118 Eigen::MatrixBase<ScalarDerived>& scalarPermutation = 119 const_cast<Eigen::MatrixBase<ScalarDerived>&>(scalar); 120 if (scalarPermutation.size() == 0) scalarPermutation.derived().resize(n); 121 if (scalarPermutation.size() < n) scalarPermutation.derived().resize(2 * n); 122 size_t scalarIdx = 0; 123 for (size_t i = 0; i < A.colBlockIndices().size(); ++i) { 124 int base = A.colBaseOfBlock(p(i)); 125 int nCols = A.colsOfBlock(p(i)); 126 for (int j = 0; j < nCols; ++j) { 127 scalarPermutation(scalarIdx++) = base++; 128 } 129 } 130 assert((int)scalarIdx == n); 131 } 132 133 protected: 134 bool _writeDebug; 135 }; 136 137 /** 138 * \brief Solver with faster iterating structure for the linear matrix 139 */ 140 template <typename MatrixType> 141 class LinearSolverCCS : public LinearSolver<MatrixType> { 142 public: LinearSolverCCS()143 LinearSolverCCS() : LinearSolver<MatrixType>(), _ccsMatrix(0), _blockOrdering(true) {} ~LinearSolverCCS()144 ~LinearSolverCCS() { delete _ccsMatrix; } 145 solveBlocks(number_t ** & blocks,const SparseBlockMatrix<MatrixType> & A)146 virtual bool solveBlocks(number_t**& blocks, const SparseBlockMatrix<MatrixType>& A) { 147 auto compute = [&](MarginalCovarianceCholesky& mcc) { 148 if (!blocks) LinearSolverCCS<MatrixType>::allocateBlocks(A, blocks); 149 mcc.computeCovariance(blocks, A.rowBlockIndices()); 150 }; 151 return solveBlocks_impl(A, compute); 152 } 153 solvePattern(SparseBlockMatrix<MatrixX> & spinv,const std::vector<std::pair<int,int>> & blockIndices,const SparseBlockMatrix<MatrixType> & A)154 virtual bool solvePattern(SparseBlockMatrix<MatrixX>& spinv, 155 const std::vector<std::pair<int, int> >& blockIndices, 156 const SparseBlockMatrix<MatrixType>& A) { 157 auto compute = [&](MarginalCovarianceCholesky& mcc) { 158 mcc.computeCovariance(spinv, A.rowBlockIndices(), blockIndices); 159 }; 160 return solveBlocks_impl(A, compute); 161 } 162 163 //! do the AMD ordering on the blocks or on the scalar matrix blockOrdering()164 bool blockOrdering() const { return _blockOrdering; } setBlockOrdering(bool blockOrdering)165 void setBlockOrdering(bool blockOrdering) { _blockOrdering = blockOrdering; } 166 167 protected: 168 SparseBlockMatrixCCS<MatrixType>* _ccsMatrix; 169 bool _blockOrdering; 170 initMatrixStructure(const SparseBlockMatrix<MatrixType> & A)171 void initMatrixStructure(const SparseBlockMatrix<MatrixType>& A) { 172 delete _ccsMatrix; 173 _ccsMatrix = new SparseBlockMatrixCCS<MatrixType>(A.rowBlockIndices(), A.colBlockIndices()); 174 A.fillSparseBlockMatrixCCS(*_ccsMatrix); 175 } 176 177 virtual bool solveBlocks_impl(const SparseBlockMatrix<MatrixType>& A, 178 std::function<void(MarginalCovarianceCholesky&)> compute) = 0; 179 }; 180 181 } // namespace g2o 182 183 #endif 184