SymmetricBlockMatrix.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/types.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cassert>
#include <stdexcept>
#include <array>

namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */

namespace gtsam {

  // Forward declarations
  class VerticalBlockMatrix;

  class GTSAM_EXPORT SymmetricBlockMatrix
  {
  public:
    typedef SymmetricBlockMatrix This;
    typedef Eigen::Block<Matrix> Block;
    typedef Eigen::Block<const Matrix> constBlock;

  protected:
    Matrix matrix_; 
    FastVector<DenseIndex> variableColOffsets_; 

    DenseIndex blockStart_; 

  public:
    SymmetricBlockMatrix() :
      blockStart_(0)
    {
      variableColOffsets_.push_back(0);
      assertInvariants();
    }

    template<typename CONTAINER>
    SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) :
      blockStart_(0)
    {
      fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
      matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
      assertInvariants();
    }

    template<typename ITERATOR>
    SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) :
      blockStart_(0)
    {
      fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
      matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
      assertInvariants();
    }

    template<typename CONTAINER>
    SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
      blockStart_(0)
    {
      matrix_.resize(matrix.rows(), matrix.cols());
      matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
      fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
      if(matrix_.rows() != matrix_.cols())
        throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
      if(variableColOffsets_.back() != matrix_.cols())
        throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix.");
      assertInvariants();
    }

    static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other);

    static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other);

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

    DenseIndex cols() const { return rows(); }

    DenseIndex nBlocks() const { return nActualBlocks() - blockStart_; }

    DenseIndex getDim(DenseIndex block) const {
      return calcIndices(block, block, 1, 1)[2];
    }


    Matrix block(DenseIndex I, DenseIndex J) const;

    Eigen::SelfAdjointView<Block, Eigen::Upper> diagonalBlock(DenseIndex J) {
      return block_(J, J).selfadjointView<Eigen::Upper>();
    }

    Eigen::SelfAdjointView<constBlock, Eigen::Upper> diagonalBlock(DenseIndex J) const {
      return block_(J, J).selfadjointView<Eigen::Upper>();
    }

    Vector diagonal(DenseIndex J) const {
      return block_(J, J).diagonal();
    }

    constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const {
      assert(I < J);
      return block_(I, J);
    }

    Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView(
        DenseIndex I, DenseIndex J) const {
      assert(J > I);
      return block_(I, I, J - I, J - I).selfadjointView<Eigen::Upper>();
    }

    Eigen::TriangularView<constBlock, Eigen::Upper> triangularView(DenseIndex I,
                                                                   DenseIndex J) const {
      assert(J > I);
      return block_(I, I, J - I, J - I).triangularView<Eigen::Upper>();
    }

    constBlock aboveDiagonalRange(DenseIndex i_startBlock,
                                  DenseIndex i_endBlock,
                                  DenseIndex j_startBlock,
                                  DenseIndex j_endBlock) const {
      assert(i_startBlock < j_startBlock);
      assert(i_endBlock <= j_startBlock);
      return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
                   j_endBlock - j_startBlock);
    }

    Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock,
                             DenseIndex j_startBlock, DenseIndex j_endBlock) {
      assert(i_startBlock < j_startBlock);
      assert(i_endBlock <= j_startBlock);
      return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
                   j_endBlock - j_startBlock);
    }


    template <typename XprType>
    void setDiagonalBlock(DenseIndex I, const XprType& xpr) {
      block_(I, I).triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
    }

    template <typename XprType>
    void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
      assert(I != J);
      if (I < J) {
        block_(I, J) = xpr;
      } else {
        block_(J, I) = xpr.transpose();
      }
    }

    template <typename XprType>
    void updateDiagonalBlock(DenseIndex I, const XprType& xpr) {
      // TODO(gareth): Eigen won't let us add triangular or self-adjoint views
      // here, so we do it manually.
      auto dest = block_(I, I);
      assert(dest.rows() == xpr.rows());
      assert(dest.cols() == xpr.cols());
      for (DenseIndex col = 0; col < dest.cols(); ++col) {
        for (DenseIndex row = 0; row <= col; ++row) {
          dest(row, col) += xpr(row, col);
        }
      }
    }

    template <typename XprType>
    void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
      assert(I != J);
      if (I < J) {
        block_(I, J).noalias() += xpr;
      } else {
        block_(J, I).noalias() += xpr.transpose();
      }
    }


    Eigen::SelfAdjointView<Block, Eigen::Upper> selfadjointView() {
      return full().selfadjointView<Eigen::Upper>();
    }

    Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView() const {
      return full().selfadjointView<Eigen::Upper>();
    }

    template <typename XprType>
    void setFullMatrix(const XprType& xpr) {
      full().triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
    }

    void setZero() {
      full().triangularView<Eigen::Upper>().setZero();
    }

    void negate() {
      full().triangularView<Eigen::Upper>() *= -1.0;
    }

    void invertInPlace() {
      const auto identity = Matrix::Identity(rows(), rows());
      full().triangularView<Eigen::Upper>() =
          selfadjointView()
              .llt()
              .solve(identity)
              .triangularView<Eigen::Upper>();
    }


    DenseIndex& blockStart() { return blockStart_; }

    DenseIndex blockStart() const { return blockStart_; }

    void choleskyPartial(DenseIndex nFrontals);

    VerticalBlockMatrix split(DenseIndex nFrontals);

  protected:

    DenseIndex nOffsets() const {
      return variableColOffsets_.size();
    }

    DenseIndex nActualBlocks() const {
      return nOffsets() - 1;
    }

    DenseIndex offset(DenseIndex block) const {
      assert(block >= 0);
      const DenseIndex actual_index = block + blockStart();
      assert(actual_index < nOffsets());
      return variableColOffsets_[actual_index];
    }

    constBlock block_(DenseIndex iBlock, DenseIndex jBlock,
                      DenseIndex blockRows = 1, DenseIndex blockCols = 1) const {
      const std::array<DenseIndex, 4> indices =
          calcIndices(iBlock, jBlock, blockRows, blockCols);
      return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
    }

    Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows = 1,
                 DenseIndex blockCols = 1) {
      const std::array<DenseIndex, 4> indices =
          calcIndices(iBlock, jBlock, blockRows, blockCols);
      return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
    }

    constBlock full() const {
      return block_(0, 0, nBlocks(), nBlocks());
    }

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

    std::array<DenseIndex, 4> calcIndices(DenseIndex iBlock, DenseIndex jBlock,
                                          DenseIndex blockRows,
                                          DenseIndex blockCols) const {
      assert(blockRows >= 0);
      assert(blockCols >= 0);

      // adjust indices to account for start and size of blocks
      const DenseIndex denseI = offset(iBlock);
      const DenseIndex denseJ = offset(jBlock);
      const DenseIndex denseRows = offset(iBlock + blockRows) - denseI;
      const DenseIndex denseCols = offset(jBlock + blockCols) - denseJ;
      return {{denseI, denseJ, denseRows, denseCols}};
    }

    void assertInvariants() const
    {
      assert(matrix_.rows() == matrix_.cols());
      assert(matrix_.cols() == variableColOffsets_.back());
      assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
    }

    template<typename ITERATOR>
    void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension)
    {
      variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
      variableColOffsets_[0] = 0;
      DenseIndex j=0;
      for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
        variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
        ++ j;
      }
      if(appendOneDimension)
      {
        variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
        ++ j;
      }
    }

    friend class VerticalBlockMatrix;
    template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;

  private:
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      // Fill in the lower triangle part of the matrix, so boost::serialization won't
      // complain about uninitialized data with an input_stream_error exception
      // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error
      matrix_.triangularView<Eigen::Lower>() = matrix_.triangularView<Eigen::Upper>().transpose();
      ar & BOOST_SERIALIZATION_NVP(matrix_);
      ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
      ar & BOOST_SERIALIZATION_NVP(blockStart_);
    }
  };

  class CholeskyFailed;

}