SO(3)与Sim(3)插值

在常规的插值中，我们会根据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}$$