点云处理基础

概述

点云（Point Cloud）是三维空间中离散点的集合，每个点包含位置坐标 \((x, y, z)\) 以及可选属性（强度、颜色、法线等）。点云处理是 3D LiDAR 应用的基础，涵盖滤波、分割、配准、特征提取等核心操作。

点云数据结构

基本表示

点云本质上是一个 \(N \times D\) 的矩阵，其中 \(N\) 为点数，\(D\) 为每个点的维度：

\[ \mathbf{P} = \begin{bmatrix} x_1 & y_1 & z_1 & i_1 & \cdots \\ x_2 & y_2 & z_2 & i_2 & \cdots \\ \vdots & \vdots & \vdots & \vdots & \ddots \\ x_N & y_N & z_N & i_N & \cdots \end{bmatrix} \]

有组织 vs 无组织点云

类型	形状	说明	来源
无组织（Unorganized）	\((N, D)\)	无空间排列顺序	多数 LiDAR
有组织（Organized）	\((H, W, D)\)	类似图像的行列结构	Ouster、深度相机

有组织点云可以利用图像处理方法（如 2D 卷积），且邻域查找为 \(O(1)\)。

常用文件格式

格式	后缀	说明
PCD	`.pcd`	PCL 原生格式，支持 ASCII/Binary
PLY	`.ply`	Stanford 格式，通用性好
LAS/LAZ	`.las/.laz`	航测标准格式，LAZ 为压缩版
XYZ	`.xyz`	纯文本，最简单
numpy	`.npy/.npz`	Python 生态常用

PCL（Point Cloud Library）

PCL 是 C++ 编写的点云处理开源库，功能全面，是 ROS 生态中点云处理的标准选择。

安装

# Ubuntu
sudo apt install libpcl-dev

# ROS2 中使用
sudo apt install ros-humble-pcl-ros ros-humble-pcl-conversions

基础操作

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

// 创建点云
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
    new pcl::PointCloud<pcl::PointXYZI>);

// 读取 PCD 文件
pcl::io::loadPCDFile("input.pcd", *cloud);

// 基本信息
std::cout << "点数: " << cloud->size() << std::endl;
std::cout << "有组织: " << cloud->isOrganized() << std::endl;

// 保存
pcl::io::savePCDFileBinary("output.pcd", *cloud);

体素栅格降采样（Voxel Grid Downsampling）

将空间划分为体素（3D 网格），每个体素内的点用质心替代：

\[ \mathbf{p}_{\text{voxel}} = \frac{1}{|\mathcal{V}|} \sum_{i \in \mathcal{V}} \mathbf{p}_i \]

#include <pcl/filters/voxel_grid.h>

pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
voxel_filter.setInputCloud(cloud);
voxel_filter.setLeafSize(0.1f, 0.1f, 0.1f);  // 10cm 体素

pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(
    new pcl::PointCloud<pcl::PointXYZI>);
voxel_filter.filter(*filtered);

直通滤波（PassThrough Filter）

按坐标范围裁剪点云：

#include <pcl/filters/passthrough.h>

pcl::PassThrough<pcl::PointXYZI> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(-1.0, 3.0);  // 保留 z 在 [-1, 3] 的点
pass.filter(*filtered);

统计离群点去除

基于每个点到其 \(k\) 近邻的平均距离，去除偏差过大的离群点：

\[ d_i = \frac{1}{k} \sum_{j=1}^{k} \| \mathbf{p}_i - \mathbf{p}_{j} \| \]

若 \(d_i > \mu + n \cdot \sigma\)，则判定为离群点。

#include <pcl/filters/statistical_outlier_removal.h>

pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
sor.setInputCloud(cloud);
sor.setMeanK(50);           // k 近邻数
sor.setStddevMulThresh(1.0); // n 倍标准差
sor.filter(*filtered);

RANSAC 平面拟合

使用 RANSAC（Random Sample Consensus）从点云中检测平面。平面方程：

\[ ax + by + cz + d = 0 \]

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>

pcl::SACSegmentation<pcl::PointXYZI> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.05);  // 内点距离阈值 5cm

pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);

// coefficients->values = {a, b, c, d}
std::cout << "平面系数: " 
          << coefficients->values[0] << ", "
          << coefficients->values[1] << ", "
          << coefficients->values[2] << ", "
          << coefficients->values[3] << std::endl;

法线估计

通过点的局部邻域拟合平面来估计法线方向：

\[ \mathbf{n}_i = \text{eigenvector}_{\min}\left( \text{Cov}(\mathcal{N}_i) \right) \]

其中 \(\mathcal{N}_i\) 为点 \(\mathbf{p}_i\) 的 \(k\) 近邻，\(\text{Cov}\) 为协方差矩阵。

#include <pcl/features/normal_3d.h>

pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> ne;
ne.setInputCloud(cloud);

pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(
    new pcl::search::KdTree<pcl::PointXYZI>);
ne.setSearchMethod(tree);
ne.setKSearch(20);  // 使用 20 个近邻

pcl::PointCloud<pcl::Normal>::Ptr normals(
    new pcl::PointCloud<pcl::Normal>);
ne.compute(*normals);

欧氏聚类分割

基于距离的聚类，将点云分割为多个独立物体：

#include <pcl/segmentation/extract_clusters.h>

pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
ec.setClusterTolerance(0.5);  // 点间距离阈值 50cm
ec.setMinClusterSize(100);     // 最小聚类点数
ec.setMaxClusterSize(25000);   // 最大聚类点数
ec.setInputCloud(filtered);

std::vector<pcl::PointIndices> cluster_indices;
ec.extract(cluster_indices);

std::cout << "发现 " << cluster_indices.size() << " 个聚类" << std::endl;

Open3D

Open3D 是 Python/C++ 的 3D 数据处理库，特别适合可视化和快速原型开发。

安装

pip install open3d

基础操作

import open3d as o3d
import numpy as np

# 读取点云
pcd = o3d.io.read_point_cloud("input.pcd")
print(f"点数: {len(pcd.points)}")

# 从 numpy 创建
points = np.random.rand(10000, 3)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)

# 可视化
o3d.visualization.draw_geometries([pcd])

体素降采样

# 体素降采样
downsampled = pcd.voxel_down_sample(voxel_size=0.05)
print(f"降采样后点数: {len(downsampled.points)}")

法线估计与可视化

# 法线估计
pcd.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(
        radius=0.1, max_nn=30
    )
)

# 可视化（包含法线）
o3d.visualization.draw_geometries(
    [pcd], point_show_normal=True
)

ICP 点云配准

ICP（Iterative Closest Point）用于将两帧点云对齐：

\[ T^* = \arg\min_{T} \sum_{i} \| T \cdot \mathbf{p}_i^s - \mathbf{p}_{c(i)}^t \|^2 \]

其中 \(T\) 为刚体变换，\(c(i)\) 为最近点对应关系。

# 点到点 ICP
source = o3d.io.read_point_cloud("source.pcd")
target = o3d.io.read_point_cloud("target.pcd")

# 初始变换（可以是粗配准结果或单位矩阵）
init_transform = np.eye(4)

# 执行 ICP
result = o3d.pipelines.registration.registration_icp(
    source, target,
    max_correspondence_distance=0.5,
    init=init_transform,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(),
    criteria=o3d.pipelines.registration.ICPConvergenceCriteria(
        max_iteration=200
    )
)

print(f"Fitness: {result.fitness:.4f}")
print(f"RMSE: {result.inlier_rmse:.4f}")
print(f"变换矩阵:\n{result.transformation}")

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

点到平面 ICP

精度更高的 ICP 变体，利用法线信息：

\[ T^* = \arg\min_{T} \sum_{i} \left( (T \cdot \mathbf{p}_i^s - \mathbf{p}_{c(i)}^t) \cdot \mathbf{n}_{c(i)}^t \right)^2 \]

# 需要先估计法线
source.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
target.estimate_normals(
    search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

result = o3d.pipelines.registration.registration_icp(
    source, target,
    max_correspondence_distance=0.5,
    init=init_transform,
    estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane()
)

RANSAC 平面分割

# 平面分割
plane_model, inliers = pcd.segment_plane(
    distance_threshold=0.05,
    ransac_n=3,
    num_iterations=1000
)

[a, b, c, d] = plane_model
print(f"平面方程: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0")

# 分离平面点和非平面点
inlier_cloud = pcd.select_by_index(inliers)
outlier_cloud = pcd.select_by_index(inliers, invert=True)

# 着色可视化
inlier_cloud.paint_uniform_color([1, 0, 0])   # 平面为红色
outlier_cloud.paint_uniform_color([0, 1, 0])   # 非平面为绿色
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])

DBSCAN 聚类

# DBSCAN 聚类
labels = np.array(pcd.cluster_dbscan(
    eps=0.5, min_points=10, print_progress=True
))

max_label = labels.max()
print(f"点云有 {max_label + 1} 个聚类")

# 着色可视化
colors = plt.get_cmap("tab20")(labels / (max_label + 1))
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
o3d.visualization.draw_geometries([pcd])

ROS2 点云处理集成

PCL-ROS 转换

#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>

// ROS2 PointCloud2 → PCL
pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_cloud(
    new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*ros_msg, *pcl_cloud);

// PCL → ROS2 PointCloud2
sensor_msgs::msg::PointCloud2 output_msg;
pcl::toROSMsg(*pcl_cloud, output_msg);
output_msg.header = ros_msg->header;
publisher->publish(output_msg);

Python 中处理 ROS2 点云

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2
import numpy as np
import open3d as o3d

class PointCloudProcessor(Node):
    def __init__(self):
        super().__init__('pc_processor')
        self.sub = self.create_subscription(
            PointCloud2, '/lidar/points', self.callback, 10)

    def callback(self, msg: PointCloud2):
        # 转换为 numpy
        points = pc2.read_points_numpy(
            msg, field_names=('x', 'y', 'z', 'intensity'))

        xyz = points[:, :3]

        # 转换为 Open3D
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(xyz)

        # 降采样
        downsampled = pcd.voxel_down_sample(voxel_size=0.1)

        self.get_logger().info(
            f'原始: {len(xyz)} 点, 降采样后: {len(downsampled.points)} 点')

点云处理流水线

graph TD
    A[原始点云] --> B[预处理]
    B --> C[特征提取]
    C --> D[高层任务]

    B --> B1[距离过滤]
    B --> B2[体素降采样]
    B --> B3[离群点去除]
    B --> B4[地面去除]

    C --> C1[法线估计]
    C --> C2[关键点检测]
    C --> C3[特征描述子<br/>FPFH / SHOT]

    D --> D1[点云配准<br/>ICP / NDT]
    D --> D2[语义分割<br/>PointNet / PointNet++]
    D --> D3[目标检测<br/>PointPillars / CenterPoint]
    D --> D4[SLAM<br/>LIO-SAM / FAST-LIO2]

常见问题与优化

问题	原因	解决方案
处理速度慢	点数过多	体素降采样、ROI 裁剪
地面误检为障碍	未去除地面	RANSAC 平面拟合或基于高度过滤
运动畸变	LiDAR 扫描期间运动	运动补偿（使用 IMU 或里程计）
稀疏远距离	点密度随距离下降	多帧累积、自适应阈值
内存占用大	点云数据量大	使用 float32、体素滤波、流式处理

参考资料

PCL 官方文档：https://pointclouds.org/documentation/
Open3D 文档：https://www.open3d.org/docs/
ROS2 pcl_ros 包文档
《3D Point Cloud Analysis》 - Shan Liu et al.
PointNet 论文：Qi et al., CVPR 2017

点云处理基础

概述

点云数据结构

基本表示

有组织 vs 无组织点云

常用文件格式

PCL（Point Cloud Library）

安装

基础操作

体素栅格降采样（Voxel Grid Downsampling）

直通滤波（PassThrough Filter）

统计离群点去除

RANSAC 平面拟合

法线估计

欧氏聚类分割

Open3D

安装

基础操作

体素降采样

法线估计与可视化

ICP 点云配准

点到平面 ICP

RANSAC 平面分割

DBSCAN 聚类

ROS2 点云处理集成

PCL-ROS 转换

Python 中处理 ROS2 点云

点云处理流水线

常见问题与优化

参考资料

评论 #