四元数插值、对偶四元数插值、DQB/DLB

一、四元数与对偶四元数（双四元数）

传送门：四元数，对偶四元数，三维旋转，平移

二、四元数插值

四元数插值 与 向量插值（传送门：[插值算法 - Lerp, NLerp, SLerp]）非常类似，
只需要把其中向量换成四元数即可：
$$Lerp({\mathbf{q}}_1,{\mathbf{q}}_2;t)=(1-t){\mathbf{q}}_1+t{\mathbf{q}}_2\text{\hspace{1em}(1)}$$

$$NLerp({\mathbf{q}}_1,{\mathbf{q}}_2;t)=\frac{(1-t){\mathbf{q}}_1+t{\mathbf{q}}_2}{\Vert (1-t){\mathbf{q}}_1+t{\mathbf{q}}_2\Vert }\text{\hspace{1em}(2)}$$

$$SLerp({\mathbf{q}}_1,{\mathbf{q}}_2;t)=\frac{\sin ((1-t)\theta )}{\sin \theta }{\mathbf{q}}_1+\frac{\sin (t\theta )}{\sin \theta }{\mathbf{q}}_2=({\mathbf{q}}_2{\mathbf{q}}_1^{\ast }{)}^t{\mathbf{q}}_1\text{\hspace{1em}(3)}$$

这里主要说一下 $(3)$ 式：

式中，$\theta$ 是 ${\mathbf{q}}_1,{\mathbf{q}}_2$ 之间的夹角，定义为：$\theta =\arccos ({\mathbf{q}}_1\cdot {\mathbf{q}}_2)$

不难看出，
在 ${\mathbf{q}}_1$ 与 ${\mathbf{q}}_2$ 之间插值 $\iff$ 在 $1$ 与 ${\mathbf{q}}_2{\mathbf{q}}_1^{\ast }$ 之间插值
即 ${\mathbf{q}}_2{\mathbf{q}}_1^{\ast }$ 表示了从 ${\mathbf{q}}_1$ 到 ${\mathbf{q}}_2$ 的变化
显然 ${\mathbf{q}}_2{\mathbf{q}}_1^{\ast }$ 是单位四元数，那么 ${\mathbf{q}}_2{\mathbf{q}}_1^{\ast }$ 可以表示为：
$${\mathbf{q}}_2{\mathbf{q}}_1^{\ast }=\cos \frac{\theta }{2}+\mathbf{u}\sin \frac{\theta }{2}$$

则
$$({\mathbf{q}}_2{\mathbf{q}}_1^{\ast }{)}^t=\cos (\frac{t\theta }{2})+\mathbf{u}\sin (\frac{t\theta }{2})$$

可以看出 四元数的 Slerp 是角速度匀速的。

三、对偶四元数插值

3.1 沙勒定理(Chasles Theorem)

沙勒定理是欧拉旋转定理的一个推论。
根据沙勒定理，刚体的最广义位移等价于一个平移加上一个旋转。
因此，刚体运动可分为平移运动与旋转运动。

而这个移动可以合成为 通过绕某根轴的螺旋运动。

3.2 螺旋运动(Screw motion) 与 ScLerp

沙勒定理描述的螺旋运动 (Screw) 如下：

物体经过 $\stackrel{⌢}{\mathrm{OA}}$，$A{O}^{\prime }$ $\iff$ 物体直接经过 $\stackrel{⌢}{O{O}^{\prime }}$

这条浅绿色的螺旋(Screw)路径可以使用对偶四元数来描述${}^{[附B.1]}$：

$$\hat{\mathbf{q}}=\cos \frac{\hat{\theta }}{2}+\hat{\mathbf{u}}\sin \frac{\hat{\theta }}{2}$$
会发现，这个形式和普通的旋转四元数非常相似。
但是此时这里的 $\hat{\theta }$ 和普通旋转四元数里的 $\theta$ 已经不是一个意思了。
其中：
$$\{\begin{array}{rl}\hat{\theta }& =\theta +\epsilon d\\ \hat{\mathbf{u}}& =\mathbf{u}+\epsilon \mathbf{m}=(0+\mathbf{u})+\epsilon (0+\mathbf{m})\end{array}$$
（其中 $d=\Vert {\mathbf{t}}_{\parallel }\Vert ,$ 矩 $\mathbf{m}=\mathbf{c}\times \mathbf{u}$）

可以看出，Screw Motion是一个比较理想的插值路径，因此衍生出一种插值方法—— ScLerp (Screw Linear Interpolation)，
他的形式与 SLerp 非常相似：
$$ScLerp({\hat{\mathbf{q}}}_0,{\hat{\mathbf{q}}}_1;t)=({\hat{\mathbf{q}}}_1{\hat{\mathbf{q}}}_0^{\ast }{)}^t{\hat{\mathbf{q}}}_0$$

同样也是角速度匀速的：
$$({\hat{\mathbf{q}}}_1{\hat{\mathbf{q}}}_0^{\ast }{)}^t⟶t\hat{\theta }⟶t\theta ,td$$

3.3 对偶四元数混合(Dual Quaternion Blending)

在 对偶四元数混合(DQB)中 比较常用的就是 对偶四元数线性混合DLB(Dual Quaternion Linear Blending)，
其实它就是 NLerp 的对偶四元数版本：
$$DLB({\hat{\mathbf{q}}}_1,{\hat{\mathbf{q}}}_2;t)=NLerp({\hat{\mathbf{q}}}_1,{\hat{\mathbf{q}}}_2;t)=\frac{(1-t){\hat{\mathbf{q}}}_1+t{\hat{\mathbf{q}}}_2}{\Vert (1-t){\hat{\mathbf{q}}}_1+t{\hat{\mathbf{q}}}_2\Vert }\text{\hspace{1em}(4)}$$
扩展版本：
$$DLB({\hat{\mathbf{q}}}_1,\dots ,{\hat{\mathbf{q}}}_n;\mathbf{w})=\frac{w_1{\hat{\mathbf{q}}}_1+w_2{\hat{\mathbf{q}}}_2+\cdots +w_n{\hat{\mathbf{q}}}_n}{\Vert w_1{\hat{\mathbf{q}}}_1+w_2{\hat{\mathbf{q}}}_2+\cdots +w_n{\hat{\mathbf{q}}}_n\Vert }\text{\hspace{1em}(5)}$$

3.4 ScLerp 与 DLB 比较

以两个变量为例：
$$\{\begin{array}{rl}DLB(1,{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast };t)& =\frac{(1-t)+t{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast }}{\Vert (1-t)+t{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast }\Vert }\\ ScLerp(1,{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast };t)& =({\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast }{)}^t\end{array}$$
记 ${\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast }=\cos \frac{\hat{\alpha }}{2}+\hat{\mathbf{n}}\sin \frac{\hat{\alpha }}{2}$，有
$$\{\begin{array}{rl}DLB(1,{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast };t)& =\frac{(1-t)+t\cos \frac{\hat{\alpha }}{2}}{\Vert (1-t)+t{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast }\Vert }+\hat{\mathbf{n}}\frac{t\sin \frac{\hat{\alpha }}{2}}{\Vert (1-t)+t{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast }\Vert }\\ ScLerp(1,{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast };t)& =\cos \frac{t\hat{\alpha }}{2}+\hat{\mathbf{n}}\sin \frac{t\hat{\alpha }}{2}\end{array}$$

可以看出来，$DLB$ 的转动轴和 $ScLerp$ 是同一个（都是$\hat{\mathbf{n}}$）
他们的区别只是角度的不同。

与 $NLerp$ 一样，$DLB$ 显然不是角速度匀速的，
记 $\cos \frac{{\hat{\beta }}_t}{2}=\frac{(1-t)+t\cos \frac{\hat{\alpha }}{2}}{\Vert (1-t)+t{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast }\Vert },\sin \frac{{\hat{\beta }}_t}{2}=\frac{t\sin \frac{\hat{\alpha }}{2}}{\Vert (1-t)+t{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast }\Vert }$，有

$$\{\begin{array}{rl}DLB(1,{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast };t)& =\cos \frac{{\hat{\beta }}_t}{2}+\hat{\mathbf{n}}\sin \frac{{\hat{\beta }}_t}{2}\\ ScLerp(1,{\hat{\mathbf{q}}}_2{\hat{\mathbf{q}}}_1^{\ast };t)& =\cos \frac{t\hat{\alpha }}{2}+\hat{\mathbf{n}}\sin \frac{t\hat{\alpha }}{2}\end{array}$$

经过实验发现，$\Vert {\hat{\beta }}_t-t\hat{\alpha }\Vert$ 很小，可以认为 DLB 路径 近似于 ScLerp 的最短路径效果。

---

附A 相关公式

A.1

$\mathbf{n}$为单位向量，那么
$$\begin{gathered} \mathbf{n}\times (\mathbf{n}\times \mathbf{a})=({\mathbf{n}}^{\mathrm{T}}\mathbf{a})\mathbf{n}-\mathbf{a} \\ or \\ \mathbf{n}\times (\mathbf{a}\times \mathbf{n})=\mathbf{a}-({\mathbf{n}}^{\mathrm{T}}\mathbf{a})\mathbf{n} \end{gathered}$$

附B 推导

B.1 Screw Motion：

图中，向量 $\mathbf{c}$ 是垂直于轴 $\mathbf{u}$ 的，
在 $\mathrm{OAB}$ 平面上过点 $\mathrm{A}$ 作 $\mathrm{OA}$ 的垂线，交 $\mathrm{OB}$ 的延长线与 $\mathrm{C}$
易得：
$$\mathrm{OA}+\mathrm{AC}=2\mathrm{OB}$$

记 $\mathbf{x}=\mathbf{u}\times \mathbf{t}$，显然 $\mathbf{x}$ 与 $\mathrm{AC}$ 方向相同，且 $\Vert \mathbf{x}\Vert =\Vert {\mathbf{t}}_{\perp }\Vert$，而 $\Vert \mathrm{AC}\Vert =\Vert {\mathbf{t}}_{\perp }\Vert \cdot \cot \frac{\theta }{2}$，所以

$$\begin{gathered} \mathrm{AC}=\mathbf{u}\times \mathbf{t}\cdot \cot \frac{\theta }{2} \\ \mathrm{OA}=\mathbf{u}\times (\mathbf{t}\times \mathbf{u})=\mathbf{t}-({\mathbf{u}}^{\mathrm{T}}\mathbf{t})\mathbf{u} \end{gathered}$$

所以
$$\mathbf{t}-({\mathbf{u}}^{\mathrm{T}}\mathbf{t})\mathbf{u}+\mathbf{u}\times \mathbf{t}\cdot \cot \frac{\theta }{2}=2\mathbf{c}$$

那么轴的矩 $\mathbf{m}$ 即为：
$$\begin{array}{rl}\mathbf{m}& =\mathbf{c}\times \mathbf{u}\\ & =\frac{1}{2}(\mathbf{t}\times \mathbf{u}+\mathbf{u}\times \mathbf{t}\times \mathbf{u}\cdot \cot \frac{\theta }{2})\\ & =\frac{1}{2}(\mathbf{t}\times \mathbf{u}+(\mathbf{t}-({\mathbf{u}}^{\mathrm{T}}\mathbf{t})\mathbf{u})\cdot \cot \frac{\theta }{2})\end{array}$$

记四元数 $\mathbf{q}=q_0+\mathbf{q}=\cos \frac{\theta }{2}+\mathbf{u}\sin \frac{\theta }{2}$，则有：

$$\sin \frac{\theta }{2}\mathbf{m}=\frac{1}{2}(\mathbf{t}\times \mathbf{q}+(\mathbf{t}-({\mathbf{u}}^{\mathrm{T}}\mathbf{t})\mathbf{u})\cdot \cos \frac{\theta }{2})$$

我们知道，${\mathbf{u}}^{\mathrm{T}}\mathbf{t}=\Vert {\mathbf{t}}_{\parallel }\Vert$，记它为 $d$，则有：

$$\sin \frac{\theta }{2}\mathbf{m}+\frac{d}{2}\mathbf{u}\cos \frac{\theta }{2}=\frac{1}{2}(\mathbf{t}\times \mathbf{q}+q_0\mathbf{t})\text{\hspace{1em}(1)}$$

由于：
$$\begin{array}{rl}\hat{\mathbf{q}}& =\mathbf{q}+\epsilon {\mathbf{q}}^{\prime }=\mathbf{q}+\frac{\epsilon }{2}\mathbf{t}\mathbf{q}\\ & =q_0+\mathbf{q}+\frac{\epsilon }{2}(-{\mathbf{t}}^{\mathrm{T}}\mathbf{q}+q_0\mathbf{t}+\mathbf{t}\times \mathbf{q})\\ & =q_0-\epsilon \frac{1}{2}{\mathbf{t}}^{\mathrm{T}}\mathbf{q}+\mathbf{q}+\epsilon \frac{1}{2}(q_0\mathbf{t}+\mathbf{t}\times \mathbf{q})\end{array}$$

由于 ${\mathbf{t}}^{\mathrm{T}}\mathbf{q}={\mathbf{t}}^{\mathrm{T}}\mathbf{u}\sin \frac{\theta }{2}=d\sin \frac{\theta }{2}$，并带入(1)式得：
$$\begin{array}{rl}\hat{\mathbf{q}}& =(\cos \frac{\theta }{2}-\epsilon \frac{d}{2}\sin \frac{\theta }{2})+((\sin \frac{\theta }{2}+\epsilon \frac{d}{2}\cos \frac{\theta }{2})\mathbf{u}+\epsilon \sin \frac{\theta }{2}\mathbf{m})\\ & =(\cos \frac{\theta }{2}-\epsilon \frac{d}{2}\sin \frac{\theta }{2})+((\sin \frac{\theta }{2}+\epsilon \frac{d}{2}\cos \frac{\theta }{2})\mathbf{u}+\epsilon \sin \frac{\theta }{2}\mathbf{m}+\epsilon \frac{d}{2}\cos \frac{\theta }{2}\epsilon \mathbf{m})\\ & =(\cos \frac{\theta }{2}-\epsilon \frac{d}{2}\sin \frac{\theta }{2})+(\sin \frac{\theta }{2}+\epsilon \frac{d}{2}\cos \frac{\theta }{2})(\mathbf{u}+\epsilon \mathbf{m})\end{array}$$

由泰勒展开式知：
$$\{\begin{array}{rl}\cos \left(\frac{\theta +\epsilon d}{2}\right)& =\cos \frac{\theta }{2}-\epsilon \frac{d}{2}\sin \frac{\theta }{2}\\ \sin \left(\frac{\theta +\epsilon d}{2}\right)& =\sin \frac{\theta }{2}+\epsilon \frac{d}{2}\cos \frac{\theta }{2}\end{array}$$

则上式则可以写为：
$$\begin{array}{rl}\hat{\mathbf{q}}& =\cos \left(\frac{\theta +\epsilon d}{2}\right)+(\mathbf{u}+\epsilon \mathbf{m})\sin \left(\frac{\theta +\epsilon d}{2}\right)\\ & =\cos \frac{\hat{\theta }}{2}+\hat{\mathbf{u}}\sin \frac{\hat{\theta }}{2}\end{array}$$

参考

四元数插值与均值（姿态平滑） https://www.cnblogs.com/21207-iHome/p/6952004.html
Skinning with Dual Quaternions
Geometric Skinning with Approximate Dual Quaternion Blending
Hand-eye calibration using dual quaternions