Description
Describe the bug
In the rotatePoint() function of the OSMPDummySensor, a rotation matrix is calculated. Then the rotation matrix is multiplied by the relative translation of an object to the ego vehicle to get the relativ position considering the orientation of the ego.
However, since this is a transformation from a global coordinate system to a local coordinate system, it has to be the inverse (or transpose) of the rotation matrix multiplied by the translation. This is described in the OSI documentation of the orientation field:
Furthermore, when the rotatePoint() function is called in the doCalc() function, the orientation of the object is passed to the function instead of the ego orientation.
Describe how to reproduce the bug
Run a unit test on rotatePoint() and feed it e.g. a relative translation of (5,3,0) and a rotation of yaw = pi/2. This should result in a position of (3,-5,0). But instead it yields (-3,-5,0).
This example was run in a unit test in the OpenMSL template repo, which is based on the OSMPDummySensor. You can find the failed test result here.
Describe the expected behavior
see example above
Describe the OS you are using
- OS: Ubuntu 22.04
- Language: C++
- Version 1.3.0