坐标变化

世界坐标

  • \(\vec{x_w} = [1,0,0]^T\)
  • \(\vec{y_w} = [0,1,0]^T\)
  • \(\vec{z_w} = [0,0,1]^T\)
  • \(o_w = [0,0,0]^T\)

物体（局部）坐标

  • \(\vec{x_l} = [e_{11},e_{12},e_{13}]^T\)
  • \(\vec{y_l} = [e_{21},e_{22},e_{23}]^T\)
  • \(\vec{z_l} = [e_{31},e_{32},e_{33}]^T\)
  • \(o_l = [l_x,l_y,l_z]^T\)

不考虑原点

在世界坐标下的向量（位置）\(\vec{p}=[x_p,y_p,z_p]^T\)，满足： \[ \vec{p} = x_p\vec{x_w} + y_p\vec{y_w} + z_p\vec{z_w} \] 并且由于坐标轴正交，有： \[ \vec{p}\cdot \vec{x_w} = x_p\vec{x_w} \cdot \vec{x_w} + y_p\vec{y_w} \cdot \vec{x_w} + z_p\vec{z_w} \cdot \vec{x_w} = x_p \] 同理： \[ \vec{p}\cdot \vec{y_w} = y_p \]

\[ \vec{p}\cdot \vec{z_w} = z_p \]

向量在某坐标系下的坐标即是在该坐标方向下的投影，令： \[ \vec{p} = x_{pl} \cdot \vec{x_l} + y_{pl} \cdot \vec{y_l} + z_{pl} \cdot \vec{z_l} \] 因此有： \[ x_{pl} = \vec{p} \cdot \vec{x_l} = [x_p,y_p,z_p]^T \cdot [e_{11}, e_{12}, e_{13}]^T \]

\[ y_{pl} = \vec{p} \cdot \vec{y_l} = [x_p,y_p,z_p]^T \cdot [e_{21}, e_{22}, e_{23}]^T \]

\[ z_{pl} = \vec{p} \cdot \vec{z_l} = [x_p,y_p,z_p]^T \cdot [e_{31}, e_{32}, e_{33}]^T \]

以矩阵形式表达： \[ \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} = \begin{bmatrix} e_{11} & e_{12} & e_{13} \\ e_{21} & e_{22} & e_{23}\\ e_{31} & e_{32} & e_{33} \end{bmatrix} \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} \] 记： \[ M = \begin{bmatrix} e_{11} & e_{12} & e_{13} \\ e_{21} & e_{22} & e_{23}\\ e_{31} & e_{32} & e_{33} \end{bmatrix} \] 即： \[ \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} = M \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} \] 因此： \[ M^{-1} M \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} = M^{-1} \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} \] 即： \[ \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} = M^{-1} \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} \] 由于 \(M\) 为正交矩阵： \[ M^{-1} = M^T \] 因此： \[ \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} =\begin{bmatrix} e_{11} & e_{21} & e_{31} \\ e_{12} & e_{22} & e_{32} \\ e_{13} & e_{23} & e_{33} \end{bmatrix} \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} \] 考虑原点

由于同一向量在坐标轴平行且原点不同的坐标系下表示相同，对于任何一点 \(p\)，都可以用该坐标系下的原点指向 \(p\) 的向量来表示。

向量 \(\vec{O_l P}\) 在物体坐标系下为： \[ \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} = M (\begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} - \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix}) = M \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} - M \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} \] 向量 \(\vec{O_w P}\) 在世界坐标系下为： \[ \begin{bmatrix} x_{p}\\ y_{p}\\ z_{p} \end{bmatrix} = M^T (\vec{O_w O_l} + \vec{O_lP}) =M^T\vec{O_lP} + \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} = M^T \begin{bmatrix} x_{pl}\\ y_{pl}\\ z_{pl} \end{bmatrix} + \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} \] 可见有矩阵加法，考虑用齐次坐标将其统一起来，这里记： \[ \begin{bmatrix} t_{x}\\ t_{y}\\ t_{z} \end{bmatrix} = -M \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} =- \begin{bmatrix} e_{11} & e_{12} & e_{13} \\ e_{21} & e_{22} & e_{23}\\ e_{31} & e_{32} & e_{33} \end{bmatrix} \begin{bmatrix} l_{x}\\ l_{y}\\ l_{z} \end{bmatrix} \] 世界坐标转换到物体（局部）坐标： \[ M_{W2L} =\begin{bmatrix} e_{11} & e_{12} & e_{13} & t_x\\ e_{21} & e_{22} & e_{23} & t_y\\ e_{31} & e_{32} & e_{33} & t_z\\ 0 & 0 & 0 & 1 \end{bmatrix} \] 物体（局部）坐标转换到世界坐标： \[ M_{L2W} =\begin{bmatrix} e_{11} & e_{21} & e_{31} & l_x\\ e_{12} & e_{22} & e_{32} & l_y\\ e_{13} & e_{23} & e_{33} & l_z\\ 0 & 0 & 0 & 1 \end{bmatrix} \]