Fix 3x3 covariance matrix rotation/transformation

[leocencetti]

The template specializations of transform_static_frame() and transform_frame() for the type Covariance3d improperly rotate the covariance matrix.

Implementation details

Old (broken) implementation

The old implementation is wrong and returns an ill-conditioned covariance matrix, breaking symmetry and positive-semi-definitiveness.

Taking for example the following covariance matrix P and quaternion q:

    | 0.1 0.2 0.3 |
P = | 0.2 0.4 0.5 |
    | 0.3 0.5 0.6 |

q = 1i + 0j + 0k + 0  # 180deg around X

The old implementation would return the matrix P':

                        | 0.1 -0.2 -0.3 |
P' = P * q = P * R(q) = | 0.2 -0.4 -0.5 |
                        | 0.3 -0.5 -0.6 |

which is clearly invalid (asymmetric and negative).

New (proper) implementation

The new implementation correctly rotates the 3x3 covariance matrix by left- and right-multiplying by the rotation quaternion (as done for the 6x6 and 9x9 cases):

                                                 |  0.1 -0.2 -0.3 |
P' = q * P * q.conjugate() = R(q) * P * R(q).T = | -0.2  0.4  0.5 |
                                                 | -0.3  0.5  0.6 |

which is a valid covariance matrix.

Useful resources

GodBolt example: link

Covariance matrix rotation derivation: https://robotics.stackexchange.com/questions/2556/how-to-rotate-covariance

Edit: Updated GodBolt link to use un-shortened version for posterity

[leocencetti]

This PR is currently targeting the ros2 branch, but the issue is present everywhere else. Might be a good idea to back-port these fixes to master (ROS1).

[vooon]

LGTM, Thanks!

@TSC21

[leocencetti]

@vooon FYI there were 4 errors in the test code (reason for the failing CI). I hadn't built the tests locally so they slipped through...

Since you merged already, I'll open a second PR to patch them.

[vooon]

I'm on release process, though to fix myself.