三维空间刚体运动4-5：四元数多点离散数值解插值方法：Sping_多个控制点的四元数插值

序：本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动，为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解，其中四元数部分较多较复杂，又分为四部分。如果读者急于实践，可直接阅读第五部分的机器人运动轨迹，此部分详细讲解了安装准备工作。此系列总体目录如下：

1. 旋转矩阵和变换矩阵；
2. 旋转向量表示旋转；
3. 欧拉角表示旋转；
4. 四元数包括以下部分：
   4-1. 四元数表示变换；
   4-2. 四元数线性插值方法：LinEuler/LinMat/Lerp/Nlerp/Slerp；
   4-3. 四元数多点插值方法：Squad；
   4-4. 四元数多点连续解析解插值方法：Spicv；
   4-5. 四元数多点离散数值解插值方法：Sping。
5. 实践：SLAM中显示机器人运动轨迹及相机位姿。

摘要：四元数插值四个方法Slerp、Squad、Spicv和Sping既复杂又很重要，为了详细阐述，故每个方法独立成一篇博文讲解。没有插值的四元数是没有灵魂的，插值的重要性不言而喻。Slerp是经典的两点间一阶连续可导插值方法，Squad方法在Slerp的基础上实现多点间的一阶连续可导，Spicv是多点间连续解析解插值方法，而Sping则是多点间离散数值解插值方法，更适合复杂曲线，此博文也是国内首篇介绍Spicv和Sping的中文资料。

1. 正切曲率$\kappa (\gamma ,t)$在$H_1$上的离散数值解——Sping

由《四元数多点连续解析解插值方法：Spicv》可知：虽然连续解析解过程推导严密，但遗憾的是，我们不能给出最优插值曲线的解析表达式。因此，我们将尝试提出一个用数值方法来解决问题的离散数值解版本。

首先我们把这个问题写成等价的离散形式，然后尝试梯度下降解决这个新版本的问题。由于插值算法使用数值解并用到梯度，所以命名为Sping(Spherical Interpolation using Nmerical Gradient descent)。

注释：原论文将此算法命名为Spring，但这个名字太普遍，容易混淆，所以笔者将其精简为Sping，更容易记忆，也减少重名。

1.1 离散版解决方案

现在回顾一下曲率平方的积分$K(\gamma )$的概念：给定控制点$Q_1,...,Q_k\in H_1$，求$\gamma (t)\in C^2(I,H_1)$，使存在$t_1,...,t_k\in (I)$，满足$\gamma (t_i)=Q_i$，并使下列表达式最小化：$$K(\gamma )={\int }_{t_1}^{t_N}{\Vert \kappa (\gamma ,t)\Vert }^{2}dt\text{\hspace{1em}(1.1)}$$据此在离散版本中进行改写，我们将尝试解决以下问题：
给定控制点$Q_1,...,Q_k\in H_1$，寻找$q_1,...,q_N\in H_1(N\ge k)$，对于$t=1,...,k$有$q_{i_t}=Q_t$，并使下列表达式最小化：$$E=\sum _{i=1}^{N}l(q_i){\Vert \tilde{\kappa }(q_i)\Vert }^2\text{\hspace{1em}(1.2)}$$可见积分被求和代替了，我们积分区间的参数宽度称为$l(q_i)$，它可以表示为第$i$个四元数前后间隔内参数宽度的中值：$$l(q_i)=\frac{\Vert q_i-q_{i-1}\Vert +\Vert q_i-q_{i+1}\Vert }{2}\text{\hspace{1em}(1.3)}$$在$l(q_i)$的近似中，$q_i$和$q_{i-1}$之间的参数化距离的另一种度量方法是角度${\theta }_i$，它须满足$\cos {\theta }_i=q_i\cdot q_{i+1}$。
方程$(1.2)$中另一个参数$\tilde{\kappa }$，它表示局部曲率的离散版：$$\tilde{k}(q_i)=q_i^{{}^{\prime \prime }}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{q}_i\text{\hspace{1em}(1.4)}$$请注意，在定义局部曲率时，分母$q_i\cdot q_i$没有被忽略(如上篇中方程$(3.1)$)。这是因为插值的四元数不一定为单位四元数，此部分请参考第2.1节。因此分母的值一般不等于1，所以它不能被省略。

在方程$(1.4)$中，使用的是插值曲线的离散近似的二阶导数。二阶导数的一个很好的中心近似是¹：$$q_i^{{}^{\prime \prime }}=\frac{q_{i-1}-2q_i+q_{i+1}}{l(q_i{)}^2}\text{\hspace{1em}(1.5)}$$

1.2 梯度下降法求解

本节尝试求解方程$(1.2)$，它可以用梯度下降法最小化。一般来说，梯度下降法可以描述如下：将被最小化的函数图像(通常称为能量函数)视为有坡度的丘陵，其中的函数值是每组坐标上的丘陵高度，函数在某点的梯度表示它最陡的上坡方向。梯度下降是基于对解的初始估计，从最初的估计开始计算梯度，并在梯度的相反方向上迈出一小步(即下山)，从而产生一个新的点。这个过程在新的点上不断重复，直到函数值不再通过走一步变得更小，在这个过程中，梯度下降法产生一个近似的局部最小值。梯度下降理论的思想大致如此，不再做详细解释，但是我们将对推导方法进行足够详细的描述，以便那些在该领域没有前置知识的读者仍然能理解其推导过程。

当使用如梯度下降法的数值近似方法时，通常需要对解空间的进行限制；相反，如果解位于期望解空间之外，则会添加一个使解付出更大代价的附加项。据此我们寻找一个函数，它可以确定离散版本的插值曲线是否在$H_1$中，因此此函数是由单位四元数组成的，可通过下式实现：$$g(q)=q\cdot q-1\text{\hspace{1em}(1.6)}$$其中： H 1 = { q ∈ H ∣ g ( q ) = 0 } H_{1}= \{ q\in H|g(q)=0 \} H1​={q∈H∣g(q)=0}，即当$g(q)=0$时，$q$是单位四元数。确定四元数是否为单位四元数的方法，可以与能量函数$E$结合成新的能量函数$F$：$$F=\sum _{i=1}^{N}l(q_i){\Vert \widetilde{\kappa (q_i)}\Vert }^2+cg(q_i{)}^2\text{\hspace{1em}(1.7)}$$假设$c\in \mathbb{R}$为相匹配的实数，当$E$在某处取最小值时，能量函数$F$也近似有一个最小值，在这里所有的$q_i$为近似单位四元数。

我们想用梯度下降法找到方程$(1.7)$中$F$的最小值，因此我们必须求出梯度。下面，我们将$q_{i,x}$记为离散插值曲线第$i$个四元数中的第$x$坐标，其中$x\in 1,2,3,4$，将四元数视为四维向量，因此梯度是$4N$维的。$F$关于每个坐标的偏导可以写成：$$\frac{\partial F}{\partial q_{i,x}}=\frac{\partial }{\partial q_{i,x}}\left(\sum _{j=1}^{N}l(q_j){\Vert \tilde{\kappa }(q_j)\Vert }^2+cg(q_j{)}^2\right)$$在方程$(1.3),(1.4),(1.5)$中，$q_i$存在于算法中的$l(q_{i-1}),l(q_i),l(q_{i+1}),\tilde{\kappa }(q_{i-1}),\tilde{\kappa }(q_i),\tilde{\kappa }(q_{i+1})$，因此相应的项必然出现在偏导中，因此有： ∂ F ∂ q i , x = ∂ ∂ q i , x ( l ( q i − 1 ) ∥ κ ~ ( q i − 1 ) ∥ 2 + l ( q i ) ∥ κ ~ ( q i ) ∥ 2 + l ( q i + 1 ) ∥ κ ~ ( q i + 1 ) ∥ 2 + c g ( q i ) 2 ) = ∂ ∂ q i , x ( l ( q i − 1 ) κ ~ ( q i − 1 ) ⋅ κ ~ ( q i − 1 ) + l ( q i ) κ ~ ( q i ) ⋅ κ ~ ( q i ) + l ( q i + 1 ) κ ~ ( q i + 1 ) ⋅ κ ~ ( q i + 1 ) + c g ( q i ) 2 ) = ∂ l ( q i − 1 ) ∂ q i , x κ ~ ( q i − 1 ) ⋅ κ ~ ( q i − 1 ) + ∂ l ( q i ) ∂ q i , x κ ~ ( q i ) ⋅ κ ~ ( q i ) + ∂ l ( q i + 1 ) ∂ q i , x κ ~ ( q i + 1 ) ⋅ κ ~ ( q i + 1 ) + 2 l ( q i − 1 ) κ ~ ( q i − 1 ) ∂ κ ~ ( q i − 1 ) ∂ q i , x + 2 l ( q i ) κ ~ ( q i ) ∂ κ ~ ( q i ) ∂ q i , x + 2 l ( q i + 1 ) κ ~ ( q i + 1 ) ∂ κ ~ ( q i + 1 ) ∂ q i , x + 2 c g ( q i ) ∂ g ( q i ) ∂ q i , x (1.8)

$$\begin{aligned} \frac{\partial F}{\partial q_{i,x}} &= \frac{\partial }{\partial q_{i,x}}\left ( l(q_{i-1})\left \| \tilde{\kappa}(q_{i-1}) \right \|^{2} + l(q_{i})\left \| \tilde{\kappa}(q_{i}) \right \|^{2} + l(q_{i+1})\left \| \tilde{\kappa}(q_{i+1}) \right \|^{2} + cg(q_{i})^{2} \right ) \\ &= \frac{\partial }{\partial q_{i,x}}\left ( l(q_{i-1})\tilde{\kappa}(q_{i-1})\cdot \tilde{\kappa}(q_{i-1}) + l(q_{i})\tilde{\kappa}(q_{i})\cdot \tilde{\kappa}(q_{i}) + l(q_{i+1})\tilde{\kappa}(q_{i+1})\cdot \tilde{\kappa}(q_{i+1}) + cg(q_{i})^{2} \right ) \\ &= \frac{\partial l(q_{i-1})}{\partial q_{i,x}}\tilde{\kappa}(q_{i-1})\cdot \tilde{\kappa}(q_{i-1}) + \frac{\partial l(q_{i})}{\partial q_{i,x}}\tilde{\kappa}(q_{i})\cdot \tilde{\kappa}(q_{i}) + \frac{\partial l(q_{i+1})}{\partial q_{i,x}}\tilde{\kappa}(q_{i+1})\cdot \tilde{\kappa}(q_{i+1}) + \\& \quad \quad 2l(q_{i-1})\tilde{\kappa}(q_{i-1})\frac{\partial \tilde{\kappa}(q_{i-1})}{\partial q_{i,x}} + 2l(q_{i})\tilde{\kappa}(q_{i})\frac{\partial \tilde{\kappa}(q_{i})}{\partial q_{i,x}} + 2l(q_{i+1})\tilde{\kappa}(q_{i+1})\frac{\partial \tilde{\kappa}(q_{i+1})}{\partial q_{i,x}} + \\ & \quad \quad 2c g(q_{i})\frac{\partial g(q_{i})}{\partial q_{i,x}} \end{aligned}$$ \tag{1.8} ∂qi,x​∂F​​=∂qi,x​∂​(l(qi−1​)∥κ~(qi−1​)∥2+l(qi​)∥κ~(qi​)∥2+l(qi+1​)∥κ~(qi+1​)∥2+cg(qi​)2)=∂qi,x​∂​(l(qi−1​)κ~(qi−1​)⋅κ~(qi−1​)+l(qi​)κ~(qi​)⋅κ~(qi​)+l(qi+1​)κ~(qi+1​)⋅κ~(qi+1​)+cg(qi​)2)=∂qi,x​∂l(qi−1​)​κ~(qi−1​)⋅κ~(qi−1​)+∂qi,x​∂l(qi​)​κ~(qi​)⋅κ~(qi​)+∂qi,x​∂l(qi+1​)​κ~(qi+1​)⋅κ~(qi+1​)+2l(qi−1​)κ~(qi−1​)∂qi,x​∂κ~(qi−1​)​+2l(qi​)κ~(qi​)∂qi,x​∂κ~(qi​)​+2l(qi+1​)κ~(qi+1​)∂qi,x​∂κ~(qi+1​)​+2cg(qi​)∂qi,x​∂g(qi​)​​(1.8)下面我们将推导方程$(1.8)$中子表达式的偏导数。
我们引入符号：$1_x$，它表示某个向量在第$x$坐标是1，在其他坐标是0。现在我们可以求出$g(q_i),l(q_{i-1}),l(q_i),l(q_{i+1}),q_{i-1}^{{}^{\prime \prime }},q_i^{{}^{\prime \prime }},q_{i+1}^{{}^{\prime \prime }}$以及$\tilde{\kappa }(q_{i-1}),\tilde{\kappa }(q_i),\tilde{\kappa }(q_{i+1})$的偏导了：$$\begin{array}{rl}\frac{\partial g(q_i)}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}(q_i\cdot q_i-1)\\ & =2q_i\cdot \frac{\partial q_i}{\partial q_{i,x}}=2q_{i,x}\end{array}\text{\hspace{1em}(1.9)}$$$$\begin{array}{rl}\frac{\partial l(q_{i-1})}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{\Vert q_{i-1}-q_{i-2}\Vert +\Vert q_{i-1}-q_i\Vert }{2}\\ & =\frac{1}{2}\frac{\partial }{\partial q_{i,x}}\left(\sqrt{(q_{i-1}-q_{i-2})\cdot (q_{i-1}-q_{i-2})}+\sqrt{(q_{i-1}-q_i)\cdot (q_{i-1}-q_i)}\right)\\ & =\frac{1}{2}\left(-\frac{1_x\cdot (q_{i-1}-q_i)}{\sqrt{(q_{i-1}-q_i)\cdot (q_{i-1}-q_i)}}\right)\\ & =\frac{1_x\cdot (q_i-q_{i-1})}{2\Vert q_{i-1}-q_i\Vert }\end{array}\text{\hspace{1em}(1.10)}$$$$\begin{array}{rl}\frac{\partial l(q_i)}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{\Vert q_i-q_{i-1}\Vert +\Vert q_i-q_{i+1}\Vert }{2}\\ & =\frac{1}{2}\frac{\partial }{\partial q_{i,x}}\left(\sqrt{(q_i-q_{i-1})\cdot (q_i-q_{i-1})}+\sqrt{(q_i-q_{i+1})\cdot (q_i-q_{i+1})}\right)\\ & =\frac{1}{2}\left(\frac{1_x\cdot (q_i-q_{i-1})}{\sqrt{(q_i-q_{i-1})\cdot (q_i-q_{i-1})}}+\frac{1_x\cdot (q_i-q_{i+1})}{\sqrt{(q_i-q_{i+1})\cdot (q_i-q_{i+1})}}\right)\\ & =\frac{1_x\cdot (q_i-q_{i-1})}{2\Vert q_i-q_{i-1}\Vert }+\frac{1_x\cdot (q_i-q_{i+1})}{2\Vert q_{i+1}-q_i\Vert }\end{array}\text{\hspace{1em}(1.11)}$$$$\begin{array}{rl}\frac{\partial l(q_{i+1})}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{\Vert q_{i+1}-q_i\Vert +\Vert q_{i+1}-q_{i+2}\Vert }{2}\\ & =\frac{1}{2}\frac{\partial }{\partial q_{i,x}}\left(\sqrt{(q_{i+1}-q_i)\cdot (q_{i+1}-q_i)}+\sqrt{(q_{i+1}-q_{i+2})\cdot (q_{i+1}-q_{i+2})}\right)\\ & =\frac{1}{2}\left(-\frac{1_x\cdot (q_{i+1}-q_i)}{\sqrt{(q_{i+1}-q_i)\cdot (q_{i+1}-q_i)}}\right)\\ & =\frac{1_x\cdot (q_i-q_{i+1})}{2\Vert q_{i+1}-q_i\Vert }\end{array}\text{\hspace{1em}(1.12)}$$$$\begin{array}{rl}\frac{\partial q_{i-1}^{{}^{\prime \prime }}}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{q_{i-2}-2q_{i-1}+q_i}{l(q_{i-1}{)}^2}\\ & =\frac{l(q_{i-1}{)}^2\frac{\partial }{\partial q_{i,x}}(q_{i-2}-2q_{i-1}+q_i)-(q_{i-2}-2q_{i-1}+q_i)\frac{\partial }{\partial q_{i,x}}l(q_{i-1}{)}^2}{l(q_{i-1}{)}^4}\\ & =\frac{l(q_{i-1}{)}^2\frac{\partial }{\partial q_{i,x}}{q}_i-(q_{i-2}-2q_{i-1}+q_i)2l(q_{i-1})\frac{\partial }{\partial q_{i,x}}l(q_{i-1})}{l(q_{i-1}{)}^4}\\ & =\frac{l(q_{i-1}{)}^2{1}_x-2(q_{i-2}-2q_{i-1}+q_i)l(q_{i-1})\frac{\partial }{\partial q_{i,x}}l(q_{i-1})}{l(q_{i-1}{)}^4}\end{array}\text{\hspace{1em}(1.13)}$$$$\begin{array}{rl}\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{q_{i-1}-2q_i+q_{i+1}}{l(q_i{)}^2}\\ & =\frac{l(q_i{)}^2\frac{\partial }{\partial q_{i,x}}(q_{i-1}-2q_i+q_{i+1})-(q_{i-1}-2q_i+q_{i+1})\frac{\partial }{\partial q_{i,x}}l(q_i{)}^2}{l(q_i{)}^4}\\ & =\frac{l(q_i{)}^2\frac{\partial }{\partial q_{i,x}}(-2q_i)-(q_{i-1}-2q_i+q_{i+1})2l(q_i)\frac{\partial }{\partial q_{i,x}}l(q_i)}{l(q_i{)}^4}\\ & =-\frac{2l(q_i{)}^2{1}_x+2(q_{i-1}-2q_i+q_{i+1})l(q_i)\frac{\partial }{\partial q_{i,x}}l(q_i)}{l(q_i{)}^4}\end{array}\text{\hspace{1em}(1.14)}$$$$\begin{array}{rl}\frac{\partial q_{i+1}^{{}^{\prime \prime }}}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{q_i-2q_{i+1}+q_{i+2}}{l(q_{i+1}{)}^2}\\ & =\frac{l(q_{i+1}{)}^2\frac{\partial }{\partial q_{i,x}}(q_i-2q_{i+1}+q_{i+2})-(q_i-2q_{i+1}+q_{i+2})\frac{\partial }{\partial q_{i,x}}l(q_{i+1}{)}^2}{l(q_{i+1}{)}^4}\\ & =\frac{l(q_{i+1}{)}^2\frac{\partial }{\partial q_{i,x}}{q}_i-(q_i-2q_{i+1}+q_{i+2})2l(q_{i+1})\frac{\partial }{\partial q_{i,x}}l(q_{i+1})}{l(q_{i+1}{)}^4}\\ & =\frac{2l(q_{i+1}{)}^2{1}_x-2(q_i-2q_{i+1}+q_{i+2})l(q_{i+1})\frac{\partial }{\partial q_{i,x}}l(q_{i+1})}{l(q_{i+1}{)}^4}\end{array}\text{\hspace{1em}(1.15)}$$$$\begin{array}{rl}\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\left(q_{i-1}^{{}^{\prime \prime }}-\frac{q_{i-1}^{{}^{\prime \prime }}\cdot q_{i-1}}{q_{i-1}\cdot q_{i-1}}{q}_{i-1}\right)\\ & =\frac{\partial q_{i-1}^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{\frac{\partial q_{i-1}^{{}^{\prime \prime }}}{\partial q_{i,x}}{q}_{i-1}}{q_{i-1}\cdot q_{i-1}}{q}_{i-1}\end{array}\text{\hspace{1em}(1.16)}$$$$\begin{array}{rl}\frac{\partial \tilde{\kappa }(q_i)}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\left(q_i^{{}^{\prime \prime }}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{q}_i\right)\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}\frac{\partial q_i}{\partial q_{i,x}}-\frac{\partial }{\partial q_{i,x}}\left(\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}\right)q_i\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{1}_x-\frac{\partial }{\partial q_{i,x}}\left(\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}\right)q_i\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{1}_x-\left(\frac{\frac{\partial q_i^{{}^{\prime \prime }}\cdot q_i}{\partial q_{i,x}}{q}_i\cdot q_i-\frac{\partial q_i\cdot q_i}{\partial q_{i,x}}{q}_i^{{}^{\prime \prime }}\cdot q_i}{(q_i\cdot q_i{)}^2}\right)q_i\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{1}_x-\left(\frac{\left(\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}\cdot q_i+q_i^{{}^{\prime \prime }}\cdot \frac{\partial q_i}{\partial q_{i,x}}\right)q_i\cdot q_i-2\left(q_i\cdot \frac{\partial q_i}{\partial q_{i,x}}\right)q_i^{{}^{\prime \prime }}\cdot q_i}{(q_i\cdot q_i{)}^2}\right)q_i\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{1}_x-\left(\frac{\left(\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}\cdot q_i+q_i^{{}^{\prime \prime }}\cdot 1_x\right)q_i\cdot q_i-2\left(q_i\cdot 1_x\right)q_i^{{}^{\prime \prime }}\cdot q_i}{(q_i\cdot q_i{)}^2}\right)q_i\end{array}\text{\hspace{1em}(1.17)}$$$$\begin{array}{rl}\frac{\partial \tilde{\kappa }(q_{i+1})}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\left(q_{i+1}^{{}^{\prime \prime }}-\frac{q_{i+1}^{{}^{\prime \prime }}\cdot q_{i+1}}{q_{i+1}\cdot q_{i+1}}{q}_{i+1}\right)\\ & =\frac{\partial q_{i+1}^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{\frac{\partial q_{i+1}^{{}^{\prime \prime }}}{\partial q_{i,x}}{q}_{i+1}}{q_{i+1}\cdot q_{i+1}}{q}_{i+1}\end{array}\text{\hspace{1em}(1.18)}$$
现在我们对梯度中的所有项都有了简单且长的表达式。我们回到梯度方程$(1.8)$，可看到其所有组成项均已导出，将方程$(1.4)$中$\tilde{\kappa }(q_{i-1}),\tilde{\kappa }(q_i),\tilde{\kappa }(q_{i+1})$的导出项和偏导数方程$(1.16),(1.17),(1.18)$中$\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}},\frac{\partial \tilde{\kappa }(q_i)}{\partial q_{i,x}},\frac{\partial \tilde{\kappa }(q_{i+1})}{\partial q_{i,x}}$的方程代入梯度方程$(1.8)$，即可显式计算出梯度。
下面我们看算法的具体步骤。

1.3 Sping算法步骤

上面我们推导出了Sping算法中梯度的显式离散表达式，现在我们可以根据梯度下降法给出Sping算法的详细步骤：

1. 初始化
   给出一个合理的初始估计值${\mathbf{q}}^0=(q_1,...,q_N)$。容易想到的是使用已介绍过的方法$Slerp$或$Squad$。初始估计值越好，方法收敛越快。
2. 迭代
   根据公式$(1.8)$的推导，计算出梯度：$$▽F=((\frac{\partial F}{\partial q_{1,1}},\frac{\partial F}{\partial q_{1,2}},\frac{\partial F}{\partial q_{1,3}},\frac{\partial F}{\partial q_{1,4}}),...,(\frac{\partial F}{\partial q_{N,1}},\frac{\partial F}{\partial q_{N,2}},\frac{\partial F}{\partial q_{N,3}},\frac{\partial F}{\partial q_{N,4}}))$$注：所有关键帧梯度初始估计值均设为零。
   对初始估计值：${\mathbf{q}}^{\mathbf{i}+1}={\mathbf{q}}^{\mathbf{i}}-e▽F$进行迭代，其中$e$定义为步长。
   或者:在初始估计值上执行迭代：${\mathbf{q}}^{\mathbf{i}+1}={\mathbf{q}}^{\mathbf{i}}-e\frac{▽F}{\Vert ▽F\Vert }$。该方法可以提供更大的数值稳定性。
3. 终止条件
   重复第二步，直到达到预定的终止条件。终止条件可以使用迭代次数，总能量$F$，与控制点数量有关的能量$F$，梯度的大小，或者更高级的策略。

这就是算法的基本步骤，下面我们看一下算法中对范数的改进及算法的扩展。

2. 优化策略——计算Sping插值范数的替代方法及算法扩展

2.1 计算Sping插值范数的三种替代方法

如上所述，我们希望插值曲线位于单位四元数的$H_1$空间，这是通过使用方程$(1.7)$中的惩罚函数$g(q_i)$做到的。但是很难确定方程$(1.7)$中惩罚函数的权重因子$c\in \mathbb{R}$ 多大才能与$E$“相匹配”。理论上，$c$必须是无穷大，才能保证$q_i\in H_1$，有几种方法可以从不同角度解决这个问题，下面分别介绍：

1. 投影
   在解空间内和解空间外的点之间有一个简单且定义明确的连接，即投影。如前所述，通过原点的直线上的所有四元数都执行相同的旋转。因此，通过对生成的四元数进行归一化，可以简单地将解估计值投影到解空间中。这可以在每次迭代中执行一次投影，也可以在最后一次迭代之后再投影。
2. 拉格朗日算子²
   惩罚函数$g(q_i)$也可以用拉格朗日算子${\lambda }_i$来引入，方法如下：$$F=E+\sum _{i=1}^N{\lambda }_{i}g(q_i)$$方程$(1.1)$的解是$F$的一个奇异点。然而奇异点并不是最小值，而是一个鞍点。因此梯度下降法对于此组合四元数$F$仍然适用，需要注意的是梯度上升必须在辅助变量${\lambda }_i$上执行。
   通过在方程$(1.7)$中加入惩罚函数和拉格朗日算法，该方法变得更具有数值鲁棒性，收敛速度更快。该方法的细节可以在附录中的[Platt & Barr, 1988]和[Barr et al.,1992]中找到。
3. 极坐标
   在上述的求解方法中，我们重新表述了问题，使解空间上的限制被整合到最小化的能量函数中（加入$g(q_i)$）。我们还可以重新定义对解空间的限制，降低最小化表达式的复杂度（投影）。这也可以通过用极坐标来表示四元数实现，此时四元数被写为：$q=[r,(\rho ,\theta ,\varphi )]$，其中$r$是半径，$(\rho ,\theta ,\varphi )$是三个必要的旋转角度。
   当使用这种表示方式时，可以很容易地保持在解空间的限制内$H_1$上求解。这可以通过不对半径坐标$r$执行梯度下降来实现，使该坐标保持恒定值1，这样就保持了解空间的限制。

从理论上看，我们认为对生成的四元数进行归一化是“作弊”行为，但也不是不可用。在上述表示中，极坐标将确保插值曲线停留在单位四元数的$H_1$空间中；使用拉格朗日算子将确保一个更鲁棒和更快的收敛算法。为了使算法尽可能简单，我们不会再进一步探索上述可能性，即我们将不再进一步讨论这三种替代方法中的任何一种。

2.2 角速度惩罚项

一般认为，梯度下降法是一种很容易推广的方法。但是对于插值曲线的预期性质，必须可以简单地描述为某些函数的零交叉或恒定状态。例如，我们可能想要确保整个插值曲线的角速度是恒定的，这可以使用另一个惩罚函数得到：$$w(q_i)=\Vert q_{i-1}-q_i\Vert -\Vert q_i-q_{i+1}\Vert \text{\hspace{1em}(2.1)}$$可以看到，惩罚函数$w(q_i)$随上步长$q_{i-1}$与下步长$q_{i+1}$之差而增加。该惩罚项可以简单地引入到算法中，比如将其引入到方程$(1.7)$的原始能量函数中：$$F=\sum _{i=1}^{N}l(q_i){\Vert \widetilde{\kappa (q_i)}\Vert }^2+c_{g}g(q_i{)}^2+c_{w}w(q_i{)}^2\text{\hspace{1em}(2.2)}$$因此必须在梯度方程$(1.8)$上加上一项。这很容易推导，因为$w(q_i)=2l(q_i)$，因此可以用方程$(1.11)$得到其导数：$$\frac{\partial w(q_i)}{\partial q_{i,x}}=\frac{1_x\cdot (q_i-q_{i-1})}{\Vert q_i-q_{i-1}\Vert }+\frac{1_x\cdot (q_i-q_{i+1})}{\Vert q_{i+1}-q_i\Vert }\text{\hspace{1em}(2.3)}$$添加上述惩罚项并不能保证插补曲线速度不变，只能使角速度平缓些。这是由于梯度包含了其他影响各个关键帧之间距离的项。如Spicv中第3.2节所述，局部曲率的定义包括几何曲率（一阶导）和插值曲线的角加速度（二阶导）的大小。在实践中，这意味着在关键帧周围，插值曲线的角速度将会更小（速度也小），而曲线曲率最大（关键帧通常为拐弯处）。

当梯度包含反作用的项时，算法的鲁棒性就会降低。因此，设置$c_w$“相匹配”并不一定保证插值帧之间的距离大致相同。相反，这可能导致该方法在数值上变得不稳定，产生不想要的结果。

3. 实践Sping算法时的改进

以上对算法的描述都是纯理论的，我们不希望仅仅从理论上分析算法的收敛性和稳定性，这样不能保证该方法在实践中有效。因此，我们将提出一些在实现算法时具体需要考虑的因素和改进措施。

3.1 多步放置多次迭代

在实践中，当初始估计给定很多帧时，该算法表现得很糟糕，因为每一帧只受它相邻帧影响，并且最初只有关键帧被正确定位。因此，插值曲线对关键帧的所有适配性必须通过关键帧和给定帧之间的帧进行传播，介于两者之间的帧越多，系统就需要更多的迭代才能达到能量最小值，这实际上给出了每个关键帧之间可接受的帧数的上限。
注意：每添加一次关键帧/给定帧称为放置一次，对估计值${\mathbf{q}}^{\mathbf{i}+1}={\mathbf{q}}^{\mathbf{i}}-e▽F$进行一次更新记为一次迭代。

解决这个问题的一个有效方法是将问题简化为多步放置多次迭代，类似m*n次循环。在第一步放置中使用相对较少的帧，这样系统在几次迭代后达到能量最小值。在第一步中计算出的帧在第二步中作为关键帧使用。在第二步放置中添加更多的中间帧，并且在几次迭代之后再次达到能量最小值。这个过程可以重复任意次数，直到达到所需的帧数。

在图3-1中可以看到算法的结果，在第一步中放置了所有帧。即使经过多次迭代，也会给出一个糟糕的近似曲线。如果用Slerp生成传递给优化算法的初始估计帧，那么该结果也与Slerp非常相似。

图3-1 一步放置多次迭代
说明：图3-1是一步放置多次迭代后插值曲线的图像。第一步插值350帧，迭代500次后的显示。

如果使用多步放置多次迭代算法，经过较少的迭代就可以得到更好的插值曲线。在图3.2中可以看到第一步插值帧数很少，这作为第二步的初始配置。最终的插值曲线如图3.3所示。当使用多步算法时，角速度图表现也更好。如果使用一步放置法(见图3.1)，速度曲线类似于心电图，而多步算法(见图3.3)的速度曲线稍好一些，但仍然有些不均匀。这是我们算法的一个缺点，所以我们期待未来能出现一个具有更好收敛性更健壮的算法，能产生更好的速度图。

图3-2 多步放置多次迭代 第一步图像

说明：图3-2是多步放置多次迭代的第一步图像。第一步插值22帧，迭代200次后的显示。

总的来说，在多步放置方法中使用了550次迭代，当使用多步方法时，结果明显更好。

图3-3 多步放置多次迭代 最终图像
说明：图3-3是多步放置迭代的最终图像。第二步插值110帧，迭代150次；在第三步和最后一步，插值550帧并迭代100次。

3.2 简化参数宽度——$l(q_i)$

公式$(1.3)$给出了插补曲线参数的“参数宽度”的数值近似。实践证明，该表达式对插值曲线的形状没有影响。然而，该表达式使算法在数值上变得不那么稳定。因此，我们使用更简单的表达式：$$l(q_i)=1\text{\hspace{1em}(3.1)}$$
对其求偏导：$$\frac{\partial l(q_i)}{\partial q_{i,x}}=\frac{\partial l(q_{i-1})}{\partial q_{i,x}}=\frac{\partial l(q_{i+1})}{\partial q_{i,x}}=0\text{\hspace{1em}(3.2)}$$梯度的表达式方程$(1.8)$也相应简化为：$$\frac{\partial F}{\partial q_{i,x}}=2\tilde{\kappa }(q_{i-1})\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}+2\tilde{\kappa }(q_i)\frac{\partial \tilde{\kappa }(q_i)}{\partial q_{i,x}}+2\tilde{\kappa }(q_{i+1})\frac{\partial \tilde{\kappa }(q_{i+1})}{\partial q_{i,x}}+2cg(q_i)\frac{\partial g(q_i)}{\partial q_{i,x}}\text{\hspace{1em}(3.3)}$$

3.3 对关键帧曲率加权

曲率最小化的连续解析解Spicv算法可以确保曲率是最小的，而离散数值方法Sping算法只给出了最小曲率的近似解。而近似解的有效性取决于对其数学组成表达式的近似程度。例如，插值曲线的二阶导数近似表示为：$$q_i^{{}^{\prime \prime }}=\frac{q_{i-1}-2q_i+q_{i+1}}{l(q_i{)}^2}$$上式即为方程$(1.5)$。这种近似是插值曲线局部曲率$\kappa (\gamma ,t)$近似的核心。使用关键帧在特定曲线之间插值时，预测这种近似是否会给数值解带来“缺陷”是很有必要的。

比如尖锐曲线问题。在实际应用中，数值方法对尖锐曲线有些敏感。例如，图3-4中的曲线在关键帧的位置就太“尖锐”了：

图3-4 多步放置多次迭代 尖锐曲线图像
说明：图3-4是对尖锐曲线的关键帧使用多步放置算法的图像，插值200帧。

对曲率的近似不能充分地在关键帧上传播。让我们重新检查梯度(原版公式1.8的简化版，公式3.3)：
$$\frac{\partial F}{\partial q_{i,x}}=2\tilde{\kappa }(q_{i-1})\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}+2\tilde{\kappa }(q_i)\frac{\partial \tilde{\kappa }(q_i)}{\partial q_{i,x}}+2\tilde{\kappa }(q_{i+1})\frac{\partial \tilde{\kappa }(q_{i+1})}{\partial q_{i,x}}+2cg(q_i)\frac{\partial g(q_i)}{\partial q_{i,x}}$$最后一项确保四元数仍然是单位四元数，与曲率无关，它可以忽略掉，这样在确定梯度以及迭代过程中单个帧的移动时，就只留下了三个因素需要考虑：$2\tilde{\kappa }(q_{i-1})\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}+2\tilde{\kappa }(q_i)\frac{\partial \tilde{\kappa }(q_i)}{\partial q_{i,x}}+2\tilde{\kappa }(q_{i+1})\frac{\partial \tilde{\kappa }(q_{i+1})}{\partial q_{i,x}}$，这三个元素代表四元数本身以及相邻四元数的曲率变化。

我们通常认为考虑四元数本身的曲率就足够了。然而，如果不考虑相邻帧，算法结果将退化为$Slerp$，$Slerp$中除了关键帧之外的每一帧都是零曲率的，它不会对关键帧中的大曲率进行补偿。

因此，每个关键帧的相邻帧决定了关键帧的曲率是否最小，所以必须“注意”它们相邻帧的曲率，而这是由方程中的项$2\tilde{\kappa }(q_{i-1})\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}$和$2\tilde{\kappa }(q_{i+1})\frac{\partial \tilde{\kappa }(q_{i+1})}{\partial q_{i,x}}$来保证的。如图3-4所示，但这还不够，关键帧的图像还是太“尖锐”。

因此，插值曲线二阶导数的近似值在这种情况下不是一个好的选择，这个近似太局部化了，应该有更宽的范围。但这不是一篇关于数值计算方法的报告，因此不在深究最佳的近似表达式。

不过，我们所描述的问题也可以通过简单的方法解决：每个关键帧可以简单地“要求”它的相邻帧更加“贴合”，换句话说，可以在梯度的每个表达式上添加一个权重函数，权重值取决于一个给定的帧是否是某个关键帧的邻居。例如，如果$q_{i-1}$是关键帧，则将$2\tilde{\kappa }(q_{i-1})\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}$替换为$2{\mathbf{c}}_{\mathbf{k}}\tilde{\kappa }(q_{i-1})\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}$。在实际应用中，$c_k$取值约为$1.2\sim 1.5$，加权函数的效果如图3-5所示：

图3-5 多步放置多次迭代 加权尖锐曲线图像
说明：图3-5是对尖锐曲线的关键帧使用多步放置算法并加权的图像，对其插值200帧，关键帧周围的曲率加权系数为1.2。

然而“通过眼睛”（即$2\tilde{\kappa }(q_{i-1})\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}$）在算法中添加一个常数$c_k$，与我们声明的从客观标准推导插值曲线的目的是不一致的，但并不影响效果。另一种方法是分析不同数值近似法的性质，但这和收敛性和稳定性一样，不在这个项目的讨论范围之内。

3.4 改进后的图像

在图3-6中可以看到使用Sping并改进后的总体效果。结果只比在关键帧上不包含曲率特殊处理的版本的图3-3略好。在下一章中，我们将看到Sping更清晰地展示其价值的例子。

图3-6 Sping效果图
说明：图3-6是Sping效果图，对其插值550帧，关键帧周围的曲率加权系数为1.3。

3.5 其它细节说明

目前为止，我们还没有描述上述算法的终止要求。在实践中，我们选择了最简单的方法：迭代次数。相应地，我们也没有定义算法使用的产生初始估计值的方法。我们选择在每一对关键帧之间使用Slerp，这个起点显然不是最优的，使用Squad可以实现更快的收敛。但此处作为对比，我们的目的是获得比Squad更优秀的插值曲线，所以使用Slerp作为初始估计似乎更公平。当单独使用Sping时，采用Squad作为初始估计显然更好。

算法的其余常数(迭代中的步长$e$和惩罚因子$c$)都可以根据鲁棒性和收敛性准则计算出来。正如前面提到的，我们将不再进一步描述这些属性，并将常量视为实现细节，详情请参见博主上传资源中的程序介绍——四元数插值绘图论文原始代码。

4. Squad与Sping对比

在四元数部分，我们讨论了一系列的插值算法。在已知的算法中，最有说服力的是Squad。在本章中，我们将比较Squad和我们自己的算法Sping，比较将以一些具有代表性的例子为基础进行说明。

4.1 例1：准圆

首先，我们检查一下Squad和Sping是否可以产生漂亮的圆形曲线。我们将关键帧放置在一个球面上正方形的直角上，围绕中心关键帧的插值曲线应该近似为一个准圆。图4-1和图4-2显示，这两条曲线都很好地满足了这一要求，相比较而言，Sping的曲线更接近正圆。

图4-1 带有柔和圆角的Squad简单曲线
说明：图4-1是带有柔和圆角的简单曲线，柔和圆角上使用Squad进行插值。插值中共使用550帧。

图4-2 带有柔和圆角的Sping简单曲线
说明：图4-2是带有柔和圆角的简单曲线，柔和圆角中使用Sping进行插值。生成步骤中总共使用了430帧和700次迭代，关键帧的曲率没有使用额外的权重。

4.2 例2：柔和曲线

这个例子对这两种算法都不是真正的挑战。预期的插值曲线是没有尖角的圆形曲线，曲线在交点处应该不成问题。

图4-3和图4-4的插值曲线都很好。然而，即使关键帧的曲率没有添加额外的权重，Sping的曲线依然比Squad的角更圆润，这意味着Sping的曲率更小。此外，Sping的速度图比Squad的速度图更稳定，这意味着Sping的行为更符合预期。

图4-3 带有交点的Squad柔和曲线
说明：图4-3是对一组有交点的柔和曲线进行Squad插值。在插值中总共使用了550帧。

图4-4 带有交点的Sping柔和曲线
说明：图4-4是对一组有交点的柔和曲线进行Sping插值，在插值步骤上共有550帧和700次迭代，关键帧周围的曲率没有增加额外权重。

4.3 例3：带尖点的插值曲线

Squad可以在关键帧处产生非常尖锐的插值曲线。插值曲线Squad(图4-5)呈现出一条尖尖的曲线。然而，Sping能够产生一个很光滑的圆角插值曲线(图4-6)。

另外，Squad的插值曲线有一个尖角并不与Squad可微分的事实相矛盾。在有尖角的关键帧，曲线的速度为零。因此，尽管曲线的几何外观不是直观上可微分的，但函数Squad仍然是可微分的。

图4-5 带有尖点的Squad插值曲线
说明：图4-5是用Squad插值了一个带有尖点的曲线，使用了200帧。

图4-6 带有尖点的Sping插值曲线
说明：图4-6是用Sping插值了一个带有尖点的曲线，总共有200帧和700次迭代分布在三个步骤中，关键帧周围曲率的相对权重为1.3。

4.4 例4：钟摆

我们继续研究曲率较大的曲线。在这个例子中，我们用的是曲率无穷大的曲线——钟摆运动，它通过三个关键帧实现的，其中第一帧和最后一帧是相等的。

期望的插值曲线的行为在视觉上并不明显，因为所有的关键帧都在一条弧线上，所以曲线应该保持在这个弧线上。

图4-7和图4-8显示了这两条曲线的相同行为——钟摆运动。注意：Lerp和Slerp将产生相同的曲线。

图4-7 Squad插值显示钟摆运动
说明：图4-7是Squad插值显示钟摆运动，其插值超过250帧

图4-8 Sping插值显示钟摆运动
说明：图4-8是Sping插值显示钟摆运动，它在三个步骤内插值超过200帧和700次迭代，关键帧周围曲率的相对权重为1.5。

4.5 例5：扰动摆

在钟摆运动中，尽管当关键帧都在同一弧线上时，插值曲线保持在同一弧线上是很合理的，但并不总是这样。在实际的钟摆曲线上，曲线在尖端关键帧处的曲率是无限大的。本期望Sping应该曲率更小，但实际上并不是，这多少有点令人失望。
而对于扰动摆，要理解它有必要记住这个算法：使用梯度下降，每一帧将在每一步轻微移动，以减少曲率。但是靠近中间关键帧的帧应该向哪个方向移动呢?因为曲线是对称的，所以每一帧的梯度都是零，但这并不表示曲率是一个局部极小值，恰恰相反，它是一个局部极大值。
因此，钟摆就是一个使Sping失去作用的例子。我们进一步研究这一点，通过略微扰乱第一帧和最后关键帧，使曲线不再是一个纯粹的钟摆，如图4-9所示，注意只是略微不同。
图4-10显示梯度下降算法Sping不再排斥纯圆弧的固定点，产生了较圆滑的曲线。因此这里我们以最小的扰动为代价，让插值曲线在中间关键帧处做到漂亮圆滑。

图4-9 Squad插值的扰动摆
说明：图4-9中Squad插值的扰动摆，使用250帧进行插值。

图4-10 Sping插值的扰动摆
说明：图4-10是用Sping插值的扰动摆，它在三个步骤中插入200帧和700次迭代，关键帧周围曲率的相对权重为1.5。

4.6 例6：全局属性

最后一个例子展示了Squad和Sping的根本区别。在每个间隔中，Squad的插值曲线都是由之前的两个关键帧和之后的两个关键帧定义的，比如局部曲率定义。相比之下，Sping的插值曲线是全局定义的。

图4-11显示了在一组共5个关键帧上的插值曲线。前三个关键帧近似地位于一条较平的弧线上，因此插值曲线在第一个间隔内是一条弧线。同样的，插值曲线在最后一个间隔内也形成一条弧线。

图4-11 Squad插值尖曲线
说明：图4-11是Squad使用550帧产生尖曲线。

相比之下，Sping的插值曲线(图4-12)是漂亮且平滑的。该算法的全局结构允许曲线在所有间隔上均匀分布曲率，因此在中间关键帧不存在过度曲率，而是将部分曲率传播到外部间隔。

图4-12 Sping插值尖曲线
说明：图4-12为Sping插值尖曲线，它使用430帧和900次迭代避免了产生尖端。关键帧周围曲率的相对权重为4。

需要注意的是，在这个例子中，有必要在关键帧周围的曲率上增加一个相对较大的权重。然而，毫无疑问，对组成表达式(特别是方程1.5中的$q_i^{{}^{\prime \prime }}$)有更好的数值近似的算法将产生相同的结果——而不必将程序的参数整合到示例中。

4.7 结论

下面显示了这两种方法的基本区别：

在Squad和Sping之间的做选择的标准并不明显。如果在大多数情况下需要一个简单的算法来产生良好的结果，那么简单的Squad就足够了。如果要求更好的插值算法，在所有情况下，更复杂的Sping将更合适。

5. 插值曲线总结

本篇的目标是客观地探索最优插值方法，从一般求解规则推导出插值曲线。我们首先研究了在文献中发现的或多或少的萌芽式插值曲线，其中包括初级的LinMat, LinEuler, Lerp, 还有较简单的Slerp，最后的结果是令人较满意的Squad。

以这些简单的插值曲线作为基础，我们试图定义插值曲线应该属于哪一类函数（上一篇Spicv的第2.2节），然而我们无法确定哪一种平滑性（$C^n$）适合于定义期望的这类函数。

然后我们试图定义一条插值曲线，使插值曲线的局部曲率积分$K(\gamma )$最小化。这些推导要求插值曲线具有四阶可导的连续导函数（注意是导函数，不是导数）：$\gamma (t)\in C^4(I,H^1)$。不幸的是，这种推导产生了一个我们无法求解的四阶微分方程。因此，对于Spicv的第2.2节中的开放问题：“曲线应可导多少次，是否允许奇点?”，不再有讨论的必要。

因此，我们推导出了一个离散的数值解，并提出了一种基于梯度下降的方法，我们检查并改进了这种方法。最终的结果是一些非常令人满意的插值曲线。

作为我们最终的插值算法，我们将选择本篇的梯度下降算法，同时在子间隔内使用Squad产生相对应分布的插值帧，并在关键帧周围使用加权曲率，我们将这条插值曲线命名为Sping。

本文基于《视觉SLAM十四讲：从理论到实践》和《Quaternions, Interpolation and Animation》编写，但相对于原文会适当精简，同时为便于全面理解，会收集其他网络好文，根据作者理解，加入一些注解和扩展知识点，如果您觉得还不错，请一键四连（点赞关注收藏评论），让更多的人看到。

参考文献：

1. 《视觉SLAM十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社
2. Quaternions, Interpolation and Animation
3. [Kincaid & Cheney, 1991] David Kincaid & Ward Cheney. Numerical Analysis. Brooks/Cole Publishing Company, Pacific Grove, California, 1991.
4. [Barr et al., 1992] Alan H. Barr, Bena Currin, Steven Gabriel, & John F. Hughes. Smooth interpolation of orientations with angular velocity constraints using quaternions. Computer Graphics, 26(2):313-320, July 1992.
5. [Platt & Barr, 1988] John C. Platt & Alan H. Barr. Constraint methods for exible models.Computer Graphics, 22(4):279{288, August 1988.

---

1. 推导请参考[Kincaid & Cheney, 1991], [Barr et al., 1992] ↩︎

2. 关于拉格朗日算子请参考《如何理解拉格朗日乘子法？》。 ↩︎