对偶四元数的应用

对偶四元数的定义

对偶四元数的一般形式为：$\hat{q}=r+\varepsilon s$,其中$r=r_0+ir_1+jr_2+kr_3$、$s=s_0+is_1+js_2+ks_3$均为四元数，$\varepsilon$为对偶运算符，满足$\varepsilon \neq 0,\varepsilon^2=0$。

对偶四元数与旋转矩阵$R$,平移向量$t$的关系为：
$\begin{bmatrix}1&0^T\\0&R\end{bmatrix}=W^T(r)Q(r)$
$\frac{1}{2}\begin{bmatrix}t\\0 \end{bmatrix}=W^T(r)s$
上式中，$Q(r)=\begin{bmatrix}r_0&-r_{123}^T\\r_{123}&r_0I_{3\times3}+K(r_{123})\end{bmatrix}$，$W(r)=\begin{bmatrix}r_0&-r_{123}^T\\r_{123}&r_0I_{3\times3}-K(r_{123})\end{bmatrix}$，$r_{123}=[r_1,r_2,r_3]^T$，$K(r_{123})=\begin{bmatrix}0&-r_3&r_2\\r_3&0&-r_1\\-r_2&r_1&0\end{bmatrix}$
整理得：
$R=\begin{bmatrix}r_0^2+r_1^2-r_2^2-r_3^2&2(r_1r_2-r_0r_3)&2(q_1q_3+q_0q_2)\\ 2(r_1r_2+r_0r_3)&r_0^2-r_1^2+r_2^2-r_3^2&2(r_2r_3-r_0r_1)\\2(r_1r_3-r_0r_2)&2(r_2r_3+r_0r_1)&r_0^2-r_1^2-r_2^2+r_3^2\end{bmatrix}$
$t=\begin{bmatrix}2(r_0s_1-r_1s_0+r_2s_3-r_3s_2)\\2(r_0s_2-r_1s_3-r_2s_0+r_3s_1)\\2(r_0s_3-r_1s_2-r_2s_1-r_3s_0)\end{bmatrix}$

对偶四元数应用在位姿恢复中的例子：

标定模型定义如下：
$P_{MT}=R_{MS}P_{ST}+P_{MS}$
目标函数为：
$f(R_{MS},P_{MS})=\frac{1}{N}\sum_{k=1}^N \|P_{MT}-R_{MS}P_{ST}-P_{MS}\|^2$
转换到对偶四元数表示：
$f(r,s)=\frac{1}{N}(r^TD_1r+s^TD_2r+Ns^Ts+D_3)$
其中：$D_1=-2\sum_{k=1}^N[Q^T(P'_{MT})W(P'_{ST})]$
$D_2=2\sum_{k=1}^N[W(P'_{ST})-Q^T(P'_{MT})]$
$D_3=\sum_{k=1}^N ({P'_{ST}}^T P'_{ST}+{P'_{MT}}^TP'_{MT})=const$
$P'_{MT}$与$P'_{ST}$为$P_{MT}$和$P_{ST}$对应的纯虚数。

利用拉格朗日乘数法，构造辅助函数：
$f(r,s,\lambda_1,\lambda_2)=\frac{1}{N}[r^TD_1r+s^TD_2r+Ns^Ts+D_3+\lambda_1(r^Tr-1)+\lambda_2(r^Ts)$
对$r$、$s$求偏导后整理得：
$Ar=\lambda_1r$
其中$A=\frac{1}{2}[\frac{1}{2N}D_2^TD_2-(D_1+D_1^T)]$
而目标函数可以转化为$f(r,s)=\frac{1}{N}(D_3-\lambda_1)$
所以，$r$为$A$得最大特征值对应的特征向量。
得到$r,s$后，即可推出$R,t$。

参考源：http://html.rhhz.net/buptjournal/html/20170102.htm