Decided how to handle different storing order in the repo

[GiulioRomualdi]

As written in the Eigen documentation, the default storage order of matrices is ColMajor, however in iDynTree the matrices are stored as RowMajor.

This raises a problem when we try to use the Eigen::Ref. Right now, in all the interfaces we use Eigen::Ref<Eigen::MatrixXd> for passing matrices, thanks to this we can write:

void cov(const Ref<const MatrixXf> x, const Ref<const MatrixXf> y, Ref<MatrixXf> C)
{
  const float num_observations = static_cast<float>(x.rows());
  const RowVectorXf x_mean = x.colwise().sum() / num_observations;
  const RowVectorXf y_mean = y.colwise().sum() / num_observations;
  C = (x.rowwise() - x_mean).transpose() * (y.rowwise() - y_mean) / num_observations;
}

MatrixXf m1, m2, m3
cov(m1, m2, m3);
cov(m1.leftCols<3>(), m2.leftCols<3>(), m3.topLeftCorner<3,3>());

The same applies also with maps

float m1[9]. m2[9], m3[9];

Eigen::Map<Eigen::MatrixXf> map1(m1, 3, 3);
Eigen::Map<Eigen::MatrixXf> map2(m3, 3, 3);
Eigen::Map<Eigen::MatrixXf> map3(m3, 3, 3);

cov(map1, map2, map3);

Unfortunately iDynTree store the matrices in RowMajor and the following lines of code will not compile

void foo(Eigen::Ref<Eigen::MatrixXd> m)
{
     return;
}

int main()
{
   iDynTree::MatrixDynSize matrix(2,2);
   Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> map = iDynTree::toEigen(matrix);

   foo(map);
}

/home/gromualdi/robot-code/test_eigen/eigen.cpp:26:12: error: could not convert ‘map’ from ‘Eigen::Map<Eigen::Matrix<double, -1, -1, 1> >’ to ‘Eigen::Ref<Eigen::Matrix<double, -1, -1> >’
     foo(map);

This is a huge problem because we cannot call the functions we implemented so far with Map of RowMajor object (we cannot use iDynTree easily).

Discussing with @S-Dafarra we noticed that it is possible to play with the Eigen::Stride for viewing an RowMajor matrix to a ColMajor one. For instance:

double array1[6];
    for(int i = 0; i < 6; ++i)
        array1[i] = i;

    cout << Map<MatrixXd, 0, Stride<Dynamic,Dynamic>>(array1, 2, 3, Stride<Dynamic,Dynamic>(1, 3))<< endl;

    cout << Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>, 0, Stride<Dynamic,Dynamic>>(array1, 2, 3, Stride<Dynamic,Dynamic>(3, 1)) << endl;

gives the following output

0 1 2
3 4 5

0 1 2
3 4 5

Unfortunately, this does not solve our problem since

void foo(Eigen::Ref<Eigen::MatrixXd> m)
{
     return;
}

foo( Map<MatrixXd, 0, Stride<Dynamic,Dynamic>>(array1, 2, 3, Stride<Dynamic,Dynamic>(1, 3)));

does not compile and give us the following error:

/home/gromualdi/robot-code/test_eigen/manif.cpp: In function ‘int main()’:
/home/gromualdi/robot-code/test_eigen/manif.cpp:26:12: error: could not convert ‘map’ from ‘Eigen::Map<Eigen::Matrix<double, -1, -1, 1> >’ to ‘Eigen::Ref<Eigen::Matrix<double, -1, -1> >’
     foo(map);
            ^
/home/gromualdi/robot-code/test_eigen/manif.cpp:79:9: error: could not convert ‘Eigen::Map<Eigen::Matrix<double, -1, -1>, 0, Eigen::Stride<-1, -1> >(((double*)(& array2)), 2, 3, Eigen::Stride<-1, -1>(1, 3))’ from ‘Eigen::Map<Eigen::Matrix<double, -1, -1>, 0, Eigen::Stride<-1, -1> >’ to ‘Eigen::Ref<Eigen::Matrix<double, -1, -1> >’
     foo(Map<MatrixXd, 0, Stride<Dynamic,Dynamic> >
         ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
         (array2, 2, 3, Stride<Dynamic,Dynamic>(1, 3)));
         ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

So far I found two possible solutions:

1. We start using only the RowMajor matrices in blf

   • pros:
     • compatible with iDynTree
     • easy to change
   • cons:
     • is manif still working?
     • if one day we need a different library (e.g. Pinocchio) we need to change all the code again.

2. We templatize everything

   • pros:
     • we should be able to handle different matrices
   • cons:
     • Are we ready for this?

@traversaro @prashanth @s-dafarra

[traversaro]

Is this really a problem? I mean, now that KinDynComputations will support MatrixView, I don't see a lot of reasons for using iDynTree Matrices over regular Eigen matrices, so it is not clear to me where you need to have an iDynTree matrix in blf and then pass it to a function that takes in input Eigen::Ref .

[S-Dafarra]

Unfortunately, this does not solve our problem since

void foo(Eigen::Ref<Eigen::MatrixXd> m)
{
     return;
}

foo( Map<MatrixXd, 0, Stride<Dynamic,Dynamic>>(array1, 2, 3, Stride<Dynamic,Dynamic>(1, 3)));

does not compile and give us the following error:

What if you use

void foo(Eigen::Ref<Eigen::MatrixXd, 0, Stride<Dynamic,Dynamic>> m)
{
     return;
}

foo( Map<MatrixXd, 0, Stride<Dynamic,Dynamic>>(array1, 2, 3, Stride<Dynamic,Dynamic>(1, 3)));

?

See https://eigen.tuxfamily.org/dox/classEigen_1_1Ref.html

[GiulioRomualdi]

I mean, now that KinDynComputations will support MatrixViex

I hope it will happen soon but I've several problems with the implementation.

@S-Dafarra https://github.com/dic-iit/bipedal-locomotion-framework/issues/100#issuecomment-688913457 seems to solve the problem.

I try to focus on the MatrixView https://github.com/robotology/idyntree/pull/730 even if I think we will have a similar problem

[S-Dafarra]

@GiulioRomualdi probably we can close this right?

[GiulioRomualdi]

@GiulioRomualdi probably we can close this right?

yes exactly.

Just a small recap. As @traversaro wrote in https://github.com/dic-iit/bipedal-locomotion-framework/issues/100#issuecomment-688907687 is not really necessary. Furthermore now thanks to https://github.com/robotology/idyntree/pull/734 and https://github.com/robotology/idyntree/pull/736 the usage of iDynTree objects in the repo can be sensitively reduced