gtsam 4.1.1
gtsam
VerticalBlockMatrix.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
20#include <gtsam/base/Matrix.h>
22
23namespace gtsam {
24
25 // Forward declarations
26 class SymmetricBlockMatrix;
27
41 class GTSAM_EXPORT VerticalBlockMatrix
42 {
43 public:
45 typedef Eigen::Block<Matrix> Block;
46 typedef Eigen::Block<const Matrix> constBlock;
47
48 protected:
49 Matrix matrix_;
50 FastVector<DenseIndex> variableColOffsets_;
51
55
56 public:
57
60 rowStart_(0), rowEnd_(0), blockStart_(0)
61 {
62 variableColOffsets_.push_back(0);
63 assertInvariants();
64 }
65
67 template<typename CONTAINER>
68 VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height,
69 bool appendOneDimension = false) :
70 variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
71 rowStart_(0), rowEnd_(height), blockStart_(0) {
72 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
73 matrix_.resize(height, variableColOffsets_.back());
74 assertInvariants();
75 }
76
78 template<typename CONTAINER, typename DERIVED>
79 VerticalBlockMatrix(const CONTAINER& dimensions,
80 const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
81 matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
82 rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) {
83 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
84 if (variableColOffsets_.back() != matrix_.cols())
85 throw std::invalid_argument(
86 "Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
87 assertInvariants();
88 }
89
91 template<typename ITERATOR>
92 VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
93 DenseIndex height, bool appendOneDimension = false) :
94 variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)),
95 rowStart_(0), rowEnd_(height), blockStart_(0) {
96 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
97 matrix_.resize(height, variableColOffsets_.back());
98 assertInvariants();
99 }
100
106 static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
107
111 static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height);
112
114 DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
115
117 DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
118
120 DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
121
123 Block operator()(DenseIndex block) { return range(block, block+1); }
124
126 const constBlock operator()(DenseIndex block) const { return range(block, block+1); }
127
129 Block range(DenseIndex startBlock, DenseIndex endBlock) {
130 assertInvariants();
131 DenseIndex actualStartBlock = startBlock + blockStart_;
132 DenseIndex actualEndBlock = endBlock + blockStart_;
133 if(startBlock != 0 || endBlock != 0) {
134 checkBlock(actualStartBlock);
135 assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
136 }
137 const DenseIndex startCol = variableColOffsets_[actualStartBlock];
138 const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
139 return matrix_.block(rowStart_, startCol, this->rows(), rangeCols);
140 }
141
142 const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const {
143 assertInvariants();
144 DenseIndex actualStartBlock = startBlock + blockStart_;
145 DenseIndex actualEndBlock = endBlock + blockStart_;
146 if(startBlock != 0 || endBlock != 0) {
147 checkBlock(actualStartBlock);
148 assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
149 }
150 const DenseIndex startCol = variableColOffsets_[actualStartBlock];
151 const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
152 return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols);
153 }
154
156 Block full() { return range(0, nBlocks()); }
157
159 const constBlock full() const { return range(0, nBlocks()); }
160
161 DenseIndex offset(DenseIndex block) const {
162 assertInvariants();
163 DenseIndex actualBlock = block + blockStart_;
164 checkBlock(actualBlock);
165 return variableColOffsets_[actualBlock];
166 }
167
169 const DenseIndex& rowStart() const { return rowStart_; }
170
172 DenseIndex& rowStart() { return rowStart_; }
173
175 const DenseIndex& rowEnd() const { return rowEnd_; }
176
178 DenseIndex& rowEnd() { return rowEnd_; }
179
181 const DenseIndex& firstBlock() const { return blockStart_; }
182
184 DenseIndex& firstBlock() { return blockStart_; }
185
187 const Matrix& matrix() const { return matrix_; }
188
190 Matrix& matrix() { return matrix_; }
191
192 protected:
193 void assertInvariants() const {
194 assert(matrix_.cols() == variableColOffsets_.back());
195 assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
196 assert(rowStart_ <= matrix_.rows());
197 assert(rowEnd_ <= matrix_.rows());
198 assert(rowStart_ <= rowEnd_);
199 }
200
201 void checkBlock(DenseIndex block) const {
202 static_cast<void>(block); //Disable unused varibale warnings.
203 assert(matrix_.cols() == variableColOffsets_.back());
204 assert(block < (DenseIndex)variableColOffsets_.size() - 1);
205 assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
206 }
207
208 template<typename ITERATOR>
209 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
210 variableColOffsets_[0] = 0;
211 DenseIndex j=0;
212 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j)
213 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
214 if(appendOneDimension)
215 variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
216 }
217
218 friend class SymmetricBlockMatrix;
219
220 private:
222 friend class boost::serialization::access;
223 template<class ARCHIVE>
224 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
225 ar & BOOST_SERIALIZATION_NVP(matrix_);
226 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
227 ar & BOOST_SERIALIZATION_NVP(rowStart_);
228 ar & BOOST_SERIALIZATION_NVP(rowEnd_);
229 ar & BOOST_SERIALIZATION_NVP(blockStart_);
230 }
231 };
232
233}
A thin wrapper around std::vector that uses a custom allocator.
typedef and functions to augment Eigen's MatrixXd
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:75
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:112
Definition: SymmetricBlockMatrix.h:52
Definition: VerticalBlockMatrix.h:42
VerticalBlockMatrix()
Construct an empty VerticalBlockMatrix.
Definition: VerticalBlockMatrix.h:59
Block full()
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition: VerticalBlockMatrix.h:156
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition: VerticalBlockMatrix.h:129
const DenseIndex & firstBlock() const
Get the apparent first block for all operations.
Definition: VerticalBlockMatrix.h:181
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition: VerticalBlockMatrix.h:187
VerticalBlockMatrix(const CONTAINER &dimensions, DenseIndex height, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block.
Definition: VerticalBlockMatrix.h:68
DenseIndex rowEnd_
Changes apparent matrix view, see main class comment.
Definition: VerticalBlockMatrix.h:53
const DenseIndex & rowEnd() const
Get the apparent last row (exclusive, i.e.
Definition: VerticalBlockMatrix.h:175
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:114
Block operator()(DenseIndex block)
Access a single block in the underlying matrix with read/write access.
Definition: VerticalBlockMatrix.h:123
DenseIndex & firstBlock()
Get or set the apparent first block for all operations.
Definition: VerticalBlockMatrix.h:184
DenseIndex cols() const
Column size.
Definition: VerticalBlockMatrix.h:117
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition: VerticalBlockMatrix.h:92
const constBlock operator()(DenseIndex block) const
Access a const block view.
Definition: VerticalBlockMatrix.h:126
const DenseIndex & rowStart() const
Get the apparent first row of the underlying matrix for all operations.
Definition: VerticalBlockMatrix.h:169
Matrix matrix_
The full matrix.
Definition: VerticalBlockMatrix.h:49
DenseIndex nBlocks() const
Block count.
Definition: VerticalBlockMatrix.h:120
const constBlock full() const
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition: VerticalBlockMatrix.h:159
DenseIndex & rowStart()
Get or set the apparent first row of the underlying matrix for all operations.
Definition: VerticalBlockMatrix.h:172
Matrix & matrix()
Non-const access to full matrix (including any portions excluded by rowStart(), rowEnd(),...
Definition: VerticalBlockMatrix.h:190
DenseIndex & rowEnd()
Get or set the apparent last row (exclusive, i.e.
Definition: VerticalBlockMatrix.h:178
DenseIndex rowStart_
Changes apparent matrix view, see main class comment.
Definition: VerticalBlockMatrix.h:52
VerticalBlockMatrix(const CONTAINER &dimensions, const Eigen::MatrixBase< DERIVED > &matrix, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
Definition: VerticalBlockMatrix.h:79
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition: VerticalBlockMatrix.h:54
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition: VerticalBlockMatrix.h:50