位姿变换

假设在$T_1$的原点在$T_0$坐标系下为$(1,2,-\pi/2)$, 有点P在$T_1$坐标系下的坐标为$(P(-0.5, -0.5, -\pi/2)$, 求点P在$T_0$坐标系下的坐标

$$T_p^{T_0}=T_{T_1}^{T_0} \cdot T_P^{T_1}$$

其中

$$T_{T_1}^{T_0} =\begin{bmatrix} \cos\theta & -\sin\theta &x\\ \sin\theta & \cos\theta &y \\ 0&0&1 \end{bmatrix}= \begin{bmatrix} \cos(-\pi/2) & -\sin(-\pi/2) &1\\ \sin(-\pi/2) & \cos(-\pi/2) &2 \\ 0&0&1 \end{bmatrix}\\ T_{T_1}^{p} =\begin{bmatrix} \cos0 & -\sin0 &-0.5\\ \sin0 & \cos0 &-0.5 \\ 0&0&1 \end{bmatrix}$$