gtsam  4.1.0
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>
21 #include <gtsam/base/FastVector.h>
22 
23 namespace gtsam {
24 
25  // Forward declarations
26  class SymmetricBlockMatrix;
27 
41  class GTSAM_EXPORT VerticalBlockMatrix
42  {
43  public:
44  typedef VerticalBlockMatrix This;
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 }
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
gtsam::VerticalBlockMatrix::variableColOffsets_
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition: VerticalBlockMatrix.h:50
gtsam::VerticalBlockMatrix::rowEnd
DenseIndex & rowEnd()
Get or set the apparent last row (exclusive, i.e.
Definition: VerticalBlockMatrix.h:178
gtsam::VerticalBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition: VerticalBlockMatrix.h:120
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:114
gtsam::VerticalBlockMatrix::rowEnd
const DenseIndex & rowEnd() const
Get the apparent last row (exclusive, i.e.
Definition: VerticalBlockMatrix.h:175
gtsam::VerticalBlockMatrix::full
const constBlock full() const
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition: VerticalBlockMatrix.h:159
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition: VerticalBlockMatrix.h:129
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
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
gtsam::VerticalBlockMatrix::full
Block full()
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition: VerticalBlockMatrix.h:156
gtsam::VerticalBlockMatrix::rowStart_
DenseIndex rowStart_
Changes apparent matrix view, see main class comment.
Definition: VerticalBlockMatrix.h:52
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix()
Construct an empty VerticalBlockMatrix.
Definition: VerticalBlockMatrix.h:59
gtsam::VerticalBlockMatrix::matrix_
Matrix matrix_
The full matrix.
Definition: VerticalBlockMatrix.h:49
gtsam::VerticalBlockMatrix::operator()
const constBlock operator()(DenseIndex block) const
Access a const block view.
Definition: VerticalBlockMatrix.h:126
gtsam::VerticalBlockMatrix::rowStart
const DenseIndex & rowStart() const
Get the apparent first row of the underlying matrix for all operations.
Definition: VerticalBlockMatrix.h:169
gtsam::VerticalBlockMatrix::blockStart_
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition: VerticalBlockMatrix.h:54
gtsam::VerticalBlockMatrix::firstBlock
const DenseIndex & firstBlock() const
Get the apparent first block for all operations.
Definition: VerticalBlockMatrix.h:181
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition: VerticalBlockMatrix.h:92
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition: VerticalBlockMatrix.h:187
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition: VerticalBlockMatrix.h:117
gtsam::VerticalBlockMatrix::matrix
Matrix & matrix()
Non-const access to full matrix (including any portions excluded by rowStart(), rowEnd(),...
Definition: VerticalBlockMatrix.h:190
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:52
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix(const CONTAINER &dimensions, DenseIndex height, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block.
Definition: VerticalBlockMatrix.h:68
gtsam::VerticalBlockMatrix::rowStart
DenseIndex & rowStart()
Get or set the apparent first row of the underlying matrix for all operations.
Definition: VerticalBlockMatrix.h:172
gtsam::VerticalBlockMatrix::firstBlock
DenseIndex & firstBlock()
Get or set the apparent first block for all operations.
Definition: VerticalBlockMatrix.h:184
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam::VerticalBlockMatrix::rowEnd_
DenseIndex rowEnd_
Changes apparent matrix view, see main class comment.
Definition: VerticalBlockMatrix.h:53
gtsam::VerticalBlockMatrix::operator()
Block operator()(DenseIndex block)
Access a single block in the underlying matrix with read/write access.
Definition: VerticalBlockMatrix.h:123