koide3 / hdl_localization

Real-time 3D localization using a (velodyne) 3D LIDAR
BSD 2-Clause "Simplified" License
775 stars 310 forks source link

Questions about Quaternion's difference equation #125

Open nyxrobotics opened 5 months ago

nyxrobotics commented 5 months ago

I am studying localization with your program. Let me ask you a question about the implementation details.

I am not sure how to derive this equation for the following quaternion calculation, although the result seems to be correct. https://github.com/koide3/hdl_localization/blob/8ba183ca49a1d6b290d965d3852793287b31a9f4/include/hdl_localization/pose_system.hpp#L79-L81

I believe the following equation can be implemented using Euler angles

dq = Eigen::AngleAxisf(gyro[0] * dt_, Vector3t::UnitX()) * Eigen::AngleAxisf(gyro[1] * dt_, Vector3t::UnitY()) *
     Eigen::AngleAxisf(gyro[2] * dt_, Vector3t::UnitZ());

Therefore, my questions are two

nyxrobotics commented 5 months ago

I refer to the following article for the quaternion definition formula (the order of the w components has been replaced) https://qiita.com/drken/items/0639cf34cce14e8d58a5

koide3 commented 5 months ago
nyxrobotics commented 5 months ago

Thank you for your reply. It is very helpful. Please allow me to ask an additional question. In the article you gave me, I think the calculation would be as follows since it is the derivative of each element of the quaternion. Can I derive the same equation from here as you?

  Quaterniont dq = Quaterniont(0, gyro[0] * dt_ / 2.0, gyro[1] * dt_ / 2.0, gyro[2] * dt_ / 2.0) * qt;
  Quaterniont qt_next = Quaterniont(qt.w() + dq.w(), qt.x() + dq.x(), qt.y() + dq.y(), qt.z() + dq.z()).normalized();