4、点云数据基础：点云数据结构（PCL/Open3D）、点云可视化、点云坐标系对齐

点云数据，说白了就是一堆三维空间中的点。每个点都带着 XYZ 坐标，有些还带着颜色、强度之类的附加信息。我刚开始接触点云时，觉得它不就是一堆点嘛，有啥好学的？后来做动态障碍物追踪才发现，点云的数据结构、可视化、坐标系对齐，每一个都是坑。今天咱们就把这些基础打牢。

4.1 点云数据结构：PCL vs Open3D

目前主流的点云处理库有两个：PCL（Point Cloud Library）和 Open3D。我个人习惯用 Open3D 做快速原型验证，用 PCL 做工业级部署。为什么？因为 Open3D 的 API 更 Pythonic，上手快；PCL 功能更全，但 C++ 为主，编译起来能让你怀疑人生。

4.1.1 PCL 的点云结构

PCL 里最核心的就是 pcl::PointCloud<T> 这个模板类。T 可以是各种点类型，比如：

点类型	包含字段	典型用途
pcl::PointXYZ	x, y, z	纯几何位置
pcl::PointXYZRGB	x, y, z, r, g, b	带颜色的点云
pcl::PointXYZI	x, y, z, intensity	激光雷达强度数据
pcl::PointNormal	x, y, z, normal_x, normal_y, normal_z	带法向量的点云

嗯，这里要注意：PCL 的点云数据是连续存储在内存中的，访问速度很快。但如果你要动态增删点，记得用 reserve() 预分配空间，不然频繁 realloc 会卡死你。

4.1.2 Open3D 的点云结构

Open3D 就简单多了。它的核心类是 open3d.geometry.PointCloud，本质上就是一个 NumPy 数组的封装。你想想看，这意味着什么？意味着你可以直接用 NumPy 的切片、索引、数学运算来处理点云，爽不爽？

import open3d as o3d
import numpy as np

# 创建一个点云
points = np.random.rand(1000, 3)  # 1000个随机点
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)

# 添加颜色
colors = np.random.rand(1000, 3)
pcd.colors = o3d.utility.Vector3dVector(colors)

# 添加法向量
normals = np.random.randn(1000, 3)
normals = normals / np.linalg.norm(normals, axis=1, keepdims=True)
pcd.normals = o3d.utility.Vector3dVector(normals)

print(f"点云包含 {len(pcd.points)} 个点")
print(f"点的坐标范围: X [{points[:,0].min():.2f}, {points[:,0].max():.2f}]")

我个人建议：如果你只是做算法验证、可视化、快速原型，用 Open3D。如果你要做工业级的滤波、分割、配准，PCL 更靠谱。但说实话，现在 Open3D 的功能也越来越强了，很多 PCL 的功能它都有 Python 接口。

4.2 点云可视化

可视化是点云处理中最直观的调试手段。你想想看，一堆数字你看不出啥问题，但一可视化，离群点、噪声、缺失区域一目了然。

4.2.1 Open3D 可视化基础

Open3D 的可视化非常方便，几行代码就能搞定：

import open3d as o3d

# 读取点云
pcd = o3d.io.read_point_cloud("example.pcd")

# 可视化
o3d.visualization.draw_geometries([pcd],
                                  window_name="点云可视化",
                                  width=800, height=600,
                                  point_show_normal=False)

但如果你只是这样用，那太浪费了。我常用的几个技巧：

• 按高度着色：用 colormap 把 Z 轴映射到颜色，一眼就能看出地形起伏
• 按强度着色：激光雷达的强度值能反映材质，比如路面和金属的强度差异很大
• 交互式选择：按住 Shift + 鼠标左键可以框选点，方便调试

4.2.2 高级可视化：多窗口与动画

做动态障碍物追踪时，经常需要同时看多个点云。比如原始点云、滤波后的点云、聚类后的点云。Open3D 支持多窗口：

import open3d as o3d

# 创建两个点云
pcd1 = o3d.io.read_point_cloud("frame_001.pcd")
pcd2 = o3d.io.read_point_cloud("frame_002.pcd")

# 给点云上色以便区分
pcd1.paint_uniform_color([1, 0, 0])  # 红色
pcd2.paint_uniform_color([0, 1, 0])  # 绿色

# 同时可视化
o3d.visualization.draw_geometries([pcd1, pcd2],
                                  window_name="两帧对比",
                                  width=1200, height=800)

嗯，这里要注意：如果你要可视化动画（比如连续帧），别用 draw_geometries，它是一次性的。要用 Visualizer 类，手动控制刷新：

vis = o3d.visualization.Visualizer()
vis.create_window()

for i in range(100):
    pcd = o3d.io.read_point_cloud(f"frame_{i:03d}.pcd")
    vis.clear_geometries()
    vis.add_geometry(pcd)
    vis.poll_events()
    vis.update_renderer()

vis.destroy_window()

4.3 点云坐标系对齐

坐标系对齐，说白了就是把不同传感器、不同时刻的点云放到同一个坐标系下。这是动态障碍物追踪的核心前提——你坐标系都没对齐，谈何追踪？

4.3.1 常见的坐标系

在机器人领域，常见的坐标系有：

坐标系	原点	X轴	Y轴	Z轴
世界坐标系	地图原点	东	北	天
机器人坐标系	机器人中心	前进方向	左侧	天
激光雷达坐标系	激光雷达中心	前方	左侧	天
相机坐标系	相机光心	右	下	前

你想想看，激光雷达和相机的坐标系就差很多。激光雷达是 Z 轴朝上，相机是 Z 轴朝前。如果不做对齐，融合出来的数据就是乱的。

4.3.2 使用 Open3D 进行坐标变换

Open3D 里做坐标变换很简单，核心就是 4x4 的齐次变换矩阵：

import open3d as o3d
import numpy as np

# 创建一个变换矩阵（平移 + 旋转）
T = np.eye(4)
T[:3, :3] = o3d.geometry.get_rotation_matrix_from_xyz([0, 0, np.pi/4])  # 绕Z轴旋转45度
T[:3, 3] = [1.0, 2.0, 0.5]  # 平移

# 应用变换
pcd = o3d.io.read_point_cloud("example.pcd")
pcd.transform(T)

# 或者用更直观的方式
pcd.translate([1.0, 2.0, 0.5])  # 平移
pcd.rotate(o3d.geometry.get_rotation_matrix_from_xyz([0, 0, np.pi/4]), 
           center=(0, 0, 0))  # 绕原点旋转

4.3.3 ICP 配准：自动对齐

如果你不知道两个点云之间的变换关系，可以用 ICP（迭代最近点）算法自动对齐。Open3D 提供了现成的接口：

import open3d as o3d

# 读取源点云和目标点云
source = o3d.io.read_point_cloud("source.pcd")
target = o3d.io.read_point_cloud("target.pcd")

# 下采样加速
source_down = source.voxel_down_sample(0.05)
target_down = target.voxel_down_sample(0.05)

# ICP 配准
threshold = 0.1  # 对应点距离阈值
trans_init = np.eye(4)  # 初始变换（可以给个粗略估计）

reg_p2p = o3d.pipelines.registration.registration_icp(
    source_down, target_down, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint())

print(f"ICP 收敛后的变换矩阵:\n{reg_p2p.transformation}")
print(f"配准误差: {reg_p2p.fitness:.4f}")

# 应用变换
source.transform(reg_p2p.transformation)

# 可视化对齐结果
source.paint_uniform_color([1, 0, 0])
target.paint_uniform_color([0, 1, 0])
o3d.visualization.draw_geometries([source, target])

嗯，这里要注意：ICP 对初始位置很敏感。如果两个点云离得太远，ICP 会陷入局部最优。我一般先用粗略的方法（比如基于里程计或 GPS）给个初始变换，再用 ICP 精调。

4.3.4 坐标系对齐的避坑指南

• 单位统一：激光雷达常用米，相机常用毫米。不统一的话，点云会飞到天上去。我曾经犯过这个错，可视化时点云全在屏幕外面，找了半天才发现是单位问题。
• 右手定则：确保所有坐标系都遵循右手定则。有些传感器用左手系，需要额外转换。
• 时间戳同步：多传感器对齐时，不仅要空间对齐，还要时间对齐。不同传感器的采样频率不同，需要用插值或最近邻匹配。