Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Apache License 2.0
7.09k
stars
2.25k
forks
source link
Using the IMU correction output by Cartographer #1788
Hello, I realize that this issue has been raised before, however I was unable to find a direct answer in any of the previous issues raised on this subject. I am trying to figure out how to apply the IMU correction quaternion to the rpy offset in my URDF file between the imu and base_link. My RPY offset is currently "-0.002173165 -0.02482185 -3.106918". The IMU quaternion correction output by cartographer is 1.27612 deg (0.999938, 0.00703409, -0.00603942, 0.00616903).
What I have tried is to use the Python tf.transformations package to convert my RPY offset into quaternion and then multiplying the correction quaternion by the converted value, and finally converting the result back to Euler angle. This seems to have the effect of flipping my IMU over and causing the SLAM to go crazy. Here is exactly what I did:
import numpy as np
import tf.transformations as t
q1 = t.quaternion_from_euler(-0.002173165, -0.02482185, -3.106918)
q2 = np.array([0.999938, 0.00703409, -0.00603942, 0.00616903])
q3 = t.quaternion_multiply(q2, q1)
result = t.euler_from_quaternion(q3
This yields the following result: [-3.13110, 0.01309, 3.1223], which is clearly not correct, even visually.
Hello, I realize that this issue has been raised before, however I was unable to find a direct answer in any of the previous issues raised on this subject. I am trying to figure out how to apply the IMU correction quaternion to the rpy offset in my URDF file between the imu and base_link. My RPY offset is currently "-0.002173165 -0.02482185 -3.106918". The IMU quaternion correction output by cartographer is 1.27612 deg (0.999938, 0.00703409, -0.00603942, 0.00616903).
What I have tried is to use the Python tf.transformations package to convert my RPY offset into quaternion and then multiplying the correction quaternion by the converted value, and finally converting the result back to Euler angle. This seems to have the effect of flipping my IMU over and causing the SLAM to go crazy. Here is exactly what I did:
This yields the following result: [-3.13110, 0.01309, 3.1223], which is clearly not correct, even visually.
Thank you for any help in advance.