# 1. SE(3)

$T=\left[\begin{array}{ll} R & p \\ 0 & 1 \end{array}\right]=\left[\begin{array}{llll} r_{11} & r_{12} & r_{13} & p_{1} \\ r_{21} & r_{22} & r_{23} & p_{2} \\ r_{31} & r_{32} & r_{33} & p_{3} \\ 0 & 0 & 0 & 1 \end{array}\right] \tag{1}$

$T^{-1}=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]^{-1}=\left[\begin{array}{cc} R^{\mathrm{T}} & -R^{\mathrm{T}} p \\ 0 & 1 \end{array}\right] \tag{2}$
$(T_1T_2)T_3=T_1(T_2T_3) \tag{3}$

$\|T x-T y\|=\|x-y\| \tag{4}$
$\langle T x-T z, T y-T z\rangle=\langle x-z, y-z\rangle \tag{5}$

# 2. 齐次变换矩阵的用法

### 2.1 描述坐标系

\begin{aligned} T_{sa}&=(R_{sa},p_{sa}) \\ T_{sb}&=(R_{sb},p_{sb}) \\ T_{sc}&=(R_{sc},p_{sc}) \end{aligned} \tag{4}

$R_{s a}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right], \quad R_{s b}=\left[\begin{array}{rrr} 0 & 0 & 1 \\ 0 & -1 & 0 \\ 1 & 0 & 0 \end{array}\right], \quad R_{s c}=\left[\begin{array}{rrr} -1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 1 & 0 \end{array}\right]$

$p_{s a}=\left[\begin{array}{c} 0 \\ 0 \\ 0 \end{array}\right], \quad p_{s b}=\left[\begin{array}{c} 0 \\ -2 \\ 0 \end{array}\right], \quad p_{s c}=\left[\begin{array}{c} -1 \\ 1 \\ 0 \end{array}\right]$

### 2.2 向量(坐标系)在不同坐标系下的描述

\begin{aligned} T_{a b} T_{b c} &=T_{a \not {b}} T_{\not {b} c}=T_{a c} \\ T_{a b} v_{b} &=T_{a b} v_{b}=v_{a} \end{aligned} \tag{5}
$v$$\{a\}$下为$v_a$

### 2.3 对向量(坐标系)进行平移和旋转

\begin{aligned} T&=\operatorname{Trans}(p)\operatorname{Rot}(\hat{\omega}, \theta)\\ &=\left[\begin{array}{llll} 1 & 0 & 0 & p_{x} \\ 0 & 1 & 0 & p_{y} \\ 0 & 0 & 1 & p_{z} \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{ll} R & 0 \\ 0 & 1 \end{array}\right] \end{aligned} \tag{6}

\begin{aligned} &T_{s b^{\prime}}=T T_{s b}=\operatorname{Trans}(p) \operatorname{Rot}(\hat{\omega}, \theta) T_{s b} \quad \text { (fixed frame) }\\ &=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} R_{s b} & p_{s b} \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} R R_{s b} & R p_{s b}+p \\ 0 & 1 \end{array}\right] \end{aligned} \tag{7}
\begin{aligned} &T_{s b^{\prime \prime}}=T_{s b} T=T_{s b} \operatorname{Trans}(p) \operatorname{Rot}(\hat{\omega}, \theta) \quad \text { (body frame) }\\ &=\left[\begin{array}{cc} R_{s b} & p_{s b} \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} R_{s b} R & R_{s b} p+p_{s b} \\ 0 & 1 \end{array}\right] \end{aligned} \tag{8}

©️2019 CSDN 皮肤主题: 创作都市 设计师: CSDN官方博客