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).