imu_pipeline
imu_pipeline copied to clipboard
ros-perception
Quaternion rotation in tf2_sensors_msgs.h
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?
No, I borked it, my linear algebra is lacking.
https://github.com/ros/geometry2/pull/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
@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!
@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!
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.
