sparse_block_matrix_ccs.h

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
#ifndef G2O_SPARSE_BLOCK_MATRIX_CCS_H
#define G2O_SPARSE_BLOCK_MATRIX_CCS_H
 
#include "Base/Headers/Buffer.hpp"
#include "Base/Headers/Dictionary.h"
#include <Eigen/Core>
 
#include "matrix_operations.h"
 
namespace NDEVR 
{

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

      int cols() const {return _colBlockIndices->size() ? _colBlockIndices->last() : 0;}
      int rows() const {return _rowBlockIndices->size() ? _rowBlockIndices->last() : 0;}

      struct RowBlock
      {
        int row;              
        MatrixType block;    
        RowBlock() 
            : row(-1)
        {}
        RowBlock(int r)
            : row(r)
        {
            //if (r < 0 || r > Constant<sint02>::Max)
              //  throw "Bad Call";
        }
        RowBlock(int r, MatrixType b) 
            : row(r)
            , block(b) 
        {
            //if (r < 0 || r > Constant<sint02>::Max)
              //  throw "Bad Call";
        }
        bool operator<(const RowBlock& other) const { return row < other.row;}
      };
      typedef Buffer<RowBlock> SparseColumn;
 
      SparseBlockMatrixCCS()
        : _rowBlockIndices(nullptr), _colBlockIndices(nullptr)
      {}
      SparseBlockMatrixCCS(const Buffer<int>& rowIndices, const Buffer<int>& colIndices) :
        _rowBlockIndices(&rowIndices), _colBlockIndices(&colIndices)
      {}

      int rowsOfBlock(int r) const { return r ? (*_rowBlockIndices)[r] - (*_rowBlockIndices)[r-1] : (*_rowBlockIndices)[0] ; }

      int colsOfBlock(int c) const { return c ? (*_colBlockIndices)[c] - (*_colBlockIndices)[c-1] : (*_colBlockIndices)[0]; }

      int rowBaseOfBlock(int r) const { return r ? (*_rowBlockIndices)[r-1] : 0 ; }

      int colBaseOfBlock(int c) const { return c ? (*_colBlockIndices)[c-1] : 0 ; }

      const Buffer<SparseColumn>& columns() const { return m_block_cols;}
      Buffer<SparseColumn>& columns() { return m_block_cols;}

      const Buffer<int>& rowBlockIndices() const { return (*_rowBlockIndices);}

      const Buffer<int>& colBlockIndices() const { return (*_colBlockIndices);}
 
      void rightMultiply(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());
 
        for (uint04 i=0; i < m_block_cols.size(); ++i)
        {
          int destOffset = colBaseOfBlock(i);
          for (auto it = m_block_cols[i].begin(); it != m_block_cols[i].end(); ++it)
          {
            const SparseMatrixBlock& a = it->block;
            int srcOffset = rowBaseOfBlock(it->row);
            // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
            internal::atxpy(a, srcVec, srcOffset, destVec, destOffset);
          }
        }
      }

      void sortColumns()
      {
        for (uint04 i=0; i < m_block_cols.size(); ++i)
        {
          std::sort(m_block_cols[i].begin(), m_block_cols[i].end());
        }
      }

      int fillCCS(int* Cp, int* Ci, g_type* Cx, bool upperTriangle = false) const
      {
        assert(Cp && Ci && Cx && "Target destination is NULL");
        int nz=0;
        for (uint04 i=0; i < m_block_cols.size(); ++i)
        {
          int cstart=i ? (*_colBlockIndices)[i-1] : 0;
          int csize=colsOfBlock(i);
          for (int c=0; c<csize; ++c) {
            *Cp=nz;
            for (auto it = m_block_cols[i].begin(); it != m_block_cols[i].end(); ++it)
            {
              const SparseMatrixBlock* b = it->block;
              int rstart=it->row ? (*_rowBlockIndices)[it->row-1] : 0;
 
              int elemsToCopy = b->rows();
              if (upperTriangle && rstart == cstart)
                elemsToCopy = c + 1;
              for (int r=0; r<elemsToCopy; ++r){
                *Cx++ = (*b)(r,c);
                *Ci++ = rstart++;
                ++nz;
              }
            }
            ++Cp;
          }
        }
        *Cp=nz;
        return nz;
      }

      int fillCCS(g_type* Cx, bool upperTriangle = false) const
      {
        assert(Cx && "Target destination is NULL");
        g_type* CxStart = Cx;
        int cstart = 0;
        for (uint04 i=0; i < m_block_cols.size(); ++i)
        {
          int csize = (*_colBlockIndices)[i] - cstart;
          for (int c = 0; c < csize; ++c) 
          {
            for (auto it = m_block_cols[i].begin(); it != m_block_cols[i].end(); ++it)
            {
              const SparseMatrixBlock* b = it->block;
              int rstart = it->row ? (*_rowBlockIndices)[it->row-1] : 0;
 
              int elemsToCopy = b->rows();
              if (upperTriangle && rstart == cstart)
                elemsToCopy = c + 1;
              memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(g_type));
              Cx += elemsToCopy;
 
            }
          }
          cstart = (*_colBlockIndices)[i];
        }
        return Cx - CxStart;
      }
 
    protected:
      const Buffer<int>* _rowBlockIndices; 
      const Buffer<int>* _colBlockIndices; 
      Buffer<SparseColumn> m_block_cols;     
  };
 
 

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

      int cols() const {return _colBlockIndices->size() ? _colBlockIndices->last() : 0;}
      int rows() const {return _rowBlockIndices->size() ? _rowBlockIndices->last() : 0;}
 
      typedef Dictionary<int, MatrixType> SparseColumn;
 
      SparseBlockMatrixHashMap(const Buffer<int>& rowIndices, const Buffer<int>& colIndices) :
        _rowBlockIndices(&rowIndices), _colBlockIndices(&colIndices)
      {}

      int rowsOfBlock(int r) const { return r ? (*_rowBlockIndices)[r] - (*_rowBlockIndices)[r-1] : (*_rowBlockIndices)[0] ; }

      int colsOfBlock(int c) const { return c ? (*_colBlockIndices)[c] - (*_colBlockIndices)[c-1] : (*_colBlockIndices)[0]; }

      int rowBaseOfBlock(int r) const { return r ? (*_rowBlockIndices)[r-1] : 0 ; }

      int colBaseOfBlock(int c) const { return c ? (*_colBlockIndices)[c-1] : 0 ; }

      const Buffer<SparseColumn>& columns() const { return m_block_cols;}
      Buffer<SparseColumn>& columns() { return m_block_cols;}

      const Buffer<int>& rowBlockIndices() const { return (*_rowBlockIndices);}

      const Buffer<int>& colBlockIndices() const { return (*_colBlockIndices);}

      MatrixType* addBlock(int r, uint04 c, bool zeroBlock = false)
      {
        SparseColumn& sparseColumn = m_block_cols[c];
        auto iter = sparseColumn.find(r);
        if (iter == sparseColumn.end())
        {
          int rb = rowsOfBlock(r);
          int cb = colsOfBlock(c);
          MatrixType m(rb, cb);
          if (zeroBlock)
            m.setZero();
          sparseColumn[r] = m;
          return &sparseColumn[r];
        }
        return &iter.value();
      }
 
    protected:
      const Buffer<int>* _rowBlockIndices; 
      const Buffer<int>* _colBlockIndices; 
      Buffer<SparseColumn> m_block_cols;     
  };
 
} //end namespace
 
#endif