| 20 | // q = q0 + vt + 1/2 at^2 |
| 21 | |
| 22 | void PoseEstimQueue::enqueue(const Quaternionf &quat, float deltaT) { |
| 23 | pos[idx] = quat; |
| 24 | t[idx] = deltaT / avgDeltaT; |
| 25 | // spd[idx] = Quaternionf::Identity().slerp(1.f / t[idx], pos[mod3(idx - 1)].inverse() * quat); |
| 26 | idx = mod3(idx + 1); |
| 27 | } |
| 28 | |
| 29 | Quaternionf PoseEstimQueue::getQuatIn(float deltaT) { |
| 30 | return pos[idx] * (pos[mod3(idx - 1)].inverse() * pos[idx]) |