Implement `submatrix` style helper for accessing submatrix of a bigger matrix

[traversaro]

After chasing a bug related to that for a long time in the new linearization code, I think it is now time to solve the problem at the root.

When writing algorithms, we often need to fill "submatrix" of a bigger matrix. Eigen already provide really nice block helpers, see http://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html. Note that this methods work also on Eigen::Map objects, so they can directly used on iDynTree matrices. Even using this methods, the logic for computing the boundary of a "block" can be complex and bug-prone.

A nice solution for this problem is the submatrix matlab class implemented in https://github.com/iron76/bnt_time_varying/blob/master/src/%40submatrix/submatrix.m by @iron76 . Citing its documentation:

% This class allows to access a given matrix A subdivided into blocks of
% arbitrary (and not necessarily constant dimension). Lets assume that we
% want to access a matrix A with 'm' rows and 'n' columns. Rows are
% subdivided into M blocks of dimensions m_1, ..., m_M. Columns are
% subdivide into blocks of dimensions n_1, ..., n_N. Of course, we have
% that m = m_1 + ... + m_M and n = n_1 + ... + n_N. The class can be
% instantiated with the following definition:
%
% As = submatrix(A, [m_1 ... m_M], [n_1 ... n_N])
%
% or alternatively
%
% As = submatrix([m_1 ... m_M], [n_1 ... n_N])
%
% in the latter case the matrix being instantiated identically null of
% suitable dimensions given the vectors [m_1 ... m_M], [n_1 ... n_N].
% The submatrix A_ij corresponding to the blocks m_i, n_j can be obtained
% by simply using:
%
% A_ij = As(i,j)
%

I think we need something similar wrapping Eigen Matrices, starting from Dense matrix and perhaps extending it to sparse matrices. It will be a completely private header (as it uses Eigen) that it would not be exposed to the users, but just used in the algorithm implementations.

cc @nunoguedelha you already planned something like this for the BERDY implementation?

[iron76]

:+1:

[nunoguedelha]

@traversaro Yes, actually I was planning to use some kind of wrapping although I hadn't looked into the respective implementation details yet. We definitely need it. Regarding the block sizes, as for instance, matrix D is split into the sub-matrices D{x,x} , D{x,a} , ... , the sub-blocks in those sub-matrices are of constant size. This should simplify the wrapping.

[traversaro]

I looked into it, and apparently all the block methods of Eigen return an object of type Eigen::Block<Derived> (see http://eigen.tuxfamily.org/dox/classEigen_1_1Block.html and http://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html .

The submatrix helper could then have a interface like:

template<typename Derived>
class SubMatrixHelper
{
 private:
     //private stuff .. .
 public:
        MatrixFixSize();

        MatrixFixSize(Derived & wrappedMatrix, const std::vector<size_t> & rowBlockSizes, const std::vector<size_t> & colBlockSizes);

        Eigen::Block<Derived> operator(size_t rowBlock, size_t colBlock); 
}