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>

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:
#ifdef 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
  };

}