Document how to write allocation-free code using iDynTree's data structures and iDynTree::toEigen

[traversaro]

Discussing with @prashanthr05 , it turns out that it is not clear why classes such as iDynTree::VectorDynSize or iDynTree::MatrixDynSize do not implement the overload for operator*, and why iDynTree::toEigen (or some equivalent method for any existing matrix library) should be used instead. We should document that.

In a nutshell, the point is that if A is a iDynTree::MatrixDynSize and b and c are iDynTree::VectorDynSize of consistent size, any naive implementation of iDynTree::MatrixDynSize::operator* will result in a memory allocation for any code such as:

b = A*c;

while the equivalent code using toEigen is allocation-free:

toEigen(b) = toEigen(A)*toEigen(c)

the reason for this are two:

• Eigen operations do not return an actual vector object, but they return an object that represents an expression that will be evaluated during the assignement to an actual vector (see discussion in https://eigen.tuxfamily.org/dox/TopicInsideEigenExample.html )
• iDynTree::toEigen returns Eigen::Map objects (see https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html), so no memory is actually copied unless it is strictly necessary.

[DanielePucci]

CC
@GiulioRomualdi @S-Dafarra @diegoferigo @kouroshD @InesSorrentino @lrapetti @Yeshasvitvs @nunoguedelha @fjandrad @MiladShafiee

[traversaro]

I think this pattern is quite nice to avoid having Eigen vectors as class members, especially in public headers (and to avoid the need to have EIGEN_MAKE_ALIGNED_OPERATOR_NEW everywhere, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html and https://github.com/search?q=EIGEN_MAKE_ALIGNED_OPERATOR_NEW&type=Issues for an idea of an amount of bugs that this creates).

However, reviewing #516 it is possible to see that on complex expressions, this can create enormous readability problems. See for example in https://github.com/robotology/idyntree/pull/516/files#diff-9b4797931af64cac6597368c06f44dbeR132 the following expression:
https://github.com/robotology/idyntree/pull/516/files#diff-9b4797931af64cac6597368c06f44dbeR132 :

iDynTree::toEigen(m_P) = iDynTree::toEigen(m_Phat) - (iDynTree::toEigen(m_K)*iDynTree::toEigen(m_S)*iDynTree::toEigen(m_K).transpose()); // P_k+1 = Phat_k+1 - K_k+1 S (K_k+1)^T

The amount of iDynTree::toEigen calls quite complicates the code.
Readability is not a secondary aspect: if a mathematical expression is not readable, it is much more probable that it hides some error. In the following I list a few solutions (and a possible improvement in iDynTree) to mitigate this problem.

Use using directives

using namespace is bad, and everyone knows that. But a few strategic using iDynTree::toEigen, especially if confined in just a function, already improve a lot the situation:

toEigen(m_P) = toEigen(m_Phat) - toEigen(m_K)*toEigen(m_S)*toEigen(m_K).transpose()); // P_k+1 = Phat_k+1 - K_k+1 S (K_k+1)^T

Using a shorter toEigen alias

This is something that I think we should add in EigenMathHelpers: a shorter alias for toEigen , for example iDynTree::ei, to simplify this kind of expression even more:

ei(m_P) = ei(m_Phat) - ei(m_K)*ei(m_S)*ei(m_K).transpose()); // P_k+1 = Phat_k+1 - K_k+1 S (K_k+1)^T

any opinion on this @S-Dafarra @GiulioRomualdi @prashanthr05 ?

Use local Eigen::Map objects

If you have read carefully https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html, you should know that local Eigen::Map object are relatively inexpensive, as they do not allocate any memory. For this reason, especially if you have a method in which multiple mathematical expression are contained (in case of the PR aforementioned, ekfUpdate in https://github.com/robotology/idyntree/pull/516/files#diff-9b4797931af64cac6597368c06f44dbeR100 is a good example) you can define a local Eigen::Map object for each iDynTree attribute, and then just use the local object to do the computation:

Eigen::Map<Eigen::MatrixXd> P(toEigen(m_P));
Eigen::Map<Eigen::MatrixXd> Phat(toEigen(m_Phat));
Eigen::Map<Eigen::MatrixXd> K(toEigen(m_K));
Eigen::Map<Eigen::MatrixXd> S(toEigen(m_S));

P = Phat - K*S*K.transpose()); // P_k+1 = Phat_k+1 - K_k+1 S (K_k+1)^T

[prashanthr05]

In my opinion, using local Eigen::Map objects is more convincing than using using iDynTree::toEigen directives, because the former makes the underlying math in the statements obvious to the reader.
This can also have a psychological effect of reading the code. If I was a beginner, I would feel lighter to read this

P = Phat - K*S*K.transpose()); // P_k+1 = Phat_k+1 - K_k+1 S (K_k+1)^T

than this

iDynTree::toEigen(m_P) = iDynTree::toEigen(m_Phat) - (iDynTree::toEigen(m_K)*iDynTree::toEigen(m_S)*iDynTree::toEigen(m_K).transpose()); // P_k+1 = Phat_k+1 - K_k+1 S (K_k+1)^T

or this

toEigen(m_P) = toEigen(m_Phat) - toEigen(m_K)*toEigen(m_S)*toEigen(m_K).transpose()); // P_k+1 = Phat_k+1 - K_k+1 S (K_k+1)^T

The latter two pieces of code adds extra layers of complexity in reading the code. I have felt this personally when I was trying to read a piece of code as a newbie coder, although the differences are very minor.

However, these can be used interchangeably depending on the complexity of the statement and depending on the need to add extra lines to the code.

By using a shorter toEigen alias , I have a feeling that we would not be conveying the intention in a straightforward manner.

In the order of precedence, I would opt for,

• local Eigen::Map objects
• using iDyntree::toEigen directive
• alias to toEigen (or not have an alias at all unless it is really intuitive as to what is going to happen by using the alias)

[S-Dafarra]

I fully agree with @prashanthr05!

[traversaro]

Indeed. Actually @francesco-romano suggested that this is a perfect example of pattern where using auto makes a lot of sense, i.e.:

auto P(toEigen(m_P));
auto Phat(toEigen(m_Phat));
auto K(toEigen(m_K));
auto S(toEigen(m_S));

P = Phat - K*S*K.transpose()); // P_k+1 = Phat_k+1 - K_k+1 S (K_k+1)^T

should work fine.

[GiulioRomualdi]

Probably it's just a crazy idea. But why not redefine the basic operators (I e. + * -) to embed the Eigen map?

[traversaro]

Probably it's just a crazy idea. But why not redefine the basic operators (I e. + * -) to embed the Eigen map?

It is not so crazy. You can actually define operator overloading in a separate namespace, so that operator would be overloaded only if you do using namespace iDynTreeWithEigenOperatorOverloaded. Clearly understanding what's going on at the first glance would not be so intuitive, but that is a tradeoff. Furthermore, we would still need to define the map for the quantity that is assigned, as it is not possible to overload the operator= in a separate namespace.

[kouroshD]

cc @kouroshD

[prashanthr05]

cc @fjandrad

[GiulioRomualdi]

Probably overloading some operators (by using a suitable namespace #510 (comment)) may be really useful also for supporting different types of object in templates class. ami-iit/bipedal-locomotion-framework#46 (comment)

@traversaro do you think it is suitable?

[traversaro]

Probably overloading some operators (by using a suitable namespace #510 (comment)) may be really useful also for supporting different types of object in templates class. dic-iit/bipedal-locomotion-framework#46 (comment)

@traversaro do you think it is suitable?

Yes, feel free to open a PR for that, thanks!