3. 刚体变换与李群李代数：旋转矩阵、平移向量、齐次坐标、SE(3)与so(3)基础
说到外参标定，绕不开的就是刚体变换。说白了，就是描述一个物体在三维空间里怎么转、怎么挪。我刚开始做标定那会儿，总觉得这些数学概念离工程很远，后来被现实狠狠教育了一回——标定结果总是不对，最后发现是旋转矩阵用错了。
嗯，今天咱们就把这块硬骨头啃下来。
3.1 旋转矩阵：别搞错方向
旋转矩阵，一个3x3的方阵。它描述的是两个坐标系之间的旋转关系。你想想看，一个刚体在空间里转，无非就是绕X轴、Y轴、Z轴转。
绕X轴转θ角：
R_x(θ) = [1, 0, 0;
0, cosθ, -sinθ;
0, sinθ, cosθ]绕Y轴转θ角：
R_y(θ) = [cosθ, 0, sinθ;
0, 1, 0;
-sinθ, 0, cosθ]绕Z轴转θ角：
R_z(θ) = [cosθ, -sinθ, 0;
sinθ, cosθ, 0;
0, 0, 1]注意：旋转矩阵是正交矩阵，行列式为+1。我曾经在项目中直接用转置当逆矩阵用，结果标定出来的外参全反了。记住：R的逆等于R的转置，但前提是它确实是旋转矩阵。
3.2 平移向量：别忽略偏移
平移向量就简单多了，就是一个3x1的向量，表示两个坐标系原点之间的偏移。比如从相机坐标系到激光雷达坐标系，平移向量t = [x, y, z]^T。
我个人习惯把平移向量放在旋转矩阵后面，组成一个4x4的齐次变换矩阵。这样处理起来特别方便。
3.3 齐次坐标：为什么要多一个1？
你可能会问，为什么非要用4维向量表示3维点？其实就是为了把旋转和平移统一成一个矩阵乘法。
3维点P = [X, Y, Z]^T，齐次坐标就是P_h = [X, Y, Z, 1]^T。变换后：
P' = [R | t] * P_h
[0 | 1]这样一次矩阵乘法，旋转和平移都搞定了。我在做多传感器融合时，经常要来回切换齐次坐标和普通坐标，写了个小工具函数，省了不少事。
小技巧：齐次坐标的最后一个分量是1，表示是点；如果是0，表示是方向向量。方向向量只旋转不平移，这个细节在标定时容易忽略。
3.4 SE(3)与so(3)：李群李代数的工程意义
SE(3)是特殊欧氏群，就是所有刚体变换的集合。so(3)是它的李代数，说白了就是旋转的“速度”或者“小扰动”。
为什么要引入李代数？因为在优化标定参数时，旋转矩阵有约束（正交、行列式为1），直接优化很麻烦。而李代数是无约束的，可以随便优化，再映射回旋转矩阵。
指数映射：从so(3)到SO(3)
R = exp(φ^) // φ是so(3)中的向量，φ^是它的反对称矩阵对数映射：从SO(3)到so(3)
φ = log(R) // 把旋转矩阵转回李代数我记得第一次用李代数做标定优化时，收敛速度比直接优化旋转矩阵快了好几倍。嗯，这就是数学的力量。
3.5 实战：刚体变换的代码实现
下面是我常用的一个Python片段，用来构建和操作刚体变换：
import numpy as np
def build_transform(R, t):
"""从旋转矩阵和平移向量构建4x4齐次变换矩阵"""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
return T
def transform_point(T, p):
"""用齐次变换矩阵变换一个3D点"""
p_h = np.append(p, 1)
p_transformed = T @ p_h
return p_transformed[:3] / p_transformed[3]
# 示例：绕Z轴旋转90度，平移(1,2,3)
theta = np.pi/2
R = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]])
t = np.array([1, 2, 3])
T = build_transform(R, t)
p = np.array([0, 0, 0])
p_new = transform_point(T, p)
print(f"原点变换后: {p_new}") # 输出应该是 [1, 2, 3]核心要点：刚体变换保持物体的形状和大小不变，只改变位置和朝向。外参标定本质上就是求两个传感器之间的刚体变换。
3.6 知识体系总览
下面这张图把本章的核心逻辑串起来了：
这张图把本章的核心脉络理清了。从SE(3)出发，分成旋转、平移、齐次坐标三条线，最后汇聚到实际应用。你想想看，外参标定不就是求这个T矩阵吗？
我的经验：刚开始学李群李代数时，别被数学公式吓住。先理解它解决了什么问题——让旋转可以像普通向量一样优化。等用熟了，再回头啃理论，会轻松很多。
好了，刚体变换这块就聊到这儿。记住：旋转矩阵别搞错方向，平移向量别漏掉，齐次坐标用起来，李代数帮你优化。这些基础打牢了，后面标定实操才能不翻车。