23 #include <gtsam/dllexport.h> 24 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 25 #include <boost/serialization/nvp.hpp> 32 namespace serialization {
40 class VerticalBlockMatrix;
57 typedef Eigen::Block<Matrix> Block;
58 typedef Eigen::Block<const Matrix> constBlock;
71 template<
typename CONTAINER>
75 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
76 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
81 template<
typename ITERATOR>
85 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
86 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
91 template<
typename CONTAINER>
95 matrix_.resize(matrix.rows(), matrix.cols());
96 matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
97 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
98 if(matrix_.rows() != matrix_.cols())
99 throw std::invalid_argument(
"Requested to create a SymmetricBlockMatrix from a non-square matrix.");
100 if(variableColOffsets_.back() != matrix_.cols())
101 throw std::invalid_argument(
"Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix.");
116 DenseIndex rows()
const { assertInvariants();
return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
126 return calcIndices(block, block, 1, 1)[2];
138 return block_(J, J).selfadjointView<Eigen::Upper>();
143 return block_(J, J).selfadjointView<Eigen::Upper>();
148 return block_(J, J).diagonal();
161 return block_(I, I, J - I, J - I).selfadjointView<Eigen::Upper>();
168 return block_(I, I, J - I, J - I).triangularView<Eigen::Upper>();
176 assert(i_startBlock < j_startBlock);
177 assert(i_endBlock <= j_startBlock);
178 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
179 j_endBlock - j_startBlock);
185 assert(i_startBlock < j_startBlock);
186 assert(i_endBlock <= j_startBlock);
187 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
188 j_endBlock - j_startBlock);
196 template <
typename XprType>
198 block_(I, I).triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
202 template <
typename XprType>
208 block_(J, I) = xpr.transpose();
213 template <
typename XprType>
217 auto dest = block_(I, I);
218 assert(dest.rows() == xpr.rows());
219 assert(dest.cols() == xpr.cols());
220 for (
DenseIndex col = 0; col < dest.cols(); ++col) {
222 dest(
row, col) += xpr(
row, col);
229 template <
typename XprType>
233 block_(I, J).noalias() += xpr;
235 block_(J, I).noalias() += xpr.transpose();
245 return full().selfadjointView<Eigen::Upper>();
250 return full().selfadjointView<Eigen::Upper>();
254 template <
typename XprType>
256 full().triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
261 full().triangularView<Eigen::Upper>().setZero();
268 void invertInPlace();
304 return variableColOffsets_.size();
309 return nOffsets() - 1;
315 const DenseIndex actual_index = block + blockStart();
316 assert(actual_index < nOffsets());
317 return variableColOffsets_[actual_index];
323 const std::array<DenseIndex, 4> indices =
324 calcIndices(iBlock, jBlock, blockRows, blockCols);
325 return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
331 const std::array<DenseIndex, 4> indices =
332 calcIndices(iBlock, jBlock, blockRows, blockCols);
333 return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
338 return block_(0, 0, nBlocks(), nBlocks());
343 return block_(0, 0, nBlocks(), nBlocks());
350 assert(blockRows >= 0);
351 assert(blockCols >= 0);
356 const DenseIndex denseRows = offset(iBlock + blockRows) - denseI;
357 const DenseIndex denseCols = offset(jBlock + blockCols) - denseJ;
358 return {{denseI, denseJ, denseRows, denseCols}};
361 void assertInvariants()
const 363 assert(matrix_.rows() == matrix_.cols());
364 assert(matrix_.cols() == variableColOffsets_.back());
365 assert(blockStart_ < (
DenseIndex)variableColOffsets_.size());
368 template<
typename ITERATOR>
369 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
bool appendOneDimension)
371 variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
372 variableColOffsets_[0] = 0;
374 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
375 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
378 if(appendOneDimension)
380 variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
386 template<
typename SymmetricBlockMatrixType>
friend class SymmetricBlockMatrixBlockExpr;
389 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 391 friend class boost::serialization::access;
392 template<
class ARCHIVE>
393 void serialize(ARCHIVE & ar,
const unsigned int ) {
397 matrix_.triangularView<Eigen::Lower>() = matrix_.triangularView<Eigen::Upper>().transpose();
398 ar & BOOST_SERIALIZATION_NVP(matrix_);
399 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
400 ar & BOOST_SERIALIZATION_NVP(blockStart_);
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Definition: Matrix.h:221
Typedefs for easier changing of types.
constBlock full() const
Get the full matrix as a block.
Definition: SymmetricBlockMatrix.h:337
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:203
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition: SymmetricBlockMatrix.h:62
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
Definition: VerticalBlockMatrix.h:42
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:172
Block full()
Get the full matrix as a block.
Definition: SymmetricBlockMatrix.h:342
Matrix matrix_
The full matrix.
Definition: SymmetricBlockMatrix.h:61
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition: SymmetricBlockMatrix.h:64
GTSAM_EXPORT bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft=0)
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:329
DenseIndex nActualBlocks() const
Number of actual blocks in the full matrix.
Definition: SymmetricBlockMatrix.h:308
Eigen::SelfAdjointView< constBlock, Eigen::Upper > diagonalBlock(DenseIndex J) const
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:142
DenseIndex blockStart() const
Definition: SymmetricBlockMatrix.h:279
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition: SymmetricBlockMatrix.h:82
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:92
Definition: SymmetricBlockMatrix.h:53
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Definition: graph-inl.h:245
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:158
DenseIndex nBlocks() const
Block count.
Definition: SymmetricBlockMatrix.h:122
DenseIndex rows() const
Row size.
Definition: SymmetricBlockMatrix.h:116
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition: SymmetricBlockMatrix.h:197
DenseIndex cols() const
Column size.
Definition: SymmetricBlockMatrix.h:119
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:183
DenseIndex nOffsets() const
Number of offsets in the full matrix.
Definition: SymmetricBlockMatrix.h:303
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:165
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:214
typedef and functions to augment Eigen's MatrixXd
Eigen::SelfAdjointView< Block, Eigen::Upper > selfadjointView()
Get self adjoint view.
Definition: SymmetricBlockMatrix.h:244
void setFullMatrix(const XprType &xpr)
Set the entire active matrix. Only reads the upper triangular part of xpr.
Definition: SymmetricBlockMatrix.h:255
A thin wrapper around std::vector that uses a custom allocator.
Definition: chartTesting.h:28
DenseIndex offset(DenseIndex block) const
Get an offset for a block index (in the active view).
Definition: SymmetricBlockMatrix.h:313
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView() const
Get self adjoint view.
Definition: SymmetricBlockMatrix.h:249
void setZero()
Set the entire active matrix zero.
Definition: SymmetricBlockMatrix.h:260
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:347
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition: SymmetricBlockMatrix.h:147
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Definition: SymmetricBlockMatrix.h:230
DenseIndex & blockStart()
Definition: SymmetricBlockMatrix.h:275
Indicate Cholesky factorization failure.
Definition: ThreadsafeException.h:115
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:137
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Definition: SymmetricBlockMatrix.h:125
SymmetricBlockMatrix(const CONTAINER &dimensions, bool appendOneDimension=false)
Construct from a container of the sizes of each block.
Definition: SymmetricBlockMatrix.h:72
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition: SymmetricBlockMatrix.h:152
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:321