四元数插值、对偶四元数插值、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+\vec{\mathbf{u}})+\epsilon (0+\vec{\mathbf{m}})\end{array}$$
（其中 $d=\Vert {\vec{\mathbf{t}}}_{\parallel }\Vert ,$ 矩 $\vec{\mathbf{m}}=\vec{\mathbf{c}}\times \vec{\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

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

附B 推导

B.1 Screw Motion：

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

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

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

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

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

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

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

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

$$\sin \frac{\theta }{2}\vec{\mathbf{m}}+\frac{d}{2}\vec{\mathbf{u}}\cos \frac{\theta }{2}=\frac{1}{2}(\vec{\mathbf{t}}\times \vec{\mathbf{q}}+q_0\vec{\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+\vec{\mathbf{q}}+\frac{\epsilon }{2}(-{\vec{\mathbf{t}}}^{\mathrm{T}}\vec{\mathbf{q}}+q_0\vec{\mathbf{t}}+\vec{\mathbf{t}}\times \vec{\mathbf{q}})\\ & =q_0-\epsilon \frac{1}{2}{\vec{\mathbf{t}}}^{\mathrm{T}}\vec{\mathbf{q}}+\vec{\mathbf{q}}+\epsilon \frac{1}{2}(q_0\vec{\mathbf{t}}+\vec{\mathbf{t}}\times \vec{\mathbf{q}})\end{array}$$

由于 ${\vec{\mathbf{t}}}^{\mathrm{T}}\vec{\mathbf{q}}={\vec{\mathbf{t}}}^{\mathrm{T}}\vec{\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})\vec{\mathbf{u}}+\epsilon \sin \frac{\theta }{2}\vec{\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})\vec{\mathbf{u}}+\epsilon \sin \frac{\theta }{2}\vec{\mathbf{m}}+\epsilon \frac{d}{2}\cos \frac{\theta }{2}\epsilon \vec{\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})(\vec{\mathbf{u}}+\epsilon \vec{\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)+(\vec{\mathbf{u}}+\epsilon \vec{\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