#include <SymmetricBlockMatrix.h>
Public Types | |
typedef Eigen::Block< Matrix > | Block |
typedef Eigen::Block< const Matrix > | constBlock |
typedef SymmetricBlockMatrix | This |
Public Member Functions | |
DenseIndex & | blockStart () |
DenseIndex | blockStart () const |
void | choleskyPartial (DenseIndex nFrontals) |
DenseIndex | cols () const |
Column size. More... | |
DenseIndex | getDim (DenseIndex block) const |
Number of dimensions for variable on this diagonal block. More... | |
DenseIndex | nBlocks () const |
Block count. More... | |
DenseIndex | rows () const |
Row size. More... | |
VerticalBlockMatrix | split (DenseIndex nFrontals) |
SymmetricBlockMatrix () | |
Construct from an empty matrix (asserts that the matrix is empty) More... | |
template<typename CONTAINER > | |
SymmetricBlockMatrix (const CONTAINER &dimensions, bool appendOneDimension=false) | |
Construct from a container of the sizes of each block. More... | |
template<typename CONTAINER > | |
SymmetricBlockMatrix (const CONTAINER &dimensions, const Matrix &matrix, bool appendOneDimension=false) | |
Construct from a container of the sizes of each vertical block and a pre-prepared matrix. More... | |
template<typename ITERATOR > | |
SymmetricBlockMatrix (ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension=false) | |
Construct from iterator over the sizes of each vertical block. More... | |
Block getter methods. | |
Matrix | block (DenseIndex I, DenseIndex J) const |
Eigen::SelfAdjointView< Block, Eigen::Upper > | diagonalBlock (DenseIndex J) |
Return the J'th diagonal block as a self adjoint view. More... | |
Eigen::SelfAdjointView< constBlock, Eigen::Upper > | diagonalBlock (DenseIndex J) const |
Return the J'th diagonal block as a self adjoint view. More... | |
Vector | diagonal (DenseIndex J) const |
Get the diagonal of the J'th diagonal block. More... | |
constBlock | aboveDiagonalBlock (DenseIndex I, DenseIndex J) const |
Get block above the diagonal (I, J). More... | |
Eigen::SelfAdjointView< constBlock, Eigen::Upper > | selfadjointView (DenseIndex I, DenseIndex J) const |
Return the square sub-matrix that contains blocks(i:j, i:j). More... | |
Eigen::TriangularView< constBlock, Eigen::Upper > | triangularView (DenseIndex I, DenseIndex J) const |
Return the square sub-matrix that contains blocks(i:j, i:j) as a triangular view. More... | |
constBlock | aboveDiagonalRange (DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const |
Get a range [i,j) from the matrix. Indices are in block units. More... | |
Block | aboveDiagonalRange (DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) |
Get a range [i,j) from the matrix. Indices are in block units. More... | |
Block setter methods. | |
template<typename XprType > | |
void | setDiagonalBlock (DenseIndex I, const XprType &xpr) |
Set a diagonal block. Only the upper triangular portion of xpr is evaluated. More... | |
template<typename XprType > | |
void | setOffDiagonalBlock (DenseIndex I, DenseIndex J, const XprType &xpr) |
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated. More... | |
template<typename XprType > | |
void | updateDiagonalBlock (DenseIndex I, const XprType &xpr) |
Increment the diagonal block by the values in xpr . Only reads the upper triangular part of xpr . More... | |
template<typename XprType > | |
void | updateOffDiagonalBlock (DenseIndex I, DenseIndex J, const XprType &xpr) |
Accessing the full matrix. | |
Eigen::SelfAdjointView< Block, Eigen::Upper > | selfadjointView () |
Get self adjoint view. More... | |
Eigen::SelfAdjointView< constBlock, Eigen::Upper > | selfadjointView () const |
Get self adjoint view. More... | |
template<typename XprType > | |
void | setFullMatrix (const XprType &xpr) |
Set the entire active matrix. Only reads the upper triangular part of xpr . More... | |
void | setZero () |
Set the entire active matrix zero. More... | |
void | negate () |
Negate the entire active matrix. More... | |
void | invertInPlace () |
Invert the entire active matrix in place. More... | |
Static Public Member Functions | |
static SymmetricBlockMatrix | LikeActiveViewOf (const SymmetricBlockMatrix &other) |
static SymmetricBlockMatrix | LikeActiveViewOf (const VerticalBlockMatrix &other) |
Protected Member Functions | |
void | assertInvariants () const |
Block | block_ (DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, DenseIndex blockCols=1) |
Get an arbitrary block from the matrix. Indices are in block units. More... | |
constBlock | block_ (DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, DenseIndex blockCols=1) const |
Get an arbitrary block from the matrix. Indices are in block units. More... | |
std::array< DenseIndex, 4 > | calcIndices (DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows, DenseIndex blockCols) const |
Compute the indices into the underlying matrix for a given block. More... | |
template<typename ITERATOR > | |
void | fillOffsets (ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) |
Block | full () |
Get the full matrix as a block. More... | |
constBlock | full () const |
Get the full matrix as a block. More... | |
DenseIndex | nActualBlocks () const |
Number of actual blocks in the full matrix. More... | |
DenseIndex | nOffsets () const |
Number of offsets in the full matrix. More... | |
DenseIndex | offset (DenseIndex block) const |
Get an offset for a block index (in the active view). More... | |
Protected Attributes | |
DenseIndex | blockStart_ |
Changes apparent matrix view, see main class comment. More... | |
Matrix | matrix_ |
The full matrix. More... | |
FastVector< DenseIndex > | variableColOffsets_ |
the starting columns of each block (0-based) More... | |
Friends | |
template<typename SymmetricBlockMatrixType > | |
class | SymmetricBlockMatrixBlockExpr |
class | VerticalBlockMatrix |
This class stores a dense matrix and allows it to be accessed as a collection of blocks. When constructed, the caller must provide the dimensions of the blocks.
The block structure is symmetric, but the underlying matrix does not necessarily need to be.
This class also has a parameter that can be changed after construction to change the apparent matrix view. firstBlock() determines the block that appears to have index 0 for all operations (except re-setting firstBlock()).
Definition at line 53 of file SymmetricBlockMatrix.h.
Definition at line 57 of file SymmetricBlockMatrix.h.
typedef Eigen::Block<const Matrix> gtsam::SymmetricBlockMatrix::constBlock |
Definition at line 58 of file SymmetricBlockMatrix.h.
Definition at line 56 of file SymmetricBlockMatrix.h.
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix | ( | ) |
Construct from an empty matrix (asserts that the matrix is empty)
Definition at line 28 of file SymmetricBlockMatrix.cpp.
|
inline |
Construct from a container of the sizes of each block.
Definition at line 72 of file SymmetricBlockMatrix.h.
|
inline |
Construct from iterator over the sizes of each vertical block.
Definition at line 82 of file SymmetricBlockMatrix.h.
|
inline |
Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
Definition at line 92 of file SymmetricBlockMatrix.h.
|
inline |
Get block above the diagonal (I, J).
Definition at line 152 of file SymmetricBlockMatrix.h.
|
inline |
Get a range [i,j) from the matrix. Indices are in block units.
Definition at line 183 of file SymmetricBlockMatrix.h.
|
inline |
Get a range [i,j) from the matrix. Indices are in block units.
Definition at line 172 of file SymmetricBlockMatrix.h.
|
inlineprotected |
Definition at line 361 of file SymmetricBlockMatrix.h.
Matrix gtsam::SymmetricBlockMatrix::block | ( | DenseIndex | I, |
DenseIndex | J | ||
) | const |
Get a copy of a block (anywhere in the matrix). This method makes a copy - use the methods below if performance is critical.
Definition at line 60 of file SymmetricBlockMatrix.cpp.
|
inlineprotected |
Get an arbitrary block from the matrix. Indices are in block units.
Definition at line 329 of file SymmetricBlockMatrix.h.
|
inlineprotected |
Get an arbitrary block from the matrix. Indices are in block units.
Definition at line 321 of file SymmetricBlockMatrix.h.
|
inline |
Retrieve or modify the first logical block, i.e. the block referenced by block index 0. Blocks before it will be inaccessible, except by accessing the underlying matrix using matrix().
Definition at line 275 of file SymmetricBlockMatrix.h.
|
inline |
Retrieve the first logical block, i.e. the block referenced by block index 0. Blocks before it will be inaccessible, except by accessing the underlying matrix using matrix().
Definition at line 279 of file SymmetricBlockMatrix.h.
|
inlineprotected |
Compute the indices into the underlying matrix for a given block.
Definition at line 347 of file SymmetricBlockMatrix.h.
void gtsam::SymmetricBlockMatrix::choleskyPartial | ( | DenseIndex | nFrontals | ) |
Given the augmented Hessian [A1'A1 A1'A2 A1'b A2'A1 A2'A2 A2'b b'A1 b'A2 b'b] on x1 and x2, does partial Cholesky in-place to obtain [R Sd;0 L] such that R'R = A1'A1 R'Sd = [A1'A2 A1'b] L'L is the augmented Hessian on the the separator x2 R and Sd can be interpreted as a GaussianConditional |R*x1 + S*x2 - d]^2
Definition at line 83 of file SymmetricBlockMatrix.cpp.
|
inline |
Column size.
Definition at line 119 of file SymmetricBlockMatrix.h.
|
inline |
Get the diagonal of the J'th diagonal block.
Definition at line 147 of file SymmetricBlockMatrix.h.
|
inline |
Return the J'th diagonal block as a self adjoint view.
Definition at line 137 of file SymmetricBlockMatrix.h.
|
inline |
Return the J'th diagonal block as a self adjoint view.
Definition at line 142 of file SymmetricBlockMatrix.h.
|
inlineprotected |
Definition at line 369 of file SymmetricBlockMatrix.h.
|
inlineprotected |
Get the full matrix as a block.
Definition at line 342 of file SymmetricBlockMatrix.h.
|
inlineprotected |
Get the full matrix as a block.
Definition at line 337 of file SymmetricBlockMatrix.h.
|
inline |
Number of dimensions for variable on this diagonal block.
Definition at line 125 of file SymmetricBlockMatrix.h.
void gtsam::SymmetricBlockMatrix::invertInPlace | ( | ) |
Invert the entire active matrix in place.
Definition at line 76 of file SymmetricBlockMatrix.cpp.
|
static |
Copy the block structure, but do not copy the matrix data. If blockStart() has been modified, this copies the structure of the corresponding matrix view. In the destination SymmetricBlockMatrix, blockStart() will be 0.
Definition at line 34 of file SymmetricBlockMatrix.cpp.
|
static |
Copy the block structure, but do not copy the matrix data. If blockStart() has been modified, this copies the structure of the corresponding matrix view. In the destination SymmetricBlockMatrix, blockStart() will be 0.
Definition at line 47 of file SymmetricBlockMatrix.cpp.
|
inlineprotected |
Number of actual blocks in the full matrix.
Definition at line 308 of file SymmetricBlockMatrix.h.
|
inline |
Block count.
Definition at line 122 of file SymmetricBlockMatrix.h.
void gtsam::SymmetricBlockMatrix::negate | ( | ) |
Negate the entire active matrix.
Definition at line 71 of file SymmetricBlockMatrix.cpp.
|
inlineprotected |
Number of offsets in the full matrix.
Definition at line 303 of file SymmetricBlockMatrix.h.
|
inlineprotected |
Get an offset for a block index (in the active view).
Definition at line 313 of file SymmetricBlockMatrix.h.
|
inline |
Row size.
Definition at line 116 of file SymmetricBlockMatrix.h.
|
inline |
Get self adjoint view.
Definition at line 244 of file SymmetricBlockMatrix.h.
|
inline |
Get self adjoint view.
Definition at line 249 of file SymmetricBlockMatrix.h.
|
inline |
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition at line 158 of file SymmetricBlockMatrix.h.
|
inline |
Set a diagonal block. Only the upper triangular portion of xpr
is evaluated.
Definition at line 197 of file SymmetricBlockMatrix.h.
|
inline |
Set the entire active matrix. Only reads the upper triangular part of xpr
.
Definition at line 255 of file SymmetricBlockMatrix.h.
|
inline |
Set an off-diagonal block. Only the upper triangular portion of xpr
is evaluated.
Definition at line 203 of file SymmetricBlockMatrix.h.
|
inline |
Set the entire active matrix zero.
Definition at line 260 of file SymmetricBlockMatrix.h.
VerticalBlockMatrix gtsam::SymmetricBlockMatrix::split | ( | DenseIndex | nFrontals | ) |
After partial Cholesky, we can optionally split off R and Sd, to be interpreted as a GaussianConditional |R*x1 + S*x2 - d]^2. We leave the symmetric lower block L in place, and adjust block_start so now *this refers to it.
Definition at line 92 of file SymmetricBlockMatrix.cpp.
|
inline |
Return the square sub-matrix that contains blocks(i:j, i:j) as a triangular view.
Definition at line 165 of file SymmetricBlockMatrix.h.
|
inline |
Increment the diagonal block by the values in xpr
. Only reads the upper triangular part of xpr
.
Definition at line 214 of file SymmetricBlockMatrix.h.
|
inline |
Update an off diagonal block. NOTE(emmett): This assumes noalias().
Definition at line 230 of file SymmetricBlockMatrix.h.
|
friend |
Definition at line 386 of file SymmetricBlockMatrix.h.
|
friend |
Definition at line 385 of file SymmetricBlockMatrix.h.
|
protected |
Changes apparent matrix view, see main class comment.
Definition at line 64 of file SymmetricBlockMatrix.h.
|
protected |
The full matrix.
Definition at line 61 of file SymmetricBlockMatrix.h.
|
protected |
the starting columns of each block (0-based)
Definition at line 62 of file SymmetricBlockMatrix.h.