3、点云基础操作：读取与保存（PCD/PLY/BIN格式）、点云可视化、坐标系与变换矩阵

好，咱们进入正题。点云处理的第一步是什么？说白了，就是得先把数据弄进来，看完长什么样，再考虑怎么折腾它。这一章我带你过一遍最基础的操作：读文件、写文件、可视化，还有那个绕不开的坐标系变换。

我个人习惯，拿到一个点云数据集，第一件事不是急着跑算法，而是先看看数据质量。嗯，这一步能帮你省掉后面很多排查问题的功夫。

3.1 点云文件的读取与保存

点云的存储格式五花八门，但最常用的就三种：PCD、PLY、BIN。我当年刚入行时，光搞懂这些格式就花了两天。其实没那么复杂，咱们一个一个说。

3.1.1 PCD格式

PCD是Point Cloud Library（PCL）的亲儿子。它的特点是文件头是纯文本，数据部分可以是ASCII或者二进制。我建议你平时调试用ASCII，方便用文本编辑器打开看一眼；正式跑数据用二进制，省空间、速度快。

读取PCD文件，用PCL的API非常直接：

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("scan.pcd", *cloud) == -1) {
    PCL_ERROR("读文件失败，检查路径\n");
    return -1;
}
std::cout << "加载了点云，点数：" << cloud->points.size() << std::endl;

保存也一样简单：

pcl::io::savePCDFileASCII("output.pcd", *cloud);  // ASCII格式
// 或者
pcl::io::savePCDFileBinary("output.pcd", *cloud); // 二进制格式
我的小经验： 如果你要保存带颜色的点云，记得用 pcl::PointXYZRGB 类型。我曾经在项目里用 PointXYZ 保存彩色点云，结果颜色信息全丢了，排查了半天才发现是类型不匹配。

3.1.2 PLY格式

PLY格式在计算机视觉领域用得很多，尤其是三维重建。它比PCD更灵活，可以存顶点、面片、颜色、法向量等等。PCL也支持直接读写PLY：

#include <pcl/io/ply_io.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ply(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPLYFile("mesh.ply", *cloud_ply);
pcl::io::savePLYFileBinary("output.ply", *cloud_ply);

这里要注意一点：PLY文件有时会包含三角网格信息，而不仅仅是点云。如果你只关心点，读取时PCL会自动忽略面片信息，只提取顶点。

3.1.3 BIN格式

BIN格式，说白了就是纯二进制文件。没有文件头，没有元数据，就是一串float或者double挨个排着。KITTI数据集用的就是这种格式。为什么？因为读取速度快，而且文件体积小。

读取BIN文件需要你自己解析字节流。我写了一个常用的函数，你直接拿去用：

#include <fstream>
#include <vector>

std::vector<float> readBinFile(const std::string& filename) {
    std::ifstream file(filename, std::ios::binary);
    if (!file.is_open()) {
        std::cerr << "打开文件失败: " << filename << std::endl;
        return {};
    }
    file.seekg(0, std::ios::end);
    size_t size = file.tellg();
    file.seekg(0, std::ios::beg);

    std::vector<float> buffer(size / sizeof(float));
    file.read(reinterpret_cast<char*>(buffer.data()), size);
    file.close();
    return buffer;
}

// 使用示例：KITTI格式，每行4个float（x, y, z, reflectance）
auto data = readBinFile("000000.bin");
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_kitti(new pcl::PointCloud<pcl::PointXYZI>);
for (size_t i = 0; i < data.size(); i += 4) {
    pcl::PointXYZI point;
    point.x = data[i];
    point.y = data[i + 1];
    point.z = data[i + 2];
    point.intensity = data[i + 3];
    cloud_kitti->points.push_back(point);
}
避坑指南： 我曾经在读取KITTI的BIN文件时，直接按 sizeof(float) 去读，结果发现点数对不上。后来才意识到，KITTI的BIN文件里每行是4个float，但有些版本是5个（多了一个时间戳）。所以，一定要先确认数据格式，别想当然。

3.2 点云可视化

点云是三维的，光看数字你根本看不出门道。可视化是点云处理的「眼睛」。PCL自带的 pcl::visualization::PCLVisualizer 就够用了。

3.2.1 快速显示

#include <pcl/visualization/cloud_viewer.h>

pcl::visualization::CloudViewer viewer("点云查看器");
viewer.showCloud(cloud);
while (!viewer.wasStopped()) {
    // 保持窗口打开
}

嗯，这个最简单，但功能也最弱。你没法交互式地旋转、缩放？其实可以，鼠标拖拽就行。但如果你想加一些自定义控件，就得用 PCLVisualizer。

3.2.2 高级可视化

#include <pcl/visualization/pcl_visualizer.h>

pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);  // 黑色背景
viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
viewer->setPointCloudRenderingProperties(
    pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");
viewer->addCoordinateSystem(1.0);  // 添加坐标轴
viewer->initCameraParameters();

while (!viewer->wasStopped()) {
    viewer->spinOnce(100);
}

我个人习惯，可视化时一定会加上坐标轴。为什么？因为这样你能直观地看到点云在空间中的朝向，尤其是做SLAM时，判断车头方向对不对，一眼就能看出来。

3.2.3 颜色映射

如果你有强度值或者高度信息，用颜色映射会直观很多。比如按高度着色：

pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(cloud, "z");
viewer->addPointCloud<pcl::PointXYZ>(cloud, color_handler, "colored cloud");

这样，高的点显示红色，低的点显示蓝色，地形起伏一目了然。

3.3 坐标系与变换矩阵

点云处理里，坐标系变换是家常便饭。你想想看，激光雷达装在车上，车在动，雷达也在动，每一帧点云都在不同的坐标系下。怎么把它们对齐？靠的就是变换矩阵。

3.3.1 常见的坐标系

坐标系 原点 常见用途
激光雷达坐标系 激光雷达中心 原始点云数据
车体坐标系 车辆后轴中心 SLAM、控制
世界坐标系 地图原点 全局定位、建图
相机坐标系 相机光心 视觉-激光雷达融合

这里有个坑：不同传感器的坐标系定义可能不一样。比如，激光雷达通常是x向前、y向左、z向上；但有些相机是z向前、x向右。做融合时，一定要先统一坐标系。

3.3.2 变换矩阵（4x4齐次矩阵）

变换矩阵长这样：

| R11 R12 R13 tx |
| R21 R22 R23 ty |
| R31 R32 R33 tz |
| 0   0   0   1  |

左上角3x3是旋转矩阵，右上角3x1是平移向量。用齐次坐标的好处是，旋转和平移可以写在一个矩阵里，连续变换就是矩阵乘法。

在PCL中，用 Eigen::Matrix4f 来表示：

#include <Eigen/Dense>

Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
transform(0, 0) = 0.707; transform(0, 1) = -0.707; transform(0, 3) = 1.0;
transform(1, 0) = 0.707; transform(1, 1) = 0.707;  transform(1, 3) = 0.5;
// 绕z轴旋转45度，然后平移(1.0, 0.5, 0)

pcl::transformPointCloud(*cloud, *cloud_transformed, transform);
核心要点： 变换矩阵是左乘还是右乘？PCL的 transformPointCloud 默认是 p' = T * p，也就是左乘。如果你搞反了，点云会飞到天上去。我刚开始学的时候，就因为这个bug，点云直接跑到了视野外面，找了半天才发现是矩阵乘反了。

3.3.3 实际应用：将激光雷达点云转换到车体坐标系

假设你的激光雷达安装在车顶，相对于车体坐标系有一个固定的偏移和旋转。这个偏移量通常由标定得到。举个例子：

// 激光雷达到车体的变换矩阵（假设标定得到）
Eigen::Matrix4f lidar_to_vehicle;
lidar_to_vehicle << 
    1, 0, 0, 0.5,   // x方向偏移0.5米
    0, 1, 0, 0.0,   // y方向无偏移
    0, 0, 1, 1.2,   // z方向偏移1.2米（车顶高度）
    0, 0, 0, 1;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_vehicle(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud_lidar, *cloud_in_vehicle, lidar_to_vehicle);

这样，每一帧点云都被转换到了车体坐标系下。后续做SLAM时，你只需要估计车体在世界坐标系下的位姿，不用再关心雷达的安装位置了。

3.3.4 逆变换

有时候你需要把点云从车体坐标系转回雷达坐标系，比如做可视化时想看到原始视角。逆变换就是求变换矩阵的逆：

Eigen::Matrix4f vehicle_to_lidar = lidar_to_vehicle.inverse();
pcl::transformPointCloud(*cloud_in_vehicle, *cloud_back_to_lidar, vehicle_to_lidar);

注意：旋转矩阵的逆等于它的转置，但平移部分不是简单的取负。所以直接用 .inverse() 最保险。

我的习惯： 每次做变换前，我都会先可视化一下变换后的点云，确认它没有「飞走」。如果发现点云位置不对，先检查矩阵是不是单位阵，再检查平移量是不是写反了。这两个问题占了90%的bug。

3.4 本章小结

这一章的内容，说白了就是点云处理的「基本功」。读文件、写文件、可视化、坐标系变换，这些操作你以后每天都会用到。我个人建议你把这些代码封装成工具函数，比如 loadPointCloud()、visualize()、transformCloud()，以后用起来就顺手多了。

下一章，咱们开始聊点云的滤波。嗯，那才是真正开始「处理」点云了。

← 上一章 📖 返回目录 下一章 →