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/Matrix.h>
#include <gtsam/base/MatrixSerialization.h>
#include <gtsam/base/FastVector.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);
      assertInvariants();
    }
 
    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);
    }
 
    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
  };
 
}