四元数插值
四元数插值
ref：
- 轨迹规划之姿态插补https://zhuanlan.zhihu.com/p/47396001
- 四元数插值https://blog.51cto.com/u_14929337/4716382
最简单的线性插值（Linear Interpolation，简称Lerp）
假设有两个四元数 q0,q1，想要在位置 t 处求插值 qt，用线性插值可以这样计算：
q t = L e r p ( q 0 , q 1 , t ) = ( 1 − t ) q 0 + t q 1 q_t=Lerp(q_0,q_1,t)=(1-t)q_0+tq_1 qt=Lerp(q0,q1,t)=(1−t)q0+tq1
如下图所示，四元数表示旋转时是单位四元数，这种插值方式，相当于我们是沿着一条直线（也就是圆上的一个弦）进行插值的，这样插值出来的四元数不是单位四元数，而且还有其他问题（后面会说）。
归一化线性插值（Normalized LinearInterpolation，简称Nlerp)
前面说过Lerp这样插值出来的并不是单位四元数，但只要将 q t q_t qt除以它的模 ∣ ∣ q t ∣ ∣ ||q_t|| ∣∣qt∣∣就能够将其转化为一个单位四元数了：
q t = N L e r p ( q 0 , q 1 , t ) = ( 1 − t ) q 0 + t q 1 ∣ ∣ ( 1 − t ) q 0 + t q 1 ∣ ∣ q_t=NLerp(q_0,q_1,t)=\frac{(1-t)q_0+tq_1}{||(1-t)q_0+tq_1||} qt=NLerp(q0,q1,t)=∣∣(1−t)q0+tq1∣∣(1−t)q0+tq1
如下图所示，在同等时间内， vt 扫过的⻆度是不同的, vt 扫过的速度（或者说⻆速度）首先会不断地增加，到t = 0.50之后会开始减速，所以Nlerp插值不能保证均匀的⻆速度。
球面线性插值(Spherical Linear Interpolation，简称Slerp)
Slerp插值可以解决前面的均匀角速度问题，它能够保证 每两个四元数之间的⻆速度是固定的，这就从原理上保证了插值的效果。如下图所示，如果 v1 和 v2 之间的夹⻆为 θ，那么：
θ t = ( 1 − t ) ⋅ 0 + t θ = t θ \theta _t=(1-t)\cdot0+t\theta=t\theta θt=(1−t)⋅0+tθ=tθ
插值公式
$$
q_t=Slerp(q_0,q_1,t)=\frac{sin((1-t)\theta)}{sin\theta}q_0+\frac{sin(t\theta)}{sin\theta}q_1\
\theta=acos(q_0\cdot q_1)
$$
编程实现Slerp插值的时候还是有几个问题需要注意一下。
1、如果单位四元数之间的夹角θ非常小，那么sin(θ)可能会由于浮点数的误差被近似为0.0，从而导致除以0的错误．所以，我们在实施 Slerp 之前，需要检查两个四元数的夹角是否过小（或者完全相同）。一旦发现这种问题，我们就必须改用 Nlerp 对两个四元数进行插值，这时候 Nlerp 的误差非常小，所以基本不会与真正的 Slerp 有什么区别。
2、在对两个单位四元数进行插值之前，我们需要先检测q0与q1之间是否是钝角，即检测它们点积的结果q0⋅q1 是否为负数。如果 q0⋅q1<0，那么我们就反转其中的一个四元数，比如说将q1改为−q1 ，并使用q0与−q1之间新的夹角来进行插值，这样才能保证插值的路径是最短的．
作者：计算机视觉life链接：https://juejin.cn/post/6844903919492169741来源：稀土掘金著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。
编程练习：
这个方法可行，但是编程稍微复杂点，计算量也大，还有一种实现四元数的球面插值计算方式，要简单很多，留给你当做作业练习啦，搞定作业，你就可以直接用来做Slerp插值啦！
作业练习1：前面四元数球面线性插值方法比较复杂，下面是它的简化版求解方法，请证明。
假设v0, v1是两个四元数，其夹角为θ，假设在它们中间进行四元数插值结果为v’，v’和v1之间夹角为θ‘ < θ，记v⊥是垂直于v1的四元数向量，证明：
v ′ = v 1 c o s θ ′ + v ⊥ ∗ s i n θ ′ v'=v_1cos\theta'+v_\perp*sin\theta' v′=v1cosθ′+v⊥∗sinθ′
作业练习2：编程实现四元数球面线性插值。
我们用智能手机采集了图像序列和IMU数据，由于IMU帧率远大于图像帧率，需要你用Slerp方法进行四元数插值，使得插值后的IMU和图像帧对齐。
已知某帧图像的时间戳为：t =700901880170406
离该图像帧最近的前后两个时刻IMU时间戳为：t1 = 700901879318945，t2 = 700901884127851
IMU在t1, t2时刻测量得的旋转四元数为：
q1x=0.509339, q1y=0.019188, q1z=0.049596, q1w=0.858921
q2x=0.509443, q2y=0.018806, q2z=0.048944,q2w=0.858905
根据上述信息求IMU对齐到图像帧的插值后的四元数。
参考结果已经给出。
温馨提示：
代码框架、数据及预期结果已经为你准备好了，公众号「计算机视觉life」后台回复：插值，即可获得。
https://blog.51cto.com/u_14929337/4716382