sparse_block_matrix_diagonal.h

#pragma once
#include "Base/Headers/Buffer.hpp"
#include <Eigen/Core>
#include <Eigen/StdVector>
 
#include "matrix_operations.h"
 
namespace NDEVR {

  template <class MatrixType>
  class SparseBlockMatrixDiagonal
  {
    public:
      typedef MatrixType SparseMatrixBlock;

      int cols() const {return _blockIndices && (*_blockIndices).size() ? (*_blockIndices).last() : 0;}
      int rows() const {return _blockIndices && (*_blockIndices).size() ? (*_blockIndices).last() : 0;}
 
      typedef Buffer<MatrixType>      DiagonalVector;
 
      SparseBlockMatrixDiagonal()
      {}
      SparseBlockMatrixDiagonal(const Buffer<int>& blockIndices) :
        _blockIndices(&blockIndices)
      {}

      inline int dimOfBlock(int r) const { return r ? (*_blockIndices)[r] - (*_blockIndices)[r-1] : (*_blockIndices)[0] ; }

      inline int baseOfBlock(int r) const { return r ? (*_blockIndices)[r-1] : 0 ; }

      const DiagonalVector& diagonal() const { return _diagonal;}
      DiagonalVector& diagonal() { return _diagonal;}

      const Buffer<int>& blockIndices() const { return (*_blockIndices);}
 
      void multiply(g_type*& dest, const g_type* src) const
      {
        int destSize=cols();
        if (! dest) {
          dest=new g_type[destSize];
          memset(dest,0, destSize*sizeof(g_type));
        }
 
        // map the memory by Eigen
        Eigen::Map<Eigen::VectorX<g_type>> destVec(dest, destSize);
        Eigen::Map<const Eigen::VectorX<g_type>> srcVec(src, rows());
 
#      ifdef G2O_OPENMP
#      pragma omp parallel for default (shared) schedule(dynamic, 10)
#      endif
        for (int i=0; i < static_cast<int>(_diagonal.size()); ++i){
          int destOffset = baseOfBlock(i);
          int srcOffset = destOffset;
          const SparseMatrixBlock& A = _diagonal[i];
          // destVec += *A.transpose() * srcVec (according to the sub-vector parts)
          internal::axpy(A, srcVec, srcOffset, destVec, destOffset);
        }
      }
 
    protected:
      const Buffer<int>* _blockIndices = nullptr; 
      DiagonalVector _diagonal;
  };
 
}