2019年6月 – MathSword数值计算软件

在常规的插值中，我们会根据n个已知点的值，插值出给定点的结果。其中主要利用的思想是，通过这n个点建立某种数学模型，进而推导出这个待插值点的结果。本文这里仅考虑2个已知点的情况，对应到我们常规插值中，就表现为线性插值。但本文这里，由于插值出的结果不是一个数值，是一个需要满足某种性质的矩阵，因此，不能单纯采用矩阵的线性加减。

1 SO(3)旋转矩阵插值

这里直接给出一种插值模型

(1)$$\begin{aligned}
R &= (R_1R_0^T)^aR_0\\
&= R_{10}^aR_0\\
&=\exp(\varphi ^ \wedge)^aR_0\\
&=\exp(a \varphi ^ \wedge)R_0 \\
&=\exp((a \varphi )^ \wedge)R_0
\end{aligned}$$

上面公式中的$a$表示插值点的值,其取值范围在[0,1]，当$a = 0$时，对应值为旋转矩阵$R_0$，当$a = 1$时，对应值为旋转矩阵$R_1$，公式中的$\varphi$对应的矩阵为 $R_1R_0^T$

2 Sim(3)位姿矩阵插值

先申明如下SE(3)矩阵格式

(2)$$
T_0 = \begin{bmatrix}
s_0R_0 & t_0\\
0 & 1
\end{bmatrix}\\T_1 = \begin{bmatrix}
s_1R_1 & t_1\\
0 & 1
\end{bmatrix}\\T = \begin{bmatrix}
sR & t\\
0 & 1
\end{bmatrix}$$

即，现在我们想找到一种插值模型，使得$a = 0$时，插值的$T = T_0$;当$a = 1$时，$T = T_1$，当$a$取其它值时，插值的$T$满足Sim(3)性质。其中插值点$a$取值在[0,1]。

即现在需要通过$T_0,T_1$插值出$T$当中的$s,R,t$

2.1 无光心约束插值

与旋转矩阵插值类似，可以构建如下的插值模型

(3)$$\begin{aligned}
T &= (T_1T_0^{-1})^aT_0\\
&= T_{10}^aT_0\\
&=\exp(\xi ^ \wedge)^aT_0\\
&=\exp(a \xi ^ \wedge)T_0 \\
&=\exp((a \xi) ^ \wedge)T_0
\end{aligned}$$

这里将公式(3)展开有

(4)$$T=\begin{bmatrix}
(s_1/s_0)^a(R_1R_0^T)^a & a(t_1 – (s_1/s_0)R_1R_0^Tt_0)\\
0 & 1
\end{bmatrix}
\begin{bmatrix}
s_0R_0 & t_0\\
0 & 1
\end{bmatrix}$$

通过公式(4)可以得到最终的插值结果

(5)$$\begin{aligned}
s &= s_0(\frac{s_1}{s_0})^a\\
R &= (R_1R_0^T)^aR_0\\
t &= (\frac{s_1}{s_0})^a(R_1R_0^T)^at_0 + a(t_1-\frac{s_1}{s_0}R_1R_0^Tt_0)
\end{aligned}$$

2.2 有光心约束插值

当我们使用Sim(3)来表示一个位姿的时候，我们知道一种位姿在空间会对应一个相机中心坐标。当给定两个位姿，然后在它们之间进行插值时，我们可以假设它们插值的相机中心也满足两个位姿相机中心的线性插值。即有如下约束

(6)$$\frac{R^Tt}{s} =(1-a) \frac{R_0^Tt_0}{s_0} + a\frac{R_1^Tt_1}{s_1}$$

公式(6)是一个含有3个等式的约束，如果插值结果的旋转$R$与缩放部分$s$采用公式(5)的结果，那结合公式(6)刚好可以求出插值的$t$,因此最终的插值结果可表示为

(7)$$\begin{aligned}
s &= s_0(\frac{s_1}{s_0})^a\\
R &= (R_1R_0^T)^aR_0\\
t &=sR ((1-a) \frac{R_0^Tt_0}{s_0} + a\frac{R_1^Tt_1}{s_1})
\end{aligned}$$

PS：这篇文章来自于2011年我写在网易博客上的内容，由于网易博客现已关闭，因此特把此文章搬迁到这里，供有缘人参考，指正！

提醒:当你某一天发现同一个复数领域内的算式表达得出不同的结果的时候,请不要太过怀疑,因为这就是复数的多值性!

已知:

这里设$x$、$y$、$m$、$n$为实数，$i$表示虚数单位，即$i = \sqrt{-1}$

$$
\begin{aligned}
A &= x + yi\\
&= e^{a + \psi i}\\
&=|A|e^{\psi i}\\
&=|A|(\cos \psi + \sin \psi i)
\end{aligned}
$$

$$
\begin{aligned}
B &= m + ni\\
&= e^{b + \beta i}\\
&=|B|e^{\beta i}\\
&=|B|(\cos \beta + sin \beta i)
\end{aligned}
$$

其中

$$
|A| = e^a = \sqrt{x^2+y^2}, a = \ln(|A|), \psi =\arctan(y/x)\\
|B| = e^b = \sqrt{m^2+n^2}, b = \ln(|B|), \beta = \arctan(n/m)
$$

以$A$为例，$|A|$叫做复数$A$的模，$\psi$为复数$A$的复角，一般把$-\pi < \psi < \pi$的值叫做主值，此时定义$arg(A)=\psi$，当$A \neq 0$的时候，主值由如下方式确定

(1)、当$x > 0$时,$\psi = \arctan(y/x)$

(2)、当$x=0,y>0$时,$\psi =\frac{\pi}{2}$

(3)、当$x=0,y<0$时,$\psi =-\frac{\pi}{2}$

(4)、当$x<0,y \geqslant 0$时,$\psi =\arctan(y/x)+\pi$

(5)、当$x<0,y<0$时,$\psi =\arctan(y/x)-\pi$

基本运算:

注意，下面的最后的结果都会变成$x +yi$的形式)

$$
A+B=(x+m)+(y+n)i\\
A-B=(x-m)+(y-n)i
$$

$$
\begin{aligned}
AB &= (x+yi)(m+ni)\\
&= xm+xni+ymi+ynii\\
&=(xm-yn)+(xn+ym)i
\end{aligned}
$$

$$
\begin{aligned}
\frac{A}{B} &= \frac{x+yi}{m+ni}\\
&= \frac{(x+yi)(m-ni)}{(m+mi)(m-ni)}\\
&=\frac{(xm+yn)+(ym-xn)}{m^2+n^2}\\
&=\frac{xm+yn}{m^2+n^2}+\frac{ym-xn}{m^2+n^2}i
\end{aligned}
$$

$$
\begin{aligned}
\log(A) &=\log(|A|e^{\psi i})\\
&=\log(|A|)+\log(e^{\psi i})\\
&=\frac{\log(x^2+y^2)}{2}+\psi i\\
&=\log(|A|)+\mathbf{arg}(A)i
\end{aligned}
$$

$$
\begin{aligned}
A^B &= (e^{a+\psi i})^{m+ni}\\
&=e^{(a+\psi i)(m+ni)}\\
&=e^{(am-\psi n)+(an+\psi m)i}\\
&=e^{(am-\psi n)}e^{(an+\psi m)i}\\
&=e^{(am-\psi n)}(\cos(an+\psi m)+\sin(an+\psi m)i)\\
&=e^{(am-\psi n)}\cos(an+\psi m)+e^{(am-\psi n)}\sin(an+\psi m)i
\end{aligned}
$$

对于$A^{0.5} = \sqrt(A)$这种特殊情况，由于开方在一些计算中调用频繁，因此不建议直接使用上面求次方的方式进行计算，而建议直接使用下面提到的算法。

设$B=\sqrt{A}$，即$A=B^2$可以通过对于表达式最终反求出结果，这里就不贴过程，直接给出答案

$$
\sqrt{A}=\sqrt{\frac{\sqrt{x^2+y^2}+x}{2}} + \mathbf{sign}(y)\sqrt{\frac{\sqrt{x^2+y^2}-x}{2}}i
$$

注意，本文在在上面公式中，当$y<0$时，$\mathbf{sign}(y)=-1$；当$y=0$时，$\mathbf{sign}(y)=0$；当$y>0$时，$\mathbf{sign}(y)=1$

$$
\sin(A)=\frac{e^{Ai}-e^{-Ai}}{2i}
$$

$$
\cos(A)=\frac{e^{Ai}+e^{-Ai}}{2}
$$

$$
\tan(A)= \frac{\sin(A)}{\cos(A)}
$$

$$
\cot(A)= \frac{\cos(A)}{\sin(A)}
$$

$$
\sec(A)=\frac{1}{\cos(A)}
$$

$$
\csc(A)=\frac{1}{\sin(A)}
$$

$$
\sinh(A)=\frac{e^A-e^{-A}}{2}
$$

$$
\cosh(A)=\frac{e^A+e^{-A}}{2}
$$

$$
\tanh(A)=\frac{\sinh(A)}{\cosh(A)}
$$

$$
\coth(A)=\frac{\cosh(A)}{\sinh(A)}
$$

$$
\ln_B(A)=\frac{\ln(A)}{\ln(B)}
$$

$$
\begin{aligned}
\arcsin(A)&=-\log(Ai+\sqrt{1-A^2})i
\\
\\
\arccos(A)&=-\log(A+\sqrt{A^2-1})i
\\
\\
\arctan(A)&=-\dfrac{1}{2}\log(\dfrac{1+Ai}{1-Ai})i
\\
\\
\mathbf{arccot}(A)&=-\dfrac{1}{2}\log(\dfrac{Ai-1}{Ai+1})i
\\
\\
\mathbf{arcsinh}(A)&=\log(A+\sqrt{1+A^2})
\\
\\
\mathbf{arccosh}(A)&=\log(A+\sqrt{A^2-1})
\\
\\
\mathbf{arctanh}(A)&=\dfrac{1}{2}\log(\dfrac{1+A}{1-A})
\\
\\
\mathbf{arccoth}(A)&=\dfrac{1}{2}\log(\dfrac{A+1}{A-1})
\end{aligned}
$$

地震来了，公式更新延后

月度归档： 2019年6月

SO(3)与Sim(3)插值

复数的基本运算