Performance: Body::setPose() should use Eigen::Isometry3d::linear() instead of ::rotation().

[peci1]

Here: https://github.com/ros-planning/geometric_shapes/blob/bca6b88be8ba70851c79f785e0d45b12cb1917df/src/bodies.cpp#L318

And here: https://github.com/ros-planning/geometric_shapes/blob/bca6b88be8ba70851c79f785e0d45b12cb1917df/src/bodies.cpp#L549

When using rotation(), Eigen unnecessarily does SVD decomposition via Eigen::Transform::computeRotationScaliing(). But we're working with isometries, so linear part of the transform is equal to the rotation. I've done some performance tests and calling setPose() on 20 000 bodies takes 250 ms, which is a lot (and according to the profiler, the time is dominated by SVD computations).

[peci1]

A guy on SO says newer Eigen should recognize and use the potential optimization, but at least the version used in Melodic doesn't do that.

[v4hn]

Could you create a pull-request and in the best case benchmark the performance improvement?
If the Isometry template indeed does not optimize away the SVD, this impacts MoveIt in general just as well.

[peci1]

I'll try that. Just as a confirmation, here's the disassembly of /opt/ros/melodic/lib/libgeometric_shapes.so version 0.6.1-0bionic.20191205.163901 (created by objdump -d):

0000000000014370 <_ZN6bodies8Cylinder18updateInternalDataEv@@Base>:
    ...
    1442e:	e8 dd 82 ff ff       	callq  c710 <_ZNK5Eigen9TransformIdLi3ELi1ELi0EE22computeRotationScalingINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEES4_EEvPT_PT0_@plt>

The docs for Transform::rotation() in Eigen master say:

If Mode==Isometry, then this method is an alias for linear(), otherwise it calls computeRotationScaling() to extract the rotation through a SVD decomposition.

But apparently, computeRotationAndScaling gets called.

Eigen development branch 3.3 doesn't contain this optimization: https://gitlab.com/libeigen/eigen/-/blob/3.3/Eigen/src/Geometry/Transform.h#L1061 .
Eigen master does: https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Transform.h#L1066 (it was added in commit 2b70b2).

So no officially released version of Eigen contains this optimization. And even if it does, I think that since Moveit supports a wide variety of platforms, there's no other way than manually switching to linear() where applicable, since in the future, there will be platforms with this optimization and platforms without it.

[v4hn]

This is indeed the case and it is very frustrating that nobody noticed this during the transition from Affine3d to Isometry3d... I would have expected some gperf analysis to point this out.

Could you prepare the corresponding pull-request?
Preferably also for the main MoveIt repository, but someone else could do that too...

[peci1]

Yes, I'll have a look at it. Do you know of a good example of performance tests, ideally in moveit? I haven't written these yet so I'd like to get them right... I assume the timeouts in tests should be "calibrated" for Release builds, is that right?

[v4hn]

I would not expect automated tests for the testsuite for performance expectations. MoveIt does not do this kind of testing (yet). Some measurements demonstrating the performance difference would be great.

[peci1]

Okay, I tested setPose() in a release build and the performance improvement is massive.

I constructed a "test case" which calls setPose() 10.000.000 times for each of the defined bodies (for ConvexMesh I created 2 cases, one for unscaled, and one for scaled mesh). The mesh I used was the box.dae from the tests directory.

Before the fix:

BodiesPerformance.Sphere (38 ms)
BodiesPerformance.Box (775 ms)
BodiesPerformance.Cylinder (843 ms)
BodiesPerformance.Mesh (3827 ms)
BodiesPerformance.ScaledMesh (4348 ms)

With the fix:

BodiesPerformance.Sphere (40 ms)
BodiesPerformance.Box (84 ms)
BodiesPerformance.Cylinder (71 ms)
BodiesPerformance.Mesh (757 ms)
BodiesPerformance.ScaledMesh (1209 ms)

And with another improvement I found for ConvexMesh:

BodiesPerformance.Sphere (41 ms)
BodiesPerformance.Box (88 ms)
BodiesPerformance.Cylinder (78 ms)
BodiesPerformance.Mesh (437 ms)
BodiesPerformance.ScaledMesh (934 ms)

That yields setPose() duration about 10% of the original for Box, Cylinder and unscaled ConvexMesh, and about 20% for scaled ConvexMesh (the saved time doesn't influence this method that much because it anyways spends lots of time on recomputing the scaled vertices, and the benefit will be the smaller the more vertices the mesh has).

The proposed fix is in #126.

[rhaschke]

Fixed via #126.