一、随机样本共识
三维点云处理通常采用RANSAC算法而非霍夫变换，因霍夫变换要求模型参数不超过三个，而三维点云模型参数常超过此限制。RANSAC的优势在于不受模型复杂度或参数数量限制，仅需满足内点比例足够大即可适用。
1.直线拟合
1) 随机选择样本
以直线拟合为例说明RANSAC步骤：
- 第一步需选择能拟合模型的最少数据点数量（称为一个sample），直线拟合需两个点（如p0、p1），因两点可唯一确定一条直线。
2) 求解模型
- 根据样本点（如p0、p1）计算直线参数，采用参数化形式（含变量t）。
- 法向量约束：通过Δx与Δy比例确定参数a、b，并规定法向量模长为1，最终求解直线方程。
- 通用性：RANSAC适用于任意模型，仅需满足根据样本点计算模型参数的条件。
3) 计算误差函数
- 共识计算：通过点到直线距离判断数据点是否支持当前模型，距离公式为向量投影（法向量方向）。
- 内点判定：若距离小于阈值τ，则标记为内点（支持模型），否则为外点。
4) 计算与模型的一致的点
- 统计内点数量：每次采样后记录支持当前模型的内点数（如示例中11个）。
- 迭代优化：重复采样与统计，最终选择内点数量最多的模型作为最优解。
2.总结
1) 优点
- ransac算法具有简单易用的特点
- 在实践应用中表现良好，即使间接比率低至10%仍能有效工作
2) 缺点
- 距离阈值需通过实验确定，缺乏理论依据
- 当间接比率较低时，迭代次数增加会导致算法效率下降
二 代码实践
#!/opt/conda/envs/point-cloud/bin/python
# 文件功能：
# 1. 从数据集中加载点云数据
# 2. 从点云数据中滤除地面点云
# 3. 从剩余的点云中提取聚类
import argparse
import os
import glob
import random
import struct
import numpy as np
# Open3D:
import open3d as o3d
# PCL utils:
import pcl
from utils.segmenter import GroundSegmenter
# sklearn:
from sklearn.cluster import DBSCAN
from itertools import cycle, islice
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# 功能：从kitti的.bin格式点云文件中读取点云
# 输入：
# path: 文件路径
# 输出：
# 点云数组
def read_velodyne_bin(path):
'''
:param path:
:return: homography matrix of the point cloud, N*3
'''
pc_list = []
with open(path, 'rb') as f:
content = f.read()
pc_iter = struct.iter_unpack('ffff', content)
for idx, point in enumerate(pc_iter):
pc_list.append([point[0], point[1], point[2]])
return np.asarray(pc_list, dtype=np.float32)
def ground_segmentation(data):
"""
Segment ground plane from Velodyne measurement
Parameters
----------
data: numpy.ndarray
Velodyne measurements as N-by-3 numpy.ndarray
Returns
----------
segmented_cloud: numpy.ndarray
Segmented surrounding objects as N-by-3 numpy.ndarray
segmented_ground: numpy.ndarray
Segmented ground as N-by-3 numpy.ndarray
"""
# TODO 01 -- ground segmentation
N, _ = data.shape
#
# pre-processing: filter by surface normals
#
# first, filter by surface normal
pcd_original = o3d.geometry.PointCloud()
pcd_original.points = o3d.utility.Vector3dVector(data)
pcd_original.estimate_normals(
search_param=o3d.geometry.KDTreeSearchParamHybrid(
radius=5.0, max_nn=9
)
)
# keep points whose surface normal is approximate to z-axis for ground plane segementation:
normals = np.asarray(pcd_original.normals)
angular_distance_to_z = np.abs(normals[:, 2])
idx_downsampled = angular_distance_to_z > np.cos(np.pi/6)
downsampled = data[idx_downsampled]
#
# plane segmentation with RANSAC
#
# ground segmentation using PLANE RANSAC from PCL:
cloud = pcl.PointCloud()
cloud.from_array(downsampled)
ground_segmenter = GroundSegmenter(cloud=cloud)
inliers, model = ground_segmenter.segment()
#
# post-processing: get ground output by distance to segemented plane
#
distance_to_ground = np.abs(
np.dot(data,np.asarray(model[:3])) + model[3]
)
idx_ground = distance_to_ground <= ground_segmenter.get_max_distance()
idx_segmented = np.logical_not(idx_ground)
segmented_cloud = data[idx_segmented]
segmented_ground = data[idx_ground]
print(
f'[Ground Segmentation]: \n\tnum. origin measurements: {N}\n\tnum. segmented cloud: {segmented_cloud.shape[0]}\n\tnum. segmented ground: {segmented_ground.shape[0]}\n'
)
return segmented_cloud, segmented_ground
def clustering(data):
"""
Segment surrounding objects using DBSCAN
Parameters
----------
data: numpy.ndarray
Segmented point cloud as N-by-3 numpy.ndarray
Returns
----------
cluster_index: list of int
Cluster ID for each point
"""
# TODO 02 -- surrounding object segmentation
cluster_index = DBSCAN(
eps=0.25, min_samples=5, n_jobs=-1
).fit_predict(data)
return cluster_index
def plot_clusters(segmented_ground, segmented_cloud, cluster_index):
"""
Visualize segmentation results using Open3D
Parameters
----------
segmented_cloud: numpy.ndarray
Segmented surrounding objects as N-by-3 numpy.ndarray
segmented_ground: numpy.ndarray
Segmented ground as N-by-3 numpy.ndarray
cluster_index: list of int
Cluster ID for each point
"""
def colormap(c, num_clusters):
"""
Colormap for segmentation result
Parameters
----------
c: int
Cluster ID
C
"""
# outlier:
if c == -1:
color = [1]*3
# surrouding object:
else:
color = [0] * 3
color[c % 3] = c/num_clusters
return color
# ground element:
pcd_ground = o3d.geometry.PointCloud()
pcd_ground.points = o3d.utility.Vector3dVector(segmented_ground)
pcd_ground.colors = o3d.utility.Vector3dVector(
[
[0.372]*3 for i in range(segmented_ground.shape[0])
]
)
# surrounding object elements:
pcd_objects = o3d.geometry.PointCloud()
pcd_objects.points = o3d.utility.Vector3dVector(segmented_cloud)
num_clusters = max(cluster_index) + 1
pcd_objects.colors = o3d.utility.Vector3dVector(
[
colormap(c, num_clusters) for c in cluster_index
]
)
# visualize:
o3d.visualization.draw_geometries([pcd_ground, pcd_objects])
def get_arguments():
"""
Get command-line arguments
"""
# init parser:
parser = argparse.ArgumentParser("Perform ground & surrounding object segmentation on KITTI 3D Object Detection.")
# add required and optional groups:
required = parser.add_argument_group('Required')
# add required:
required.add_argument(
"-i", dest="input", help="Input path of velodyne point cloud.",
required=True
)
required.add_argument(
"-n", dest="num_cases", help="The number of samples.",
required=True, type=int
)
# parse arguments:
return parser.parse_args()
def main(input_dir, num_cases):
pattern = os.path.join(input_dir, '*.bin')
samples = random.sample(glob.glob(pattern), num_cases)
print('KITTI 3D Object Detection Pipeline')
for sample in samples:
print(f'\t Process {sample} ...')
# read Velodyne measurements:
lidar_measurements = read_velodyne_bin(sample)
# segment ground:
segmented_cloud, segmented_ground = ground_segmentation(data=lidar_measurements)
# segment surrouding objects:
cluster_index = clustering(segmented_cloud)
# visualize with Open3D:
plot_clusters(segmented_ground, segmented_cloud, cluster_index)
if __name__ == '__main__':
# parse arguments:
arguments = get_arguments()
# get input point cloud filename:
main(arguments.input, arguments.num_cases)
这段代码 [clustering.py](file:///home/hal18/Downloads/3D-Point-Cloud-Analytics-master/workspace/assignments/04-model-fitting/clustering.py) 的主要功能是对 KITTI 数据集中的激光雷达（LiDAR）点云数据进行处理，具体包括地面分割和周围物体聚类。
以下是对代码各个部分的详细解释：
1. 核心依赖库
numpy: 用于高效的数组运算。- [open3d](file:///home/hal18/Downloads/3D-Point-Cloud-Analytics-master/workspace/assignments/01-introduction/venv/bin/open3d) (
o3d): 用于点云的可视化、法向量估计以及几何处理。 pcl(Python PCL bindings): 用于高性能的点云处理，这里主要用于 RANSAC平面拟合。sklearn.cluster.DBSCAN: 用于基于密度的空间聚类算法，将非地面点划分为不同的物体簇。utils.segmenter.GroundSegmenter: 自定义的地面分割工具类（封装了 PCL 的 RANSAC 功能）。
2. 主要函数详解
[read_velodyne_bin(path)](file:///home/hal18/Downloads/3D-Point-Cloud-Analytics-master/workspace/assignments/04-model-fitting/clustering.py#L33-L44)
- 功能: 读取 KITTI 数据集特有的 `.bin格式点云文件。
- 逻辑:
- 以二进制模式打开文件。
2使用struct.iter_unpack('ffff', content)解析数据。KITTI 的 bin 文件每行包含4个 float32 数值：x, y, z, intensity。 - 只提取前三个值
(x, y, z)存入列表。 - 返回形状为 N×3N \times 3N×3 的 numpy 数组。
- 以二进制模式打开文件。
ground_segmentation(data)
- 功能:从原始点云中分离出“地面点”和“非地面点（周围物体）”。
- 步骤:
- 预处理（法向量过滤）:
- 将数据转换为 Open3D 点云对象。
- 估计每个点的法向量。
- 计算法向量与 Z 轴（垂直方向）的夹角余弦值
angular_distance_to_z。 - 筛选: 保留那些法向量接近垂直向上的点（
> cos(pi/6)，即夹角小于30度）。这通常对应于平坦的地面区域，目的是减少后续 RANSAC 的计算量并提高鲁棒性。
- 平面分割 (RANSAC):
- 将筛选后的点转换为 PCL 点云对象。
- 使用 [GroundSegmenter](file:///home/hal18/Downloads/3D-Point-Cloud-Analytics-master/workspace/assignments/04-model-fitting/utils/segmenter.py#L13-L46)（内部调用 PCL 的 SACSegmentation）进行平面模型拟合。
- 获取平面模型参数
model(Ax+By+Cz+D=0Ax + By + Cz + D = 0Ax+By+Cz+D=0) 和内点索引。
- 后处理（距离判断）:
- 计算所有原始点到拟合平面的距离：distance=∣Ax+By+Cz+D∣distance = |Ax + By + Cz + D|distance=∣Ax+By+Cz+D∣。
- 如果距离小于阈值
ground_segmenter.get_max_distance()，则标记为地面点 (idx_ground)。 - 其余点标记为非地面点/周围物体 (
idx_segmented)。
- 返回:
segmented_cloud(周围物体),segmented_ground(地面)。
- 预处理（法向量过滤）:
[clustering(data)](file:///home/hal18/Downloads/3D-Point-Cloud-Analytics-master/workspace/assignments/04-model-fitting/clustering.py#L113-L133)
- 功能: 对去除地面后的点云进行聚类，识别独立的物体。
- 算法: DBSCAN (Density-Based Spatial Clustering of Applications with Noise)。
- 参数:
eps=0.25: 邻域半径。两点距离小于 0.25 米视为邻居。min_samples=5: 形成核心点所需的最小邻居数。n_jobs=-1: 使用所有 CPU 核心并行计算。
- 返回:
cluster_index，一个整数数组。-1: 表示噪声点（不属于任何簇）。>= 0: 表示该点所属的簇 ID。
plot_clusters(segmented_ground, segmented_cloud, cluster_index)
- 功能: 使用 Open3D 可视化分割结果。
- 颜色映射逻辑 (
colormap):- 地面: 固定为灰色
[0.372, 0.372, 0.372]。 - 噪声点 (-1): 白色
[1, 1, 1]。 - 物体簇:根据簇 ID 动态生成颜色。
color[c % 3] = c / num_clusters: 这是一种简单的着色策略，通过修改 RGB 通道中的某一个分量来区分不同的簇，使得相邻 ID 的簇颜色不同。
- 地面: 固定为灰色
- 可视化: 创建两个 PointCloud 对象（地面和物体），设置颜色，然后调用
o3d.visualization.draw_geometries显示窗口。
[main(input_dir, num_cases)](file:///home/hal18/Downloads/3D-Point-Cloud-Analytics-master/workspace/assignments/04-model-fitting/clustering.py#L217-L233)
- 流程控制:
- 从指定目录随机选取
num_cases个.bin文件。 - 遍历每个文件：
- 读取点云。
- 执行地面分割。
- 执行聚类。
- 弹出窗口展示结果。
- 从指定目录随机选取
3. 代码逻辑总结图
4. 关键点与潜在优化建议
-
法向量预筛选的作用:
- 直接对所有点进行 RANSAC 可能会受到车辆、树木等非平面结构的干扰。先通过法向量筛选出“大致水平”的点，可以显著提高平面拟合的准确性和速度。
-
DBSCAN 参数敏感性:
eps=0.25是一个经验值。如果点云密度变化大（例如远处点稀疏），固定的eps可能导致远处物体被分裂成多个小簇或被视为噪声。在实际生产中，可能需要根据距离自适应调整eps。
-
性能瓶颈:
estimate_normals在 Open3D 中可能较慢。- PCL 的 Python 绑定有时存在内存拷贝开销。
- DBSCAN 的时间复杂度较高，对于大规模点云，可以考虑使用
HDBSCAN或基于体素网格下采样后的聚类。
-
可视化颜色:
- 当前的
colormap比较简单，相邻簇的颜色可能对比度不够明显。可以使用matplotlib的 colormap (如plt.cm.hsv) 来生成更鲜明的区分颜色。
- 当前的
这段代码是一个典型的传统点云处理流水线：去噪/预处理 -> 地面移除 -> 欧氏/密度聚类，常用于自动驾驶场景下的静态障碍物检测初步阶段。
扫一扫
4740
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
