1 问题
有时,我们需要将一个3D空间的SE3变量映射到指定的2D平面,然后再加以求解。
为了方便说明,这里假设一个SE3的表现形式为\(T\), 假设\(T\) 以如下值来表示
(1)
$$
T_1 =\begin{bmatrix}
-0.69492055764131 & 0.71352099052778 & 0.08929285886191 & 4
\\
-0.19200697279199 & -0.30378504433947 & 0.93319235382364
& 5
\\
0.69297816774177 & 0.63134969938371 & 0.34810747783026 & 8
\\
0 & 0 & 0 & 1
\end{bmatrix}
$$
1.1 x-y平面
为了取出x-y对应的旋转部分分量,这里定义如下变换矩阵
(2)
$$
T_{xy1} =\begin{bmatrix}
1 & 0 & 0 & 0
\\
0 & 1 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\end{bmatrix}
$$
则经过 \(T_{xy1}TT_{xy1}\)可以取到\(T\)中2*2旋转部分,执行后得到类似如下结果
(3)
$$
T_{xy1}TT_{xy1} =\begin{bmatrix}
-0.69492055764131 & 0.71352099052778 & 0 & 0
\\
-0.19200697279199 & -0.30378504433947 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\end{bmatrix}
$$
为了取出x-y偏移部分,这里定义如下矩阵
(4)
$$
T_2 =\begin{bmatrix}
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 1
\end{bmatrix}
$$
则经过 \(T_{xy1}TT_2\)可以取到\(T\)中2*1偏移部分,执行后得到类似如下结果
(5)
$$
T_{xy1}TT_2 =\begin{bmatrix}
0 & 0 & 0 & 4
\\
0 & 0 & 0 & 5
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\end{bmatrix}
$$
这里定义矩阵
(6)
$$
\begin{aligned}
T_{xy3}=T_{xy1}+T_2&=
\begin{bmatrix}
1 & 0 & 0 & 0
\\
0 & 1 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 1
\end{bmatrix}
\\
\\
T_{xy4} &=\begin{bmatrix}
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 1 & 0
\\
0 & 0 & 0 & 1
\end{bmatrix}
\end{aligned}
$$
最终可以按如下方式得到我们想要的矩阵
(7)
$$
T_{xy}'=T_{xy1}TT_{xy3}+T_{xy4}=\begin{bmatrix}
-0.69492055764131 & 0.71352099052778 & 0 & 4
\\
-0.19200697279199 & -0.30378504433947 & 0
& 5
\\
0 & 0 & 1 & 0
\\
0 & 0 & 0 & 1
\end{bmatrix}
$$
因此,对于一个SE3\(T\),通过公式(7)取出了x-y平面的变换量。
同样,对于一个3D点\(\mathbf{p}\)可以通过如下变换矩阵得到x-y部分分量。
(8)
$$
\mathbf{p}'_{xy} = T_{xy1}\mathbf{p}+\mathbf{c}
$$
上面的向量\(\mathbf{c}\)为如下向量
(9)
$$
\mathbf{c} = \begin{bmatrix}0\\0\\0\\1\end{bmatrix}
$$
1.2 x-z平面
为了取出x-z对应的旋转部分分量,这里定义如下变换矩阵
(10)
$$
T_{xz1} =\begin{bmatrix}
1 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 1 & 0
\\
0 & 0 & 0 & 0
\end{bmatrix}
$$
则经过 \(T_{xz1}TT_{xz1}\)可以取到\(T\)中2*2旋转部分,执行后得到类似如下结果
(11)
$$
T_{xz1}TT_{xz1} =\begin{bmatrix}
-0.69492055764131 & 0 & 0.08929285886191 & 0
\\
0 & 0 & 0 & 0
\\
0.69297816774177 & 0 & 0.34810747783026 & 0
\\
0 & 0 & 0 & 0 &
\end{bmatrix}
$$
则经过 \(T_{xz1}TT_2\)可以取到T中2*1偏移部分,执行后得到类似如下结果
(12)
$$
T_{xz1}TT_2 =\begin{bmatrix}
0 & 0 & 0 & 4
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 8
\\
0 & 0 & 0 & 0
\end{bmatrix}
$$
这里定义矩阵
(13)
$$
\begin{aligned}
T_{xz3}=T_{xz1}+T_2&=
\begin{bmatrix}
1 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 1 & 0
\\
0 & 0 & 0 & 1
\end{bmatrix}
\\
\\
T_{xz4} &=\begin{bmatrix}
0 & 0 & 0 & 0
\\
0 & 1 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 1
\end{bmatrix}
\end{aligned}
$$
最终可以按如下方式得到我们想要的矩阵
(14)
$$
T_{xz}'=T_{xz1}TT_{xz3}+T_{xz4}=\begin{bmatrix}
-0.69492055764131 & 0 & 0.08929285886191 & 4
\\
0 & 1 & 0 & 0
\\
0.69297816774177 & 0 & 0.34810747783026 & 8
\\
0 & 0 & 0 & 1
\end{bmatrix}
$$
因此,对于一个SE3\(T\),通过公式(13)取出了x-z平面的变换量。
同样,对于一个3D点\(\mathbf{p}\)可以通过如下变换矩阵得到x-z部分分量。
(15)
$$
\mathbf{p}'_{xz} = T_{xz1}\mathbf{p}+\mathbf{c}
$$
1.3 y-z平面
为了取出y-z对应的旋转部分分量,这里定义如下变换矩阵
(16)
$$
T_{xz1} =\begin{bmatrix}
0 & 0 & 0 & 0
\\
0 & 1 & 0 & 0
\\
0 & 0 & 1 & 0
\\
0 & 0 & 0 & 0
\end{bmatrix}
$$
则经过 \(T_{yz1}TT_{yz1}\)可以取到\(T\)中2*2旋转部分,执行后得到类似如下结果
(17)
$$
T_{yz1}TT_{yz1} =\begin{bmatrix}
0 & 0 & 0 & 0
\\
0 & -0.30378504433947 & 0.93319235382364 & 0
\\
0 & 0.63134969938371 & 0.34810747783026 & 0
\\
0 & 0 & 0 & 0
\end{bmatrix}
$$
则经过 \(T_{yz1}TT_2\)可以取到\(T\)中2*1偏移部分,执行后得到类似如下结果
(18)
$$
T_{yz1}TT_2 =\begin{bmatrix}
0 & 0 & 0 & 0
\\0 & 0 & 0 & 5
\\0 & 0 & 0 & 8
\\0 & 0 & 0 & 0
\end{bmatrix}
$$
这里定义矩阵
(19)
$$
\begin{aligned}
T_{yz3}=T_{yz1}+T_2&=
\begin{bmatrix}
0 & 0 & 0 & 0
\\
0 & 1 & 0 & 0
\\
0 & 0 & 1 & 0
\\
0 & 0 & 0 & 1
\end{bmatrix}
\\
\\
T_{yz4} &=\begin{bmatrix}
1 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 0
\\
0 & 0 & 0 & 1
\end{bmatrix}
\end{aligned}
$$
最终可以按如下方式得到我们想要的矩阵
(20)
$$
T_{yz}'=T_{yz1}TT_{yz3}+T_{yz4}=\begin{bmatrix}
1 & 0 & 0 & 0
\\ 0 & -0.30378504433947 & 0.93319235382364 & 5
\\ 0 & 0.63134969938371 & 0.34810747783026 & 8
\\ 0 & 0 & 0 & 1 \end{bmatrix}
$$
因此,对于一个SE3\(T\),通过公式(19)取出了y-z平面的变换量。
同样,对于一个3D点\(\mathbf{p}\)可以通过如下变换矩阵得到y-z部分分量。
(21)
$$
\mathbf{p}'_{yz} = T_{yz1}\mathbf{p}+\mathbf{c}
$$
2 结论
1. 对于x-y平面映射,位姿或者变换矩阵使用公式(7)进行转换、空间点使用(8)进行转换。
2. 对于x-z平面映射,位姿或者变换矩阵使用公式(13)进行转换、空间点使用(14)进行转换。
3. 对于y-z平面映射,位姿或者变换矩阵使用公式(19)进行转换、空间点使用(20)进行转换。
3 案例
3.1 问题
空间中有一个射线A(其表示位姿为\(T_A\))、与射线B(其表示位姿为\(T_B\)),将射线A通过某个变换\(T\)可以使得射线A与射线B在x-y平面完全重合。现在想建立\(T_A\)、\(T_B\),以及变换\(T\)的关系式。
3.2 求解
实际可以使用如下公式构建求解目标
(22)
$$
F=T_{xy1}(T_AT^{-1}-T_B)T_{xy3}=\mathbf{0}_{4\times4}
$$
为了方便李代数计算,分别取出第1、2、4列数据作为误差项,使用如下方式等效(21)的方程
(23)
$$
\begin{cases}
\mathbf{e}_1=F\mathbf{a}=T_{xy1}(T_AT^{-1}-T_B)T_{xy3}\mathbf{a}=\mathbf{0}
\\
\mathbf{e}_2=F\mathbf{b}=T_{xy1}(T_AT^{-1}-T_B)T_{xy3}\mathbf{b}=\mathbf{0}
\\
\mathbf{e}_3=F\mathbf{c}=T_{xy1}(T_AT^{-1}-T_B)T_{xy3}\mathbf{c}=\mathbf{0}
\end{cases}
$$
或者取出第1、2行数据作为误差项,使用如下方式等效(22)的方程
(24)
$$
\begin{cases}
\mathbf{e}_1=F^T\mathbf{a}=T_{xy3}^T((T^{-1})^T T_A^T-T_B^T)T_{xy1}^T\mathbf{a}=\mathbf{0}
\\
\mathbf{e}_2=F^T\mathbf{b}=T_{xy3}^T((T^{-1})^TT_A^T-T_B^T)T_{xy1}^T\mathbf{b}=\mathbf{0}
\end{cases}
$$
上面的\(\mathbf{a},\mathbf{b}\)定义如下
(25)
$$
\mathbf{a}=[1,0,0,0]^T
\\
\mathbf{b}=[0,1,0,0]^T
$$
因此最终可以使用(23)或(24)算式来进行求解。