NeBula-Autonomy / LOCUS

Robust Lidar Odometry System
MIT License
358 stars 49 forks source link

Is this Quaternion math ok? #7

Open willat343 opened 2 years ago

willat343 commented 2 years ago

A line of code in LoFrontend::GetImuQuaternion caught my eye: https://github.com/NeBula-Autonomy/LOCUS/blob/16d147e0a6253d30c2cef52ffdb6532e63696071/lo_frontend/src/LoFrontend.cc#L809

This line of code transforms the IMU orientation into the base link frame, and only runs if the user has set frame_conversions/b_convert_imu_to_base_link_frame to true (it is false in lo_frames.yaml). The line of code does: q' = q_e q q_e^-1 where q is the imu data (in the IMU frame), q' is the imu data transformed to the base link frame, and q_e is the extrinsic mapping the rotation from IMU to base link.

I had recently seen how the IMU to lidar frame conversion is done in LIO-SAM at https://github.com/TixiaoShan/LIO-SAM/blob/0cdc9e8b010fff947275d68aa3545062afb059da/include/utility.h#L269 where they do q' = q * q_e and so this line stuck out to me because of the different operatoin.

My understanding of quaternion math is pretty basic, so forgive me if I am completely wrong here, but after looking at a couple of online resources, I think that the q' = pqp^-1 operation is for rotating vectors and q' = q*p is for chaining rotations. Having said that, when running LIO-SAM I did have to pre-transform the IMU data into a body frame (from an NED frame) in a pre-processing step before applying the extrinsic in their code to get things working, so I'm not completely confident in my assessment.

It would be good to get your thoughts on this and/or confirm that LOCUS is doing the right thing.