3、点云坐标系与变换：世界坐标系与传感器坐标系、欧拉角与旋转矩阵、使用Open3D进行点云旋转与平移
做激光SLAM这几年，我踩过最大的坑，就是坐标系搞混了。
你想想看，雷达扫出来一堆点，明明感觉位置是对的，但一拼接就错位。查了半天，最后发现是旋转方向搞反了。嗯，这种经历，我相信做点云的朋友多少都遇到过。
所以这一章，咱们就把坐标系和变换这件事彻底聊透。说白了，就是搞清楚「点从哪里来，要到哪里去」。
3.1 世界坐标系 vs 传感器坐标系
先问个问题：雷达扫到的点，是以谁为原点？
答案是雷达自己。雷达每次发射激光，测出距离和角度，算出来的坐标(x, y, z)，都是以雷达的物理中心为原点的。这就是传感器坐标系，也叫局部坐标系。
但问题来了——雷达装在车上，车在动。雷达每一帧扫到的点，都是相对于当时那个位置的。如果你直接把两帧的点云叠在一起，肯定对不上。
所以我们需要一个固定的参考系，叫世界坐标系。所有传感器数据，最终都要变换到世界坐标系下，才能做拼接、建图。
核心概念：
- 传感器坐标系：以传感器自身为原点，随传感器移动而移动
- 世界坐标系：固定不变，通常以机器人启动位置或地图原点为基准
- 变换：把传感器坐标系下的点，通过旋转和平移，映射到世界坐标系
我在项目里见过有人直接把雷达坐标当成世界坐标用，结果建出来的地图全是歪的。后来我养成了一个习惯：拿到点云数据，第一件事就是确认坐标系定义。
3.2 欧拉角与旋转矩阵
坐标系变换，核心就两件事：旋转和平移。
平移好理解，就是加个偏移量。旋转呢？我们得用数学来描述「一个坐标系怎么转才能和另一个坐标系对齐」。
3.2.1 欧拉角
欧拉角是最直观的旋转表示方式。说白了，就是绕三个轴分别转多少度。
- 绕X轴旋转：称为翻滚角（Roll）
- 绕Y轴旋转：称为俯仰角（Pitch）
- 绕Z轴旋转：称为偏航角（Yaw）
但欧拉角有个大坑——万向锁。当俯仰角接近±90°时，翻滚和偏航会变得无法区分。我曾经在调试一个无人机项目时，就因为这个原因，姿态解算直接炸了。从那以后，我内部计算一律用旋转矩阵或四元数，欧拉角只用来给人看。
避坑指南：欧拉角适合做可视化展示，不适合做插值和迭代计算。如果你要做连续旋转，建议用旋转矩阵或四元数。
3.2.2 旋转矩阵
旋转矩阵是一个3x3的正交矩阵，行列式为+1。它把一个三维向量旋转到另一个方向。
绕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]]多个旋转组合时，只需要把矩阵乘起来就行。注意顺序：先绕Z转，再绕Y转，最后绕X转，对应的矩阵乘法是 R = R_x * R_y * R_z。
我的习惯：写代码时，我会把旋转矩阵和位移向量拼成一个4x4的齐次变换矩阵。这样一次矩阵乘法就能完成旋转+平移，代码更干净。
3.3 使用Open3D进行点云旋转与平移
理论讲完了，咱们直接上代码。Open3D处理点云变换非常方便。
3.3.1 加载点云并可视化
import open3d as o3d
import numpy as np
# 读取点云
pcd = o3d.io.read_point_cloud("example.pcd")
print(f"点云点数: {len(pcd.points)}")
# 可视化原始点云
o3d.visualization.draw_geometries([pcd], window_name="原始点云")3.3.2 点云平移
# 定义平移向量 (沿X轴移动2米，沿Y轴移动1米，沿Z轴移动0.5米)
translation = np.array([2.0, 1.0, 0.5])
# 平移点云
pcd_translated = pcd.translate(translation)
# 可视化平移后的点云
o3d.visualization.draw_geometries([pcd_translated], window_name="平移后点云")3.3.3 点云旋转
# 定义旋转角度 (弧度)
theta = np.pi / 4 # 45度
# 构建绕Z轴的旋转矩阵
R = pcd.get_rotation_matrix_from_xyz((0, 0, theta))
# 旋转点云
pcd_rotated = pcd.rotate(R, center=(0, 0, 0))
# 可视化旋转后的点云
o3d.visualization.draw_geometries([pcd_rotated], window_name="旋转后点云")注意：rotate方法的center参数指定旋转中心。如果不指定，默认绕原点旋转。如果你想让点云绕自身中心旋转，可以先计算点云质心，再以质心为旋转中心。
3.3.4 使用齐次变换矩阵
# 构建4x4齐次变换矩阵
T = np.eye(4)
T[:3, :3] = R # 旋转部分
T[:3, 3] = [2.0, 1.0, 0.5] # 平移部分
# 应用变换
pcd_transformed = pcd.transform(T)
# 可视化
o3d.visualization.draw_geometries([pcd_transformed], window_name="变换后点云")我个人更推荐用齐次变换矩阵的方式。为什么？因为一次变换就能搞定旋转+平移，而且多个变换可以连乘，代码可读性高很多。
3.4 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
3.5 实战中的注意事项
最后，分享几个我在实际项目中总结的经验：
- 坐标系定义要统一：项目一开始就定好坐标系规则，比如X轴朝前、Y轴朝左、Z轴朝上。团队所有人都用同一套标准。
- 旋转顺序要明确：欧拉角的旋转顺序不同，结果完全不同。我习惯用Z-Y-X顺序，这也是机器人领域最常用的。
- 验证变换是否正确：每次做完变换，我都会可视化一下，看看点云是否在预期位置。肉眼检查虽然土，但很有效。
- 注意单位：角度用弧度还是度数？Open3D里默认是弧度。我曾经因为忘记转换，把点云转到了天上去。
小技巧：如果你不确定旋转矩阵对不对，可以用单位向量做测试。比如把(1,0,0)旋转90度，看是不是变成(0,1,0)。这种小测试能帮你快速定位问题。
好了，坐标系和变换这部分就聊到这里。记住一句话：坐标系是点云处理的基石，变换是连接不同坐标系的桥梁。搞懂了这些，后面的点云配准、建图才会顺风顺水。
公众号:蓝海资料掘金营，微信deep3321