Quaternion rotation in tf2_sensors_msgs.h

[kogut]

Is the rotation on line 112 of tf2_sensor_msgs.h correct?

The form of that rotation is the one commonly used to rotate a point in 3D space, where x, y, z are a point in space and w=0.

I get expected results from my sensor URDF if a pure quaternion multiplication is applied here instead.

My test case is with input IMU RPY of 0,0,0 in the sensor frame, and a sensor->vehicle URDF that applies RPY of PI,0,0 , I expect output IMU data of PI,0,0 in the vehicle frame. (with the sign of pitch/yaw reversed for non-zero values).

Am I correct, or am I mis-interpreting the intent of this rotation?

[paulbovbel]

No, I borked it, my linear algebra is lacking.

ros/geometry2#78

Needs to be fixed here, PRed up to tf2, and a separate node needs to exist specifically for doing the ned->enu conversion as (poorly) implemented here.

@tonybaltovski FYI

[ClaudioCimarelli]

@paulbovbel Hello, I would like to use this package that seems useful to me, as I am just starting with Ros.

I do not understand if there is actually a problem in the way you do the rotations with quaternion, as @kogut pointed out , and also in your pull request to tf2.
Then, should I remove, in line 112 of tf2_sensor_msgs.h, the first quaternion multiplication between the transform rotation and the IMU rotation; that is, i should do only: imu_rot_quat * transf_quat.inverse()?
About the covariance matrix rotation, I looked that it should be rotated as RCR^t (^t: the transpose) where R is a rotation matrix? Could be that you rotated only once?
I do not understand why you used the quaternion instead of rotation matrices in the first place (as you did in the beginning with angular velocity and acc.).

Thank you in advance!

[paulbovbel]

@ClaudioCimarelli, to be perfectly honest, I remember nothing about how or why anymore. I haven't touched an IMU in about 3 years.

PRs are most certainly welcome if you have the time. A gtest or two to validate the math would probably go a long way!

[peci1]

Just lost a day on this one! I'll consider sending a PR with the fix and tests if there's chance it'll be merged in a reasonable time frame.

[peci1]

Done: #15 .

[peci1]

Fixed by #15. (after almost 7 years :-D )