computing the marginal covariance given a cholesky factor (lower triangle of the factor) More...
#include <marginal_covariance_cholesky.h>
Public Member Functions | |
void | computeCovariance (const std::vector< OptimizableGraph::Vertex * > &vertices) |
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 | |
Protected Attributes | |
int * | _Ai |
row indices of the CCS storage | |
int * | _Ap |
column pointer of the CCS storage | |
double * | _Ax |
values of the cholesky factor | |
std::vector< double > | _diag |
cache 1 / H_ii to avoid recalculations | |
LookupMap | _map |
hash look up table for the already computed entries | |
int | _n |
L is an n X n matrix. | |
int * | _perm |
permutation of the cholesky factor. Variable re-ordering for better fill-in |
computing the marginal covariance given a cholesky factor (lower triangle of the factor)
Definition at line 37 of file marginal_covariance_cholesky.h.
typedef std::tr1::unordered_map<int, double> g2o::MarginalCovarianceCholesky::LookupMap [protected] |
hash struct for storing the matrix elements needed to compute the covariance
Definition at line 42 of file marginal_covariance_cholesky.h.
Definition at line 35 of file marginal_covariance_cholesky.cpp.
Definition at line 40 of file marginal_covariance_cholesky.cpp.
void g2o::MarginalCovarianceCholesky::computeCovariance | ( | const std::vector< OptimizableGraph::Vertex * > & | vertices | ) |
compute the covariance for the given vertices, directly store inside the uncertainty field of the vertex
Definition at line 61 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 152 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 204 of file marginal_covariance_cholesky.cpp.
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 121 of file marginal_covariance_cholesky.cpp.
int g2o::MarginalCovarianceCholesky::computeIndex | ( | int | r, |
int | c | ||
) | const [inline, protected] |
compute the index used for hashing
Definition at line 84 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.
Definition at line 44 of file marginal_covariance_cholesky.cpp.
int* g2o::MarginalCovarianceCholesky::_Ai [protected] |
row indices of the CCS storage
Definition at line 76 of file marginal_covariance_cholesky.h.
int* g2o::MarginalCovarianceCholesky::_Ap [protected] |
column pointer of the CCS storage
Definition at line 75 of file marginal_covariance_cholesky.h.
double* g2o::MarginalCovarianceCholesky::_Ax [protected] |
values of the cholesky factor
Definition at line 77 of file marginal_covariance_cholesky.h.
std::vector<double> g2o::MarginalCovarianceCholesky::_diag [protected] |
cache 1 / H_ii to avoid recalculations
Definition at line 81 of file marginal_covariance_cholesky.h.
LookupMap g2o::MarginalCovarianceCholesky::_map [protected] |
hash look up table for the already computed entries
Definition at line 80 of file marginal_covariance_cholesky.h.
int g2o::MarginalCovarianceCholesky::_n [protected] |
L is an n X n matrix.
Definition at line 74 of file marginal_covariance_cholesky.h.
int* g2o::MarginalCovarianceCholesky::_perm [protected] |
permutation of the cholesky factor. Variable re-ordering for better fill-in
Definition at line 78 of file marginal_covariance_cholesky.h.