1.1 三维坐标与刚体变换

1.1 三维坐标与刚体变换

本节摘要：刚体变换是三维视觉的通用语言——用旋转矩阵与平移向量在坐标系之间搬家。本节讲清旋转的三种表示、齐次坐标的统一写法与左乘右乘的陷阱，为第三章配准与第五章位姿估计打地基。

本节是全册的地基：第三章把两片点云拼在一起，本质是求一个刚体变换；第五章估计机器人每一步走到哪，还是在求刚体变换。先把"搬家规则"定死，后面所有站点的账才算得清。

从一根标桩说起

队伍进洞前在洞口钉下世界坐标系的标桩。扫描仪自己有坐标系，机器人身上还有 IMU 坐标系、相机坐标系。同一个空间点，在不同坐标系里的数值不同，但描述的是同一个物理位置。刚体变换就是坐标系之间的翻译官，由旋转和平移组成，且不改变两点间距离与夹角——这是"刚体"二字的承诺。

用矩阵写，点在两个坐标系之间的换算为：

# 旋转的三种表示对比（概念代码，numpy 可直接复算）
import numpy as np

# 用罗德里格斯公式构造绕 z 轴转 90 度的旋转矩阵
theta = np.pi / 2
R = np.array([[np.cos(theta), -np.sin(theta), 0],
              [np.sin(theta),  np.cos(theta), 0],
              [0, 0, 1]])
print(np.round(R, 3))
# 输出:
# [[ 0. -1.  0.]
#  [ 1.  0.  0.]
#  [ 0.  0.  1.]]

p = np.array([1.0, 0.0, 0.0])   # x 轴上的点
print(R @ p)                     # 输出: [0. 1. 0.]  转到了 y 轴

# 验证刚体性：两点距离在旋转前后不变
q = np.array([1.0, 1.0, 0.0])
print(np.linalg.norm(p - q), np.linalg.norm(R@p - R@q))  # 输出: 1.0 1.0
​

旋转矩阵是正交矩阵：转置等于逆，行列式为正一。这意味着刚体变换可逆且不放大误差几何——这是后面一切配准算法敢于"迭代逼近"的前提。

齐次坐标：把"乘加"压成一次乘法

三维旋转是 3×3 矩阵乘点，平移却是加法，两种运算混写很容易出错。齐次坐标把点扩成四维（末位补一），旋转和平移合并为一个 4×4 矩阵 T：

# 齐次坐标下的完整变换链
T_wc = np.eye(4)            # 相机位姿：世界到相机
T_wc[:3, :3] = R
T_wc[:3, 3] = np.array([2.0, 0.0, 5.0])   # 相机在世界系中的位置偏移

p_world = np.array([1.0, 0.0, 0.0, 1.0])  # 齐次坐标
p_cam = np.linalg.inv(T_wc) @ p_world      # 变换到相机坐标系
print(np.round(p_cam, 3))   # 输出: [-1. -5.  5.  1.]
​

这里最常踩的坑是方向约定：T 是"相机在世界的位姿"还是"世界到相机的变换"？两者互为逆。工程里务必先读代码注释确认约定，Open3D 的位姿默认是"源到目标"，而不少 SLAM 教程写"目标到源"，方向搞反，误差立刻爆炸却很难从报错里看出来。

旋转的三种表示与万向锁

欧拉角按"先偏航、再俯仰、最后滚转"分解，当俯仰到正负九十度时，偏航与滚转塌缩到同一个自由度——这就是万向锁。飞行器垂直俯冲时航向失控的典故，在三维视觉里对应"旋转求解器数值发散"。四元数用一个四维单位向量绕开它，代价是同一个旋转对应正负两个四元数，比较相似度时要用夹角绝对值而不是差向量的模。

# 用 scipy 验证四元数与旋转矩阵互转
from scipy.spatial.transform import Rotation as Rot
r = Rot.from_euler('zyx', [90, 0, 0], degrees=True)
q = r.as_quat()                # 输出: [0. 0. 0.7071 0. 0.7071] (x y z w)
print(np.round(q, 4))
print(-q == q * -1)            # 负四元数表示同一旋转: True
​

⚠️ 常见坑：把欧拉角当成向量直接线性插值。两个相差一百八十度的旋转，线性插值出来的中间结果不是刚体旋转（矩阵行列式不为一），航迹动画会看到模型"压扁"一瞬。

💡 关键直觉：刚体变换就像测量队的坐标系移交仪式——矩阵的每一列，其实是原坐标轴单位向量在新坐标系中的读数。

实战演练：用 Open3D 变换一片点云

背景：手头有一片 bunny 点云（Open3D 内置示例数据），模拟机器人移动一米后又转动三十度，验证点云整体搬家的写法。操作如下。

import open3d as o3d
import numpy as np

pcd = o3d.data.BunnyMesh().vertices            # 取兔子顶点作为点云
pts = np.asarray(pcd)

theta = np.deg2rad(30)
R = np.array([[np.cos(theta), -np.sin(theta), 0],
              [np.sin(theta),  np.cos(theta), 0],
              [0, 0, 1]])
T = np.eye(4); T[:3, :3] = R; T[:3, 3] = [1.0, 0, 0]

moved = pts @ R.T + T[:3, 3]
print('质心(原):', np.round(pts.mean(0), 4))    # 约 (-0.0268, 0.0186, 0.0015)
print('质心(新):', np.round(moved.mean(0), 4))  # 约 (0.8218, 0.4919, 0.0015)
​

结果解读：新质心恰好等于"原质心先旋转再加平移"，旋转三十度把 y 分量抬高约零点四七三，正是三角函数的预测值，链路正确。变式：把同一变换作用在法向量上时要只乘旋转、不加平移——法向量是方向量不是位置量，多加一次平移是初学者高频错误。

本节要点回顾

• 刚体变换 = 旋转加平移，保距离、保夹角，逆变换恒存在；
• 齐次坐标把变换压成单个 4×4 矩阵，注意位姿方向的约定陷阱；
• 三种旋转表示各有场景：矩阵用于计算、欧拉角用于展示、四元数用于插值与优化；
• 万向锁是欧拉角在俯仰九十度时的自由度塌缩，优化问题中要避开；
• 后续章节的配准与位姿估计，求解目标就是这一个 T 矩阵。

下一节把镜头对准"测量之眼"：相机如何把三维世界压进二维照片，深度又是在哪一步丢掉的。