4. 点云预处理（上）

各位同学，欢迎来到实战教程的第一章。

说实话，做融合定位这么多年，我踩过最大的坑就是——原始点云直接往里怼。你想想看，一帧激光雷达扫下来，几十万个点，里面还有各种噪声、离群点、甚至飞点。直接拿去做配准或者融合，结果可想而知。

所以，点云预处理不是可选项，是必选项。今天我们就来聊聊滤波和降采样这两件事。

4.1 为什么要做点云滤波？

先说说我的亲身经历。有一次在室内走廊做定位实验，激光雷达扫出来的点云里，莫名其妙多了几个离得很远的点。一查，是玻璃窗反射造成的。这些点如果不处理，配准时会把整个位姿带偏。

点云滤波要解决三个问题：

• 去除噪声——传感器本身的热噪声、环境反射造成的飞点
• 限制范围——只保留我们关心的区域，比如前方20米内
• 剔除离群点——那些孤零零飘在空中的点，基本是误检

4.2 三种常用滤波方法

4.2.1 直通滤波（PassThrough Filter）

直通滤波，说白了就是切一刀。你设定一个范围，在这个范围之外的点全部扔掉。

比如，我只关心前方10米内的障碍物，那就把x轴范围设在0到10米。或者我只关心地面以上2米内的物体，那就把z轴范围设在-0.5到2.0米。

// C++ 示例：直通滤波
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");        // 沿z轴滤波
pass.setFilterLimits(-0.5, 2.0);     // 保留z在-0.5到2.0之间的点
pass.filter(*cloud_filtered);

4.2.2 体素滤波（VoxelGrid Filter）

体素滤波这个名字听起来高大上，其实原理很简单。你把三维空间切成一个个小立方体（体素），每个立方体里只保留一个点——通常是所有点的重心。

这样做有两个好处：一是降采样，二是平滑噪声。

// C++ 示例：体素滤波
pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(cloud);
voxel.setLeafSize(0.1f, 0.1f, 0.1f);  // 体素边长10cm
voxel.filter(*cloud_filtered);

体素大小怎么选？我个人习惯：

• 室内场景：0.05m - 0.1m
• 室外场景：0.1m - 0.3m
• 远距离物体：可以适当放大，反正远距离点本来就稀疏

4.2.3 统计滤波（Statistical Outlier Removal）

统计滤波是专门对付离群点的。它的思路是：计算每个点到它k个最近邻的平均距离。如果这个距离明显大于平均值，那这个点大概率是离群点。

// C++ 示例：统计滤波
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(50);              // 考虑50个近邻
sor.setStddevMulThresh(1.0);   // 标准差倍数阈值
sor.filter(*cloud_filtered);

我曾经在户外场景用过一次统计滤波，阈值设得太小（0.5），结果把墙上的点都当成离群点给剔了。嗯，那面墙看起来就像被狗啃过一样。

4.3 点云降采样

滤波之后，点云干净了，但点数可能还是很多。一帧64线激光雷达的点云，动辄十几万个点。实时处理的话，CPU扛不住。

降采样就是要在保持几何结构的前提下，减少点的数量。

4.3.1 VoxelGrid 降采样

刚才提到的体素滤波，其实也是一种降采样。但这里要区分一下：

• 体素滤波：每个体素内取重心，有平滑效果
• 体素降采样：每个体素内随机取一个点，或者取离体素中心最近的点

我个人更推荐用体素滤波来做降采样，因为取重心能保留更多几何信息。

4.3.2 Farthest Point Sampling（FPS）

FPS 是一种更高级的降采样方法。它的思路是：每次选一个离已选点集最远的点加入。

这样做的好处是，采样点能均匀覆盖整个点云，不会出现局部密集、局部稀疏的情况。

// Python 示例：Farthest Point Sampling
def farthest_point_sample(point_cloud, n_samples):
    """
    point_cloud: (N, 3) 的点云
    n_samples: 要采样的点数
    """
    N, D = point_cloud.shape
    centroids = np.zeros((n_samples,))
    distance = np.ones((N,)) * 1e10
    
    farthest = np.random.randint(0, N)
    for i in range(n_samples):
        centroids[i] = farthest
        centroid = point_cloud[farthest, :].reshape(1, 3)
        dist = np.sum((point_cloud - centroid) ** 2, axis=1)
        distance = np.minimum(distance, dist)
        farthest = np.argmax(distance)
    
    return point_cloud[centroids.astype(np.int32)]

4.4 代码实战：完整的预处理流程

下面是一个完整的预处理流程，把滤波和降采样串起来：

# Python + Open3D 实现
import open3d as o3d
import numpy as np

def preprocess_point_cloud(pcd, config):
    """
    点云预处理流水线
    config: 配置字典
    """
    # 1. 直通滤波 - 只保留前方20米，高度-1到3米
    bbox = o3d.geometry.AxisAlignedBoundingBox(
        min_bound=(-20, -20, -1),
        max_bound=(20, 20, 3)
    )
    pcd = pcd.crop(bbox)
    
    # 2. 统计滤波 - 剔除离群点
    cl, ind = pcd.remove_statistical_outlier(
        nb_neighbors=20,
        std_ratio=2.0
    )
    pcd = pcd.select_by_index(ind)
    
    # 3. 体素降采样 - 体素大小0.1m
    pcd = pcd.voxel_down_sample(voxel_size=0.1)
    
    return pcd

# 使用示例
pcd = o3d.io.read_point_cloud("scan.pcd")
processed_pcd = preprocess_point_cloud(pcd, {})
o3d.visualization.draw_geometries([processed_pcd])

4.5 参数调优建议

场景	体素大小	统计滤波近邻数	统计滤波阈值	推荐降采样方法
室内小场景	0.05m	20	1.5	VoxelGrid
室外开阔	0.2m	30	2.0	VoxelGrid
长走廊/隧道	0.1m	50	1.0	FPS
高速移动场景	0.15m	20	2.5	VoxelGrid

这些参数不是绝对的。我建议你拿到新数据后，先可视化看看效果，再微调。毕竟，每个激光雷达的脾气都不一样。