Loading...
Searching...
No Matches
SymmetricBlockMatrix.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/types.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cassert>
#include <stdexcept>
#include <array>
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
namespace gtsam {
// Forward declarations
class VerticalBlockMatrix;
class GTSAM_EXPORT SymmetricBlockMatrix
{
public:
typedef SymmetricBlockMatrix This;
typedef Eigen::Block<Matrix> Block;
typedef Eigen::Block<const Matrix> constBlock;
protected:
Matrix matrix_;
FastVector<DenseIndex> variableColOffsets_;
DenseIndex blockStart_;
public:
SymmetricBlockMatrix();
template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) :
blockStart_(0)
{
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
assertInvariants();
}
template<typename ITERATOR>
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) :
blockStart_(0)
{
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
assertInvariants();
}
template<typename CONTAINER>
SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
blockStart_(0)
{
matrix_.resize(matrix.rows(), matrix.cols());
matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
if(matrix_.rows() != matrix_.cols())
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
if(variableColOffsets_.back() != matrix_.cols())
throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix.");
assertInvariants();
}
static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other);
static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other);
DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
DenseIndex cols() const { return rows(); }
DenseIndex nBlocks() const { return nActualBlocks() - blockStart_; }
DenseIndex getDim(DenseIndex block) const {
return calcIndices(block, block, 1, 1)[2];
}
Matrix block(DenseIndex I, DenseIndex J) const;
Eigen::SelfAdjointView<Block, Eigen::Upper> diagonalBlock(DenseIndex J) {
return block_(J, J).selfadjointView<Eigen::Upper>();
}
Eigen::SelfAdjointView<constBlock, Eigen::Upper> diagonalBlock(DenseIndex J) const {
return block_(J, J).selfadjointView<Eigen::Upper>();
}
Vector diagonal(DenseIndex J) const {
return block_(J, J).diagonal();
}
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const {
assert(I < J);
return block_(I, J);
}
Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView(
DenseIndex I, DenseIndex J) const {
assert(J > I);
return block_(I, I, J - I, J - I).selfadjointView<Eigen::Upper>();
}
Eigen::TriangularView<constBlock, Eigen::Upper> triangularView(DenseIndex I,
DenseIndex J) const {
assert(J > I);
return block_(I, I, J - I, J - I).triangularView<Eigen::Upper>();
}
constBlock aboveDiagonalRange(DenseIndex i_startBlock,
DenseIndex i_endBlock,
DenseIndex j_startBlock,
DenseIndex j_endBlock) const {
assert(i_startBlock < j_startBlock);
assert(i_endBlock <= j_startBlock);
return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
j_endBlock - j_startBlock);
}
Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock,
DenseIndex j_startBlock, DenseIndex j_endBlock) {
assert(i_startBlock < j_startBlock);
assert(i_endBlock <= j_startBlock);
return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
j_endBlock - j_startBlock);
}
template <typename XprType>
void setDiagonalBlock(DenseIndex I, const XprType& xpr) {
block_(I, I).triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
}
template <typename XprType>
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
assert(I != J);
if (I < J) {
block_(I, J) = xpr;
} else {
block_(J, I) = xpr.transpose();
}
}
template <typename XprType>
void updateDiagonalBlock(DenseIndex I, const XprType& xpr) {
// TODO(gareth): Eigen won't let us add triangular or self-adjoint views
// here, so we do it manually.
auto dest = block_(I, I);
assert(dest.rows() == xpr.rows());
assert(dest.cols() == xpr.cols());
for (DenseIndex col = 0; col < dest.cols(); ++col) {
for (DenseIndex row = 0; row <= col; ++row) {
dest(row, col) += xpr(row, col);
}
}
}
template <typename XprType>
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
assert(I != J);
if (I < J) {
block_(I, J).noalias() += xpr;
} else {
block_(J, I).noalias() += xpr.transpose();
}
}
Eigen::SelfAdjointView<Block, Eigen::Upper> selfadjointView() {
return full().selfadjointView<Eigen::Upper>();
}
Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView() const {
return full().selfadjointView<Eigen::Upper>();
}
template <typename XprType>
void setFullMatrix(const XprType& xpr) {
full().triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
}
void setZero() {
full().triangularView<Eigen::Upper>().setZero();
}
void negate();
void invertInPlace();
DenseIndex& blockStart() { return blockStart_; }
DenseIndex blockStart() const { return blockStart_; }
void choleskyPartial(DenseIndex nFrontals);
VerticalBlockMatrix split(DenseIndex nFrontals);
protected:
DenseIndex nOffsets() const {
return variableColOffsets_.size();
}
DenseIndex nActualBlocks() const {
return nOffsets() - 1;
}
DenseIndex offset(DenseIndex block) const {
assert(block >= 0);
const DenseIndex actual_index = block + blockStart();
assert(actual_index < nOffsets());
return variableColOffsets_[actual_index];
}
constBlock block_(DenseIndex iBlock, DenseIndex jBlock,
DenseIndex blockRows = 1, DenseIndex blockCols = 1) const {
const std::array<DenseIndex, 4> indices =
calcIndices(iBlock, jBlock, blockRows, blockCols);
return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
}
Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows = 1,
DenseIndex blockCols = 1) {
const std::array<DenseIndex, 4> indices =
calcIndices(iBlock, jBlock, blockRows, blockCols);
return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
}
constBlock full() const {
return block_(0, 0, nBlocks(), nBlocks());
}
Block full() {
return block_(0, 0, nBlocks(), nBlocks());
}
std::array<DenseIndex, 4> calcIndices(DenseIndex iBlock, DenseIndex jBlock,
DenseIndex blockRows,
DenseIndex blockCols) const {
assert(blockRows >= 0);
assert(blockCols >= 0);
// adjust indices to account for start and size of blocks
const DenseIndex denseI = offset(iBlock);
const DenseIndex denseJ = offset(jBlock);
const DenseIndex denseRows = offset(iBlock + blockRows) - denseI;
const DenseIndex denseCols = offset(jBlock + blockCols) - denseJ;
return {{denseI, denseJ, denseRows, denseCols}};
}
void assertInvariants() const
{
assert(matrix_.rows() == matrix_.cols());
assert(matrix_.cols() == variableColOffsets_.back());
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
}
template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension)
{
variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
variableColOffsets_[0] = 0;
DenseIndex j=0;
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j;
}
if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
}
}
friend class VerticalBlockMatrix;
template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// Fill in the lower triangle part of the matrix, so boost::serialization won't
// complain about uninitialized data with an input_stream_error exception
// http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error
matrix_.triangularView<Eigen::Lower>() = matrix_.triangularView<Eigen::Upper>().transpose();
ar & BOOST_SERIALIZATION_NVP(matrix_);
ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
ar & BOOST_SERIALIZATION_NVP(blockStart_);
}
};
class CholeskyFailed;
}
Matrix.h
typedef and functions to augment Eigen's MatrixXd
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
types.h
Typedefs for easier changing of types.
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:33
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:249
gtsam::split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds...
Definition graph-inl.h:255
gtsam::choleskyPartial
bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft)
Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
Definition cholesky.cpp:108
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
gtsam::SymmetricBlockMatrix::full
Block full()
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:340
gtsam::SymmetricBlockMatrix::blockStart_
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition SymmetricBlockMatrix.h:62
gtsam::SymmetricBlockMatrix::setDiagonalBlock
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:195
gtsam::SymmetricBlockMatrix::nActualBlocks
DenseIndex nActualBlocks() const
Number of actual blocks in the full matrix.
Definition SymmetricBlockMatrix.h:306
gtsam::SymmetricBlockMatrix::diagonal
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition SymmetricBlockMatrix.h:145
gtsam::SymmetricBlockMatrix::matrix_
Matrix matrix_
The full matrix.
Definition SymmetricBlockMatrix.h:59
gtsam::SymmetricBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition SymmetricBlockMatrix.h:117
gtsam::SymmetricBlockMatrix::getDim
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Definition SymmetricBlockMatrix.h:123
gtsam::SymmetricBlockMatrix::setZero
void setZero()
Set the entire active matrix zero.
Definition SymmetricBlockMatrix.h:258
gtsam::SymmetricBlockMatrix::setOffDiagonalBlock
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:201
gtsam::SymmetricBlockMatrix::setFullMatrix
void setFullMatrix(const XprType &xpr)
Set the entire active matrix. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:253
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< Block, Eigen::Upper > selfadjointView()
Get self adjoint view.
Definition SymmetricBlockMatrix.h:242
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
gtsam::SymmetricBlockMatrix::block_
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.
Definition SymmetricBlockMatrix.h:319
gtsam::SymmetricBlockMatrix::full
constBlock full() const
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:335
gtsam::SymmetricBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition SymmetricBlockMatrix.h:114
gtsam::SymmetricBlockMatrix::aboveDiagonalRange
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.
Definition SymmetricBlockMatrix.h:181
gtsam::SymmetricBlockMatrix::triangularView
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.
Definition SymmetricBlockMatrix.h:163
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< constBlock, Eigen::Upper > diagonalBlock(DenseIndex J) const
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:140
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
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.
Definition SymmetricBlockMatrix.h:90
gtsam::SymmetricBlockMatrix::blockStart
DenseIndex blockStart() const
Retrieve the first logical block, i.e.
Definition SymmetricBlockMatrix.h:277
gtsam::SymmetricBlockMatrix::block
Matrix block(DenseIndex I, DenseIndex J) const
Get a copy of a block (anywhere in the matrix).
Definition SymmetricBlockMatrix.cpp:60
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView() const
Get self adjoint view.
Definition SymmetricBlockMatrix.h:247
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
SymmetricBlockMatrix(const CONTAINER &dimensions, bool appendOneDimension=false)
Construct from a container of the sizes of each block.
Definition SymmetricBlockMatrix.h:70
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition SymmetricBlockMatrix.h:80
gtsam::SymmetricBlockMatrix::blockStart
DenseIndex & blockStart()
Retrieve or modify the first logical block, i.e.
Definition SymmetricBlockMatrix.h:273
gtsam::SymmetricBlockMatrix::aboveDiagonalRange
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.
Definition SymmetricBlockMatrix.h:170
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
SymmetricBlockMatrix()
Construct from an empty matrix (asserts that the matrix is empty).
Definition SymmetricBlockMatrix.cpp:28
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
gtsam::SymmetricBlockMatrix::block_
Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, DenseIndex blockCols=1)
Get an arbitrary block from the matrix. Indices are in block units.
Definition SymmetricBlockMatrix.h:327
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:212
gtsam::SymmetricBlockMatrix::nOffsets
DenseIndex nOffsets() const
Number of offsets in the full matrix.
Definition SymmetricBlockMatrix.h:301
gtsam::SymmetricBlockMatrix::calcIndices
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.
Definition SymmetricBlockMatrix.h:345
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
gtsam::SymmetricBlockMatrix::variableColOffsets_
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition SymmetricBlockMatrix.h:60
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
gtsam::SymmetricBlockMatrix::offset
DenseIndex offset(DenseIndex block) const
Get an offset for a block index (in the active view).
Definition SymmetricBlockMatrix.h:311
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43