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< MatrixXD > &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 More...

double	computeEntry (int r, int c)

Protected Attributes
int	_n
	L is an n X n matrix. More...

int *	_Ap
	column pointer of the CCS storage More...

int *	_Ai
	row indices of the CCS storage More...

double *	_Ax
	values of the cholesky factor More...

int *	_perm
	permutation of the cholesky factor. Variable re-ordering for better fill-in More...

LookupMap	_map
	hash look up table for the already computed entries More...

std::vector< double >	_diag
	cache 1 / H_ii to avoid recalculations More...

Detailed Description

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

Definition at line 45 of file marginal_covariance_cholesky.h.

Member Typedef Documentation

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 50 of file marginal_covariance_cholesky.h.

Constructor & Destructor Documentation

g2o::MarginalCovarianceCholesky::MarginalCovarianceCholesky ( )

Definition at line 45 of file marginal_covariance_cholesky.cpp.

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

g2o::MarginalCovarianceCholesky::~MarginalCovarianceCholesky ( )

Definition at line 50 of file marginal_covariance_cholesky.cpp.

51 {

52 }

Member Function Documentation

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 102 of file marginal_covariance_cholesky.cpp.

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

Referenced by g2o::LinearSolverCSparse< MatrixType >::solveBlocks(), g2o::LinearSolverCholmod< MatrixType >::solveBlocks(), g2o::LinearSolverCSparse< MatrixType >::solvePattern(), and g2o::LinearSolverCholmod< MatrixType >::solvePattern().

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

void g2o::MarginalCovarianceCholesky::computeCovariance	(	SparseBlockMatrix< MatrixXD > &	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 154 of file marginal_covariance_cholesky.cpp.

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

 {
   // allocate the sparse
   spinv = SparseBlockMatrix<MatrixXD>(&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);
     
     MatrixXD *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);
     
     MatrixXD *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;
       }
   }
 }

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 71 of file marginal_covariance_cholesky.cpp.

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

Referenced by computeCovariance().

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

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

inlineprotected

compute the index used for hashing

Definition at line 90 of file marginal_covariance_cholesky.h.

Referenced by computeCovariance(), and computeEntry().

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

g2o::MarginalCovarianceCholesky::_n

int _n

L is an n X n matrix.

Definition: marginal_covariance_cholesky.h:80

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 54 of file marginal_covariance_cholesky.cpp.

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

Referenced by g2o::LinearSolverCSparse< MatrixType >::solveBlocks(), g2o::LinearSolverCholmod< MatrixType >::solveBlocks(), g2o::LinearSolverCSparse< MatrixType >::solvePattern(), and g2o::LinearSolverCholmod< MatrixType >::solvePattern().

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