VerticalBlockMatrix.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/MatrixSerialization.h>
 
#include <cassert>
 
namespace gtsam {
 
// Forward declarations
class SymmetricBlockMatrix;

class GTSAM_EXPORT VerticalBlockMatrix {
 public:
  typedef VerticalBlockMatrix This;
  typedef Eigen::Block<Matrix> Block;
  typedef Eigen::Block<const Matrix> constBlock;
 
 protected:
  Matrix matrix_;  
  FastVector<DenseIndex>
      variableColOffsets_;  
 
  DenseIndex
      rowStart_;  
  DenseIndex
      rowEnd_;  
  DenseIndex
      blockStart_;  
 
 public:
  VerticalBlockMatrix() : rowStart_(0), rowEnd_(0), blockStart_(0) {
    variableColOffsets_.push_back(0);
  }
 
  // Destructor
  ~VerticalBlockMatrix() = default;
 
  // Copy constructor (default)
  VerticalBlockMatrix(const VerticalBlockMatrix& other) = default;
 
  // Copy assignment operator (default)
  VerticalBlockMatrix& operator=(const VerticalBlockMatrix& other) = default;
 
  // Move constructor
  VerticalBlockMatrix(VerticalBlockMatrix&& other) noexcept
      : matrix_(std::move(other.matrix_)),
        variableColOffsets_(std::move(other.variableColOffsets_)),
        rowStart_(other.rowStart_),
        rowEnd_(other.rowEnd_),
        blockStart_(other.blockStart_) {
    other.rowStart_ = 0;
    other.rowEnd_ = 0;
    other.blockStart_ = 0;
  }
 
  // Move assignment operator
  VerticalBlockMatrix& operator=(VerticalBlockMatrix&& other) noexcept {
    if (this != &other) {
      matrix_ = std::move(other.matrix_);
      variableColOffsets_ = std::move(other.variableColOffsets_);
      rowStart_ = other.rowStart_;
      rowEnd_ = other.rowEnd_;
      blockStart_ = other.blockStart_;
 
      other.rowStart_ = 0;
      other.rowEnd_ = 0;
      other.blockStart_ = 0;
    }
    return *this;
  }

  template <typename CONTAINER>
  VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height,
                      bool appendOneDimension = false)
      : variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
        rowStart_(0),
        rowEnd_(height),
        blockStart_(0) {
    fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
    matrix_.resize(height, variableColOffsets_.back());
    assertInvariants();
  }

  template <typename CONTAINER, typename DERIVED>
  VerticalBlockMatrix(const CONTAINER& dimensions,
                      const Eigen::MatrixBase<DERIVED>& matrix,
                      bool appendOneDimension = false)
      : matrix_(matrix),
        variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
        rowStart_(0),
        rowEnd_(matrix.rows()),
        blockStart_(0) {
    fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
    if (variableColOffsets_.back() != matrix_.cols())
      throw std::invalid_argument(
          "Requested to create a VerticalBlockMatrix with dimensions that do "
          "not sum to the total columns of the provided matrix.");
    assertInvariants();
  }

  template <typename ITERATOR>
  VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
                      DenseIndex height, bool appendOneDimension = false)
      : variableColOffsets_((lastBlockDim - firstBlockDim) +
                            (appendOneDimension ? 2 : 1)),
        rowStart_(0),
        rowEnd_(height),
        blockStart_(0) {
    fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
    matrix_.resize(height, variableColOffsets_.back());
    assertInvariants();
  }

  static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);

  static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs,
                                              DenseIndex height);

  DenseIndex rows() const {
    assertInvariants();
    return rowEnd_ - rowStart_;
  }

  DenseIndex cols() const {
    assertInvariants();
    return variableColOffsets_.back() - variableColOffsets_[blockStart_];
  }

  DenseIndex nBlocks() const {
    assertInvariants();
    return variableColOffsets_.size() - 1 - blockStart_;
  }

  Block operator()(DenseIndex block) { return range(block, block + 1); }

  const constBlock operator()(DenseIndex block) const {
    return range(block, block + 1);
  }

  Block range(DenseIndex startBlock, DenseIndex endBlock) {
    assertInvariants();
    DenseIndex actualStartBlock = startBlock + blockStart_;
    DenseIndex actualEndBlock = endBlock + blockStart_;
    if (startBlock != 0 || endBlock != 0) {
      checkBlock(actualStartBlock);
      assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
    }
    const DenseIndex startCol = variableColOffsets_[actualStartBlock];
    const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
    return matrix_.block(rowStart_, startCol, this->rows(), rangeCols);
  }
 
  const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const {
    assertInvariants();
    DenseIndex actualStartBlock = startBlock + blockStart_;
    DenseIndex actualEndBlock = endBlock + blockStart_;
    if (startBlock != 0 || endBlock != 0) {
      checkBlock(actualStartBlock);
      assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
    }
    const DenseIndex startCol = variableColOffsets_[actualStartBlock];
    const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
    return ((const Matrix&)matrix_)
        .block(rowStart_, startCol, this->rows(), rangeCols);
  }

  constBlock blockRows(DenseIndex block, DenseIndex beginRow,
                       DenseIndex endRow) const {
    assertInvariants();
    assert(beginRow >= 0 && beginRow <= endRow && endRow <= matrix_.rows());
    const DenseIndex actualBlock = block + blockStart_;
    checkBlock(actualBlock);
    const DenseIndex startCol = variableColOffsets_[actualBlock];
    const DenseIndex blockCols =
        variableColOffsets_[actualBlock + 1] - startCol;
    return matrix_.block(beginRow, startCol, endRow - beginRow, blockCols);
  }

  Block full() { return range(0, nBlocks()); }

  const constBlock full() const { return range(0, nBlocks()); }
 
  DenseIndex offset(DenseIndex block) const {
    assertInvariants();
    DenseIndex actualBlock = block + blockStart_;
    checkBlock(actualBlock);
    return variableColOffsets_[actualBlock];
  }

  const DenseIndex& rowStart() const { return rowStart_; }

  DenseIndex& rowStart() { return rowStart_; }

  const DenseIndex& rowEnd() const { return rowEnd_; }

  DenseIndex& rowEnd() { return rowEnd_; }

  const DenseIndex& firstBlock() const { return blockStart_; }

  DenseIndex& firstBlock() { return blockStart_; }

  const Matrix& matrix() const { return matrix_; }

  Matrix& matrix() { return matrix_; }
 
 protected:
  void assertInvariants() const {
    assert(matrix_.cols() == variableColOffsets_.back());
    assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
    assert(rowStart_ <= matrix_.rows());
    assert(rowEnd_ <= matrix_.rows());
    assert(rowStart_ <= rowEnd_);
  }
 
  void checkBlock(DenseIndex block) const {
    static_cast<void>(block);  // Disable unused varibale warnings.
    assert(matrix_.cols() == variableColOffsets_.back());
    assert(block < (DenseIndex)variableColOffsets_.size() - 1);
    assert(variableColOffsets_[block] < matrix_.cols() &&
           variableColOffsets_[block + 1] <= matrix_.cols());
  }
 
  template <typename ITERATOR>
  void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
                   bool appendOneDimension) {
    variableColOffsets_[0] = 0;
    DenseIndex j = 0;
    for (ITERATOR dim = firstBlockDim; dim != lastBlockDim; ++dim, ++j)
      variableColOffsets_[j + 1] = variableColOffsets_[j] + *dim;
    if (appendOneDimension)
      variableColOffsets_[j + 1] = variableColOffsets_[j] + 1;
  }
 
  friend class SymmetricBlockMatrix;
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    ar& BOOST_SERIALIZATION_NVP(matrix_);
    ar& BOOST_SERIALIZATION_NVP(variableColOffsets_);
    ar& BOOST_SERIALIZATION_NVP(rowStart_);
    ar& BOOST_SERIALIZATION_NVP(rowEnd_);
    ar& BOOST_SERIALIZATION_NVP(blockStart_);
  }
#endif
};
 
}  // namespace gtsam