g2o
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
g2o::LinearSolverCSparse< MatrixType > Class Template Reference

linear solver which uses CSparse More...

#include <linear_solver_csparse.h>

Inheritance diagram for g2o::LinearSolverCSparse< MatrixType >:
Inheritance graph
[legend]
Collaboration diagram for g2o::LinearSolverCSparse< MatrixType >:
Collaboration graph
[legend]

Public Member Functions

 LinearSolverCSparse ()
 
virtual ~LinearSolverCSparse ()
 
virtual bool init ()
 
bool solve (const SparseBlockMatrix< MatrixType > &A, double *x, double *b)
 
bool solveBlocks (double **&blocks, const SparseBlockMatrix< MatrixType > &A)
 
virtual bool solvePattern (SparseBlockMatrix< MatrixXD > &spinv, const std::vector< std::pair< int, int > > &blockIndices, const SparseBlockMatrix< MatrixType > &A)
 
bool blockOrdering () const
 do the AMD ordering on the blocks or on the scalar matrix More...
 
void setBlockOrdering (bool blockOrdering)
 
virtual bool writeDebug () const
 write a debug dump of the system matrix if it is not SPD in solve More...
 
virtual void setWriteDebug (bool b)
 
- Public Member Functions inherited from g2o::LinearSolverCCS< MatrixType >
 LinearSolverCCS ()
 
 ~LinearSolverCCS ()
 
- Public Member Functions inherited from g2o::LinearSolver< MatrixType >
 LinearSolver ()
 
virtual ~LinearSolver ()
 

Protected Member Functions

void computeSymbolicDecomposition (const SparseBlockMatrix< MatrixType > &A)
 
void fillCSparse (const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
 
- Protected Member Functions inherited from g2o::LinearSolverCCS< MatrixType >
void initMatrixStructure (const SparseBlockMatrix< MatrixType > &A)
 

Protected Attributes

css * _symbolicDecomposition
 
int _csWorkspaceSize
 
double * _csWorkspace
 
int * _csIntWorkspace
 
CSparseExt * _ccsA
 
bool _blockOrdering
 
MatrixStructure _matrixStructure
 
VectorXI _scalarPermutation
 
bool _writeDebug
 
- Protected Attributes inherited from g2o::LinearSolverCCS< MatrixType >
SparseBlockMatrixCCS< MatrixType > * _ccsMatrix
 

Detailed Description

template<typename MatrixType>
class g2o::LinearSolverCSparse< MatrixType >

linear solver which uses CSparse

Definition at line 71 of file linear_solver_csparse.h.

Constructor & Destructor Documentation

template<typename MatrixType>
g2o::LinearSolverCSparse< MatrixType >::LinearSolverCSparse ( )
inline

Definition at line 74 of file linear_solver_csparse.h.

74  :
75  LinearSolverCCS<MatrixType>()
76  {
77  _symbolicDecomposition = 0;
78  _csWorkspaceSize = -1;
79  _csWorkspace = 0;
80  _csIntWorkspace = 0;
81  _ccsA = new CSparseExt;
82  _blockOrdering = true;
83  _writeDebug = true;
84  }
template<typename MatrixType>
virtual g2o::LinearSolverCSparse< MatrixType >::~LinearSolverCSparse ( )
inlinevirtual

Definition at line 86 of file linear_solver_csparse.h.

87  {
88  if (_symbolicDecomposition) {
89  cs_sfree(_symbolicDecomposition);
90  _symbolicDecomposition = 0;
91  }
92  delete[] _csWorkspace; _csWorkspace = 0;
93  delete[] _csIntWorkspace; _csIntWorkspace = 0;
94  delete _ccsA;
95  }

Member Function Documentation

template<typename MatrixType>
bool g2o::LinearSolverCSparse< MatrixType >::blockOrdering ( ) const
inline

do the AMD ordering on the blocks or on the scalar matrix

Definition at line 228 of file linear_solver_csparse.h.

228 { return _blockOrdering;}
template<typename MatrixType>
void g2o::LinearSolverCSparse< MatrixType >::computeSymbolicDecomposition ( const SparseBlockMatrix< MatrixType > &  A)
inlineprotected

Definition at line 246 of file linear_solver_csparse.h.

References g2o::MatrixStructure::Aii, g2o::MatrixStructure::Ap, g2o::SparseBlockMatrix< MatrixType >::colBaseOfBlock(), g2o::SparseBlockMatrix< MatrixType >::colsOfBlock(), g2o::SparseBlockMatrix< MatrixType >::fillBlockStructure(), g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::MatrixStructure::n, g2o::MatrixStructure::nzMax(), and g2o::G2OBatchStatistics::timeSymbolicDecomposition.

247  {
248  double t=get_monotonic_time();
249  if (! _blockOrdering) {
250  _symbolicDecomposition = cs_schol (1, _ccsA) ;
251  } else {
252  A.fillBlockStructure(_matrixStructure);
253 
254  // prepare block structure for the CSparse call
255  cs auxBlock;
256  auxBlock.nzmax = _matrixStructure.nzMax();
257  auxBlock.m = auxBlock.n = _matrixStructure.n;
258  auxBlock.p = _matrixStructure.Ap;
259  auxBlock.i = _matrixStructure.Aii;
260  auxBlock.x = NULL; // no values
261  auxBlock.nz = -1; // CCS format
262 
263  // AMD ordering on the block structure
264  const int& n = _ccsA->n;
265  int* P = cs_amd(1, &auxBlock);
266 
267  // blow up the permutation to the scalar matrix
268  if (_scalarPermutation.size() == 0)
269  _scalarPermutation.resize(n);
270  if (_scalarPermutation.size() < n)
271  _scalarPermutation.resize(2*n);
272  size_t scalarIdx = 0;
273  for (int i = 0; i < _matrixStructure.n; ++i) {
274  const int& p = P[i];
275  int base = A.colBaseOfBlock(p);
276  int nCols = A.colsOfBlock(p);
277  for (int j = 0; j < nCols; ++j)
278  _scalarPermutation(scalarIdx++) = base++;
279  }
280  assert((int)scalarIdx == n);
281  cs_free(P);
282 
283  // apply the scalar permutation to finish symbolic decomposition
284  _symbolicDecomposition = (css*) cs_calloc(1, sizeof(css)); /* allocate result S */
285  _symbolicDecomposition->pinv = cs_pinv(_scalarPermutation.data(), n);
286  cs* C = cs_symperm(_ccsA, _symbolicDecomposition->pinv, 0);
287  _symbolicDecomposition->parent = cs_etree(C, 0);
288  int* post = cs_post(_symbolicDecomposition->parent, n);
289  int* c = cs_counts(C, _symbolicDecomposition->parent, post, 0);
290  cs_free(post);
291  cs_spfree(C);
292  _symbolicDecomposition->cp = (int*) cs_malloc(n+1, sizeof(int));
293  _symbolicDecomposition->unz = _symbolicDecomposition->lnz = cs_cumsum(_symbolicDecomposition->cp, c, n);
294  cs_free(c);
295  if (_symbolicDecomposition->lnz < 0) {
296  cs_sfree(_symbolicDecomposition);
297  _symbolicDecomposition = 0;
298  }
299 
300  }
301  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
302  if (globalStats){
303  globalStats->timeSymbolicDecomposition = get_monotonic_time() - t;
304  }
305 
306  /* std::cerr << "# Number of nonzeros in L: " << (int)_symbolicDecomposition->lnz << " by " */
307  /* << (_blockOrdering ? "block" : "scalar") << " AMD ordering " << std::endl; */
308  }
g2o::get_monotonic_time
double get_monotonic_time()
Definition: timeutil.cpp:113
g2o::MatrixStructure::Ap
int * Ap
column pointers for A, of size n+1
Definition: matrix_structure.h:57
g2o::MatrixStructure::n
int n
A is m-by-n. n must be >= 0.
Definition: matrix_structure.h:55
g2o::G2OBatchStatistics::globalStats
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
g2o::MatrixStructure::Aii
int * Aii
row indices of A, of size nz = Ap [n]
Definition: matrix_structure.h:58
g2o::MatrixStructure::nzMax
int nzMax() const
max number of non-zeros blocks
Definition: matrix_structure.h:61
template<typename MatrixType>
void g2o::LinearSolverCSparse< MatrixType >::fillCSparse ( const SparseBlockMatrix< MatrixType > &  A,
bool  onlyValues 
)
inlineprotected

Definition at line 310 of file linear_solver_csparse.h.

References g2o::SparseBlockMatrix< MatrixType >::cols(), g2o::CSparseExt::columnsAllocated, g2o::SparseBlockMatrix< MatrixType >::nonZeros(), and g2o::SparseBlockMatrix< MatrixType >::rows().

311  {
312  if (! onlyValues)
313  this->initMatrixStructure(A);
314  int m = A.rows();
315  int n = A.cols();
316  assert(m > 0 && n > 0 && "Hessian has 0 rows/cols");
317 
318  if (_ccsA->columnsAllocated < n) {
319  _ccsA->columnsAllocated = _ccsA->columnsAllocated == 0 ? n : 2 * n; // pre-allocate more space if re-allocating
320  delete[] _ccsA->p;
321  _ccsA->p = new int[_ccsA->columnsAllocated+1];
322  }
323 
324  if (! onlyValues) {
325  int nzmax = A.nonZeros();
326  if (_ccsA->nzmax < nzmax) {
327  _ccsA->nzmax = _ccsA->nzmax == 0 ? nzmax : 2 * nzmax; // pre-allocate more space if re-allocating
328  delete[] _ccsA->x;
329  delete[] _ccsA->i;
330  _ccsA->i = new int[_ccsA->nzmax];
331  _ccsA->x = new double[_ccsA->nzmax];
332  }
333  }
334  _ccsA->m = m;
335  _ccsA->n = n;
336 
337  if (onlyValues) {
338  this->_ccsMatrix->fillCCS(_ccsA->x, true);
339  } else {
340  int nz = this->_ccsMatrix->fillCCS(_ccsA->p, _ccsA->i, _ccsA->x, true); (void) nz;
341  assert(nz <= _ccsA->nzmax);
342  }
343  _ccsA->nz=-1; // tag as CCS formatted matrix
344  }
g2o::LinearSolverCCS::_ccsMatrix
SparseBlockMatrixCCS< MatrixType > * _ccsMatrix
Definition: linear_solver.h:97
g2o::LinearSolverCCS::initMatrixStructure
void initMatrixStructure(const SparseBlockMatrix< MatrixType > &A)
Definition: linear_solver.h:99
template<typename MatrixType>
virtual bool g2o::LinearSolverCSparse< MatrixType >::init ( )
inlinevirtual

init for operating on matrices with a different non-zero pattern like before

Implements g2o::LinearSolver< MatrixType >.

Definition at line 97 of file linear_solver_csparse.h.

Referenced by g2o::SolverSLAM2DLinear::solveOrientation().

98  {
99  if (_symbolicDecomposition) {
100  cs_sfree(_symbolicDecomposition);
101  _symbolicDecomposition = 0;
102  }
103  return true;
104  }
template<typename MatrixType>
void g2o::LinearSolverCSparse< MatrixType >::setBlockOrdering ( bool  blockOrdering)
inline

Definition at line 229 of file linear_solver_csparse.h.

229 { _blockOrdering = blockOrdering;}
g2o::LinearSolverCSparse::blockOrdering
bool blockOrdering() const
do the AMD ordering on the blocks or on the scalar matrix
Definition: linear_solver_csparse.h:228
template<typename MatrixType>
virtual void g2o::LinearSolverCSparse< MatrixType >::setWriteDebug ( bool  b)
inlinevirtual

Reimplemented from g2o::LinearSolver< MatrixType >.

Definition at line 233 of file linear_solver_csparse.h.

233 { _writeDebug = b;}
template<typename MatrixType>
bool g2o::LinearSolverCSparse< MatrixType >::solve ( const SparseBlockMatrix< MatrixType > &  A,
double *  x,
double *  b 
)
inlinevirtual

Assumes that A is the same matrix for several calls. Among other assumptions, the non-zero pattern does not change! If the matrix changes call init() before. solve system Ax = b, x and b have to allocated beforehand!!

Implements g2o::LinearSolver< MatrixType >.

Definition at line 106 of file linear_solver_csparse.h.

References g2o::G2OBatchStatistics::choleskyNNZ, g2o::csparse_extension::cs_cholsolsymb(), g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::G2OBatchStatistics::timeNumericDecomposition, and g2o::csparse_extension::writeCs2Octave().

107  {
108  fillCSparse(A, _symbolicDecomposition != 0);
109  // perform symbolic cholesky once
110  if (_symbolicDecomposition == 0) {
111  computeSymbolicDecomposition(A);
112  }
113  // re-allocate the temporary workspace for cholesky
114  if (_csWorkspaceSize < _ccsA->n) {
115  _csWorkspaceSize = 2 * _ccsA->n;
116  delete[] _csWorkspace;
117  _csWorkspace = new double[_csWorkspaceSize];
118  delete[] _csIntWorkspace;
119  _csIntWorkspace = new int[2*_csWorkspaceSize];
120  }
121 
122  double t=get_monotonic_time();
123  // _x = _b for calling csparse
124  if (x != b)
125  memcpy(x, b, _ccsA->n * sizeof(double));
126  int ok = csparse_extension::cs_cholsolsymb(_ccsA, x, _symbolicDecomposition, _csWorkspace, _csIntWorkspace);
127  if (! ok) {
128  if (_writeDebug) {
129  std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl;
130  csparse_extension::writeCs2Octave("debug.txt", _ccsA, true);
131  }
132  return false;
133  }
134 
135  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
136  if (globalStats){
137  globalStats->timeNumericDecomposition = get_monotonic_time() - t;
138  globalStats->choleskyNNZ = static_cast<size_t>(_symbolicDecomposition->lnz);
139  }
140 
141  return ok != 0;
142  }
g2o::get_monotonic_time
double get_monotonic_time()
Definition: timeutil.cpp:113
g2o::LinearSolverCSparse::fillCSparse
void fillCSparse(const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
Definition: linear_solver_csparse.h:310
g2o::csparse_extension::cs_cholsolsymb
int cs_cholsolsymb(const cs *A, double *b, const css *S, double *x, int *work)
Definition: csparse_helper.cpp:56
g2o::G2OBatchStatistics::globalStats
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
g2o::LinearSolverCSparse::computeSymbolicDecomposition
void computeSymbolicDecomposition(const SparseBlockMatrix< MatrixType > &A)
Definition: linear_solver_csparse.h:246
g2o::csparse_extension::writeCs2Octave
bool writeCs2Octave(const char *filename, const cs *A, bool upperTriangular)
Definition: csparse_helper.cpp:145
template<typename MatrixType>
bool g2o::LinearSolverCSparse< MatrixType >::solveBlocks ( double **&  blocks,
const SparseBlockMatrix< MatrixType > &  A 
)
inlinevirtual

Inverts the diagonal blocks of A

Returns
false if not defined.

Reimplemented from g2o::LinearSolver< MatrixType >.

Definition at line 144 of file linear_solver_csparse.h.

References g2o::G2OBatchStatistics::choleskyNNZ, g2o::SparseBlockMatrix< MatrixType >::colsOfBlock(), g2o::MarginalCovarianceCholesky::computeCovariance(), g2o::csparse_extension::cs_chol_workspace(), g2o::G2OBatchStatistics::globalStats(), g2o::SparseBlockMatrix< MatrixType >::rowBlockIndices(), g2o::SparseBlockMatrix< MatrixType >::rows(), g2o::SparseBlockMatrix< MatrixType >::rowsOfBlock(), and g2o::MarginalCovarianceCholesky::setCholeskyFactor().

144  {
145  fillCSparse(A, _symbolicDecomposition != 0);
146  // perform symbolic cholesky once
147  if (_symbolicDecomposition == 0) {
148  computeSymbolicDecomposition(A);
149  assert(_symbolicDecomposition && "Symbolic cholesky failed");
150  }
151  // re-allocate the temporary workspace for cholesky
152  if (_csWorkspaceSize < _ccsA->n) {
153  _csWorkspaceSize = 2 * _ccsA->n;
154  delete[] _csWorkspace;
155  _csWorkspace = new double[_csWorkspaceSize];
156  delete[] _csIntWorkspace;
157  _csIntWorkspace = new int[2*_csWorkspaceSize];
158  }
159 
160  if (! blocks){
161  blocks=new double*[A.rows()];
162  double **block=blocks;
163  for (size_t i=0; i < A.rowBlockIndices().size(); ++i){
164  int dim = A.rowsOfBlock(i) * A.colsOfBlock(i);
165  *block = new double [dim];
166  block++;
167  }
168  }
169 
170  int ok = 1;
171  csn* numericCholesky = csparse_extension::cs_chol_workspace(_ccsA, _symbolicDecomposition, _csIntWorkspace, _csWorkspace);
172  if (numericCholesky) {
173  MarginalCovarianceCholesky mcc;
174  mcc.setCholeskyFactor(_ccsA->n, numericCholesky->L->p, numericCholesky->L->i, numericCholesky->L->x, _symbolicDecomposition->pinv);
175  mcc.computeCovariance(blocks, A.rowBlockIndices());
176  cs_nfree(numericCholesky);
177  } else {
178  ok = 0;
179  std::cerr << "inverse fail (numeric decomposition)" << std::endl;
180  }
181 
182  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
183  if (globalStats){
184  globalStats->choleskyNNZ = static_cast<size_t>(_symbolicDecomposition->lnz);
185  }
186 
187  return ok != 0;
188  }
g2o::LinearSolverCSparse::fillCSparse
void fillCSparse(const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
Definition: linear_solver_csparse.h:310
g2o::G2OBatchStatistics::globalStats
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
g2o::LinearSolverCSparse::computeSymbolicDecomposition
void computeSymbolicDecomposition(const SparseBlockMatrix< MatrixType > &A)
Definition: linear_solver_csparse.h:246
g2o::csparse_extension::cs_chol_workspace
csn * cs_chol_workspace(const cs *A, const css *S, int *cin, double *xin)
Definition: csparse_helper.cpp:88
template<typename MatrixType>
virtual bool g2o::LinearSolverCSparse< MatrixType >::solvePattern ( SparseBlockMatrix< MatrixXD > &  spinv,
const std::vector< std::pair< int, int > > &  blockIndices,
const SparseBlockMatrix< MatrixType > &  A 
)
inlinevirtual

Inverts the a block pattern of A in spinv

Returns
false if not defined.

Reimplemented from g2o::LinearSolver< MatrixType >.

Definition at line 190 of file linear_solver_csparse.h.

References g2o::G2OBatchStatistics::choleskyNNZ, g2o::MarginalCovarianceCholesky::computeCovariance(), g2o::csparse_extension::cs_chol_workspace(), g2o::G2OBatchStatistics::globalStats(), g2o::SparseBlockMatrix< MatrixType >::rowBlockIndices(), and g2o::MarginalCovarianceCholesky::setCholeskyFactor().

190  {
191  fillCSparse(A, _symbolicDecomposition != 0);
192  // perform symbolic cholesky once
193  if (_symbolicDecomposition == 0) {
194  computeSymbolicDecomposition(A);
195  assert(_symbolicDecomposition && "Symbolic cholesky failed");
196  }
197  // re-allocate the temporary workspace for cholesky
198  if (_csWorkspaceSize < _ccsA->n) {
199  _csWorkspaceSize = 2 * _ccsA->n;
200  delete[] _csWorkspace;
201  _csWorkspace = new double[_csWorkspaceSize];
202  delete[] _csIntWorkspace;
203  _csIntWorkspace = new int[2*_csWorkspaceSize];
204  }
205 
206 
207  int ok = 1;
208  csn* numericCholesky = csparse_extension::cs_chol_workspace(_ccsA, _symbolicDecomposition, _csIntWorkspace, _csWorkspace);
209  if (numericCholesky) {
210  MarginalCovarianceCholesky mcc;
211  mcc.setCholeskyFactor(_ccsA->n, numericCholesky->L->p, numericCholesky->L->i, numericCholesky->L->x, _symbolicDecomposition->pinv);
212  mcc.computeCovariance(spinv, A.rowBlockIndices(), blockIndices);
213  cs_nfree(numericCholesky);
214  } else {
215  ok = 0;
216  std::cerr << "inverse fail (numeric decomposition)" << std::endl;
217  }
218 
219  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
220  if (globalStats){
221  globalStats->choleskyNNZ = static_cast<size_t>(_symbolicDecomposition->lnz);
222  }
223 
224  return ok != 0;
225  }
g2o::LinearSolverCSparse::fillCSparse
void fillCSparse(const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
Definition: linear_solver_csparse.h:310
g2o::G2OBatchStatistics::globalStats
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
g2o::LinearSolverCSparse::computeSymbolicDecomposition
void computeSymbolicDecomposition(const SparseBlockMatrix< MatrixType > &A)
Definition: linear_solver_csparse.h:246
g2o::csparse_extension::cs_chol_workspace
csn * cs_chol_workspace(const cs *A, const css *S, int *cin, double *xin)
Definition: csparse_helper.cpp:88
template<typename MatrixType>
virtual bool g2o::LinearSolverCSparse< MatrixType >::writeDebug ( ) const
inlinevirtual

write a debug dump of the system matrix if it is not SPD in solve

Reimplemented from g2o::LinearSolver< MatrixType >.

Definition at line 232 of file linear_solver_csparse.h.

232 { return _writeDebug;}

Member Data Documentation

template<typename MatrixType>
bool g2o::LinearSolverCSparse< MatrixType >::_blockOrdering
protected

Definition at line 241 of file linear_solver_csparse.h.

template<typename MatrixType>
CSparseExt* g2o::LinearSolverCSparse< MatrixType >::_ccsA
protected

Definition at line 240 of file linear_solver_csparse.h.

template<typename MatrixType>
int* g2o::LinearSolverCSparse< MatrixType >::_csIntWorkspace
protected

Definition at line 239 of file linear_solver_csparse.h.

template<typename MatrixType>
double* g2o::LinearSolverCSparse< MatrixType >::_csWorkspace
protected

Definition at line 238 of file linear_solver_csparse.h.

template<typename MatrixType>
int g2o::LinearSolverCSparse< MatrixType >::_csWorkspaceSize
protected

Definition at line 237 of file linear_solver_csparse.h.

template<typename MatrixType>
MatrixStructure g2o::LinearSolverCSparse< MatrixType >::_matrixStructure
protected

Definition at line 242 of file linear_solver_csparse.h.

template<typename MatrixType>
VectorXI g2o::LinearSolverCSparse< MatrixType >::_scalarPermutation
protected

Definition at line 243 of file linear_solver_csparse.h.

template<typename MatrixType>
css* g2o::LinearSolverCSparse< MatrixType >::_symbolicDecomposition
protected

Definition at line 236 of file linear_solver_csparse.h.

template<typename MatrixType>
bool g2o::LinearSolverCSparse< MatrixType >::_writeDebug
protected

Definition at line 244 of file linear_solver_csparse.h.

The documentation for this class was generated from the following file:
Generated on Mon Mar 18 2019 20:14:49 for g2o by   doxygen 1.8.11