g2o::MarginalCovarianceCholesky Class Reference

computing the marginal covariance given a cholesky factor (lower triangle of the factor) More...

#include <marginal_covariance_cholesky.h>

Public Member Functions
	MarginalCovarianceCholesky ()
	~MarginalCovarianceCholesky ()
void	computeCovariance (double **covBlocks, const std::vector< int > &blockIndices)
void	computeCovariance (SparseBlockMatrix< MatrixX > &spinv, const std::vector< int > &rowBlockIndices, const std::vector< std::pair< int, int > > &blockIndices)
void	setCholeskyFactor (int n, int *Lp, int *Li, double *Lx, int *permInv)

Protected Types
typedef std::unordered_map< int, double >	LookupMap

Protected Member Functions
int	computeIndex (int r, int c) const
	compute the index used for hashing
double	computeEntry (int r, int c)

Protected Attributes
int	_n
	L is an n X n matrix.
int *	_Ap
	column pointer of the CCS storage
int *	_Ai
	row indices of the CCS storage
double *	_Ax
	values of the cholesky factor
int *	_perm
LookupMap	_map
	hash look up table for the already computed entries
std::vector< double >	_diag
	cache 1 / H_ii to avoid recalculations

Detailed Description

computing the marginal covariance given a cholesky factor (lower triangle of the factor)

Definition at line 42 of file marginal_covariance_cholesky.h.

Member Typedef Documentation

LookupMap

typedef std::unordered_map<int, double> g2o::MarginalCovarianceCholesky::LookupMap

protected

hash struct for storing the matrix elements needed to compute the covariance

Definition at line 48 of file marginal_covariance_cholesky.h.

Constructor & Destructor Documentation

MarginalCovarianceCholesky()

g2o::MarginalCovarianceCholesky::MarginalCovarianceCholesky

(

)

Definition at line 46 of file marginal_covariance_cholesky.cpp.

    : _n(0), _Ap(0), _Ai(0), _Ax(0), _perm(0) {}

~MarginalCovarianceCholesky()

g2o::MarginalCovarianceCholesky::~MarginalCovarianceCholesky

(

)

Definition at line 49 of file marginal_covariance_cholesky.cpp.

{}

Member Function Documentation

computeCovariance() [1/2]

void g2o::MarginalCovarianceCholesky::computeCovariance	(	double **	covBlocks,
		const std::vector< int > &	blockIndices
	)

compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to be provided by the caller).

Definition at line 100 of file marginal_covariance_cholesky.cpp.

                                                            {
  _map.clear();
  int base = 0;
  vector<MatrixElem> elemsToCompute;
  for (size_t i = 0; i < blockIndices.size(); ++i) {
    int nbase = blockIndices[i];
    int vdim = nbase - base;
    for (int rr = 0; rr < vdim; ++rr)
      for (int cc = rr; cc < vdim; ++cc) {
        int r = _perm ? _perm[rr + base] : rr + base;  // apply permutation
        int c = _perm ? _perm[cc + base] : cc + base;
        if (r > c)  // make sure it's still upper triangular after applying the
                    // permutation
          swap(r, c);
        elemsToCompute.push_back(MatrixElem(r, c));
      }
    base = nbase;
  }
 
  // sort the elems to reduce the recursive calls
  sort(elemsToCompute.begin(), elemsToCompute.end());
 
  // compute the inverse elements we need
  for (size_t i = 0; i < elemsToCompute.size(); ++i) {
    const MatrixElem& me = elemsToCompute[i];
    computeEntry(me.r, me.c);
  }
 
  // set the marginal covariance for the vertices, by writing to the blocks
  // memory
  base = 0;
  for (size_t i = 0; i < blockIndices.size(); ++i) {
    int nbase = blockIndices[i];
    int vdim = nbase - base;
    double* cov = covBlocks[i];
    for (int rr = 0; rr < vdim; ++rr)
      for (int cc = rr; cc < vdim; ++cc) {
        int r = _perm ? _perm[rr + base] : rr + base;  // apply permutation
        int c = _perm ? _perm[cc + base] : cc + base;
        if (r > c)  // upper triangle
          swap(r, c);
        int idx = computeIndex(r, c);
        LookupMap::const_iterator foundIt = _map.find(idx);
        assert(foundIt != _map.end());
        cov[rr * vdim + cc] = foundIt->second;
        if (rr != cc) cov[cc * vdim + rr] = foundIt->second;
      }
    base = nbase;
  }
}

References _map, _perm, g2o::MatrixElem::c, computeEntry(), computeIndex(), and g2o::MatrixElem::r.

computeCovariance() [2/2]

void g2o::MarginalCovarianceCholesky::computeCovariance	(	SparseBlockMatrix< MatrixX > &	spinv,
		const std::vector< int > &	rowBlockIndices,
		const std::vector< std::pair< int, int > > &	blockIndices
	)

compute the marginal cov for the given block indices, write the result in spinv).

Definition at line 152 of file marginal_covariance_cholesky.cpp.

                                                       {
  // allocate the sparse
  spinv = SparseBlockMatrix<MatrixX>(&rowBlockIndices[0], &rowBlockIndices[0],
                                     rowBlockIndices.size(),
                                     rowBlockIndices.size(), true);
  _map.clear();
  vector<MatrixElem> elemsToCompute;
  for (size_t i = 0; i < blockIndices.size(); ++i) {
    int blockRow = blockIndices[i].first;
    int blockCol = blockIndices[i].second;
    assert(blockRow >= 0);
    assert(blockRow < (int)rowBlockIndices.size());
    assert(blockCol >= 0);
    assert(blockCol < (int)rowBlockIndices.size());
 
    int rowBase = spinv.rowBaseOfBlock(blockRow);
    int colBase = spinv.colBaseOfBlock(blockCol);
 
    MatrixX* block = spinv.block(blockRow, blockCol, true);
    assert(block);
    for (int iRow = 0; iRow < block->rows(); ++iRow)
      for (int iCol = 0; iCol < block->cols(); ++iCol) {
        int rr = rowBase + iRow;
        int cc = colBase + iCol;
        int r = _perm ? _perm[rr] : rr;  // apply permutation
        int c = _perm ? _perm[cc] : cc;
        if (r > c) swap(r, c);
        elemsToCompute.push_back(MatrixElem(r, c));
      }
  }
 
  // sort the elems to reduce the number of recursive calls
  sort(elemsToCompute.begin(), elemsToCompute.end());
 
  // compute the inverse elements we need
  for (size_t i = 0; i < elemsToCompute.size(); ++i) {
    const MatrixElem& me = elemsToCompute[i];
    computeEntry(me.r, me.c);
  }
 
  // set the marginal covariance
  for (size_t i = 0; i < blockIndices.size(); ++i) {
    int blockRow = blockIndices[i].first;
    int blockCol = blockIndices[i].second;
    int rowBase = spinv.rowBaseOfBlock(blockRow);
    int colBase = spinv.colBaseOfBlock(blockCol);
 
    MatrixX* block = spinv.block(blockRow, blockCol);
    assert(block);
    for (int iRow = 0; iRow < block->rows(); ++iRow)
      for (int iCol = 0; iCol < block->cols(); ++iCol) {
        int rr = rowBase + iRow;
        int cc = colBase + iCol;
        int r = _perm ? _perm[rr] : rr;  // apply permutation
        int c = _perm ? _perm[cc] : cc;
        if (r > c) swap(r, c);
        int idx = computeIndex(r, c);
        LookupMap::const_iterator foundIt = _map.find(idx);
        assert(foundIt != _map.end());
        (*block)(iRow, iCol) = foundIt->second;
      }
  }
}

References _map, _perm, g2o::SparseBlockMatrix< MatrixType >::block(), g2o::MatrixElem::c, g2o::SparseBlockMatrix< MatrixType >::colBaseOfBlock(), computeEntry(), computeIndex(), g2o::MatrixElem::r, and g2o::SparseBlockMatrix< MatrixType >::rowBaseOfBlock().

computeEntry()

double g2o::MarginalCovarianceCholesky::computeEntry ( int  r, int  c  )

protected

compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular. May issue recursive calls to itself to compute the missing values.

Definition at line 69 of file marginal_covariance_cholesky.cpp.

                                                            {
  assert(r <= c);
  int idx = computeIndex(r, c);
 
  LookupMap::const_iterator foundIt = _map.find(idx);
  if (foundIt != _map.end()) {
    return foundIt->second;
  }
 
  // compute the summation over column r
  double s = 0.;
  const int& sc = _Ap[r];
  const int& ec = _Ap[r + 1];
  for (int j = sc + 1; j < ec;
       ++j) {  // sum over row r while skipping the element on the diagonal
    const int& rr = _Ai[j];
    double val = rr < c ? computeEntry(rr, c) : computeEntry(c, rr);
    s += val * _Ax[j];
  }
 
  double result;
  if (r == c) {
    const double& diagElem = _diag[r];
    result = diagElem * (diagElem - s);
  } else {
    result = -s * _diag[r];
  }
  _map[idx] = result;
  return result;
}

References _Ai, _Ap, _Ax, _diag, _map, computeEntry(), and computeIndex().

Referenced by computeCovariance(), computeCovariance(), and computeEntry().

computeIndex()

int g2o::MarginalCovarianceCholesky::computeIndex ( int  r, int  c  ) const

inlineprotected

compute the index used for hashing

Definition at line 93 of file marginal_covariance_cholesky.h.

                                       { /*assert(r <= c);*/
    return r * _n + c;
  }

Referenced by computeCovariance(), computeCovariance(), and computeEntry().

setCholeskyFactor()

void g2o::MarginalCovarianceCholesky::setCholeskyFactor	(	int	n,
		int *	Lp,
		int *	Li,
		double *	Lx,
		int *	permInv
	)

set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in. permInv might be 0, will then not permute the entries.

The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers are owned by the caller, MarginalCovarianceCholesky does not free the pointers.

Definition at line 51 of file marginal_covariance_cholesky.cpp.

                                                                             {
  _n = n;
  _Ap = Lp;
  _Ai = Li;
  _Ax = Lx;
  _perm = permInv;
 
  // pre-compute reciprocal values of the diagonal of L
  _diag.resize(n);
  for (int r = 0; r < n; ++r) {
    const int& sc = _Ap[r];  // L is lower triangular, thus the first elem in
                             // the column is the diagonal entry
    assert(r == _Ai[sc] && "Error in CCS storage of L");
    _diag[r] = 1.0 / _Ax[sc];
  }
}

References _Ai, _Ap, _Ax, _diag, _n, and _perm.

Referenced by g2o::LinearSolverCholmod< MatrixType >::solveBlocks_impl(), g2o::LinearSolverCSparse< MatrixType >::solveBlocks_impl(), and g2o::LinearSolverEigen< MatrixType >::solveBlocks_impl().

Member Data Documentation

_Ai

int* g2o::MarginalCovarianceCholesky::_Ai

protected

row indices of the CCS storage

Definition at line 84 of file marginal_covariance_cholesky.h.

Referenced by computeEntry(), and setCholeskyFactor().

_Ap

int* g2o::MarginalCovarianceCholesky::_Ap

protected

column pointer of the CCS storage

Definition at line 83 of file marginal_covariance_cholesky.h.

Referenced by computeEntry(), and setCholeskyFactor().

_Ax

double* g2o::MarginalCovarianceCholesky::_Ax

protected

values of the cholesky factor

Definition at line 85 of file marginal_covariance_cholesky.h.

Referenced by computeEntry(), and setCholeskyFactor().

_diag

std::vector<double> g2o::MarginalCovarianceCholesky::_diag

protected

cache 1 / H_ii to avoid recalculations

Definition at line 90 of file marginal_covariance_cholesky.h.

Referenced by computeEntry(), and setCholeskyFactor().

_map

LookupMap g2o::MarginalCovarianceCholesky::_map

protected

hash look up table for the already computed entries

Definition at line 89 of file marginal_covariance_cholesky.h.

Referenced by computeCovariance(), computeCovariance(), and computeEntry().

_n

int g2o::MarginalCovarianceCholesky::_n

protected

L is an n X n matrix.

Definition at line 82 of file marginal_covariance_cholesky.h.

Referenced by setCholeskyFactor().

_perm

int* g2o::MarginalCovarianceCholesky::_perm

protected

permutation of the cholesky factor. Variable re-ordering for better fill-in

Definition at line 86 of file marginal_covariance_cholesky.h.

Referenced by computeCovariance(), computeCovariance(), and setCholeskyFactor().

---

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

• /build/reproducible-path/g2o-0~20230806/g2o/core/marginal_covariance_cholesky.h
• /build/reproducible-path/g2o-0~20230806/g2o/core/marginal_covariance_cholesky.cpp