对偶四元数的应用

对偶四元数的定义

对偶四元数的一般形式为：$\hat{q}=r+\epsilon s$,其中$r=r_0+i{r}_1+j{r}_2+k{r}_3$、$s=s_0+i{s}_1+j{s}_2+k{s}_3$均为四元数，$\epsilon$为对偶运算符，满足$\epsilon \ne 0,{\epsilon }^2=0$。

对偶四元数与旋转矩阵$R$,平移向量$t$的关系为：
$$\left[\begin{array}{cc}1& 0^T\\ 0& R\end{array}\right]=W^T(r)Q(r)$$
$$\frac{1}{2}\left[\begin{array}{c}t\\ 0\end{array}\right]=W^T(r)s$$
上式中，$Q(r)=\left[\begin{array}{cc}{r}_0& -r_{123}^T\\ r_{123}& r_0{I}_{3\times 3}+K(r_{123})\end{array}\right]$，$W(r)=\left[\begin{array}{cc}{r}_0& -r_{123}^T\\ r_{123}& r_0{I}_{3\times 3}-K(r_{123})\end{array}\right]$，$r_{123}=[r_1,r_2,r_3{]}^T$，$K(r_{123})=\left[\begin{array}{ccc}0& -r_3& r_2\\ r_3& 0& -r_1\\ -r_2& r_1& 0\end{array}\right]$
整理得：
$R=\left[\begin{array}{ccc}{r}_0^2+r_1^2-r_2^2-r_3^2& 2(r_1{r}_2-r_0{r}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(r_1{r}_2+r_0{r}_3)& r_0^2-r_1^2+r_2^2-r_3^2& 2(r_2{r}_3-r_0{r}_1)\\ 2(r_1{r}_3-r_0{r}_2)& 2(r_2{r}_3+r_0{r}_1)& r_0^2-r_1^2-r_2^2+r_3^2\end{array}\right]$
$t=\left[\begin{array}{c}2(r_0{s}_1-r_1{s}_0+r_2{s}_3-r_3{s}_2)\\ 2(r_0{s}_2-r_1{s}_3-r_2{s}_0+r_3{s}_1)\\ 2(r_0{s}_3-r_1{s}_2-r_2{s}_1-r_3{s}_0)\end{array}\right]$

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

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

利用拉格朗日乘数法，构造辅助函数：
$$f(r,s,{\lambda }_1,{\lambda }_2)=\frac{1}{N}[r^T{D}_{1}r+s^T{D}_{2}r+N{s}^{T}s+D_3+{\lambda }_1(r^{T}r-1)+{\lambda }_2(r^{T}s)$$
对$r$、$s$求偏导后整理得：
$$Ar={\lambda }_{1}r$$
其中$A=\frac{1}{2}[\frac{1}{2N}{D}_2^T{D}_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
http://www.jgg09.com/CN/article/downloadArticleFile.do?attachType=PDF&id=11153