sparse_block_matrix.h

Go to the documentation of this file.

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 
// g2o is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published
// by the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
// 
// g2o is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef __SPARSE_BLOCK_MATRIX__
#define __SPARSE_BLOCK_MATRIX__

#include <map>
#include <vector>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <Eigen/Core>

#include "matrix_structure.h"
#include "g2o/config.h"

namespace g2o {
  using namespace Eigen;
template <class MatrixType = MatrixXd >
class SparseBlockMatrix {

  public:
    typedef MatrixType SparseMatrixBlock;

    inline int cols() const 
       {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
    inline int rows() const 
       {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}

    typedef std::map<int, SparseMatrixBlock*> IntBlockMap;

    SparseBlockMatrix
       (const int * rbi, const int* cbi, int rb, int cb, bool hasStorage=true);

    SparseBlockMatrix();

    ~SparseBlockMatrix();

    
    void clear(bool dealloc=false) ;

    SparseMatrixBlock* block(int r, int c, bool alloc=false);

    const SparseMatrixBlock* block(int r, int c) const;

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

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

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

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

    size_t nonZeros() const; 
    size_t nonZeroBlocks() const; 

    SparseBlockMatrix* clone() const ;

    SparseBlockMatrix* slice
       (int rmin, int rmax, int cmin, int cmax, bool alloc=true) const;

    template <class MatrixTransposedType>
    bool transpose(SparseBlockMatrix<MatrixTransposedType>*& dest) const;

    bool add(SparseBlockMatrix<MatrixType>*& dest) const ;

    template <class MatrixResultType, class MatrixFactorType>
    bool multiply
    (
       SparseBlockMatrix<MatrixResultType> *& dest,
       const SparseBlockMatrix<MatrixFactorType>* M
    ) const;

    void multiply(double*& dest, const double* src) const;

    void rightMultiply(double*& dest, const double* src) const;

    void scale( double a);

    bool symmPermutation
    (
       SparseBlockMatrix<MatrixType>*& dest,
       const int* pinv, bool onlyUpper=false
    ) const;

    int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const;

    int fillCCS(double* Cx, bool upperTriangle = false) const;

    void fillBlockStructure(MatrixStructure& ms) const;

    const std::vector<IntBlockMap>& blockCols() const { return _blockCols;}
    std::vector<IntBlockMap>& blockCols() { return _blockCols;}

    const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
    std::vector<int>& rowBlockIndices() { return _rowBlockIndices;}

    const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
    std::vector<int>& colBlockIndices() { return _colBlockIndices;}

    bool writeOctave(const char* filename, bool upperTriangle = true) const;

  protected:
    std::vector<int> _rowBlockIndices;

    std::vector<int> _colBlockIndices;

    std::vector <IntBlockMap> _blockCols;

    bool _hasStorage;
};

template < class  MatrixType >
std::ostream& operator<<
   (std::ostream&, const SparseBlockMatrix<MatrixType>& m);

  typedef SparseBlockMatrix<MatrixXd> SparseBlockMatrixXd;   

} //end namespace

#include "sparse_block_matrix.hpp"

#endif