Fix VIO quaternion
the VIO interface publishes a pose of the body w.r.t the vio start frame. The local frame is front-right-down and the vio start frame (aka world frame) is NED.
However, the orientation was being published incorrectly.
The rotation_to_quaternion
function had two errors associated with it:
- As is stated in the reference document, a transpose was required
- The order of quaternion components in ROS is x,y,z,w. This is the same order as in the reference document. However, the implementation assumed w,x,y,z
I removed this implementation in favor of the well-tested ROS tf2
API.
minor suggestion: I try to avoid language like R_imu_to_vio
because it is not clear if it is {imu w.r.t vio} or {vio w.r.t imu}. In this case it is the former. I tend to use notation like R_VI
to denote this - typeset as R^V_I
.