對偶四元數的應用

對偶四元數的定義

對偶四元數的一般形式爲：$\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