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

basic solver for Ax = b which has to reimplemented for different linear algebra libraries More...

#include <linear_solver_cholmod.h>

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

Public Member Functions

 LinearSolverCholmod ()
 
virtual ~LinearSolverCholmod ()
 
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)
 
virtual bool saveMatrix (const std::string &fileName)
 
- 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 fillCholmodExt (const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
 
- Protected Member Functions inherited from g2o::LinearSolverCCS< MatrixType >
void initMatrixStructure (const SparseBlockMatrix< MatrixType > &A)
 

Protected Attributes

cholmod_common _cholmodCommon
 
CholmodExt_cholmodSparse
 
cholmod_factor * _cholmodFactor
 
bool _blockOrdering
 
MatrixStructure _matrixStructure
 
VectorXI _scalarPermutation
 
VectorXI _blockPermutation
 
bool _writeDebug
 
- Protected Attributes inherited from g2o::LinearSolverCCS< MatrixType >
SparseBlockMatrixCCS< MatrixType > * _ccsMatrix
 

Detailed Description

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

basic solver for Ax = b which has to reimplemented for different linear algebra libraries

Definition at line 76 of file linear_solver_cholmod.h.

Constructor & Destructor Documentation

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

Definition at line 79 of file linear_solver_cholmod.h.

References g2o::CholmodExt::CholmodExt().

79  :
80  LinearSolverCCS<MatrixType>()
81  {
82  _writeDebug = true;
83  _blockOrdering = false;
84  _cholmodSparse = new CholmodExt();
85  _cholmodFactor = 0;
86  cholmod_start(&_cholmodCommon);
87 
88  // setup ordering strategy
89  _cholmodCommon.nmethods = 1 ;
90  _cholmodCommon.method[0].ordering = CHOLMOD_AMD; //CHOLMOD_COLAMD
91  //_cholmodCommon.postorder = 0;
92 
93  _cholmodCommon.supernodal = CHOLMOD_AUTO; //CHOLMOD_SUPERNODAL; //CHOLMOD_SIMPLICIAL;
94  }
template<typename MatrixType>
virtual g2o::LinearSolverCholmod< MatrixType >::~LinearSolverCholmod ( )
inlinevirtual

Definition at line 96 of file linear_solver_cholmod.h.

97  {
98  delete _cholmodSparse;
99  if (_cholmodFactor != 0) {
100  cholmod_free_factor(&_cholmodFactor, &_cholmodCommon);
101  _cholmodFactor = 0;
102  }
103  cholmod_finish(&_cholmodCommon);
104  }

Member Function Documentation

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

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

Definition at line 249 of file linear_solver_cholmod.h.

template<typename MatrixType>
void g2o::LinearSolverCholmod< MatrixType >::computeSymbolicDecomposition ( const SparseBlockMatrix< MatrixType > &  A)
inlineprotected

Definition at line 271 of file linear_solver_cholmod.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, and g2o::MatrixStructure::nzMax().

272  {
273  double t = get_monotonic_time();
274  if (! _blockOrdering) {
275  // setup ordering strategy
276  _cholmodCommon.nmethods = 1;
277  _cholmodCommon.method[0].ordering = CHOLMOD_AMD; //CHOLMOD_COLAMD
278  _cholmodFactor = cholmod_analyze(_cholmodSparse, &_cholmodCommon); // symbolic factorization
279  } else {
280 
281  A.fillBlockStructure(_matrixStructure);
282 
283  // get the ordering for the block matrix
284  if (_blockPermutation.size() == 0)
286  if (_blockPermutation.size() < _matrixStructure.n) // double space if resizing
288 
289  // prepare AMD call via CHOLMOD
290  cholmod_sparse auxCholmodSparse;
291  auxCholmodSparse.nzmax = _matrixStructure.nzMax();
292  auxCholmodSparse.nrow = auxCholmodSparse.ncol = _matrixStructure.n;
293  auxCholmodSparse.p = _matrixStructure.Ap;
294  auxCholmodSparse.i = _matrixStructure.Aii;
295  auxCholmodSparse.nz = 0;
296  auxCholmodSparse.x = 0;
297  auxCholmodSparse.z = 0;
298  auxCholmodSparse.stype = 1;
299  auxCholmodSparse.xtype = CHOLMOD_PATTERN;
300  auxCholmodSparse.itype = CHOLMOD_INT;
301  auxCholmodSparse.dtype = CHOLMOD_DOUBLE;
302  auxCholmodSparse.sorted = 1;
303  auxCholmodSparse.packed = 1;
304  int amdStatus = cholmod_amd(&auxCholmodSparse, NULL, 0, _blockPermutation.data(), &_cholmodCommon);
305  if (! amdStatus) {
306  return;
307  }
308 
309  // blow up the permutation to the scalar matrix
310  if (_scalarPermutation.size() == 0)
311  _scalarPermutation.resize(_cholmodSparse->ncol);
312  if (_scalarPermutation.size() < (int)_cholmodSparse->ncol)
313  _scalarPermutation.resize(2*_cholmodSparse->ncol);
314  size_t scalarIdx = 0;
315  for (int i = 0; i < _matrixStructure.n; ++i) {
316  const int& p = _blockPermutation(i);
317  int base = A.colBaseOfBlock(p);
318  int nCols = A.colsOfBlock(p);
319  for (int j = 0; j < nCols; ++j)
320  _scalarPermutation(scalarIdx++) = base++;
321  }
322  assert(scalarIdx == _cholmodSparse->ncol);
323 
324  // apply the ordering
325  _cholmodCommon.nmethods = 1 ;
326  _cholmodCommon.method[0].ordering = CHOLMOD_GIVEN;
327  _cholmodFactor = cholmod_analyze_p(_cholmodSparse, _scalarPermutation.data(), NULL, 0, &_cholmodCommon);
328 
329  }
330  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
331  if (globalStats)
332  globalStats->timeSymbolicDecomposition = get_monotonic_time() - t;
333 
334  //const int& bestIdx = _cholmodCommon.selected;
335  //cerr << "# Number of nonzeros in L: " << (int)_cholmodCommon.method[bestIdx].lnz << " by "
336  //<< (_cholmodCommon.method[bestIdx].ordering == CHOLMOD_GIVEN ? "block" : "scalar") << " AMD ordering " << endl;
337 
338  }
double get_monotonic_time()
Definition: timeutil.cpp:113
int * Ap
column pointers for A, of size n+1
int n
A is m-by-n. n must be >= 0.
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
int * Aii
row indices of A, of size nz = Ap [n]
int nzMax() const
max number of non-zeros blocks
template<typename MatrixType>
void g2o::LinearSolverCholmod< MatrixType >::fillCholmodExt ( const SparseBlockMatrix< MatrixType > &  A,
bool  onlyValues 
)
inlineprotected

Definition at line 340 of file linear_solver_cholmod.h.

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

341  {
342  if (! onlyValues)
343  this->initMatrixStructure(A);
344  size_t m = A.rows();
345  size_t n = A.cols();
346  assert(m > 0 && n > 0 && "Hessian has 0 rows/cols");
347 
348  if (_cholmodSparse->columnsAllocated < n) {
349  //std::cerr << __PRETTY_FUNCTION__ << ": reallocating columns" << std::endl;
350  _cholmodSparse->columnsAllocated = _cholmodSparse->columnsAllocated == 0 ? n : 2 * n; // pre-allocate more space if re-allocating
351  delete[] (int*)_cholmodSparse->p;
353  }
354  if (! onlyValues) {
355  size_t nzmax = A.nonZeros();
356  if (_cholmodSparse->nzmax < nzmax) {
357  //std::cerr << __PRETTY_FUNCTION__ << ": reallocating row + values" << std::endl;
358  _cholmodSparse->nzmax = _cholmodSparse->nzmax == 0 ? nzmax : 2 * nzmax; // pre-allocate more space if re-allocating
359  delete[] (double*)_cholmodSparse->x;
360  delete[] (int*)_cholmodSparse->i;
361  _cholmodSparse->i = new int[_cholmodSparse->nzmax];
362  _cholmodSparse->x = new double[_cholmodSparse->nzmax];
363  }
364  }
365  _cholmodSparse->ncol = n;
366  _cholmodSparse->nrow = m;
367 
368  if (onlyValues)
369  this->_ccsMatrix->fillCCS((double*)_cholmodSparse->x, true);
370  else
371  this->_ccsMatrix->fillCCS((int*)_cholmodSparse->p, (int*)_cholmodSparse->i, (double*)_cholmodSparse->x, true);
372  }
SparseBlockMatrixCCS< MatrixType > * _ccsMatrix
Definition: linear_solver.h:97
void initMatrixStructure(const SparseBlockMatrix< MatrixType > &A)
Definition: linear_solver.h:99
template<typename MatrixType>
virtual bool g2o::LinearSolverCholmod< MatrixType >::init ( )
inlinevirtual

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

Implements g2o::LinearSolver< MatrixType >.

Definition at line 106 of file linear_solver_cholmod.h.

107  {
108  if (_cholmodFactor != 0) {
109  cholmod_free_factor(&_cholmodFactor, &_cholmodCommon);
110  _cholmodFactor = 0;
111  }
112  return true;
113  }
template<typename MatrixType>
virtual bool g2o::LinearSolverCholmod< MatrixType >::saveMatrix ( const std::string &  fileName)
inlinevirtual

Definition at line 256 of file linear_solver_cholmod.h.

References g2o::writeCCSMatrix().

256  {
257  writeCCSMatrix(fileName, _cholmodSparse->nrow, _cholmodSparse->ncol, (int*)_cholmodSparse->p, (int*)_cholmodSparse->i, (double*)_cholmodSparse->x, true);
258  return true;
259  }
bool writeCCSMatrix(const string &filename, int rows, int cols, const int *Ap, const int *Ai, const double *Ax, bool upperTriangleSymmetric)
template<typename MatrixType>
void g2o::LinearSolverCholmod< MatrixType >::setBlockOrdering ( bool  blockOrdering)
inline

Definition at line 250 of file linear_solver_cholmod.h.

bool blockOrdering() const
do the AMD ordering on the blocks or on the scalar matrix
template<typename MatrixType>
virtual void g2o::LinearSolverCholmod< MatrixType >::setWriteDebug ( bool  b)
inlinevirtual

Reimplemented from g2o::LinearSolver< MatrixType >.

Definition at line 254 of file linear_solver_cholmod.h.

template<typename MatrixType>
bool g2o::LinearSolverCholmod< 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 115 of file linear_solver_cholmod.h.

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

116  {
117  //cerr << __PRETTY_FUNCTION__ << " using cholmod" << endl;
118  fillCholmodExt(A, _cholmodFactor != 0); // _cholmodFactor used as bool, if not existing will copy the whole structure, otherwise only the values
119 
120  if (_cholmodFactor == 0) {
122  assert(_cholmodFactor != 0 && "Symbolic cholesky failed");
123  }
124  double t=get_monotonic_time();
125 
126  // setting up b for calling cholmod
127  cholmod_dense bcholmod;
128  bcholmod.nrow = bcholmod.d = _cholmodSparse->nrow;
129  bcholmod.ncol = 1;
130  bcholmod.x = b;
131  bcholmod.xtype = CHOLMOD_REAL;
132  bcholmod.dtype = CHOLMOD_DOUBLE;
133 
134  cholmod_factorize(_cholmodSparse, _cholmodFactor, &_cholmodCommon);
135  if (_cholmodCommon.status == CHOLMOD_NOT_POSDEF) {
136  if (_writeDebug) {
137  std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl;
138  saveMatrix("debug.txt");
139  }
140  return false;
141  }
142 
143  cholmod_dense* xcholmod = cholmod_solve(CHOLMOD_A, _cholmodFactor, &bcholmod, &_cholmodCommon);
144  memcpy(x, xcholmod->x, sizeof(double) * bcholmod.nrow); // copy back to our array
145  cholmod_free_dense(&xcholmod, &_cholmodCommon);
146 
147  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
148  if (globalStats){
149  globalStats->timeNumericDecomposition = get_monotonic_time() - t;
150  globalStats->choleskyNNZ = static_cast<size_t>(_cholmodCommon.method[0].lnz);
151  }
152 
153  return true;
154  }
double get_monotonic_time()
Definition: timeutil.cpp:113
void computeSymbolicDecomposition(const SparseBlockMatrix< MatrixType > &A)
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
virtual bool saveMatrix(const std::string &fileName)
void fillCholmodExt(const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
template<typename MatrixType>
bool g2o::LinearSolverCholmod< 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 156 of file linear_solver_cholmod.h.

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

157  {
158  //cerr << __PRETTY_FUNCTION__ << " using cholmod" << endl;
159  fillCholmodExt(A, _cholmodFactor != 0); // _cholmodFactor used as bool, if not existing will copy the whole structure, otherwise only the values
160 
161  if (_cholmodFactor == 0) {
163  assert(_cholmodFactor && "Symbolic cholesky failed");
164  }
165 
166  if (! blocks){
167  blocks=new double*[A.rows()];
168  double **block=blocks;
169  for (size_t i = 0; i < A.rowBlockIndices().size(); ++i){
170  int dim=A.rowsOfBlock(i)*A.colsOfBlock(i);
171  *block = new double [dim];
172  block++;
173  }
174  }
175 
176  cholmod_factorize(_cholmodSparse, _cholmodFactor, &_cholmodCommon);
177  if (_cholmodCommon.status == CHOLMOD_NOT_POSDEF)
178  return false;
179 
180  // convert the factorization to LL, simplical, packed, monotonic
181  int change_status = cholmod_change_factor(CHOLMOD_REAL, 1, 0, 1, 1, _cholmodFactor, &_cholmodCommon);
182  if (! change_status) {
183  return false;
184  }
185  assert(_cholmodFactor->is_ll && !_cholmodFactor->is_super && _cholmodFactor->is_monotonic && "Cholesky factor has wrong format");
186 
187  // invert the permutation
188  int* p = (int*)_cholmodFactor->Perm;
189  VectorXI pinv; pinv.resize(_cholmodSparse->ncol);
190  for (size_t i = 0; i < _cholmodSparse->ncol; ++i)
191  pinv(p[i]) = i;
192 
193  // compute the marginal covariance
194  MarginalCovarianceCholesky mcc;
195  mcc.setCholeskyFactor(_cholmodSparse->ncol, (int*)_cholmodFactor->p, (int*)_cholmodFactor->i,
196  (double*)_cholmodFactor->x, pinv.data());
197  mcc.computeCovariance(blocks, A.rowBlockIndices());
198 
199  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
200  if (globalStats) {
201  globalStats->choleskyNNZ = static_cast<size_t>(_cholmodCommon.method[_cholmodCommon.selected].lnz);
202  }
203 
204  return true;
205  }
void computeSymbolicDecomposition(const SparseBlockMatrix< MatrixType > &A)
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
void fillCholmodExt(const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
Eigen::Matrix< int, Eigen::Dynamic, 1, Eigen::ColMajor > VectorXI
Definition: eigen_types.h:38
template<typename MatrixType>
virtual bool g2o::LinearSolverCholmod< 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 207 of file linear_solver_cholmod.h.

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

208  {
209  //cerr << __PRETTY_FUNCTION__ << " using cholmod" << endl;
210  fillCholmodExt(A, _cholmodFactor != 0); // _cholmodFactor used as bool, if not existing will copy the whole structure, otherwise only the values
211 
212  if (_cholmodFactor == 0) {
214  assert(_cholmodFactor && "Symbolic cholesky failed");
215  }
216 
217  cholmod_factorize(_cholmodSparse, _cholmodFactor, &_cholmodCommon);
218  if (_cholmodCommon.status == CHOLMOD_NOT_POSDEF)
219  return false;
220 
221  // convert the factorization to LL, simplical, packed, monotonic
222  int change_status = cholmod_change_factor(CHOLMOD_REAL, 1, 0, 1, 1, _cholmodFactor, &_cholmodCommon);
223  if (! change_status) {
224  return false;
225  }
226  assert(_cholmodFactor->is_ll && !_cholmodFactor->is_super && _cholmodFactor->is_monotonic && "Cholesky factor has wrong format");
227 
228  // invert the permutation
229  int* p = (int*)_cholmodFactor->Perm;
230  VectorXI pinv; pinv.resize(_cholmodSparse->ncol);
231  for (size_t i = 0; i < _cholmodSparse->ncol; ++i)
232  pinv(p[i]) = i;
233 
234  // compute the marginal covariance
235  MarginalCovarianceCholesky mcc;
236  mcc.setCholeskyFactor(_cholmodSparse->ncol, (int*)_cholmodFactor->p, (int*)_cholmodFactor->i,
237  (double*)_cholmodFactor->x, pinv.data());
238  mcc.computeCovariance(spinv, A.rowBlockIndices(), blockIndices);
239 
240  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
241  if (globalStats) {
242  globalStats->choleskyNNZ = static_cast<size_t>(_cholmodCommon.method[_cholmodCommon.selected].lnz);
243  }
244 
245  return true;
246  }
void computeSymbolicDecomposition(const SparseBlockMatrix< MatrixType > &A)
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
void fillCholmodExt(const SparseBlockMatrix< MatrixType > &A, bool onlyValues)
Eigen::Matrix< int, Eigen::Dynamic, 1, Eigen::ColMajor > VectorXI
Definition: eigen_types.h:38
template<typename MatrixType>
virtual bool g2o::LinearSolverCholmod< 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 253 of file linear_solver_cholmod.h.

Member Data Documentation

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

Definition at line 266 of file linear_solver_cholmod.h.

template<typename MatrixType>
VectorXI g2o::LinearSolverCholmod< MatrixType >::_blockPermutation
protected

Definition at line 268 of file linear_solver_cholmod.h.

template<typename MatrixType>
cholmod_common g2o::LinearSolverCholmod< MatrixType >::_cholmodCommon
protected

Definition at line 263 of file linear_solver_cholmod.h.

template<typename MatrixType>
cholmod_factor* g2o::LinearSolverCholmod< MatrixType >::_cholmodFactor
protected

Definition at line 265 of file linear_solver_cholmod.h.

template<typename MatrixType>
CholmodExt* g2o::LinearSolverCholmod< MatrixType >::_cholmodSparse
protected

Definition at line 264 of file linear_solver_cholmod.h.

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

Definition at line 267 of file linear_solver_cholmod.h.

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

Definition at line 268 of file linear_solver_cholmod.h.

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

Definition at line 269 of file linear_solver_cholmod.h.


The documentation for this class was generated from the following file: