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

#include <linear_solver_cholmod_online.h>

Inheritance diagram for g2o::LinearSolverCholmodOnline< MatrixType >:

[legend]

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< MatrixX > &spinv, const std::vector< std::pair< int, int > > &blockIndices, const SparseBlockMatrix< MatrixType > &A)

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

void	setWriteDebug (bool b)

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

Protected Attributes
cholmod_common	_cholmodCommon

cholmod::CholmodExt *	_cholmodSparse

cholmod_factor *	_cholmodFactor

bool	_blockOrdering

MatrixStructure	_matrixStructure

Eigen::VectorXi	_scalarPermutation

Eigen::VectorXi	_blockPermutation

Protected Attributes inherited from g2o::LinearSolver< MatrixType >
bool	_writeDebug

Additional Inherited Members
Static Public Member Functions inherited from g2o::LinearSolver< MatrixType >
static void	allocateBlocks (const SparseBlockMatrix< MatrixType > &A, double **&blocks)
	allocate block memory structure

static void	deallocateBlocks (const SparseBlockMatrix< MatrixType > &A, double **&blocks)
	de-allocate the block structure

template<typename BlockDerived , typename ScalarDerived >
static void	blockToScalarPermutation (const SparseBlockMatrix< MatrixType > &A, const Eigen::MatrixBase< BlockDerived > &p, const Eigen::MatrixBase< ScalarDerived > &scalar)

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 51 of file linear_solver_cholmod_online.h.

Constructor & Destructor Documentation

◆ LinearSolverCholmodOnline()

template<typename MatrixType >

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

inline

Definition at line 54 of file linear_solver_cholmod_online.h.

                              : LinearSolver<MatrixType>() {
    _cholmodSparse = new cholmod::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;
  }

References g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodCommon, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodFactor, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodSparse, and g2o::LinearSolverCholmodOnlineInterface::batchEveryN.

◆ ~LinearSolverCholmodOnline()

template<typename MatrixType >

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

inlinevirtual

Definition at line 69 of file linear_solver_cholmod_online.h.

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

References g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodCommon, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodFactor, and g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodSparse.

Member Function Documentation

◆ blockOrdering()

template<typename MatrixType >

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

inline

Definition at line 124 of file linear_solver_cholmod_online.h.

124{ return true; }

◆ choleskyUpdate()

template<typename MatrixType >

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

inlinevirtual

Implements g2o::LinearSolverCholmodOnlineInterface.

Definition at line 139 of file linear_solver_cholmod_online.h.

                                             {
    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;
  }

References g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodCommon, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodFactor, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodSparse, and g2o::writeCCSMatrix().

◆ computeSymbolicDecomposition()

template<typename MatrixType >

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

inline

Definition at line 181 of file linear_solver_cholmod_online.h.

                                                                            {
    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;
  }

References g2o::LinearSolverCholmodOnline< MatrixType >::_blockPermutation, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodCommon, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodFactor, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodSparse, g2o::LinearSolverCholmodOnline< MatrixType >::_matrixStructure, g2o::LinearSolverCholmodOnline< MatrixType >::_scalarPermutation, g2o::MatrixStructure::Aii, g2o::MatrixStructure::Ap, g2o::LinearSolverCholmodOnlineInterface::cmember, g2o::SparseBlockMatrix< MatrixType >::colBaseOfBlock(), g2o::SparseBlockMatrix< MatrixType >::colsOfBlock(), g2o::SparseBlockMatrix< MatrixType >::fillBlockStructure(), g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::MatrixStructure::n, and g2o::G2OBatchStatistics::timeSymbolicDecomposition.

Referenced by g2o::LinearSolverCholmodOnline< MatrixType >::solve().

◆ fillCholmodExt()

template<typename MatrixType >

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

inline

Definition at line 228 of file linear_solver_cholmod_online.h.

                                                                               {
    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;
  }

References g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodSparse, g2o::LinearSolverCholmodOnlineInterface::batchEveryN, g2o::SparseBlockMatrix< MatrixType >::cols(), g2o::cholmod::CholmodExt::columnsAllocated, g2o::SparseBlockMatrix< MatrixType >::fillCCS(), g2o::SparseBlockMatrix< MatrixType >::nonZeros(), and g2o::SparseBlockMatrix< MatrixType >::rows().

Referenced by g2o::LinearSolverCholmodOnline< MatrixType >::solve().

◆ init()

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 78 of file linear_solver_cholmod_online.h.

78{ return true; }

◆ L()

template<typename MatrixType >

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

inlinevirtual

Implements g2o::LinearSolverCholmodOnlineInterface.

Definition at line 126 of file linear_solver_cholmod_online.h.

126{ return _cholmodFactor; }

References g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodFactor.

◆ nonZerosInL()

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 131 of file linear_solver_cholmod_online.h.

                             {
    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;
  }

References g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodFactor.

◆ solve() [1/2]

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.

                                                                           {
    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;
  }

References g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodCommon, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodFactor, g2o::LinearSolverCholmodOnline< MatrixType >::_cholmodSparse, g2o::G2OBatchStatistics::choleskyNNZ, g2o::LinearSolverCholmodOnline< MatrixType >::computeSymbolicDecomposition(), g2o::LinearSolverCholmodOnline< MatrixType >::fillCholmodExt(), g2o::get_monotonic_time(), g2o::G2OBatchStatistics::globalStats(), g2o::G2OBatchStatistics::timeNumericDecomposition, and g2o::writeCCSMatrix().

◆ solve() [2/2]

template<typename MatrixType >

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

inlinevirtual

Implements g2o::LinearSolverCholmodOnlineInterface.

Definition at line 154 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;
  }