linear solver using PCG, pre-conditioner is block Jacobi More...

#include <linear_solver_pcg.h>

Inheritance diagram for g2o::LinearSolverPCG< MatrixType >:

Collaboration diagram for g2o::LinearSolverPCG< MatrixType >:

Public Member Functions
	LinearSolverPCG ()

virtual	~LinearSolverPCG ()

virtual bool	init ()

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

double	tolerance () const
	return the tolerance for terminating PCG before convergence

void	setTolerance (double tolerance)

int	maxIterations () const

void	setMaxIterations (int maxIter)

bool	absoluteTolerance () const

void	setAbsoluteTolerance (bool absoluteTolerance)

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)

Protected Types
using	MatrixVector = std::vector< MatrixType >

using	MatrixPtrVector = std::vector< const MatrixType * >

Protected Member Functions
void	multDiag (const std::vector< int > &colBlockIndices, MatrixVector &A, const VectorX &src, VectorX &dest)

void	multDiag (const std::vector< int > &colBlockIndices, MatrixPtrVector &A, const VectorX &src, VectorX &dest)

void	mult (const std::vector< int > &colBlockIndices, const VectorX &src, VectorX &dest)

Protected Attributes
double	_tolerance

double	_residual

bool	_absoluteTolerance

int	_maxIter

MatrixPtrVector	_diag

MatrixVector	_J

std::vector< std::pair< int, int > >	_indices

MatrixPtrVector	_sparseMat

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)

Detailed Description

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

linear solver using PCG, pre-conditioner is block Jacobi

Definition at line 43 of file linear_solver_pcg.h.

Member Typedef Documentation

◆ MatrixPtrVector

template<typename MatrixType >

using g2o::LinearSolverPCG< MatrixType >::MatrixPtrVector = std::vector<const MatrixType*>

protected

Definition at line 77 of file linear_solver_pcg.h.

◆ MatrixVector

template<typename MatrixType >

using g2o::LinearSolverPCG< MatrixType >::MatrixVector = std::vector<MatrixType>

protected

Definition at line 76 of file linear_solver_pcg.h.

Constructor & Destructor Documentation

◆ LinearSolverPCG()

template<typename MatrixType >

g2o::LinearSolverPCG< MatrixType >::LinearSolverPCG ( )

inline

Definition at line 45 of file linear_solver_pcg.h.

                    : LinearSolver<MatrixType>() {
    _tolerance = cst(1e-6);
    _absoluteTolerance = true;
    _residual = -1.0;
    _maxIter = -1;
  }

References g2o::LinearSolverPCG< MatrixType >::_absoluteTolerance, g2o::LinearSolverPCG< MatrixType >::_maxIter, g2o::LinearSolverPCG< MatrixType >::_residual, g2o::LinearSolverPCG< MatrixType >::_tolerance, and g2o::cst().

◆ ~LinearSolverPCG()

template<typename MatrixType >

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

inlinevirtual

Definition at line 52 of file linear_solver_pcg.h.

52{}

Member Function Documentation

◆ absoluteTolerance()

template<typename MatrixType >

bool g2o::LinearSolverPCG< MatrixType >::absoluteTolerance ( ) const

inline

Definition at line 70 of file linear_solver_pcg.h.

70{ return _absoluteTolerance; }

References g2o::LinearSolverPCG< MatrixType >::_absoluteTolerance.

Referenced by g2o::LinearSolverPCG< MatrixType >::setAbsoluteTolerance().

◆ init()

template<typename MatrixType >

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

inlinevirtual

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

Implements g2o::LinearSolver< MatrixType >.

Definition at line 54 of file linear_solver_pcg.h.

                      {
    _residual = -1.0;
    _indices.clear();
    _sparseMat.clear();
    return true;
  }

References g2o::LinearSolverPCG< MatrixType >::_indices, g2o::LinearSolverPCG< MatrixType >::_residual, and g2o::LinearSolverPCG< MatrixType >::_sparseMat.

◆ maxIterations()

template<typename MatrixType >

int g2o::LinearSolverPCG< MatrixType >::maxIterations ( ) const

inline

Definition at line 67 of file linear_solver_pcg.h.

67{ return _maxIter; }

References g2o::LinearSolverPCG< MatrixType >::_maxIter.

◆ mult()

template<typename MatrixType >

void LinearSolverPCG::mult	(	const std::vector< int > &	colBlockIndices,
		const VectorX &	src,
		VectorX &	dest
	)

protected

Definition at line 186 of file linear_solver_pcg.hpp.

                                                                          {
  // first multiply with the diagonal
  multDiag(colBlockIndices, _diag, src, dest);
 
  // now multiply with the upper triangular block
  for (size_t i = 0; i < _sparseMat.size(); ++i) {
    const int& srcOffset = _indices[i].second;
    const int& destOffsetT = srcOffset;
    const int& destOffset = _indices[i].first;
    const int& srcOffsetT = destOffset;
 
    const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a =
        _sparseMat[i];
    // destVec += *a * srcVec (according to the sub-vector parts)
    internal::pcg_axpy(*a, src, srcOffset, dest, destOffset);
    // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
    internal::pcg_atxpy(*a, src, srcOffsetT, dest, destOffsetT);
  }
}

References internal::pcg_atxpy(), and internal::pcg_axpy().

◆ multDiag() [1/2]

template<typename MatrixType >

void LinearSolverPCG::multDiag	(	const std::vector< int > &	colBlockIndices,
		MatrixPtrVector &	A,
		const VectorX &	src,
		VectorX &	dest
	)

protected

Definition at line 175 of file linear_solver_pcg.hpp.

                                       {
  int row = 0;
  for (size_t i = 0; i < A.size(); ++i) {
    internal::pcg_axy(*A[i], src, row, dest, row);
    row = colBlockIndices[i];
  }
}

References internal::pcg_axy().

◆ multDiag() [2/2]

template<typename MatrixType >

void LinearSolverPCG::multDiag	(	const std::vector< int > &	colBlockIndices,
		MatrixVector &	A,
		const VectorX &	src,
		VectorX &	dest
	)

protected

Definition at line 164 of file linear_solver_pcg.hpp.

                                       {
  int row = 0;
  for (size_t i = 0; i < A.size(); ++i) {
    internal::pcg_axy(A[i], src, row, dest, row);
    row = colBlockIndices[i];
  }
}

References internal::pcg_axy().

◆ setAbsoluteTolerance()

template<typename MatrixType >

void g2o::LinearSolverPCG< MatrixType >::setAbsoluteTolerance ( bool absoluteTolerance )

inline

Definition at line 71 of file linear_solver_pcg.h.

                                                    {
    _absoluteTolerance = absoluteTolerance;
  }

References g2o::LinearSolverPCG< MatrixType >::_absoluteTolerance, and g2o::LinearSolverPCG< MatrixType >::absoluteTolerance().

◆ setMaxIterations()

template<typename MatrixType >

void g2o::LinearSolverPCG< MatrixType >::setMaxIterations ( int maxIter )

inline

Definition at line 68 of file linear_solver_pcg.h.

68{ _maxIter = maxIter; }

References g2o::LinearSolverPCG< MatrixType >::_maxIter.

◆ setTolerance()

template<typename MatrixType >

void g2o::LinearSolverPCG< MatrixType >::setTolerance ( double tolerance )

inline

Definition at line 65 of file linear_solver_pcg.h.

65{ _tolerance = tolerance; }

g2o::LinearSolverPCG::tolerance

double tolerance() const

return the tolerance for terminating PCG before convergence

Definition linear_solver_pcg.h:64

References g2o::LinearSolverPCG< MatrixType >::_tolerance, and g2o::LinearSolverPCG< MatrixType >::tolerance().

◆ solve()

template<typename MatrixType >

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

virtual

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 88 of file linear_solver_pcg.hpp.

                                                              {
  const bool indexRequired = _indices.size() == 0;
  _diag.clear();
  _J.clear();
 
  // put the block matrix once in a linear structure, makes mult faster
  int colIdx = 0;
  for (size_t i = 0; i < A.blockCols().size(); ++i) {
    const typename SparseBlockMatrix<MatrixType>::IntBlockMap& col =
        A.blockCols()[i];
    if (col.size() > 0) {
      typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it;
      for (it = col.begin(); it != col.end(); ++it) {
        if (it->first == (int)i) {  // only the upper triangular block is needed
          _diag.push_back(it->second);
          _J.push_back(it->second->inverse());
          break;
        }
        if (indexRequired) {
          _indices.push_back(std::make_pair(
              it->first > 0 ? A.rowBlockIndices()[it->first - 1] : 0, colIdx));
          _sparseMat.push_back(it->second);
        }
      }
    }
    colIdx = A.colBlockIndices()[i];
  }
 
  int n = A.rows();
  assert(n > 0 && "Hessian has 0 rows/cols");
  Eigen::Map<VectorX> xvec(x, A.cols());
  const Eigen::Map<VectorX> bvec(b, n);
  xvec.setZero();
 
  VectorX r, d, q, s;
  d.setZero(n);
  q.setZero(n);
  s.setZero(n);
 
  r = bvec;
  multDiag(A.colBlockIndices(), _J, r, d);
  double dn = r.dot(d);
  double d0 = _tolerance * dn;
 
  if (_absoluteTolerance) {
    if (_residual > 0.0 && _residual > d0) d0 = _residual;
  }
 
  int maxIter = _maxIter < 0 ? A.rows() : _maxIter;
 
  int iteration;
  for (iteration = 0; iteration < maxIter; ++iteration) {
    G2O_DEBUG("residual [{}]: {}", iteration, dn);
    if (dn <= d0) break;  // done
    mult(A.colBlockIndices(), d, q);
    double a = dn / d.dot(q);
    xvec += a * d;
    // TODO: reset residual here every 50 iterations
    r -= a * q;
    multDiag(A.colBlockIndices(), _J, r, s);
    double dold = dn;
    dn = r.dot(s);
    double ba = dn / dold;
    d = s + ba * d;
  }
  _residual = 0.5 * dn;
  G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
  if (globalStats) {
    globalStats->iterationsLinearSolver = iteration;
  }
 
  return true;
}

References g2o::SparseBlockMatrix< MatrixType >::blockCols(), g2o::SparseBlockMatrix< MatrixType >::colBlockIndices(), g2o::SparseBlockMatrix< MatrixType >::cols(), G2O_DEBUG, g2o::G2OBatchStatistics::iterationsLinearSolver, g2o::SparseBlockMatrix< MatrixType >::rowBlockIndices(), and g2o::SparseBlockMatrix< MatrixType >::rows().

◆ tolerance()

template<typename MatrixType >

double g2o::LinearSolverPCG< MatrixType >::tolerance ( ) const

inline

return the tolerance for terminating PCG before convergence

Definition at line 64 of file linear_solver_pcg.h.

64{ return _tolerance; }

References g2o::LinearSolverPCG< MatrixType >::_tolerance.

Referenced by g2o::LinearSolverPCG< MatrixType >::setTolerance().

Member Data Documentation

◆ _absoluteTolerance

template<typename MatrixType >

bool g2o::LinearSolverPCG< MatrixType >::_absoluteTolerance

protected

Definition at line 81 of file linear_solver_pcg.h.

Referenced by g2o::LinearSolverPCG< MatrixType >::absoluteTolerance(), g2o::LinearSolverPCG< MatrixType >::LinearSolverPCG(), and g2o::LinearSolverPCG< MatrixType >::setAbsoluteTolerance().

◆ _diag

template<typename MatrixType >

MatrixPtrVector g2o::LinearSolverPCG< MatrixType >::_diag

protected

Definition at line 84 of file linear_solver_pcg.h.

◆ _indices

template<typename MatrixType >

std::vector<std::pair<int, int> > g2o::LinearSolverPCG< MatrixType >::_indices

protected

Definition at line 87 of file linear_solver_pcg.h.

Referenced by g2o::LinearSolverPCG< MatrixType >::init().

◆ _J

template<typename MatrixType >

MatrixVector g2o::LinearSolverPCG< MatrixType >::_J

protected

Definition at line 85 of file linear_solver_pcg.h.

◆ _maxIter

template<typename MatrixType >

int g2o::LinearSolverPCG< MatrixType >::_maxIter

protected

Definition at line 82 of file linear_solver_pcg.h.

Referenced by g2o::LinearSolverPCG< MatrixType >::LinearSolverPCG(), g2o::LinearSolverPCG< MatrixType >::maxIterations(), and g2o::LinearSolverPCG< MatrixType >::setMaxIterations().

◆ _residual

template<typename MatrixType >

double g2o::LinearSolverPCG< MatrixType >::_residual

protected

Definition at line 80 of file linear_solver_pcg.h.

Referenced by g2o::LinearSolverPCG< MatrixType >::init(), and g2o::LinearSolverPCG< MatrixType >::LinearSolverPCG().

◆ _sparseMat

template<typename MatrixType >

MatrixPtrVector g2o::LinearSolverPCG< MatrixType >::_sparseMat

protected

Definition at line 88 of file linear_solver_pcg.h.

Referenced by g2o::LinearSolverPCG< MatrixType >::init().

◆ _tolerance

template<typename MatrixType >

double g2o::LinearSolverPCG< MatrixType >::_tolerance

protected

Definition at line 79 of file linear_solver_pcg.h.

Referenced by g2o::LinearSolverPCG< MatrixType >::LinearSolverPCG(), g2o::LinearSolverPCG< MatrixType >::setTolerance(), and g2o::LinearSolverPCG< MatrixType >::tolerance().

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

/build/reproducible-path/g2o-0~20230806/g2o/solvers/pcg/linear_solver_pcg.h
/build/reproducible-path/g2o-0~20230806/g2o/solvers/pcg/linear_solver_pcg.hpp

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ MatrixPtrVector

◆ MatrixVector

Constructor & Destructor Documentation

◆ LinearSolverPCG()

◆ ~LinearSolverPCG()

Member Function Documentation

◆ absoluteTolerance()

◆ init()

◆ maxIterations()

◆ mult()

◆ multDiag() [1/2]

◆ multDiag() [2/2]

◆ setAbsoluteTolerance()

◆ setMaxIterations()

◆ setTolerance()

◆ solve()

◆ tolerance()

Member Data Documentation

◆ _absoluteTolerance

◆ _diag

◆ _indices

◆ _J

◆ _maxIter

◆ _residual

◆ _sparseMat

◆ _tolerance