Small matrix calc speedup in collision_distance_field_types

[AndyZe]

I thought it would be a good idea to scour the codebase for places where the fact that (rotation matrix transpose) == (rot. matrix inverse) could be used to speed up calculations. Since a transpose is cheaper than an inverse.

This is the only place I found where that applies.

Eigen has some internal optimizations for Isometry3d types, but I doubt it's smart enough to look ahead and realize it only needs to invert the 3x3 rotation matrix.

Reviewers will need a slightly advanced background in linear algebra, which is why I tagged @mamoll and @v4hn.

[codecov]

Codecov Report

Merging #666 (2f26a02) into main (11e7cf4) will decrease coverage by 0.02%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main     #666      +/-   ##
==========================================
- Coverage   54.19%   54.18%   -0.01%     
==========================================
  Files         192      192              
  Lines       20187    20187              
==========================================
- Hits        10939    10936       -3     
- Misses       9248     9251       +3     

Impacted Files	Coverage Δ
...on_distance_field/collision_distance_field_types.h	61.06% <0.00%> (ø)
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	74.72% <0.00%> (-1.13%)	⬇️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 11e7cf4...2f26a02. Read the comment docs.

[simonschmeisser]

I'm curious: did you measure a real speed-up here? I would expect Eigen to be smart enough here

[AndyZe]

I didn't actually benchmark it...