computing the marginal covariance given a cholesky factor (lower triangle of the factor) More...
#include <marginal_covariance_cholesky.h>
Public Member Functions | |
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) |
MarginalCovarianceCholesky () | |
void | setCholeskyFactor (int n, int *Lp, int *Li, double *Lx, int *permInv) |
~MarginalCovarianceCholesky () | |
Protected Types | |
typedef std::tr1::unordered_map< int, double > | LookupMap |
Protected Member Functions | |
double | computeEntry (int r, int c) |
int | computeIndex (int r, int c) const |
compute the index used for hashing More... | |
Protected Attributes | |
int * | _Ai |
row indices of the CCS storage More... | |
int * | _Ap |
column pointer of the CCS storage More... | |
double * | _Ax |
values of the cholesky factor More... | |
std::vector< double > | _diag |
cache 1 / H_ii to avoid recalculations More... | |
LookupMap | _map |
hash look up table for the already computed entries More... | |
int | _n |
L is an n X n matrix. More... | |
int * | _perm |
permutation of the cholesky factor. Variable re-ordering for better fill-in More... | |
computing the marginal covariance given a cholesky factor (lower triangle of the factor)
Definition at line 48 of file marginal_covariance_cholesky.h.
|
protected |
hash struct for storing the matrix elements needed to compute the covariance
Definition at line 53 of file marginal_covariance_cholesky.h.
g2o::MarginalCovarianceCholesky::MarginalCovarianceCholesky | ( | ) |
Definition at line 45 of file marginal_covariance_cholesky.cpp.
g2o::MarginalCovarianceCholesky::~MarginalCovarianceCholesky | ( | ) |
Definition at line 50 of file marginal_covariance_cholesky.cpp.
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.
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.
|
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.
|
inlineprotected |
compute the index used for hashing
Definition at line 93 of file marginal_covariance_cholesky.h.
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.
|
protected |
row indices of the CCS storage
Definition at line 85 of file marginal_covariance_cholesky.h.
|
protected |
column pointer of the CCS storage
Definition at line 84 of file marginal_covariance_cholesky.h.
|
protected |
values of the cholesky factor
Definition at line 86 of file marginal_covariance_cholesky.h.
|
protected |
cache 1 / H_ii to avoid recalculations
Definition at line 90 of file marginal_covariance_cholesky.h.
|
protected |
hash look up table for the already computed entries
Definition at line 89 of file marginal_covariance_cholesky.h.
|
protected |
L is an n X n matrix.
Definition at line 83 of file marginal_covariance_cholesky.h.
|
protected |
permutation of the cholesky factor. Variable re-ordering for better fill-in
Definition at line 87 of file marginal_covariance_cholesky.h.