idyntree: Create function that returns mutiple transforms based on a list of frames

You are still passing by copy grin

Oops, you are right I think the answer to this is the answer to the next question haha.

Btw…at what time did you test it? astonished

Baby woke me up at 4 couldn’t go back to sleep so might as well work a bit. Guess I wasn’t as awake as I thought. I’ll redo the tests and see if it changes.

A first dumb implementation could simply use a for cycle and call the getWorldTransform method for each of the requested frames, checking that they actually exist. Later, we can try to identify if there is some bottleneck.

Finally, I was able to finish the extra funciton in swig toMatlab for the Matrix4x4Vector. The results are the following:

traj\experiments	getHT	getHT + use Iterator	getHT + to Matlab
100 iterations	0.0005	0.0135	0.0006

As seen from the results, the main time consumption now is in the actual getWorldTransformsAsHomogenous function call. Not sure if its in the C++ side or in the swig conversion side.

The time for the updates becomes:

This time we were able to get a x5 times faster update in the visualization. The difference with respect to the previous timings seems to be matlab going inside the for loop to update the transform objects. As can be seen here:

So now matlab for calls and assignment of matrices is the bottleneck.

Added in e398eb95d3d7b8e6b644d30971c2926b7907659a

Scripts used to test: testGetWorldTransformTimes.txt testToMatlab.txt

Thanks for trying, but in the code it seems you forgot the & https://github.com/robotology/idyntree/blob/f1f61356eb7e5ad6606d218613ea90af9dc9b609/src/high-level/include/iDynTree/KinDynComputations.h#L388

You are still passing by copy 😁

Btw…at what time did you test it? 😲

While I was thinking about what could be the cause for the cellArray to be taking so much more time than the stringVector, I noticed that you are passing this input by copy here. Maybe it is taking more time because each time you call it, Matlab has to convert this data structure in a vector, allocating memory, which is then destroyed after the execution. A simple fix is to pass a const reference to the vector (like in the first comment) and define this vector only once at the beginning, before iterating getWorldTransforms. This goes in the direction of “allocating memory at every cycle is bad”.

> What is the difference between std::vector<iDynTree::Transform> getWorldTransforms( std::vector<int> frameIndexes); and std::vector<iDynTree::Transform> getWorldTransformsIndex( std::vector<int> frameIndexes);.

It was a test I did when seeing that the time between using indexes and a frame name was pretty much the same. I had the doubt if maybe because of the overloading of the function something was taking a bit of extra time. So I created one that only takes integers, but the result was the same. It was deleted later since it was purely to test that.

In C++ this implementation may be inefficient because you are returning by copy a vector. If instead you pass the output vector as a parameter, you can resize it once beforehand. Then, multiple calls of this function would avoid further memory allocation, which can be very costly in C++. Is it possible in Matlab to pass the output vector as a parameter or there are difficulties? Would it make any difference on the time?

My understanding is that we can don’t know if it would be more efficient. I can give it a try.

In case it is possible to pass the output vector as a parameter, to which object would it correspond in the C++ side? Is it possible to write an implementation which can avoid the toMatlab or fromMatlab (if it exists) calls? Maybe using templates?

Avoiding the toMatlab is something I also want to try, even without sending the output vector. For the C++ side I think is straight forward if I create a iDynTree.MatrixVector variable ( soon to be renamed to Matrix4x4Vector ). Now using both the sending of the output parameter and avoid using the toMatlab conversion is something that I’m not sure out my head how to do.

Is it worth to try a similar test fully in C++ to see if those times are so high also there?

I honestly doubt is worth it at the moment. If you see the difference between using the index and the frame name or avoiding doing the homogenousTransform call from matlab, for me is clear that the c++ code runs faster and that the swig conversions are slower. So I would further improve the swig part before like avoiding the toMatlab call. Then if we are still not satisfied with the time we can try to look more deeply at the C++ code.

How do you use the output vector in Matlab? It seems that Matlab is pretty bad in for loops and if clauses. Maybe you can move some more logic in the C++ side to reduce the amount of loops done in Matlab.

That is exactly my goal with avoiding the toMatlab call. For now, after I receive the vector of matrices, I need to transform each to a matlab variable, so I enter a for loop converting each from c++ to matlab, for efficiency in the update function I use the same for loop to set the resulting matrix as the transform for the meshes. See updateVisualization

I doubt it is possible, but what could further help for the visualization in terms of time optimization is to be able to get from a single swig function directly matlab matrices without the need to do the toMatlab call.

It is possible, but you need to inject the C++ code directly in the SWIG generated code, see for example as the toMatlab call itself is implemented: https://github.com/robotology/idyntree/blob/114671fb1a012f6214b35b8462d463fea99781fd/bindings/matlab/matlab_matvec.i#L119 .