linear solver which allows to update the cholesky factor More...

#include <linear_solver_cholmod_online.h>

Inheritance diagram for g2o::LinearSolverCholmodOnline< MatrixType >:

Collaboration diagram for g2o::LinearSolverCholmodOnline< MatrixType >:

[legend]

Public Member Functions
	LinearSolverCholmodOnline ()

virtual	~LinearSolverCholmodOnline ()

virtual bool	init ()

bool	solve (const SparseBlockMatrix< MatrixType > &A, double x, double b)

bool	blockOrdering () const

cholmod_factor *	L () const

size_t	nonZerosInL () const

int	choleskyUpdate (cholmod_sparse *update)

bool	solve (double x, double b)

void	computeSymbolicDecomposition (const SparseBlockMatrix< MatrixType > &A)

void	fillCholmodExt (const SparseBlockMatrix< MatrixType > &A, bool onlyValues)

Public Member Functions inherited from g2o::LinearSolver< MatrixType >
	LinearSolver ()

virtual	~LinearSolver ()

virtual 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)

virtual bool	writeDebug () const
	write a debug dump of the system matrix if it is not PSD in solve More...

virtual void	setWriteDebug (bool)

Public Member Functions inherited from g2o::LinearSolverCholmodOnlineInterface
	LinearSolverCholmodOnlineInterface ()

Protected Attributes
cholmod_common	_cholmodCommon

CholmodExt *	_cholmodSparse

cholmod_factor *	_cholmodFactor

bool	_blockOrdering

MatrixStructure	_matrixStructure

Eigen::VectorXi	_scalarPermutation

Eigen::VectorXi	_blockPermutation

Additional Inherited Members
Public Attributes inherited from g2o::LinearSolverCholmodOnlineInterface
Eigen::VectorXi *	cmember

int	batchEveryN

Detailed Description

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

linear solver which allows to update the cholesky factor

Definition at line 46 of file linear_solver_cholmod_online.h.

Constructor & Destructor Documentation

template<typename MatrixType>

g2o::LinearSolverCholmodOnline< MatrixType >::LinearSolverCholmodOnline ( )

inline

Definition at line 49 of file linear_solver_cholmod_online.h.

                                 :
       LinearSolver<MatrixType>()
     {
       _cholmodSparse = new CholmodExt();
       _cholmodFactor = 0;
       cholmod_start(&_cholmodCommon);
 
       // setup ordering strategy
       _cholmodCommon.nmethods = 1 ;
       _cholmodCommon.method[0].ordering = CHOLMOD_AMD; //CHOLMOD_COLAMD
       //_cholmodCommon.postorder = 0;
 
       _cholmodCommon.supernodal = CHOLMOD_AUTO; //CHOLMOD_SUPERNODAL; //CHOLMOD_SIMPLICIAL;
       batchEveryN = 100;
     }

template<typename MatrixType>

virtual g2o::LinearSolverCholmodOnline< MatrixType >::~LinearSolverCholmodOnline ( )

inlinevirtual

Definition at line 65 of file linear_solver_cholmod_online.h.

     {
       delete _cholmodSparse;
       if (_cholmodFactor) {
         cholmod_free_factor(&_cholmodFactor, &_cholmodCommon);
         _cholmodFactor = 0;
       }
       cholmod_finish(&_cholmodCommon);
     }

Member Function Documentation

template<typename MatrixType>

bool g2o::LinearSolverCholmodOnline< MatrixType >::blockOrdering ( ) const

inline

Definition at line 118 of file linear_solver_cholmod_online.h.

118 { return true;}

template<typename MatrixType>

int g2o::LinearSolverCholmodOnline< MatrixType >::choleskyUpdate ( cholmod_sparse * update )

inlinevirtual

Implements g2o::LinearSolverCholmodOnlineInterface.

Definition at line 135 of file linear_solver_cholmod_online.h.

References g2o::writeCCSMatrix().

     {
       int result = cholmod_updown(1, update, _cholmodFactor, &_cholmodCommon);
       //std::cerr << __PRETTY_FUNCTION__ << " " << result << std::endl;
       if (_cholmodCommon.status == CHOLMOD_NOT_POSDEF) {
         std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl;
         writeCCSMatrix("debug.txt", _cholmodSparse->nrow, _cholmodSparse->ncol, (int*)_cholmodSparse->p, (int*)_cholmodSparse->i, (double*)_cholmodSparse->x, true);
         return 0;
       }
       return result;
     }

template<typename MatrixType>

void g2o::LinearSolverCholmodOnline< MatrixType >::computeSymbolicDecomposition ( const SparseBlockMatrix< MatrixType > & A )

inline

Definition at line 173 of file linear_solver_cholmod_online.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(), and g2o::MatrixStructure::n.

     {
       double t = get_monotonic_time();
 
       A.fillBlockStructure(_matrixStructure);
 
       // get the ordering for the block matrix
       if (_blockPermutation.size() < _matrixStructure.n) // double space if resizing
         _blockPermutation.resize(2*_matrixStructure.n);
 
       int amdStatus = camd_order(_matrixStructure.n, _matrixStructure.Ap, _matrixStructure.Aii, _blockPermutation.data(), NULL, NULL, cmember->data());
       if (amdStatus != CAMD_OK) {
         std::cerr << "Error while computing ordering" << std::endl;
       }
 
       // blow up the permutation to the scalar matrix and extend to include the additional blocks
       if (_scalarPermutation.size() == 0)
         _scalarPermutation.resize(_cholmodSparse->ncol);
       if (_scalarPermutation.size() < (int)_cholmodSparse->ncol)
         _scalarPermutation.resize(2*_cholmodSparse->ncol);
       size_t scalarIdx = 0;
       for (int i = 0; i < _matrixStructure.n; ++i) {
         const int& p = _blockPermutation(i);
         int base  = A.colBaseOfBlock(p);
         int nCols = A.colsOfBlock(p);
         for (int j = 0; j < nCols; ++j)
           _scalarPermutation(scalarIdx++) = base++;
       }
       
       for (; scalarIdx < _cholmodSparse->ncol; ++scalarIdx) // extending for the additional blocks
         _scalarPermutation(scalarIdx) = scalarIdx;
       assert(scalarIdx == _cholmodSparse->ncol);
 
       // apply the ordering
       _cholmodCommon.nmethods = 1;
       _cholmodCommon.method[0].ordering = CHOLMOD_GIVEN;
       _cholmodFactor = cholmod_analyze_p(_cholmodSparse, _scalarPermutation.data(), NULL, 0, &_cholmodCommon);
 
       G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
       if (globalStats)
         globalStats->timeSymbolicDecomposition = get_monotonic_time() - t;
 
     }

template<typename MatrixType>

void g2o::LinearSolverCholmodOnline< MatrixType >::fillCholmodExt	(	const SparseBlockMatrix< MatrixType > &	A,
		bool	onlyValues
	)

inline

Definition at line 217 of file linear_solver_cholmod_online.h.

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

     {
       int blockDimension = MatrixType::RowsAtCompileTime;
       assert(blockDimension > 0);
       //size_t origM = A.rows();
       size_t origN = A.cols();
       int additionalSpace = blockDimension * batchEveryN;
       size_t m = A.rows() + additionalSpace;
       size_t n = A.cols() + additionalSpace;
 
       if (_cholmodSparse->columnsAllocated < n) {
         //std::cerr << __PRETTY_FUNCTION__ << ": reallocating columns" << std::endl;
         _cholmodSparse->columnsAllocated = _cholmodSparse->columnsAllocated == 0 ? n : 2 * n; // pre-allocate more space if re-allocating
         delete[] (int*)_cholmodSparse->p;
         _cholmodSparse->p = new int[_cholmodSparse->columnsAllocated+1];
       }
       if (! onlyValues) {
         size_t nzmax = A.nonZeros() + additionalSpace;
         if (_cholmodSparse->nzmax < nzmax) {
           //std::cerr << __PRETTY_FUNCTION__ << ": reallocating row + values" << std::endl;
           _cholmodSparse->nzmax = _cholmodSparse->nzmax == 0 ? nzmax : 2 * nzmax; // pre-allocate more space if re-allocating
           delete[] (double*)_cholmodSparse->x;
           delete[] (int*)_cholmodSparse->i;
           _cholmodSparse->i = new int[_cholmodSparse->nzmax];
           _cholmodSparse->x = new double[_cholmodSparse->nzmax];
         }
       }
       _cholmodSparse->ncol = n;
       _cholmodSparse->nrow = m;
 
       int nz = 0;
       if (onlyValues)
         nz = A.fillCCS((double*)_cholmodSparse->x, true);
       else
         nz = A.fillCCS((int*)_cholmodSparse->p, (int*)_cholmodSparse->i, (double*)_cholmodSparse->x, true);
 
       int* cp = (int*)_cholmodSparse->p;
       int* ci = (int*)_cholmodSparse->i;
       double* cx = (double*)_cholmodSparse->x;
 
       cp = &cp[origN];
       ci = &ci[nz];
       cx = &cx[nz];
 
       // diagonal for the next blocks
       for (int i = 0; i < additionalSpace; ++i) {
         *cp++ = nz;
         *ci++ = origN + i;
         *cx++ = 1e-6;
         ++nz;
       }
       *cp = nz;
 
     }

template<typename MatrixType>

virtual bool g2o::LinearSolverCholmodOnline< MatrixType >::init ( )

inlinevirtual

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

Implements g2o::LinearSolver< MatrixType >.

Definition at line 75 of file linear_solver_cholmod_online.h.

     {
       return true;
     }

template<typename MatrixType>

cholmod_factor* g2o::LinearSolverCholmodOnline< MatrixType >::L ( ) const

inlinevirtual

Implements g2o::LinearSolverCholmodOnlineInterface.

Definition at line 120 of file linear_solver_cholmod_online.h.

120 { return _cholmodFactor;}

g2o::LinearSolverCholmodOnline::_cholmodFactor

cholmod_factor * _cholmodFactor

Definition: linear_solver_cholmod_online.h:167

template<typename MatrixType>

size_t g2o::LinearSolverCholmodOnline< MatrixType >::nonZerosInL ( ) const

inlinevirtual

return the number of non-zeros in the current factorization

Implements g2o::LinearSolverCholmodOnlineInterface.

Definition at line 125 of file linear_solver_cholmod_online.h.

References if().

                                {
       size_t nnz= 0;
       int* nz = (int*)_cholmodFactor->nz;
       if (! nz)
         return 0;
       for (size_t i = 0; i < _cholmodFactor->n; ++i)
         nnz += nz[i];
       return nnz;
     }

template<typename MatrixType>

bool g2o::LinearSolverCholmodOnline< 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 80 of file linear_solver_cholmod_online.h.

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

     {
       cholmod_free_factor(&_cholmodFactor, &_cholmodCommon);
       _cholmodFactor = 0;
       fillCholmodExt(A, false);
 
       computeSymbolicDecomposition(A);
       assert(_cholmodFactor && "Symbolic cholesky failed");
       double t=get_monotonic_time();
 
       // setting up b for calling cholmod
       cholmod_dense bcholmod;
       bcholmod.nrow  = bcholmod.d = _cholmodSparse->nrow;
       bcholmod.ncol  = 1;
       bcholmod.x     = b;
       bcholmod.xtype = CHOLMOD_REAL;
       bcholmod.dtype = CHOLMOD_DOUBLE;
 
       cholmod_factorize(_cholmodSparse, _cholmodFactor, &_cholmodCommon);
       if (_cholmodCommon.status == CHOLMOD_NOT_POSDEF) {
         std::cerr << "solve(): Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl;
         writeCCSMatrix("debug.txt", _cholmodSparse->nrow, _cholmodSparse->ncol, (int*)_cholmodSparse->p, (int*)_cholmodSparse->i, (double*)_cholmodSparse->x, true);
         return false;
       }
 
       cholmod_dense* xcholmod = cholmod_solve(CHOLMOD_A, _cholmodFactor, &bcholmod, &_cholmodCommon);
       memcpy(x, xcholmod->x, sizeof(double) * bcholmod.nrow); // copy back to our array
       cholmod_free_dense(&xcholmod, &_cholmodCommon);
 
       G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
       if (globalStats){
         globalStats->timeNumericDecomposition = get_monotonic_time() - t;
         globalStats->choleskyNNZ = static_cast<size_t>(_cholmodCommon.method[0].lnz);
       }
 
       return true;
     }

template<typename MatrixType>

bool g2o::LinearSolverCholmodOnline< MatrixType >::solve	(	double *	x,
		double *	b
	)

inlinevirtual

Implements g2o::LinearSolverCholmodOnlineInterface.

Definition at line 147 of file linear_solver_cholmod_online.h.

     {
       // setting up b for calling cholmod
       cholmod_dense bcholmod;
       bcholmod.nrow  = bcholmod.d = _cholmodSparse->nrow;
       bcholmod.ncol  = 1;
       bcholmod.x     = b;
       bcholmod.xtype = CHOLMOD_REAL;
       bcholmod.dtype = CHOLMOD_DOUBLE;
 
       cholmod_dense* xcholmod = cholmod_solve(CHOLMOD_A, _cholmodFactor, &bcholmod, &_cholmodCommon);
       memcpy(x, xcholmod->x, sizeof(double) * bcholmod.nrow); // copy back to our array
       cholmod_free_dense(&xcholmod, &_cholmodCommon);
       return true;
     }