linear_solver_eigen.h
Go to the documentation of this file.
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_EIGEN_H
28 #define G2O_LINEAR_SOLVER_EIGEN_H
29 
30 #include <Eigen/Sparse>
31 #include <Eigen/SparseCholesky>
32 
33 #include "../core/linear_solver.h"
34 #include "../core/batch_stats.h"
35 #include "../stuff/timeutil.h"
36 
37 #include "../core/eigen_types.h"
38 
39 #include <iostream>
40 #include <vector>
41 
42 namespace g2o {
43 
50 template <typename MatrixType>
51 class LinearSolverEigen: public LinearSolver<MatrixType>
52 {
53  public:
54  typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseMatrix;
55  typedef Eigen::Triplet<double> Triplet;
56  typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> PermutationMatrix;
60  class CholeskyDecomposition : public Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>
61  {
62  public:
63  CholeskyDecomposition() : Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>() {}
64  using Eigen::SimplicialLDLT< SparseMatrix, Eigen::Upper>::analyzePattern_preordered;
65 
66  void analyzePatternWithPermutation(SparseMatrix& a, const PermutationMatrix& permutation)
67  {
68  m_Pinv = permutation;
69  m_P = permutation.inverse();
70  int size = a.cols();
71  SparseMatrix ap(size, size);
72  ap.selfadjointView<Eigen::Upper>() = a.selfadjointView<UpLo>().twistedBy(m_P);
73  analyzePattern_preordered(ap, true);
74  }
75  };
76 
77  public:
79  LinearSolver<MatrixType>(),
80  _init(true), _blockOrdering(false), _writeDebug(false)
81  {
82  }
83 
85  {
86  }
87 
88  virtual bool init()
89  {
90  _init = true;
91  return true;
92  }
93 
94  bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b)
95  {
96  if (_init)
97  _sparseMatrix.resize(A.rows(), A.cols());
99  if (_init) // compute the symbolic composition once
101  _init = false;
102 
103  double t=get_monotonic_time();
104  _cholesky.factorize(_sparseMatrix);
105  if (_cholesky.info() != Eigen::Success) { // the matrix is not positive definite
106  if (_writeDebug) {
107  std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl;
108  A.writeOctave("debug.txt");
109  }
110  return false;
111  }
112 
113  // Solving the system
114  VectorXD::MapType xx(x, _sparseMatrix.cols());
115  VectorXD::ConstMapType bb(b, _sparseMatrix.cols());
116  xx = _cholesky.solve(bb);
118  if (globalStats) {
119  globalStats->timeNumericDecomposition = get_monotonic_time() - t;
120  globalStats->choleskyNNZ = _cholesky.matrixL().nestedExpression().nonZeros() + _sparseMatrix.cols(); // the elements of D
121  }
122 
123  return true;
124  }
125 
127  bool blockOrdering() const { return _blockOrdering;}
129 
131  virtual bool writeDebug() const { return _writeDebug;}
132  virtual void setWriteDebug(bool b) { _writeDebug = b;}
133 
134  protected:
135  bool _init;
138  SparseMatrix _sparseMatrix;
140 
148  {
149  double t=get_monotonic_time();
150  if (! _blockOrdering) {
151  _cholesky.analyzePattern(_sparseMatrix);
152  } else {
153  // block ordering with the Eigen Interface
154  // This is really ugly currently, as it calls internal functions from Eigen
155  // and modifies the SparseMatrix class
156  Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic> blockP;
157  {
158  // prepare a block structure matrix for calling AMD
159  std::vector<Triplet> triplets;
160  for (size_t c = 0; c < A.blockCols().size(); ++c){
161  const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
162  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
163  const int& r = it->first;
164  if (r > static_cast<int>(c)) // only upper triangle
165  break;
166  triplets.push_back(Triplet(r, c, 0.));
167  }
168  }
169 
170  // call the AMD ordering on the block matrix.
171  // Relies on Eigen's internal stuff, probably bad idea
172  SparseMatrix auxBlockMatrix(A.blockCols().size(), A.blockCols().size());
173  auxBlockMatrix.setFromTriplets(triplets.begin(), triplets.end());
174  typename CholeskyDecomposition::CholMatrixType C;
175  C = auxBlockMatrix.selfadjointView<Eigen::Upper>();
176  Eigen::internal::minimum_degree_ordering(C, blockP);
177  }
178 
179  int rows = A.rows();
180  assert(rows == A.cols() && "Matrix A is not square");
181 
182  // Adapt the block permutation to the scalar matrix
183  PermutationMatrix scalarP;
184  scalarP.resize(rows);
185  int scalarIdx = 0;
186  for (int i = 0; i < blockP.size(); ++i) {
187  const int& p = blockP.indices()(i);
188  int base = A.colBaseOfBlock(p);
189  int nCols = A.colsOfBlock(p);
190  for (int j = 0; j < nCols; ++j)
191  scalarP.indices()(scalarIdx++) = base++;
192  }
193  assert(scalarIdx == rows && "did not completely fill the permutation matrix");
194  // analyze with the scalar permutation
195  _cholesky.analyzePatternWithPermutation(_sparseMatrix, scalarP);
196 
197  }
199  if (globalStats)
200  globalStats->timeSymbolicDecomposition = get_monotonic_time() - t;
201  }
202 
203  void fillSparseMatrix(const SparseBlockMatrix<MatrixType>& A, bool onlyValues)
204  {
205  if (onlyValues) {
206  A.fillCCS(_sparseMatrix.valuePtr(), true);
207  } else {
208 
209  // create from triplet structure
210  std::vector<Triplet> triplets;
211  triplets.reserve(A.nonZeros());
212  for (size_t c = 0; c < A.blockCols().size(); ++c) {
213  int colBaseOfBlock = A.colBaseOfBlock(c);
214  const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
215  for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
216  int rowBaseOfBlock = A.rowBaseOfBlock(it->first);
217  const MatrixType& m = *(it->second);
218  for (int cc = 0; cc < m.cols(); ++cc) {
219  int aux_c = colBaseOfBlock + cc;
220  for (int rr = 0; rr < m.rows(); ++rr) {
221  int aux_r = rowBaseOfBlock + rr;
222  if (aux_r > aux_c)
223  break;
224  triplets.push_back(Triplet(aux_r, aux_c, m(rr, cc)));
225  }
226  }
227  }
228  }
229  _sparseMatrix.setFromTriplets(triplets.begin(), triplets.end());
230 
231  }
232  }
233 };
234 
235 } // end namespace
236 
237 #endif
statistics about the optimization
Definition: batch_stats.h:39
int cols() const
columns of the matrix
Eigen::SparseMatrix< double, Eigen::ColMajor > SparseMatrix
bool writeOctave(const char *filename, bool upperTriangle=true) const
int rows() const
rows of the matrix
Sub-classing Eigen&#39;s SimplicialLDLT to perform ordering with a given ordering.
void computeSymbolicDecomposition(const SparseBlockMatrix< MatrixType > &A)
int colsOfBlock(int c) const
how many cols does the block at block-col c has?
Eigen::Triplet< double > Triplet
size_t nonZeros() const
number of non-zero elements
Eigen::PermutationMatrix< Eigen::Dynamic, Eigen::Dynamic > PermutationMatrix
size_t choleskyNNZ
number of non-zeros in the cholesky factor
Definition: batch_stats.h:70
bool blockOrdering() const
do the AMD ordering on the blocks or on the scalar matrix
basic solver for Ax = b
Definition: linear_solver.h:41
linear solver which uses the sparse Cholesky solver from Eigen
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:72
double timeNumericDecomposition
numeric decomposition (0 if not done)
Definition: batch_stats.h:57
double get_monotonic_time()
Definition: timeutil.cpp:113
std::map< int, SparseMatrixBlock * > IntBlockMap
int colBaseOfBlock(int c) const
where does the col at block-col r starts?
const std::vector< IntBlockMap > & blockCols() const
the block matrices per block-column
void fillSparseMatrix(const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
bool solve(const SparseBlockMatrix< MatrixType > &A, double *x, double *b)
double timeSymbolicDecomposition
symbolic decomposition (0 if not done)
Definition: batch_stats.h:56
virtual bool writeDebug() const
write a debug dump of the system matrix if it is not SPD in solve
void setBlockOrdering(bool blockOrdering)
CholeskyDecomposition _cholesky
int rowBaseOfBlock(int r) const
where does the row at block-row r starts?
Sparse matrix which uses blocks.
int fillCCS(int *Cp, int *Ci, double *Cx, bool upperTriangle=false) const
void analyzePatternWithPermutation(SparseMatrix &a, const PermutationMatrix &permutation)
virtual void setWriteDebug(bool b)


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47