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/Matrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/SymmetricBlockMatrixBlockExpr.h>

namespace gtsam {

  // Forward declarations
  class VerticalBlockMatrix;

  class GTSAM_EXPORT SymmetricBlockMatrix
  {
  public:
    typedef SymmetricBlockMatrix This;
    typedef SymmetricBlockMatrixBlockExpr<This> Block;
    typedef SymmetricBlockMatrixBlockExpr<const This> 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 { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }

    Block operator()(DenseIndex i_block, DenseIndex j_block) {
      return Block(*this, i_block, j_block);
    }

    constBlock operator()(DenseIndex i_block, DenseIndex j_block) const {
      return constBlock(*this, i_block, j_block);
    }

    Block range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) {
      assertInvariants();
      return Block(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock);
    }

    constBlock range(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const {
      assertInvariants();
      return constBlock(*this, i_startBlock, j_startBlock, i_endBlock - i_startBlock, j_endBlock - j_startBlock);
    }

    Block full()
    {
      return Block(*this, 0, nBlocks(), 0);
    }

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

    Eigen::SelfAdjointView<const Matrix, Eigen::Upper> matrix() const
    {
      return matrix_;
    }

    Eigen::SelfAdjointView<Matrix, Eigen::Upper> matrix()
    {
      return matrix_;
    }

    DenseIndex offset(DenseIndex block) const
    {
      assertInvariants();
      DenseIndex actualBlock = block + blockStart_;
      checkBlock(actualBlock);
      return variableColOffsets_[actualBlock];
    }

    DenseIndex& blockStart() { return blockStart_; }

    DenseIndex blockStart() const { return blockStart_; }

    VerticalBlockMatrix choleskyPartial(DenseIndex nFrontals);

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

    void checkBlock(DenseIndex block) const
    {
      assert(matrix_.rows() == matrix_.cols());
      assert(matrix_.cols() == variableColOffsets_.back());
      assert(block >= 0);
      assert(block < (DenseIndex)variableColOffsets_.size()-1);
      assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
    }

    DenseIndex offsetUnchecked(DenseIndex block) const
    {
      return variableColOffsets_[block + blockStart_];
    }

    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 : public gtsam::ThreadsafeException<CholeskyFailed>
  {
  public:
    CholeskyFailed() throw() {}
    virtual ~CholeskyFailed() throw() {}
  };

}