Part I · 第 5 章刚体运动与李群 · 5.3Part I · Ch. 5 Rigid-Body Motion & Lie Groups · 5.3

齐次变换与 SE(3)Homogeneous Transforms & SE(3)

旋转加平移不是线性变换，没法直接写成一个矩阵乘法。给坐标多添一个 1，升高一维，"旋转加平移"就变成了一个 4×4 矩阵，位姿的连乘、求逆都变成了普通的矩阵运算。这样的矩阵构成的集合就是 SE(3)，它是李群的第一个主角。 Rotation plus translation is not a linear map, so it cannot be a single matrix product. Append a 1 to the coordinates, go up one dimension, and "rotate then translate" becomes one 4×4 matrix; chaining and inverting poses turn into ordinary matrix algebra. The set of such matrices is SE(3), the first leading actor among Lie groups.

约 25 分钟~25 min 前置：1.3 矩阵即变换 · 1.6 逆 · 5.1 坐标系与位姿 · 5.2 旋转的表示Prereqs: 1.3 Matrices as transformations · 1.6 Inverse · 5.1 Frames & poses · 5.2 Representing rotations 5 个交互5 interactives

1为什么机器人需要它Why robots need this

机械臂有六七个关节，每个关节处的坐标系相对前一个都是"转一下再平移一段"。末端在底座里的位姿，是这一长串"旋转 + 平移"的连乘。用 5.1 节的写法 $(R, \mathbf t)$，每乘一次要算 $R_1R_2$ 和 $R_1\mathbf t_2 + \mathbf t_1$ 两个式子，求逆还要记 $-R\T\mathbf t$。关节多了，推导和代码都很容易写错。

齐次坐标的好处是：把"旋转 + 平移"装进同一个矩阵，连乘就是矩阵乘法，求逆有固定的分块公式。相机投影 $P = K\,[R\,|\,\mathbf t]$、机械臂的正运动学、SLAM 里的位姿图，都靠这一个小技巧。更重要的是，这样得到的矩阵集合 SE(3) 本身是一个群，下一节会看到，它也是一个弯曲的光滑流形，这正是李群理论登场的地方。

A robot arm has six or seven joints, and each joint frame sits relative to the previous one by "turn, then slide". The end effector's pose in the base frame is a long chain of such "rotation + translation" steps. With the $(R, \mathbf t)$ notation of Lesson 5.1, every product needs two formulas, $R_1R_2$ and $R_1\mathbf t_2 + \mathbf t_1$, and inversion adds $-R\T\mathbf t$. With many joints, both derivations and code are easy to get wrong.

The beauty of homogeneous coordinates is that they put "rotation + translation" in one matrix: chaining is matrix multiplication and inversion has a fixed block formula. Camera projection $P = K\,[R\,|\,\mathbf t]$, forward kinematics and SLAM pose graphs all rely on this one trick. More importantly, the set SE(3) of such matrices is itself a group, and, as the next lesson shows, a curved smooth manifold: exactly where Lie-group theory enters.

2直觉：升一维，把平移变成"剪切"Intuition: go up one dimension and translation becomes a shear

线性变换必须把原点留在原点（1.3 节），所以平移不是线性的。但看一个小把戏：把平面上的点 $(x, y)$ 放到三维空间的高度 $w = 1$ 的那张平面上，变成 $(x, y, 1)$。现在对三维空间做一个剪切：每个点沿 $xy$ 方向平移的距离，和它的高度 $w$ 成正比，$(x, y, w) \mapsto (x + t_x w,\ y + t_y w,\ w)$。高度为 1 的那张平面上，每个点恰好平移了 $(t_x, t_y)$；这个剪切是三维里的线性变换，因此可以写成矩阵乘法。

还有一个意外的好处：高度 $w = 0$ 的点，剪切对它们没有任何影响。所以 $w=0$ 的"点"可以用来表示方向向量，它们只会被旋转，不会被平移。下面的实验里，拖动 $t_x, t_y, \theta$：琥珀色的点（$w=1$）会被平移，紫色的箭头（$w=0$）不会。

A linear map must keep the origin fixed (Lesson 1.3), so translation is not linear. Now watch a small trick: put the plane point $(x, y)$ at the plane of height $w = 1$ in 3D, as $(x, y, 1)$. Then shear 3D space: every point slides in the $xy$ direction by an amount proportional to its height $w$, $(x, y, w) \mapsto (x + t_x w,\ y + t_y w,\ w)$. On the plane $w = 1$ every point moves by exactly $(t_x, t_y)$; and this shear is a linear map in 3D, hence a matrix product.

There is a pleasant side effect: points with height $w = 0$ are untouched by the shear. So "points" with $w=0$ can represent direction vectors: they get rotated but never translated. In the lab below, drag $t_x, t_y, \theta$: the amber point ($w=1$) is translated while the purple arrow ($w=0$) is not.

FIG 1升一维：平面平移 = 三维剪切Lifting: planar translation = a 3D shear

拖动画面旋转视角drag to orbit

灰色是 $w=0$ 的地面和 $w=1$ 的"平面世界"。灰色的 F 是原来的图形，琥珀色的 F 是变换后的图形，它仍然停在 $w=1$ 的平面上，只是平移（并旋转）了。三色箭头是矩阵的三列，也就是 $\hat x$、$\hat y$、$\hat w$ 的像：蓝色的 $\hat w$ 会歪向 $(t_x, t_y, 1)$，这正是平移信息所在的那一列。 Grey shows the ground $w=0$ and the "flatland" plane $w=1$. The grey F is the original shape and the amber F its image: it stays on the plane $w=1$, merely translated (and rotated). The three coloured arrows are the columns of the matrix, i.e. the images of $\hat x$, $\hat y$, $\hat w$: the blue $\hat w$ leans toward $(t_x, t_y, 1)$, which is the column that carries the translation.

试一试Try this

令 $\theta = 0$，把 $(t_x, t_y)$ 拉到长度超过 2。琥珀色的 F 平移了，蓝色箭头歪向哪里？紫色箭头动了吗？With $\theta = 0$, pull $(t_x, t_y)$ to a length above 2. The amber F slid; where does the blue arrow lean? Did the purple arrow move?
令 $t = 0$，把 $\theta$ 转到 90° 左右。整个"平面世界"绕蓝色的 $w$ 轴转动，紫色箭头也跟着转。With $t = 0$, turn $\theta$ to about 90°. The whole flatland rotates about the blue $w$ axis, and the purple arrow turns with it.

核心洞见Key insight

矩阵的最后一列是平移，因为 $\hat w$（即"原点"）被送到了 $(\mathbf t, 1)$。矩阵的左上角是旋转，右下角的 1 保证平面 $w=1$ 被送回 $w=1$。对点 $(\mathbf p, 1)$ 做乘法，得到 $(R\mathbf p + \mathbf t,\ 1)$，恰好就是"先转后移"。

The last column of the matrix is the translation, because $\hat w$ (the "origin") is sent to $(\mathbf t, 1)$. The upper-left block is the rotation, and the 1 in the lower-right corner keeps the plane $w=1$ mapped onto $w=1$. Multiplying the point $(\mathbf p, 1)$ yields $(R\mathbf p + \mathbf t,\ 1)$: exactly "rotate then translate".

3动手玩：SE(2) 与求逆Play: SE(2) and the inverse

平面里的刚体运动构成 SE(2)，位姿是 $(x, y, \theta)$，3×3 齐次矩阵是

Rigid motions of the plane form SE(2); a pose is $(x, y, \theta)$ and its 3×3 homogeneous matrix is

$$ T = \begin{bmatrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix}, \qquad T^{-1} = \begin{bmatrix} R\T & -R\T\mathbf t \\ \mathbf 0\T & 1 \end{bmatrix}. $$

$T$ 把机器人系的读数变成世界系的读数，$T^{-1}$ 反过来。直观上，$T^{-1}$ 就是"站在机器人身上看世界"：机器人变成原点，朝向变成 $+x$，世界系被放到了 $T^{-1}$ 所描述的位置。左边是世界视角，右边是机器人视角。拖动任何一边的点，另一边同步更新。

$T$ turns robot-frame readings into world-frame readings, and $T^{-1}$ does the opposite. Intuitively, $T^{-1}$ is "seeing the world from the robot's seat": the robot becomes the origin, its heading becomes $+x$, and the world frame is placed where $T^{-1}$ says. The left panel is the world view and the right one the robot view. Drag the point in either panel and the other follows.

FIG 2SE(2)：T 与 T⁻¹，世界视角与机器人视角SE(2): T and T⁻¹, world view vs. robot view

世界视角：拖动机器人和点world view: drag the robot and the point

机器人视角（用 T⁻¹）robot view (using T⁻¹)

左：世界系 W（灰）和带着机器人系 B 的三角形。右：同一个场景，但以机器人为中心；灰色的 W 系画在 $T^{-1}$ 的位置。琥珀点在两边是同一个点，读数分别是 $\mathbf p^W = T\mathbf p^B$ 和 $\mathbf p^B = T^{-1}\mathbf p^W$。下面的矩阵里，琥珀色是平移那一列。 Left: the world frame W (grey) and the triangle carrying the robot frame B. Right: the same scene centred on the robot; the grey W frame is drawn at the pose $T^{-1}$. The amber point is the same point on both sides, read as $\mathbf p^W = T\mathbf p^B$ and $\mathbf p^B = T^{-1}\mathbf p^W$. In the matrices below, amber marks the translation column.

试一试Try this

把机器人放到 $(3, 1)$、朝向 $90°$。预测 $T^{-1}$ 的平移列，再核对读数。（答案：$(-1, 3)$，而不是 $(-3,-1)$。）Put the robot at $(3, 1)$ facing $90°$. Predict $T^{-1}$'s translation column, then check. (Answer: $(-1, 3)$, not $(-3,-1)$.)
在右图里把点拖到机器人正前方 2 米，也就是 $\mathbf p^B = (2, 0)$。再看左图：它在世界里的哪里？In the right panel drag the point to 2 m straight ahead of the robot, i.e. $\mathbf p^B = (2, 0)$. Then look at the left panel: where is it in the world?

三维：SE(3) 编辑器In 3D: the SE(3) editor

三维位姿是 $T = \begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1\end{bmatrix} \in \R^{4\times4}$，其中 $R$ 的三列依次是机体 $x, y, z$ 轴在世界里的方向（红、绿、蓝），$\mathbf t$ 是机体原点的位置（琥珀）。用六个滑块摆一个位姿（$R = R_z(\text{yaw})R_y(\text{pitch})R_x(\text{roll})$，5.2 节），观察 4×4 矩阵每一块怎样对应到图上。

A 3D pose is $T = \begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1\end{bmatrix} \in \R^{4\times4}$, where the three columns of $R$ are the body's $x, y, z$ axes in the world (red, green, blue) and $\mathbf t$ is the position of the body origin (amber). Set a pose with the six sliders ($R = R_z(\text{yaw})R_y(\text{pitch})R_x(\text{roll})$, Lesson 5.2) and see how each block of the 4×4 matrix lands in the picture.

FIG 3SE(3)：4×4 矩阵的每一块SE(3): every block of the 4×4 matrix

拖动画面旋转视角drag to orbit

淡色是世界系，亮色是机体系 $\{B\}$（带箱体）。琥珀小点 $\mathbf p$ 固定在机体上，坐标是 $\mathbf p^B = (0.8, 0, 0.5)$；紫色小点 $\mathbf q$ 固定在世界里，$\mathbf q^W = (1.6, 1.0, 0.6)$。前者用 $T$ 变到世界系，后者用 $T^{-1}$ 变到机体系。把鼠标放到下方"再看数学"一节公式里带底色的 $R$、$\mathbf t$ 上，这里的箭头会加粗。 The faint frame is the world; the bright one is the body frame $\{B\}$ (with a box). The amber dot $\mathbf p$ is fixed on the body with $\mathbf p^B = (0.8, 0, 0.5)$; the purple dot $\mathbf q$ is fixed in the world with $\mathbf q^W = (1.6, 1.0, 0.6)$. The first is mapped to the world by $T$, the second to the body frame by $T^{-1}$. Hover the highlighted $R$ and $\mathbf t$ in the formulas of "The math" below and the arrows here get thicker.

试一试Try this

让机体的 $x$ 轴（红）指向正上方，即 $R$ 的第一列变成 $(0,0,1)$。需要什么 pitch？（顺便想想：这是哪一种欧拉角奇异？）Make the body's $x$ axis (red) point straight up, i.e. the first column of $R$ becomes $(0,0,1)$. Which pitch does it take? (And which Euler singularity is this?)
用平移滑块把机体原点放到紫色点 $\mathbf q$ 上（距离小于 0.12）。此时 $T^{-1}\mathbf q$ 是什么？为什么？Use the translation sliders to put the body origin on the purple point $\mathbf q$ (within 0.12). What is $T^{-1}\mathbf q$ now, and why?

连乘：平面三连杆Chaining: a planar 3-link arm

每个关节处的坐标系相对前一个是"先沿前一段连杆走 $L$，再转 $q$"：$T_{i-1,i} = \mathrm{Trans}(L_{i-1})\,\mathrm{Rot}(q_i)$。末端相对底座的位姿就是把它们依次乘起来：$T_{0E} = T_{01}\,T_{12}\,T_{23}\,T_{3E}$。拖动三个关节（圆圈）。

Each joint frame sits relative to the previous one by "walk $L$ along the previous link, then turn by $q$": $T_{i-1,i} = \mathrm{Trans}(L_{i-1})\,\mathrm{Rot}(q_i)$. The end effector's pose in the base frame is the product of all of them, $T_{0E} = T_{01}\,T_{12}\,T_{23}\,T_{3E}$. Drag the three joints (the circles).

FIG 4连乘 T₀ₑ = T₀₁T₁₂T₂₃T₃ₑChaining T₀ₑ = T₀₁T₁₂T₂₃T₃ₑ

拖动三个圆圈（关节 / 末端）drag the three circles (joint / tip)

连杆长度 $L_1, L_2, L_3 = 1.0, 0.8, 0.5$。每个关节画出它的坐标系（红 $x$、绿 $y$）。把鼠标放到公式里的 $T_{01}$、$T_{12}$、$T_{23}$ 上，会加粗对应关节的坐标系。绿色圆圈是目标点。 Link lengths $L_1, L_2, L_3 = 1.0, 0.8, 0.5$. Each joint draws its frame (red $x$, green $y$). Hover $T_{01}$, $T_{12}$, $T_{23}$ in the formula to thicken that joint's frame. The green circle is the target.

试一试Try this

让末端落到绿色目标点上（距离小于 0.08）。读一读 $T_{0E}$ 的最后一列。Bring the tip onto the green target (within 0.08). Read the last column of $T_{0E}$.
把三个关节全拉直（$q_1=q_2=q_3=0$）。这时每个 $T_{i-1,i}$ 是什么样子？Straighten all three joints ($q_1=q_2=q_3=0$). What does each $T_{i-1,i}$ look like?

4再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，FIG 3、FIG 4 或 FIG 5 中对应的元素会被高亮。Tip: hover the highlighted symbols in the formulas; the matching element in FIG 3, FIG 4 or FIG 5 lights up.

齐次坐标：点和方向Homogeneous coordinates: points and directions

给 $\mathbf p \in \R^3$ 添一个分量 1，得到齐次坐标 $\tilde{\mathbf p} = (\mathbf p, 1) \in \R^4$。方向（自由向量）则用 $(\mathbf d, 0)$。位姿 $T$ 作用在它们上：

Append a 1 to $\mathbf p \in \R^3$ to get the homogeneous coordinates $\tilde{\mathbf p} = (\mathbf p, 1) \in \R^4$. Directions (free vectors) use $(\mathbf d, 0)$. A pose $T$ acts on them by

$$ T\tilde{\mathbf p} = \begin{bmatrix} \hl{R}{R} & \hl{t}{\mathbf t} \\ \mathbf 0\T & 1 \end{bmatrix}\begin{bmatrix} \mathbf p \\ 1 \end{bmatrix} = \begin{bmatrix} R\mathbf p + \mathbf t \\ 1 \end{bmatrix}, \qquad T\begin{bmatrix} \mathbf d \\ 0 \end{bmatrix} = \begin{bmatrix} R\mathbf d \\ 0 \end{bmatrix}. $$

点 − 点 = 方向（$w$ 分量 $1 - 1 = 0$），点 + 方向 = 点（$1 + 0 = 1$），点 + 点没有意义（$w = 2$）。这和直觉一致：两个位置相减得到位移，两个位置相加没有物理意义。

Point − point = direction (the $w$ component is $1 - 1 = 0$), point + direction = point ($1 + 0 = 1$), and point + point makes no sense ($w = 2$). That matches intuition: subtracting positions gives a displacement, adding positions has no physical meaning.

SE(3) 的定义The definition of SE(3)

$$ \SE = \left\{ T = \begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix} \;\middle|\; R \in \SO,\ \mathbf t \in \R^3 \right\}, \qquad \mathrm{SE}(2) \text{ is the same with } R \in \mathrm{SO}(2),\ \mathbf t\in\R^2. $$

"S"是 special（$\det R = +1$，没有镜像），"E"是 Euclidean（保持距离）。它有 $3 + 3 = 6$ 个自由度（SE(2) 有 $1 + 2 = 3$ 个）。注意 $T$ 本身不是正交矩阵：$T\T T \ne I$，所以 $T^{-1} \ne T\T$。

"S" is special ($\det R = +1$, no reflections) and "E" is Euclidean (distances are preserved). It has $3 + 3 = 6$ degrees of freedom (SE(2) has $1 + 2 = 3$). Note that $T$ itself is not orthogonal: $T\T T \ne I$, so $T^{-1} \ne T\T$.

连乘与求逆Composition and inverse

分块矩阵相乘，正好重现了 5.1 节的两个公式：

Block multiplication reproduces the two formulas of Lesson 5.1:

$$ \begin{bmatrix} R_1 & \mathbf t_1 \\ \mathbf 0\T & 1 \end{bmatrix}\begin{bmatrix} R_2 & \mathbf t_2 \\ \mathbf 0\T & 1 \end{bmatrix} = \begin{bmatrix} R_1R_2 & R_1\mathbf t_2 + \mathbf t_1 \\ \mathbf 0\T & 1 \end{bmatrix}, \qquad T^{-1} = \begin{bmatrix} R\T & -R\T\mathbf t \\ \mathbf 0\T & 1 \end{bmatrix}. $$

验证逆：$\begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix}\begin{bmatrix} R\T & -R\T\mathbf t \\ \mathbf 0\T & 1 \end{bmatrix} = \begin{bmatrix} RR\T & -RR\T\mathbf t + \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix} = I$。求逆不需要解线性方程组，只要一次转置和一次矩阵乘向量。而且 $\det T = \det R = 1$，$T^{-1}$ 一定存在。

Check the inverse: $\begin{bmatrix} R & \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix}\begin{bmatrix} R\T & -R\T\mathbf t \\ \mathbf 0\T & 1 \end{bmatrix} = \begin{bmatrix} RR\T & -RR\T\mathbf t + \mathbf t \\ \mathbf 0\T & 1 \end{bmatrix} = I$. Inversion needs no linear solve, only a transpose and one matrix-vector product, and since $\det T = \det R = 1$ the inverse always exists.

它是一个群It is a group

上面的公式说明 SE(3) 满足群的四条要求：封闭（两个 $T$ 相乘还是 $T$）、结合律（矩阵乘法天然满足）、有单位元 $I$、每个元素有逆。但它不可交换：$T_1T_2 \ne T_2T_1$（5.1 节的左乘右乘之别，以及 FIG 4 里"先转后移"和"先移后转"的区别）。群的定义是下一节（5.4）的主题。

The formulas show that SE(3) satisfies the four requirements of a group: closure (the product of two $T$'s is a $T$), associativity (free from matrix multiplication), an identity $I$, and an inverse for every element. It is not commutative: $T_1T_2 \ne T_2T_1$ (the left/right distinction of Lesson 5.1, and the difference between "turn then move" and "move then turn" in FIG 4). Lesson 5.4 takes up the definition of a group.

平面三连杆的正运动学Forward kinematics of the planar arm

$$ \hl{k1}{T_{01}} = \mathrm{Rot}(q_1),\quad \hl{k2}{T_{12}} = \mathrm{Trans}(L_1)\,\mathrm{Rot}(q_2),\quad \hl{k3}{T_{23}} = \mathrm{Trans}(L_2)\,\mathrm{Rot}(q_3),\quad T_{3E} = \mathrm{Trans}(L_3), $$ $$ T_{0E} = T_{01}\,T_{12}\,T_{23}\,T_{3E}, \qquad \mathrm{Trans}(L) = \begin{bmatrix} 1 & 0 & L \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}. $$

读最后一列就得到末端位置，左上角的 $2\times2$ 块给出末端朝向 $\theta = q_1 + q_2 + q_3$（平面里的角度直接相加）。三维关节用的是同样的思路，5.10 节会把它改写成指数积公式。

The last column gives the tip position and the upper-left $2\times2$ block gives the tip orientation $\theta = q_1 + q_2 + q_3$ (planar angles simply add). The same idea carries to 3D joints, and Lesson 5.10 rewrites it as the product of exponentials.

5机器人中的它：手眼标定误差In robotics: hand–eye calibration error

相机装在机械臂的手上（eye-in-hand）。相机看到杯子，给出 $\mathbf p^C$；机器人要把杯子变到底座系才能规划抓取：

A camera is mounted on the robot's hand (eye-in-hand). It sees the cup and reports $\mathbf p^C$; the robot must express the cup in the base frame to plan the grasp:

$$ \tilde{\mathbf p}^{B} = T_{BE}\,\hl{cal}{T_{EC}}\,\tilde{\mathbf p}^{C}. $$

$T_{BE}$ 来自关节角和正运动学（FIG 4），非常准；$T_{EC}$ 是相机相对手的安装位姿，由手眼标定得到，一定有误差。下面让 $T_{EC}$ 的真值比标定值多转了 $\varepsilon$。注意这个小角度误差会被距离放大：相机离杯子越远，估计出来的杯子位置偏得越多。

$T_{BE}$ comes from the joint angles and forward kinematics (FIG 4) and is very accurate; $T_{EC}$ is the camera's mounting pose relative to the hand, obtained by hand–eye calibration, and is never perfect. Below, the true $T_{EC}$ is rotated by $\varepsilon$ more than the calibrated one. A small angle error is amplified by distance: the farther the camera is from the cup, the more the estimated cup position is off.

FIG 5eye-in-hand：标定误差被距离放大Eye-in-hand: calibration error amplified by distance

拖动杯子drag the cup

两连杆机械臂（底座在原点），手上装着相机。实线相机是真值（物理上相机的真实安装），虚线相机是机器人以为的安装。琥珀点是杯子的真值，红点是用标定值算出来的估计。红线的长度就是抓取偏差。把鼠标放到上面公式里的 $T_{EC}$ 上，标定的安装（手到虚线相机）会以琥珀色加粗显示。 A two-link arm (base at the origin) with a camera on the hand. The solid camera is the truth (how the camera is really mounted); the dashed one is how the robot believes it is mounted. The amber dot is the true cup, the red dot the estimate computed with the calibrated pose; the length of the red line is the grasp error. Hover $T_{EC}$ in the formula above and the calibrated mounting (hand to dashed camera) is drawn thick in amber.

试一试Try this

把标定误差 $\varepsilon$ 调到 0：估计和真值重合，红线消失。Set the calibration error $\varepsilon$ to 0: the estimate coincides with the truth and the red line vanishes.
设 $\varepsilon = 5°$，把杯子拖到离相机 1.2 m 以外，让偏差超过 10 cm。偏差 ≈ 距离 × $\varepsilon$（弧度）。Set $\varepsilon = 5°$ and drag the cup more than 1.2 m from the camera so the error exceeds 10 cm. Error ≈ range × $\varepsilon$ (in radians).

真实系统里In real systems

机械臂控制器内部把每个连杆的位姿存成 4×4 矩阵（或旋转 + 平移的对），正运动学就是一连串乘法。相机的投影矩阵 $P = K[R\,|\,\mathbf t]$ 里的 $[R\,|\,\mathbf t]$ 就是 $T_{CW}$ 去掉最后一行。点云配准（ICP）输出的是一个 4×4 的 $T$。SLAM 的位姿图每条边都是一个相对位姿 $T_{ij}$。数值上，连乘几千次后 $R$ 会慢慢漂离旋转矩阵，需要定期重新正交化（1.10 节）。

Robot arm controllers store each link's pose as a 4×4 matrix (or a rotation + translation pair), and forward kinematics is a string of multiplications. In a camera projection matrix $P = K[R\,|\,\mathbf t]$, the $[R\,|\,\mathbf t]$ is $T_{CW}$ without its last row. Point-cloud registration (ICP) returns a 4×4 $T$. In SLAM pose graphs every edge is a relative pose $T_{ij}$. Numerically, after thousands of products $R$ slowly drifts off the rotation matrices and must be re-orthonormalised periodically (Lesson 1.10).

6常见误区Common pitfalls

误区 1：用转置求 4×4 位姿的逆Pitfall 1: inverting a 4×4 pose by transposing it

只有左上角的 $R$ 满足 $R^{-1} = R\T$。整个 $T$ 的逆是 $\begin{bmatrix}R\T & -R\T\mathbf t\\ \mathbf 0\T & 1\end{bmatrix}$，而 $T\T = \begin{bmatrix}R\T & \mathbf 0\\ \mathbf t\T & 1\end{bmatrix}$ 完全是另一个东西。如果平移为零，两者碰巧一样，所以这个 bug 在原点附近测试时不会暴露。

Only the upper-left $R$ satisfies $R^{-1} = R\T$. The inverse of the whole $T$ is $\begin{bmatrix}R\T & -R\T\mathbf t\\ \mathbf 0\T & 1\end{bmatrix}$, while $T\T = \begin{bmatrix}R\T & \mathbf 0\\ \mathbf t\T & 1\end{bmatrix}$ is something else entirely. With zero translation the two happen to agree, so the bug stays hidden when you test near the origin.

误区 2：把方向向量当成点来变换Pitfall 2: transforming a direction as if it were a point

法向量、速度、力的方向、坐标轴方向都是方向，只应被旋转，不应被平移：用 $R\mathbf d$，或者用 $w = 0$ 的齐次坐标。把它们当点 $(\mathbf d, 1)$ 变换，就会凭空加上 $\mathbf t$。反过来，把点当方向变换，位置就不会被平移到正确的地方。（注意：速度作为 twist 的一部分时，变换规则更复杂，见 5.8 节的伴随。）

Normals, velocities, force directions and axis directions are directions: they should be rotated, not translated. Use $R\mathbf d$, or homogeneous coordinates with $w = 0$. Transforming them as points $(\mathbf d, 1)$ adds a spurious $\mathbf t$; transforming points as directions leaves the position untranslated. (Caveat: when a velocity is part of a twist the rule is more involved, see the adjoint in Lesson 5.8.)

误区 3：把 SE(3) 里的位姿当向量相加、平均Pitfall 3: adding or averaging poses in SE(3) as vectors

两个 $T$ 相加得到的 4×4 矩阵不是位姿，$(T_1 + T_2)/2$ 的左上角不是旋转矩阵（5.4 节会看到它怎样"缩水"）。求平均、插值、做优化都不能直接对矩阵元素做加法，需要李群的工具（5.6–5.9 节）。

The sum of two $T$'s is not a pose, and the upper-left block of $(T_1 + T_2)/2$ is not a rotation matrix (Lesson 5.4 shows how it "shrinks"). Averaging, interpolating and optimising cannot add matrix entries directly; they need Lie-group tools (Lessons 5.6–5.9).

误区 4：忘了左上角的 R 要保持正交Pitfall 4: letting the rotation block R drift out of orthogonality

矩阵乘法在浮点数里会累积误差。把一个位姿连乘上万次（比如积分里程计）之后，左上角不再是严格的旋转矩阵，长度开始缩放、直角开始倾斜。对策是定期把 $R$ 投影回 $\SO$（SVD 或 Gram–Schmidt），或者把旋转存成单位四元数，随时归一化。

Floating-point matrix products accumulate error. After a pose has been multiplied tens of thousands of times (say integrating odometry), the upper-left block is no longer a strict rotation: lengths start to scale and right angles to tilt. The remedy is to project $R$ back onto $\SO$ periodically (SVD or Gram–Schmidt), or to store the rotation as a unit quaternion and normalise it as you go.

7小练习Check yourself

1. 平面里把点平移 $(2, 3)$ 的 3×3 齐次矩阵是？1. Which 3×3 homogeneous matrix translates planar points by $(2, 3)$?

平移放在最后一列（列向量约定）：$(x, y, 1)\mapsto(x + 2, y + 3, 1)$。选项 A 是缩放，选项 C 是行向量约定下的写法（与本课程的列向量约定不同），选项 D 是剪切。The translation goes in the last column (column-vector convention): $(x, y, 1)\mapsto(x + 2, y + 3, 1)$. Option A is a scaling, option C is the row-vector convention (not this course's), and option D is a shear.

2. 机器人位姿 $T$ 的平移是 $\mathbf t = (4, 0, 0)$。机器人的前进方向 $\mathbf d = (1, 0, 0)$（$w = 0$）在世界里经 $T$ 变换后，当 $R = I$ 时是？2. A robot pose $T$ has translation $\mathbf t = (4, 0, 0)$. With $R = I$, what is the heading direction $\mathbf d = (1, 0, 0)$ (with $w = 0$) after the transform $T$?

$w = 0$ 的方向不受平移影响：$R\mathbf d + \mathbf t\cdot 0 = (1,0,0)$。选项 B 是把它当成点 $(\mathbf d, 1)$ 来变换的结果；选项 A 加上了平移却保留 $w = 0$，自相矛盾。A direction with $w = 0$ is unaffected by translation: $R\mathbf d + \mathbf t\cdot 0 = (1,0,0)$. Option B is what you get by transforming it as a point $(\mathbf d, 1)$; option A adds the translation yet keeps $w = 0$, which is inconsistent.

3. 平面位姿 $T$：$R = R(90°)$，$\mathbf t = (3, 1)$。$T^{-1}$ 的平移是？3. A planar pose $T$ has $R = R(90°)$ and $\mathbf t = (3, 1)$. What is the translation of $T^{-1}$?

$-R\T\mathbf t$，$R\T = \begin{bmatrix}0&1\\-1&0\end{bmatrix}$，$R\T(3,1)^\top = (1,-3)$，取负得 $(-1, 3)$。$(1,-3)$ 忘了负号，$(-3,-1)$ 忘了旋转。$-R\T\mathbf t$ with $R\T = \begin{bmatrix}0&1\\-1&0\end{bmatrix}$: $R\T(3,1)^\top = (1,-3)$, negated $(-1, 3)$. $(1,-3)$ forgets the minus sign and $(-3,-1)$ forgets the rotation.

4. 平面两连杆：$L_1 = 1$，$q_1 = 90°$，$q_2 = 0$，末端在第二根杆的末端，$L_2 = 1$。$T_{0E} = \mathrm{Rot}(q_1)\mathrm{Trans}(L_1)\mathrm{Rot}(q_2)\mathrm{Trans}(L_2)$ 的末端位置是？4. A planar two-link arm has $L_1 = 1$, $q_1 = 90°$, $q_2 = 0$ and the end point at the end of the second link, $L_2 = 1$. Where does $T_{0E} = \mathrm{Rot}(q_1)\mathrm{Trans}(L_1)\mathrm{Rot}(q_2)\mathrm{Trans}(L_2)$ put the end point?

$\mathrm{Trans}(1)\mathrm{Rot}(0)\mathrm{Trans}(1)$ 把原点送到 $(2, 0)$，再经 $\mathrm{Rot}(90°)$ 变成 $(0, 2)$。两根杆都沿着第一个关节转后的 $+y$ 方向伸出去。$\mathrm{Trans}(1)\mathrm{Rot}(0)\mathrm{Trans}(1)$ sends the origin to $(2, 0)$, and $\mathrm{Rot}(90°)$ turns it into $(0, 2)$. Both links point along $+y$ after the first joint turns.

5. 下列关于 SE(3) 的说法，哪一个是对的？5. Which statement about SE(3) is true?

封闭、结合、有单位元、有逆，加起来就是群；位姿的乘法不可交换。$T$ 只有 $R$ 块是正交的，$T\T T \ne I$。Closed, associative, with an identity and inverses: that is a group, and pose multiplication is not commutative. Only the $R$ block of $T$ is orthogonal, and $T\T T \ne I$.

8延伸阅读Further reading

K. Lynch & F. Park, Modern Robotics, Ch. 3.3 — 齐次变换矩阵与 SE(3) 的性质homogeneous transformation matrices and the properties of SE(3)
R. Hartley & A. Zisserman, Multiple View Geometry in Computer Vision, Ch. 2 — 齐次坐标与射影几何homogeneous coordinates and projective geometry
R. Murray, Z. Li, S. Sastry, A Mathematical Introduction to Robotic Manipulation, Ch. 2 — 刚体运动的齐次表示the homogeneous representation of rigid motion
B. Siciliano et al., Robotics: Modelling, Planning and Control, Ch. 2.6 — 齐次变换homogeneous transformations