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>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
#include <algorithm>
#include <array>
#include <cassert>
#include <cstring>
#include <stdexcept>
#include <vector>
 
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
 
namespace gtsam {
 
namespace internal {
 
class CompactLeafSchurKernel;

struct BlockColumnRange {
  DenseIndex begin;
  DenseIndex end;

  bool operator()(DenseIndex column) const {
    return column >= begin && column < end;
  }

  bool owns(DenseIndex I, DenseIndex J) const {
    return (*this)(std::max(I, J));
  }
};
}  // namespace internal
 
// 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();

  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];
  }

  DenseIndex blockScalarOffset(DenseIndex block) const { return offset(block); }


  Matrix block(DenseIndex I, DenseIndex J) const;

  constBlock blockView(DenseIndex I, DenseIndex J) const {
    return block_(I, J);
  }

  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 updateDiagonalBlockAt(DenseIndex scalarOffset, const XprType& xpr) {
    auto dest =
        matrix_.block(scalarOffset, scalarOffset, xpr.rows(), xpr.cols());
    assert(dest.rows() == dest.cols());
    for (DenseIndex col = 0; col < dest.cols(); ++col) {
      for (DenseIndex row = 0; row <= col; ++row) {
        dest(row, col) += xpr(row, col);
      }
    }
  }

  template <int Dimension, typename XprType>
  void updateFixedDiagonalBlockAt(DenseIndex scalarOffset, const XprType& xpr) {
    auto dest = matrix_.template block<Dimension, Dimension>(scalarOffset,
                                                             scalarOffset);
    for (int col = 0; col < Dimension; ++col) {
      for (int row = 0; row <= col; ++row) {
        dest(row, col) += xpr(row, col);
      }
    }
  }

  void addToDiagonalBlock(DenseIndex I, const Vector& deltaDiag) {
    auto dest = block_(I, I);
    assert(dest.rows() == deltaDiag.size());
    dest.diagonal().array() += deltaDiag.array();
  }

  void addScaledIdentity(DenseIndex I, double lambda) {
    auto dest = block_(I, I);
    dest.diagonal().array() += lambda;
  }

  template <typename XprType>
  void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
    assert(I != J);
    if constexpr (XprType::SizeAtCompileTime == 1) {
      if (I < J) {
        auto destination = block_(I, J);
        assert(destination.rows() == 1 && destination.cols() == 1);
        destination(0, 0) += xpr.coeff(0, 0);
      } else {
        auto destination = block_(J, I);
        assert(destination.rows() == 1 && destination.cols() == 1);
        destination(0, 0) += xpr.coeff(0, 0);
      }
    } else {
      if (I < J) {
        block_(I, J).noalias() += xpr;
      } else {
        block_(J, I).noalias() += xpr.transpose();
      }
    }
  }

  template <typename XprType>
  void updateOffDiagonalBlockAt(DenseIndex rowOffset, DenseIndex colOffset,
                                const XprType& xpr) {
    assert(rowOffset < colOffset);
    if constexpr (XprType::SizeAtCompileTime == 1) {
      matrix_(rowOffset, colOffset) += xpr.coeff(0, 0);
    } else {
      matrix_.block(rowOffset, colOffset, xpr.rows(), xpr.cols()) += xpr;
    }
  }

  template <int Rows, int Cols, typename XprType>
  void updateFixedOffDiagonalBlockAt(DenseIndex rowOffset, DenseIndex colOffset,
                                     const XprType& xpr) {
    assert(rowOffset < colOffset);
    if constexpr (Rows == 1 && Cols == 1) {
      assert(xpr.rows() == 1 && xpr.cols() == 1);
      matrix_(rowOffset, colOffset) += xpr.coeff(0, 0);
    } else {
      matrix_.template block<Rows, Cols>(rowOffset, colOffset) += xpr;
    }
  }

  void updateFromMappedBlocks(const SymmetricBlockMatrix& other,
                              const std::vector<DenseIndex>& blockIndices);

  void updateFromMappedBlocks(const SymmetricBlockMatrix& other,
                              const std::vector<DenseIndex>& blockIndices,
                              const std::vector<DenseIndex>& scalarOffsets);

  void updateFromMappedBlocks(const SymmetricBlockMatrix& other,
                              DenseIndex sourceBlockStart,
                              const std::vector<DenseIndex>& blockIndices,
                              const std::vector<DenseIndex>& scalarOffsets);

  void updateFromMappedBlockColumn(const SymmetricBlockMatrix& other,
                                   DenseIndex sourceBlockStart,
                                   DenseIndex ownedSourceBlock,
                                   const std::vector<DenseIndex>& blockIndices,
                                   const std::vector<DenseIndex>& scalarOffsets,
                                   bool ownLastCrossBlock = false);

  void updateFromOuterProductBlocks(const VerticalBlockMatrix& other,
                                    const std::vector<DenseIndex>& blockIndices,
                                    double alpha = 1.0);

  void updateFromOuterProductBlocks(
      const VerticalBlockMatrix& other, DenseIndex sourceRowBegin,
      DenseIndex sourceRowEnd, DenseIndex sourceBlockStart,
      const std::vector<DenseIndex>& blockIndices,
      const std::vector<DenseIndex>& scalarOffsets, double alpha = 1.0);

  void addUpperTriangular(const SymmetricBlockMatrix& other) {
    assert(nBlocks() == other.nBlocks());
    full().triangularView<Eigen::Upper>() += other.full();
  }


  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 setAllZero() { matrix_.setZero(); }

  void setZeroColumns(DenseIndex beginCol, DenseIndex endCol) {
    assert(beginCol < endCol);
    assert(beginCol >= 0);
    assert(endCol >= 0);
    assert(beginCol < nBlocks());
    assert(endCol <= nBlocks());
    static_assert(Matrix::IsRowMajor == 0,
                  "setZeroColumns requires column-major storage.");
 
    const DenseIndex denseBeginCol = offset(beginCol);
    const DenseIndex denseEndCol = offset(endCol);
 
    double* begin = matrix_.data() + denseBeginCol * matrix_.rows();
    double* end = matrix_.data() + denseEndCol * matrix_.rows();
 
    // Using memset for maximal compiler optimization.
    memset(begin, 0, (end - begin) * sizeof(*begin));
  }

  void negate();

  void invertInPlace();


  DenseIndex& blockStart() { return blockStart_; }

  DenseIndex blockStart() const { return blockStart_; }

  void choleskyPartial(DenseIndex nFrontals);

  VerticalBlockMatrix split(DenseIndex nFrontals);

  void split(DenseIndex nFrontals, VerticalBlockMatrix* RSd);
 
 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;
    friend class internal::CompactLeafSchurKernel;
  template <typename SymmetricBlockMatrixType>
  friend class SymmetricBlockMatrixBlockExpr;
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  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_);
  }
#endif
};

class CholeskyFailed;
 
}  // namespace gtsam