gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
SymmetricBlockMatrix.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
4* Atlanta, Georgia 30332-0415
5* All Rights Reserved
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8* See LICENSE for the license information
9
10* -------------------------------------------------------------------------- */
11
18#pragma once
19
21#include <gtsam/base/Matrix.h>
22#include <gtsam/base/types.h>
23#include <gtsam/dllexport.h>
24#include <boost/serialization/nvp.hpp>
25#include <cassert>
26#include <stdexcept>
27#include <array>
28
29namespace boost {
30namespace serialization {
31class access;
32} /* namespace serialization */
33} /* namespace boost */
34
35namespace gtsam {
36
37 // Forward declarations
38 class VerticalBlockMatrix;
39
51 class GTSAM_EXPORT SymmetricBlockMatrix
52 {
53 public:
55 typedef Eigen::Block<Matrix> Block;
56 typedef Eigen::Block<const Matrix> constBlock;
57
58 protected:
59 Matrix matrix_;
61
63
64 public:
67
69 template<typename CONTAINER>
70 SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) :
71 blockStart_(0)
72 {
73 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
74 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
75 assertInvariants();
76 }
77
79 template<typename ITERATOR>
80 SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) :
81 blockStart_(0)
82 {
83 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
84 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
85 assertInvariants();
86 }
87
89 template<typename CONTAINER>
90 SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
91 blockStart_(0)
92 {
93 matrix_.resize(matrix.rows(), matrix.cols());
94 matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
95 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
96 if(matrix_.rows() != matrix_.cols())
97 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
98 if(variableColOffsets_.back() != matrix_.cols())
99 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix.");
100 assertInvariants();
101 }
102
106 static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other);
107
111 static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other);
112
114 DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
115
117 DenseIndex cols() const { return rows(); }
118
120 DenseIndex nBlocks() const { return nActualBlocks() - blockStart_; }
121
124 return calcIndices(block, block, 1, 1)[2];
125 }
126
129
132 Matrix block(DenseIndex I, DenseIndex J) const;
133
135 Eigen::SelfAdjointView<Block, Eigen::Upper> diagonalBlock(DenseIndex J) {
136 return block_(J, J).selfadjointView<Eigen::Upper>();
137 }
138
140 Eigen::SelfAdjointView<constBlock, Eigen::Upper> diagonalBlock(DenseIndex J) const {
141 return block_(J, J).selfadjointView<Eigen::Upper>();
142 }
143
145 Vector diagonal(DenseIndex J) const {
146 return block_(J, J).diagonal();
147 }
148
150 constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const {
151 assert(I < J);
152 return block_(I, J);
153 }
154
156 Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView(
157 DenseIndex I, DenseIndex J) const {
158 assert(J > I);
159 return block_(I, I, J - I, J - I).selfadjointView<Eigen::Upper>();
160 }
161
163 Eigen::TriangularView<constBlock, Eigen::Upper> triangularView(DenseIndex I,
164 DenseIndex J) const {
165 assert(J > I);
166 return block_(I, I, J - I, J - I).triangularView<Eigen::Upper>();
167 }
168
170 constBlock aboveDiagonalRange(DenseIndex i_startBlock,
171 DenseIndex i_endBlock,
172 DenseIndex j_startBlock,
173 DenseIndex j_endBlock) const {
174 assert(i_startBlock < j_startBlock);
175 assert(i_endBlock <= j_startBlock);
176 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
177 j_endBlock - j_startBlock);
178 }
179
181 Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock,
182 DenseIndex j_startBlock, DenseIndex j_endBlock) {
183 assert(i_startBlock < j_startBlock);
184 assert(i_endBlock <= j_startBlock);
185 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
186 j_endBlock - j_startBlock);
187 }
188
192
194 template <typename XprType>
195 void setDiagonalBlock(DenseIndex I, const XprType& xpr) {
196 block_(I, I).triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
197 }
198
200 template <typename XprType>
201 void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
202 assert(I != J);
203 if (I < J) {
204 block_(I, J) = xpr;
205 } else {
206 block_(J, I) = xpr.transpose();
207 }
208 }
209
211 template <typename XprType>
212 void updateDiagonalBlock(DenseIndex I, const XprType& xpr) {
213 // TODO(gareth): Eigen won't let us add triangular or self-adjoint views
214 // here, so we do it manually.
215 auto dest = block_(I, I);
216 assert(dest.rows() == xpr.rows());
217 assert(dest.cols() == xpr.cols());
218 for (DenseIndex col = 0; col < dest.cols(); ++col) {
219 for (DenseIndex row = 0; row <= col; ++row) {
220 dest(row, col) += xpr(row, col);
221 }
222 }
223 }
224
227 template <typename XprType>
228 void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
229 assert(I != J);
230 if (I < J) {
231 block_(I, J).noalias() += xpr;
232 } else {
233 block_(J, I).noalias() += xpr.transpose();
234 }
235 }
236
240
242 Eigen::SelfAdjointView<Block, Eigen::Upper> selfadjointView() {
243 return full().selfadjointView<Eigen::Upper>();
244 }
245
247 Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView() const {
248 return full().selfadjointView<Eigen::Upper>();
249 }
250
252 template <typename XprType>
253 void setFullMatrix(const XprType& xpr) {
254 full().triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
255 }
256
258 void setZero() {
259 full().triangularView<Eigen::Upper>().setZero();
260 }
261
263 void negate();
264
266 void invertInPlace();
267
269
273 DenseIndex& blockStart() { return blockStart_; }
274
277 DenseIndex blockStart() const { return blockStart_; }
278
289 void choleskyPartial(DenseIndex nFrontals);
290
297
298 protected:
299
302 return variableColOffsets_.size();
303 }
304
307 return nOffsets() - 1;
308 }
309
312 assert(block >= 0);
313 const DenseIndex actual_index = block + blockStart();
314 assert(actual_index < nOffsets());
315 return variableColOffsets_[actual_index];
316 }
317
319 constBlock block_(DenseIndex iBlock, DenseIndex jBlock,
320 DenseIndex blockRows = 1, DenseIndex blockCols = 1) const {
321 const std::array<DenseIndex, 4> indices =
322 calcIndices(iBlock, jBlock, blockRows, blockCols);
323 return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
324 }
325
327 Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows = 1,
328 DenseIndex blockCols = 1) {
329 const std::array<DenseIndex, 4> indices =
330 calcIndices(iBlock, jBlock, blockRows, blockCols);
331 return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
332 }
333
335 constBlock full() const {
336 return block_(0, 0, nBlocks(), nBlocks());
337 }
338
340 Block full() {
341 return block_(0, 0, nBlocks(), nBlocks());
342 }
343
345 std::array<DenseIndex, 4> calcIndices(DenseIndex iBlock, DenseIndex jBlock,
346 DenseIndex blockRows,
347 DenseIndex blockCols) const {
348 assert(blockRows >= 0);
349 assert(blockCols >= 0);
350
351 // adjust indices to account for start and size of blocks
352 const DenseIndex denseI = offset(iBlock);
353 const DenseIndex denseJ = offset(jBlock);
354 const DenseIndex denseRows = offset(iBlock + blockRows) - denseI;
355 const DenseIndex denseCols = offset(jBlock + blockCols) - denseJ;
356 return {{denseI, denseJ, denseRows, denseCols}};
357 }
358
359 void assertInvariants() const
360 {
361 assert(matrix_.rows() == matrix_.cols());
362 assert(matrix_.cols() == variableColOffsets_.back());
363 assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
364 }
365
366 template<typename ITERATOR>
367 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension)
368 {
369 variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
370 variableColOffsets_[0] = 0;
371 DenseIndex j=0;
372 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
373 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
374 ++ j;
375 }
376 if(appendOneDimension)
377 {
378 variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
379 ++ j;
380 }
381 }
382
383 friend class VerticalBlockMatrix;
384 template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
385
386 private:
388 friend class boost::serialization::access;
389 template<class ARCHIVE>
390 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
391 // Fill in the lower triangle part of the matrix, so boost::serialization won't
392 // complain about uninitialized data with an input_stream_error exception
393 // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error
394 matrix_.triangularView<Eigen::Lower>() = matrix_.triangularView<Eigen::Upper>().transpose();
395 ar & BOOST_SERIALIZATION_NVP(matrix_);
396 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
397 ar & BOOST_SERIALIZATION_NVP(blockStart_);
398 }
399 };
400
402 class CholeskyFailed;
403
404}
typedef and functions to augment Eigen's MatrixXd
A thin wrapper around std::vector that uses a custom allocator.
Typedefs for easier changing of types.
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:34
Global functions in a separate testing namespace.
Definition chartTesting.h:28
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
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
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
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
Block full()
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:340
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition SymmetricBlockMatrix.h:62
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:195
DenseIndex nActualBlocks() const
Number of actual blocks in the full matrix.
Definition SymmetricBlockMatrix.h:306
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition SymmetricBlockMatrix.h:145
Matrix matrix_
The full matrix.
Definition SymmetricBlockMatrix.h:59
DenseIndex cols() const
Column size.
Definition SymmetricBlockMatrix.h:117
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Definition SymmetricBlockMatrix.h:123
void setZero()
Set the entire active matrix zero.
Definition SymmetricBlockMatrix.h:258
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
void setFullMatrix(const XprType &xpr)
Set the entire active matrix. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:253
Eigen::SelfAdjointView< Block, Eigen::Upper > selfadjointView()
Get self adjoint view.
Definition SymmetricBlockMatrix.h:242
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
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
constBlock full() const
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:335
DenseIndex rows() const
Row size.
Definition SymmetricBlockMatrix.h:114
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
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
Eigen::SelfAdjointView< constBlock, Eigen::Upper > diagonalBlock(DenseIndex J) const
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:140
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
DenseIndex blockStart() const
Retrieve the first logical block, i.e.
Definition SymmetricBlockMatrix.h:277
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView() const
Get self adjoint view.
Definition SymmetricBlockMatrix.h:247
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
SymmetricBlockMatrix(const CONTAINER &dimensions, bool appendOneDimension=false)
Construct from a container of the sizes of each block.
Definition SymmetricBlockMatrix.h:70
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition SymmetricBlockMatrix.h:80
DenseIndex & blockStart()
Retrieve or modify the first logical block, i.e.
Definition SymmetricBlockMatrix.h:273
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
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
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
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
DenseIndex nOffsets() const
Number of offsets in the full matrix.
Definition SymmetricBlockMatrix.h:301
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
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition SymmetricBlockMatrix.h:60
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
DenseIndex offset(DenseIndex block) const
Get an offset for a block index (in the active view).
Definition SymmetricBlockMatrix.h:311
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43