1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 H. Strasdat 3 // Copyright (C) 2012 R. Kümmerle 4 // All rights reserved. 5 // 6 // Redistribution and use in source and binary forms, with or without 7 // modification, are permitted provided that the following conditions are 8 // met: 9 // 10 // * Redistributions of source code must retain the above copyright notice, 11 // this list of conditions and the following disclaimer. 12 // * Redistributions in binary form must reproduce the above copyright 13 // notice, this list of conditions and the following disclaimer in the 14 // documentation and/or other materials provided with the distribution. 15 // 16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 17 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 18 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 19 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 22 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 23 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 24 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 28 #ifndef G2O_LINEAR_SOLVER_DENSE_H 29 #define G2O_LINEAR_SOLVER_DENSE_H 30 31 #include "g2o/core/linear_solver.h" 32 #include "g2o/core/batch_stats.h" 33 34 #include <vector> 35 #include <utility> 36 #include<Eigen/Core> 37 #include<Eigen/Cholesky> 38 39 40 namespace g2o { 41 42 /** 43 * \brief linear solver using dense cholesky decomposition 44 */ 45 template <typename MatrixType> 46 class LinearSolverDense : public LinearSolver<MatrixType> 47 { 48 public: LinearSolverDense()49 LinearSolverDense() : 50 LinearSolver<MatrixType>(), 51 _reset(true) 52 { 53 } 54 ~LinearSolverDense()55 virtual ~LinearSolverDense() 56 { 57 } 58 init()59 virtual bool init() 60 { 61 _reset = true; 62 return true; 63 } 64 solve(const SparseBlockMatrix<MatrixType> & A,number_t * x,number_t * b)65 bool solve(const SparseBlockMatrix<MatrixType>& A, number_t* x, number_t* b) 66 { 67 int n = A.cols(); 68 int m = A.cols(); 69 70 MatrixX& H = _H; 71 if (H.cols() != n) { 72 H.resize(n, m); 73 _reset = true; 74 } 75 if (_reset) { 76 _reset = false; 77 H.setZero(); 78 } 79 80 // copy the sparse block matrix into a dense matrix 81 int c_idx = 0; 82 for (size_t i = 0; i < A.blockCols().size(); ++i) { 83 int c_size = A.colsOfBlock(i); 84 assert(c_idx == A.colBaseOfBlock(i) && "mismatch in block indices"); 85 86 const typename SparseBlockMatrix<MatrixType>::IntBlockMap& col = A.blockCols()[i]; 87 if (col.size() > 0) { 88 typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it; 89 for (it = col.begin(); it != col.end(); ++it) { 90 int r_idx = A.rowBaseOfBlock(it->first); 91 // only the upper triangular block is processed 92 if (it->first <= (int)i) { 93 int r_size = A.rowsOfBlock(it->first); 94 H.block(r_idx, c_idx, r_size, c_size) = *(it->second); 95 if (r_idx != c_idx) // write the lower triangular block 96 H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose(); 97 } 98 } 99 } 100 101 c_idx += c_size; 102 } 103 104 // solving via Cholesky decomposition 105 VectorX::MapType xvec(x, m); 106 VectorX::ConstMapType bvec(b, n); 107 _cholesky.compute(H); 108 if (_cholesky.isPositive()) { 109 xvec = _cholesky.solve(bvec); 110 return true; 111 } 112 return false; 113 } 114 115 protected: 116 bool _reset; 117 MatrixX _H; 118 Eigen::LDLT<MatrixX> _cholesky; 119 120 }; 121 122 123 }// end namespace 124 125 #endif 126