三维点云处理 4.5 拟合: ransac

一、随机样本共识三维点云处理通常采用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.从剩余的点云中提取聚类importargparseimportosimportglobimportrandomimportstructimportnumpyas np#Open3D:importopen3das o3d#PCL utils:importpclfrom utils.segmenterimportGroundSegmenter#sklearn:from sklearn.clusterimportDBSCANfrom itertoolsimportcycle,isliceimportmatplotlib.pyplotas plt from mpl_toolkits.mplot3dimportAxes3D# 功能从kitti的.bin格式点云文件中读取点云 # 输入#path:文件路径# 输出 # 点云数组 defread_velodyne_bin(path)::param path::return:homography matrix of the point cloud,N*3 pc_list[]withopen(path,rb)as f:contentf.read()pc_iterstruct.iter_unpack(ffff,content)foridx,point inenumerate(pc_iter):pc_list.append([point[0],point[1],point[2]])returnnp.asarray(pc_list,dtypenp.float32)defground_segmentation(data): Segment ground plane from Velodyne measurement Parameters----------data:numpy.ndarray Velodyne measurements as N-by-3numpy.ndarray Returns----------segmented_cloud:numpy.ndarray Segmented surrounding objects as N-by-3numpy.ndarray segmented_ground:numpy.ndarray Segmented ground as N-by-3numpy.ndarray#TODO01--ground segmentationN,_data.shape ##pre-processing:filter by surface normals##first,filter by surface normalpcd_originalo3d.geometry.PointCloud()pcd_original.pointso3d.utility.Vector3dVector(data)pcd_original.estimate_normals(search_paramo3d.geometry.KDTreeSearchParamHybrid(radius5.0,max_nn9))#keeppoints whose surface normal is approximate to z-axisforground plane segementation:normalsnp.asarray(pcd_original.normals)angular_distance_to_znp.abs(normals[:,2])idx_downsampledangular_distance_to_znp.cos(np.pi/6)downsampleddata[idx_downsampled]##planesegmentation with RANSAC##groundsegmentationusingPLANE RANSAC from PCL:cloudpcl.PointCloud()cloud.from_array(downsampled)ground_segmenterGroundSegmenter(cloudcloud)inliers,modelground_segmenter.segment()##post-processing:get ground output by distance to segemented plane#distance_to_groundnp.abs(np.dot(data,np.asarray(model[:3]))model[3])idx_grounddistance_to_groundground_segmenter.get_max_distance()idx_segmentednp.logical_not(idx_ground)segmented_clouddata[idx_segmented]segmented_grounddata[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)returnsegmented_cloud,segmented_ground defclustering(data): Segment surrounding objectsusingDBSCAN Parameters----------data:numpy.ndarray Segmented point cloud as N-by-3numpy.ndarray Returns----------cluster_index:list ofintCluster IDforeach point#TODO02--surrounding object segmentationcluster_indexDBSCAN(eps0.25,min_samples5,n_jobs-1).fit_predict(data)returncluster_index defplot_clusters(segmented_ground,segmented_cloud,cluster_index): Visualize segmentation resultsusingOpen3D Parameters----------segmented_cloud:numpy.ndarray Segmented surrounding objects as N-by-3numpy.ndarray segmented_ground:numpy.ndarray Segmented ground as N-by-3numpy.ndarray cluster_index:list ofintCluster IDforeach point defcolormap(c,num_clusters): Colormapforsegmentation result Parameters----------c:intCluster ID C#outlier:ifc-1:color[1]*3#surroudingobject:else:color[0]*3color[c%3]c/num_clustersreturncolor#groundelement:pcd_groundo3d.geometry.PointCloud()pcd_ground.pointso3d.utility.Vector3dVector(segmented_ground)pcd_ground.colorso3d.utility.Vector3dVector([[0.372]*3fori inrange(segmented_ground.shape[0])])#surroundingobject elements:pcd_objectso3d.geometry.PointCloud()pcd_objects.pointso3d.utility.Vector3dVector(segmented_cloud)num_clustersmax(cluster_index)1pcd_objects.colorso3d.utility.Vector3dVector([colormap(c,num_clusters)forc in cluster_index])#visualize:o3d.visualization.draw_geometries([pcd_ground,pcd_objects])defget_arguments(): Get command-line arguments#initparser:parserargparse.ArgumentParser(Perform ground surrounding object segmentation on KITTI 3D Object Detection.)#addrequiredandoptional groups:requiredparser.add_argument_group(Required)#addrequired:required.add_argument(-i,destinput,helpInput path of velodyne point cloud.,requiredTrue)required.add_argument(-n,destnum_cases,helpThe number of samples.,requiredTrue,typeint)#parsearguments:returnparser.parse_args()defmain(input_dir,num_cases):patternos.path.join(input_dir,*.bin)samplesrandom.sample(glob.glob(pattern),num_cases)print(KITTI3D Object Detection Pipeline)forsample in samples:print(f\t Process {sample} ...)#readVelodyne measurements:lidar_measurementsread_velodyne_bin(sample)#segmentground:segmented_cloud,segmented_groundground_segmentation(datalidar_measurements)#segmentsurrouding objects:cluster_indexclustering(segmented_cloud)#visualizewith Open3D:plot_clusters(segmented_ground,segmented_cloud,cluster_index)if__name____main__:#parsearguments:argumentsget_arguments()#getinput 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(AxByCzD0Ax By Cz D 0AxByCzD0) 和内点索引。后处理距离判断:计算所有原始点到拟合平面的距离distance∣AxByCzD∣distance |Ax By Cz D|distance∣AxByCzD∣。如果距离小于阈值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)。参数:eps0.25: 邻域半径。两点距离小于 0.25 米视为邻居。min_samples5: 形成核心点所需的最小邻居数。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. 代码逻辑总结图法向量预筛选PCL RANSAC计算所有点到平面距离是否DBSCAN读取 .bin 文件原始点云 N x 3地面分割 ground_segmentation候选地面点拟合平面模型 AxByCzD0距离 阈值?地面点 segmented_ground周围物体点 segmented_cloud聚类 clustering簇索引 cluster_index可视化 plot_clusters4. 关键点与潜在优化建议法向量预筛选的作用:直接对所有点进行 RANSAC 可能会受到车辆、树木等非平面结构的干扰。先通过法向量筛选出“大致水平”的点可以显著提高平面拟合的准确性和速度。DBSCAN 参数敏感性:eps0.25是一个经验值。如果点云密度变化大例如远处点稀疏固定的eps可能导致远处物体被分裂成多个小簇或被视为噪声。在实际生产中可能需要根据距离自适应调整eps。性能瓶颈:estimate_normals在 Open3D 中可能较慢。PCL 的 Python 绑定有时存在内存拷贝开销。DBSCAN 的时间复杂度较高对于大规模点云可以考虑使用HDBSCAN或基于体素网格下采样后的聚类。可视化颜色:当前的colormap比较简单相邻簇的颜色可能对比度不够明显。可以使用matplotlib的 colormap (如plt.cm.hsv) 来生成更鲜明的区分颜色。这段代码是一个典型的传统点云处理流水线去噪/预处理 - 地面移除 - 欧氏/密度聚类常用于自动驾驶场景下的静态障碍物检测初步阶段。