一、背景

在自动驾驶和机器人感知中，雷达（LiDAR） 是一种常用的传感器，它能通过激光扫描获取周围环境的三维点云——也就是一大堆带有三维坐标（X, Y, Z）的点，每个点代表激光照射到物体表面的一个位置。

通常，要从点云中识别出障碍物（如车辆、行人、路障），最先进的方法是使用基于深度学习的点云检测模型（比如PointPillars、VoxelNet等）。这些模型能够直接处理点云，输出每个障碍物的类别和3D包围盒（一个能框住物体的最小立方体，包含中心位置、长宽高和朝向）。但是，深度学习模型需要大量的标注数据进行训练，而且对硬件有一定要求。

那么，有没有一种更直观、更轻量的方法，不需要复杂的神经网络，也能快速找到点云中的障碍物呢？
我们观察到，障碍物在点云中通常表现为一团聚集的点（有点像“团状物”）。这让我们联想到图像处理中的连通域分析——如果把点云“拍扁”成一张二维图像，那些聚集的点就会形成一块块连通的区域，就像照片中的物体一样。基于这个想法，我们设计了两种简单但有效的方案，可以直接从雷达点云中生成障碍物的3D包围盒，完全不需要深度学习。

二、效果

1、方案一效果

下面两张图分别展示了两种方案的实际效果。原始点云（灰色）中，红色的立方体就是算法自动生成的障碍物包围盒。

（顶视图投影 + 连通域分析的结果）

2、方案二效果

（RANSAC地面分割 + DBSCAN聚类 + 合并过滤的结果）

三、原理

方案一：顶视图投影 + 连通域分析

这个方案的思路非常直接：既然障碍物在点云中是“一团一团的”，我们可以从上方往下看，把三维点云投影到二维平面（X-Y平面），就像用相机俯拍一样。投影之后，每个点对应一个像素，有点的像素被标记为1，没有的标记为0，这样就得到了一张二值图像。
然后，我们用图像处理中的连通域标记（8邻域连接）来找出哪些像素是连在一起的。每个连通的像素块就对应一个潜在的障碍物。最后，根据这个像素块在原图中的位置和大小，结合原始点云中该区域点的Z坐标（高度），就能反算出一个三维的轴对齐包围盒（即长方体的各个面平行于坐标轴）。

优点：实现简单，计算快。
缺点：如果障碍物之间有重叠（比如两辆车并排且高度相近），它们在投影图上可能会连成一片，导致无法区分。此外，它只能产生轴对齐的包围盒，无法适应旋转的物体。

方案二：RANSAC地面分割 + DBSCAN聚类 + 合并与过滤

为了克服方案一的局限性，我们采用了更精细的三维处理：

1. 地面分割：点云中除了障碍物，还有大量的地面点。地面点会干扰障碍物的识别，因此需要先去除。我们使用RANSAC（随机采样一致性）算法来拟合一个平面，认为这个平面就是地面，并将其从点云中移除。剩下的就是非地面点（障碍物候选点）。

2. 欧式聚类（DBSCAN）：现在，障碍物点云是散乱的，我们需要把属于同一个物体的点聚在一起。DBSCAN是一种基于密度的聚类算法，它会根据点与点之间的距离来判断是否属于同一簇。我们需要设定一个距离阈值 eps（如果两个点的距离小于这个值，就认为它们是邻居）。为了让参数更鲁棒，我们实现了一个自适应eps估算：计算每个点的k个最近邻的平均距离，然后取所有点这个距离的中位数作为eps。

3. 合并过分割的簇：DBSCAN有时会把一个物体分成几块（比如车头和车身离得稍远），造成“过分割”。我们通过计算每个簇的包围盒，如果两个簇在X-Y平面上的投影重叠较多（IOU大于阈值），且Z方向高度相近，就将它们合并。

4. 形状过滤：最后，我们根据包围盒的体积、长宽比、高度等条件，过滤掉太小的（可能是噪声）或形状异常的（比如长条状可能是栏杆）物体，输出最终的3D包围盒。

优点：能处理复杂场景，适应旋转物体（我们可以用有向包围盒，但这里示例用了轴对齐），通过合并减少过分割，过滤去除噪声。
缺点：参数较多，需要针对不同场景调整。

四、代码实现

下面我们分别给出两种方案的Python代码，并附上详细注释，帮助理解每一步在做什么。

方案一

import matplotlib.pyplot as plt
import open3d as o3d
import numpy as np
from scipy import ndimage
import copy

def load_point_cloud(filename):
    """加载点云文件（支持.pcd等格式）"""
    print("加载点云文件...")
    pcd = o3d.io.read_point_cloud(filename)
    print(f"原始点云数量: {len(pcd.points)}")
    return pcd

def preprocess_point_cloud(pcd, voxel_size=0.05, z_min=-1.5, z_max=-0.01):
    """
    预处理点云：
    1. 根据Z坐标过滤（只保留特定高度范围内的点，例如去除地面以上的点）
    2. 体素下采样（减少点云密度，加快处理）
    3. 统计滤波去噪（移除离群点）
    """
    print("预处理点云...")
    points = np.asarray(pcd.points)

    # 1. Z值过滤：只保留高度在z_min到z_max之间的点
    mask = (points[:, 2] > z_min) & (points[:, 2] <= z_max)
    filtered_points = points[mask]
    filtered_cloud = o3d.geometry.PointCloud()
    filtered_cloud.points = o3d.utility.Vector3dVector(filtered_points)
    print(f"Z过滤后点数: {len(filtered_cloud.points)}")

    # 2. 体素下采样：将空间划分为小立方体（体素），每个体素内只保留一个点
    downpcd = filtered_cloud.voxel_down_sample(voxel_size=voxel_size)
    print(f"下采样后点数: {len(downpcd.points)}")

    # 3. 统计滤波：计算每个点周围邻居的距离，距离过大的点被认为是噪声并移除
    cl, ind = downpcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0)
    inlier_cloud = downpcd.select_by_index(ind)
    print(f"去噪后点数: {len(inlier_cloud.points)}")

    return inlier_cloud

def visualize_results(original_pcd, clusters, bounding_boxes, title="检测结果"):
    """可视化原始点云、各个聚类点云以及包围盒"""
    print("可视化结果...")
    geometries = [original_pcd.paint_uniform_color([0.5, 0.5, 0.5])]  # 原始点云设为灰色
    geometries.extend(clusters)   # 每个聚类用随机颜色显示
    geometries.extend(bounding_boxes)  # 包围盒显示为红色
    o3d.visualization.draw_geometries(
        geometries, window_name=title, width=1024, height=768
    )

def detect_objects_in_point_cloud(filename, voxel_size=0.1, z_range=(-1.5, -0.01), min_volume=0.05):
    """
    主函数：通过顶视图投影+连通域分析检测障碍物
    参数说明：
        filename: 点云文件路径
        voxel_size: 体素大小，用于下采样，也影响投影分辨率
        z_range: (z_min, z_max) 只保留该高度范围内的点
        min_volume: 最小体积阈值，小于此值的物体将被过滤
    """
    # 1. 加载点云
    pcd = load_point_cloud(filename)

    # 2. 预处理
    processed_pcd = preprocess_point_cloud(pcd, voxel_size=voxel_size, 
                                           z_min=z_range[0], z_max=z_range[1])
    points = np.asarray(processed_pcd.points)
    
    cluster_geometries = []  # 存储每个聚类的点云（用于可视化）
    bounding_boxes = []      # 存储每个物体的包围盒
    
    # 3. 顶视图投影，计算连通域
    print("开始顶视图投影和连通域分析...")
    
    # 投影分辨率：每个像素代表实际空间中的多大尺寸
    pixel_size = voxel_size * 2  # 这里设为体素大小的2倍，可根据需要调整
    
    # 获取点云在XY平面上的范围
    x_min, x_max = np.min(points[:, 0]), np.max(points[:, 0])
    y_min, y_max = np.min(points[:, 1]), np.max(points[:, 1])
    
    # 计算图像尺寸（行列数）
    width = int(np.ceil((x_max - x_min) / pixel_size)) + 1
    height = int(np.ceil((y_max - y_min) / pixel_size)) + 1
    
    # 初始化二值图像，所有像素初始为0
    img = np.zeros((height, width), dtype=np.uint8)
    
    # 将每个点的X,Y坐标映射到像素行列索引
    col_indices = np.floor((points[:, 0] - x_min) / pixel_size).astype(int)
    row_indices = np.floor((points[:, 1] - y_min) / pixel_size).astype(int)
    # 确保索引不越界
    col_indices = np.clip(col_indices, 0, width - 1)
    row_indices = np.clip(row_indices, 0, height - 1)
    
    # 标记有点的像素为1
    img[row_indices, col_indices] = 1
    
    # 连通域标记（使用8邻域连接）
    labeled_img, num_features = ndimage.label(img, structure=np.ones((3, 3)))
    print(f"检测到 {num_features} 个候选连通域")
    
    # 为每个点获取其对应的标签
    point_labels = labeled_img[row_indices, col_indices]
    
    filtered_count = 0
    # 遍历每一个连通域（标签从1开始）
    for label in range(1, num_features + 1):
        # 提取属于当前标签的点
        mask = point_labels == label
        if np.sum(mask) == 0:
            continue
        cluster_points = points[mask]
        
        # ---- 在图像上获取该连通域的二维包围盒 ----
        rows, cols = np.where(labeled_img == label)
        min_row, max_row = np.min(rows), np.max(rows)
        min_col, max_col = np.min(cols), np.max(cols)
        
        # 将像素坐标转换回物理坐标，得到二维中心、长、宽
        center_x = x_min + (min_col + max_col) / 2.0 * pixel_size
        center_y = y_min + (min_row + max_row) / 2.0 * pixel_size
        length_x = (max_col - min_col + 1) * pixel_size
        length_y = (max_row - min_row + 1) * pixel_size
        
        # ---- 获取该区域的高度范围 ----
        z_min_obj = np.min(cluster_points[:, 2])
        z_max_obj = np.max(cluster_points[:, 2])
        center_z = (z_min_obj + z_max_obj) / 2.0
        length_z = z_max_obj - z_min_obj
        
        # 计算体积
        volume = length_x * length_y * length_z
        
        # 过滤体积太小的物体（可能是噪声）
        if volume < min_volume:
            filtered_count += 1
            continue
        
        # 生成该聚类的点云（用于可视化）
        cluster_pcd = o3d.geometry.PointCloud()
        cluster_pcd.points = o3d.utility.Vector3dVector(cluster_points)
        color = np.random.rand(3)  # 随机颜色
        cluster_pcd.paint_uniform_color(color)
        cluster_geometries.append(cluster_pcd)
        
        # 生成轴对齐包围盒
        min_bound = [center_x - length_x/2, center_y - length_y/2, center_z - length_z/2]
        max_bound = [center_x + length_x/2, center_y + length_y/2, center_z + length_z/2]
        bbox = o3d.geometry.AxisAlignedBoundingBox(min_bound, max_bound)
        bbox.color = [1, 0, 0]  # 红色
        bounding_boxes.append(bbox)
    
    print(f"过滤后剩余物体数量: {len(bounding_boxes)} (过滤了 {filtered_count} 个)")
    
    # 4. 可视化结果
    visualize_results(processed_pcd, cluster_geometries, bounding_boxes)

    return {
        'point_cloud': processed_pcd,
        'clusters': cluster_geometries,
        'bounding_boxes': bounding_boxes
    }

def main():
    # 调用主函数，请将"LIDAR_TOP.pcd"替换为你自己的点云文件路径
    result = detect_objects_in_point_cloud(
        "LIDAR_TOP.pcd",
        voxel_size=0.2,           # 体素大小（米）
        z_range=(-1.5, -0.01),    # 只保留高度在-1.5米到-0.01米之间的点（假设雷达安装高度1.5米，地面以下）
        min_volume=0.05            # 最小体积0.05立方米
    )
    return result

if __name__ == "__main__":
    main()

方案二

import open3d as o3d
import numpy as np
from sklearn.neighbors import NearestNeighbors
import copy  # 用于合并时的深拷贝

def load_point_cloud(filename):
    """加载点云文件"""
    print("加载点云文件...")
    pcd = o3d.io.read_point_cloud(filename)
    print(f"原始点云数量: {len(pcd.points)}")
    return pcd

def preprocess_point_cloud(pcd, voxel_size=0.1):
    """基础预处理：下采样 + 统计滤波（不包含地面分割）"""
    print("基础预处理...")
    # 1. 体素下采样
    downpcd = pcd.voxel_down_sample(voxel_size=voxel_size)
    print(f"下采样后点数: {len(downpcd.points)}")
    # 2. 统计滤波去噪
    cl, ind = downpcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0)
    inlier_cloud = downpcd.select_by_index(ind)
    print(f"去噪后点数: {len(inlier_cloud.points)}")
    return inlier_cloud

def segment_ground_ransac(pcd, distance_threshold=0.2, ransac_n=3, num_iterations=1000):
    """
    使用RANSAC算法分割地面
    参数:
        pcd: 输入点云
        distance_threshold: 点到平面的距离阈值，小于该值的点被认为是地面点
        ransac_n: 随机采样点数，用于拟合平面
        num_iterations: 迭代次数
    返回:
        ground_cloud: 地面点云
        objects_cloud: 非地面点云
    """
    print("RANSAC地面分割...")
    plane_model, inliers = pcd.segment_plane(distance_threshold=distance_threshold,
                                             ransac_n=ransac_n,
                                             num_iterations=num_iterations)
    ground_cloud = pcd.select_by_index(inliers)
    objects_cloud = pcd.select_by_index(inliers, invert=True)
    print(f"地面点: {len(ground_cloud.points)}, 非地面点: {len(objects_cloud.points)}")
    return ground_cloud, objects_cloud

def estimate_eps(point_cloud, k=8, percentile=50):
    """
    基于k近邻距离估算DBSCAN的eps参数
    原理：计算每个点的k个最近邻的平均距离，然后取所有点这个平均距离的某个分位数作为eps。
    这样可以根据点云密度自动调整聚类半径。
    """
    points = np.asarray(point_cloud.points)
    if len(points) < k:
        return 0.5  # 点太少，返回默认值
    nbrs = NearestNeighbors(n_neighbors=k).fit(points)
    distances, _ = nbrs.kneighbors(points)
    # 每个点的平均距离（忽略自身，即取第1到k-1近邻）
    avg_dist = np.mean(distances[:, 1:], axis=1)
    eps = np.percentile(avg_dist, percentile)
    print(f"自适应eps估算: {eps:.4f} (基于{k}近邻的{percentile}分位数)")
    return eps

def cluster_dbscan(pcd, eps=None, min_points=10, eps_factor=2.0, use_auto_eps=True):
    """
    执行DBSCAN聚类，支持自动估算eps
    """
    if use_auto_eps:
        if eps is None:
            eps = estimate_eps(pcd, k=min_points, percentile=50)
    else:
        # 若使用固定倍数，需要传入体素大小等，这里简化处理
        pass
    print(f"DBSCAN聚类参数: eps={eps:.4f}, min_points={min_points}")
    labels = np.array(pcd.cluster_dbscan(eps=eps, min_points=min_points, print_progress=True))
    return labels

def merge_clusters_by_bbox(clusters_pcd, bboxes, iou_thresh=0.3, z_merge_thresh=0.5):
    """
    基于包围盒合并过分割的簇
    参数:
        clusters_pcd: 每个簇的点云列表
        bboxes: 每个簇的轴对齐包围盒列表
        iou_thresh: XY平面投影的IOU阈值，高于此值考虑合并
        z_merge_thresh: Z方向中心高度差阈值，小于此值考虑合并
    返回:
        合并后的簇点云列表和包围盒列表
    """
    if len(clusters_pcd) <= 1:
        return clusters_pcd, bboxes
    n = len(clusters_pcd)
    merged = [False] * n
    new_clusters = []
    new_bboxes = []
    for i in range(n):
        if merged[i]:
            continue
        # 当前簇作为合并的基础
        combined_pcd = copy.deepcopy(clusters_pcd[i])
        combined_bbox = bboxes[i]
        merged[i] = True
        for j in range(i+1, n):
            if merged[j]:
                continue
            # 计算两个包围盒在XY平面上的IOU
            bbox_i = bboxes[i]
            bbox_j = bboxes[j]
            min_i = bbox_i.get_min_bound()
            max_i = bbox_i.get_max_bound()
            min_j = bbox_j.get_min_bound()
            max_j = bbox_j.get_max_bound()
            # 计算重叠区间
            x_overlap = max(0, min(max_i[0], max_j[0]) - max(min_i[0], min_j[0]))
            y_overlap = max(0, min(max_i[1], max_j[1]) - max(min_i[1], min_j[1]))
            area_i = (max_i[0]-min_i[0]) * (max_i[1]-min_i[1])
            area_j = (max_j[0]-min_j[0]) * (max_j[1]-min_j[1])
            overlap_area = x_overlap * y_overlap
            iou_xy = overlap_area / (area_i + area_j - overlap_area + 1e-6)
            # Z方向中心高度差
            z_center_i = (min_i[2] + max_i[2]) / 2
            z_center_j = (min_j[2] + max_j[2]) / 2
            z_diff = abs(z_center_i - z_center_j)
            # 如果满足合并条件，则合并
            if iou_xy > iou_thresh and z_diff < z_merge_thresh:
                combined_pcd += clusters_pcd[j]
                merged[j] = True
        # 重新计算合并后的包围盒
        new_bbox = combined_pcd.get_axis_aligned_bounding_box()
        new_bbox.color = [1, 0, 0]
        new_clusters.append(combined_pcd)
        new_bboxes.append(new_bbox)
    print(f"合并前簇数量: {n}, 合并后: {len(new_clusters)}")
    return new_clusters, new_bboxes

def filter_by_shape(bbox, min_volume=0.05, max_aspect_ratio=5.0, min_height=0.2):
    """
    根据形状过滤：体积、长宽比、高度
    参数:
        bbox: 包围盒
        min_volume: 最小体积（立方米）
        max_aspect_ratio: 最大长宽比（最长边/最短边）
        min_height: 最小高度（米）
    返回:
        True表示通过过滤，保留；False表示过滤掉
    """
    extent = bbox.get_extent()
    volume = extent[0] * extent[1] * extent[2]
    if volume < min_volume:
        return False
    aspect_ratio = max(extent) / (min(extent) + 1e-6)
    if aspect_ratio > max_aspect_ratio:
        return False
    if extent[2] < min_height:
        return False
    return True

def detect_objects_optimized(filename, voxel_size=0.2, 
                             ground_dist=0.3, 
                             z_range=None,          # 例如 (-1.5, None) 或 (-1.5, 0.0)
                             z_max_global=None,     # 额外Z上限过滤（如去除Z>0的点）
                             eps=None, min_points=15,
                             min_volume=0.1, use_auto_eps=True,
                             merge_enabled=True):
    """
    优化后的物体检测主函数，支持Z范围过滤和额外Z上限过滤
    参数:
        filename: 点云文件路径
        voxel_size: 体素下采样大小
        ground_dist: RANSAC地面分割距离阈值
        z_range: 预处理阶段的Z值过滤范围，元组 (z_min, z_max)，None表示不限制
        z_max_global: 地面分割后对物体点云的Z上限过滤（如0表示只保留Z<=0的点）
        eps: DBSCAN聚类半径，若为None则自动估算
        min_points: DBSCAN最小点数
        min_volume: 最小体积阈值
        use_auto_eps: 是否自动估算eps
        merge_enabled: 是否启用簇合并
    """
    # 1. 加载点云
    pcd = load_point_cloud(filename)

    # 2. 基础预处理（下采样+去噪）
    processed_pcd = preprocess_point_cloud(pcd, voxel_size=voxel_size)

    # 3. 可选：预处理阶段的Z范围过滤（例如只保留地面附近区域）
    if z_range is not None:
        z_min, z_max = z_range
        points = np.asarray(processed_pcd.points)
        mask = np.ones(len(points), dtype=bool)
        if z_min is not None:
            mask &= (points[:, 2] > z_min)
        if z_max is not None:
            mask &= (points[:, 2] <= z_max)
        processed_pcd = processed_pcd.select_by_index(np.where(mask)[0])
        print(f"Z范围过滤后点数: {len(processed_pcd.points)}")

    # 4. RANSAC地面分割
    ground, objects = segment_ground_ransac(processed_pcd, distance_threshold=ground_dist)

    # 5. 额外Z上限过滤（例如去除高于地面的点，只保留地面以下的物体）
    if z_max_global is not None:
        obj_points = np.asarray(objects.points)
        mask_z = obj_points[:, 2] <= z_max_global
        objects = objects.select_by_index(np.where(mask_z)[0])
        print(f"Z上限过滤后物体点数量: {len(objects.points)}")

    if len(objects.points) == 0:
        print("警告：无物体点云，请检查参数")
        return None

    # 6. DBSCAN聚类
    if use_auto_eps:
        eps = estimate_eps(objects, k=min_points, percentile=50)
    else:
        if eps is None:
            eps = voxel_size * 3.0  # 默认倍数
    labels = np.array(objects.cluster_dbscan(eps=eps, min_points=min_points, print_progress=True))

    max_label = labels.max()
    print(f"初始聚类数: {max_label + 1} (不含噪声)")

    # 提取每个簇
    points = np.asarray(objects.points)
    raw_clusters = []
    raw_bboxes = []
    for label in range(max_label + 1):
        cluster_mask = labels == label
        cluster_points = points[cluster_mask]
        cluster_pcd = o3d.geometry.PointCloud()
        cluster_pcd.points = o3d.utility.Vector3dVector(cluster_points)
        cluster_pcd.paint_uniform_color(np.random.rand(3))  # 随机颜色
        bbox = cluster_pcd.get_axis_aligned_bounding_box()
        bbox.color = [1, 0, 0]
        raw_clusters.append(cluster_pcd)
        raw_bboxes.append(bbox)

    # 7. 可选：合并过分割簇
    if merge_enabled:
        merged_clusters, merged_bboxes = merge_clusters_by_bbox(raw_clusters, raw_bboxes)
    else:
        merged_clusters, merged_bboxes = raw_clusters, raw_bboxes

    # 8. 形状过滤
    final_clusters = []
    final_bboxes = []
    filtered_count = 0
    for cluster, bbox in zip(merged_clusters, merged_bboxes):
        if filter_by_shape(bbox, min_volume=min_volume):
            final_clusters.append(cluster)
            final_bboxes.append(bbox)
        else:
            filtered_count += 1

    print(f"过滤后物体数量: {len(final_bboxes)} (过滤了 {filtered_count} 个)")

    # 9. 可视化
    ground.paint_uniform_color([0.5, 0.5, 0.5])  # 灰色地面
    vis_list = [ground] + final_clusters + final_bboxes
    o3d.visualization.draw_geometries(vis_list, window_name="优化后的欧式聚类结果", width=1024, height=768)

    return {
        'ground': ground,
        'objects': objects,
        'clusters': final_clusters,
        'bounding_boxes': final_bboxes
    }

def main():
    # 调用主函数，请将"LIDAR_TOP.pcd"替换为你自己的点云文件路径
    result = detect_objects_optimized(
        "LIDAR_TOP.pcd",
        voxel_size=0.2,          # 下采样体素大小（米）
        ground_dist=-1.5,         # RANSAC地面分割距离阈值
        z_range=(None,0),    # 预处理过滤：只保留Z > -1.5米的点（即地面以下的部分）
        z_max_global=0.0,        # 额外过滤：只保留Z <= 0的非地面点（即地面以下）
        min_points=10,            # DBSCAN最小点数
        min_volume=0.05,          # 最小体积（立方米）
        use_auto_eps=True,        # 自动估算eps
        merge_enabled=True        # 启用合并过分割簇
    )
    return result

if __name__ == "__main__":
    main()

五、总结

本文介绍了两种无需深度学习的点云障碍物检测方法：

• 方案一 将三维点云投影到二维平面，利用图像连通域分析快速定位障碍物，适合简单场景。
• 方案二 采用RANSAC地面分割、DBSCAN聚类、合并与过滤，能处理更复杂的场景，且可灵活调整参数。

这两种方法实现简单、计算高效，可作为深度学习方法的补充或替代，特别适合资源受限或需要快速原型验证的场景。