点云提取车道线 识别车道线
目录
安装依赖项：
windows
1. 提取地面
2. 提取车道线
安装依赖项：
pip install CSF
windows
pip install csf
import csfcsf=csf.csf(points)1. 提取地面
import numpy as np
import laspy
import open3d as o3d
# import CSF
import csf
import matplotlib.cm as cm# ====================== 1. 读取LAS文件 ======================
def read_las_to_open3d(file_path):"""读取LAS文件并转换为Open3D点云"""las = laspy.read(file_path)xyz = np.vstack((las.x, las.y, las.z)).transpose()pcd = o3d.geometry.PointCloud()pcd.points = o3d.utility.Vector3dVector(xyz)# 反射强度处理if hasattr(las, 'intensity'):intensity = np.array(las.intensity)# 归一化处理min_val, max_val = np.min(intensity), np.max(intensity)# 另一种做法：将原始强度值归一化到0-255，再除以255得到0-1if max_val > min_val:# 归一化到0-255gray_values = (intensity - min_val) * 255.0 / (max_val - min_val)else:gray_values = np.zeros_like(intensity)# 然后除以255，得到0-1gray_values = (gray_values)+125normalized_gray = gray_values / 255.0rgb_colors = np.repeat(normalized_gray[:, np.newaxis], 3, axis=1)else:# 无反射强度时使用黑色rgb_colors = np.zeros((len(xyz), 3))# 设置点云颜色pcd.colors = o3d.utility.Vector3dVector(rgb_colors)# # 检查并转换颜色# if hasattr(las, 'red'):# rgb = np.vstack((las.red, las.green, las.blue)).T# # 若值超过 255，转为 8 位# if rgb.max() > 255:# rgb = (rgb >> 8).astype(np.uint8)# normalized_rgb = rgb / 255.0# else:# normalized_rgb = np.zeros((len(xyz), 3)) # 无颜色时设为黑色# # 创建 Open3D 点云对象# pcd.colors = o3d.utility.Vector3dVector(normalized_rgb)# 可视化o3d.visualization.draw_geometries([pcd])return pcd# ====================== 2. 地面点云提取算法 ======================
def extract_ground(pcd, method="region_ransac", **kwargs):"""提取最大地平面（二选一方法）参数：method: "region_ransac"（复杂地形） 或 "csf"（平坦/斜坡地形）kwargs: 算法参数（见下方说明）"""# 方法1：区域化RANSAC（适合起伏地形）if method == "region_ransac":return _region_ransac(pcd, voxel_size=kwargs.get('voxel_size', 0.1),distance_threshold=kwargs.get('distance_threshold', 0.2),region_size=kwargs.get('region_size', 5.0))# 方法2：布料模拟滤波（CSF）（适合连续斜坡）elif method == "csf":return _csf_filter(pcd, rigidness=kwargs.get('rigidness', 3),cloth_resolution=kwargs.get('cloth_resolution', 0.5),height_threshold=kwargs.get('height_threshold', 0.15))def _region_ransac(pcd, voxel_size, distance_threshold, region_size):"""区域化RANSAC地面分割"""# 下采样与去噪# down_pcd = pcd.voxel_down_sample(voxel_size)# cl, ind = pcd.remove_statistical_outlier(nb_neighbors=100, std_ratio=2.0)# filtered_pcd = pcd.select_by_index(ind)filtered_pcd = pcd# 分区域处理bbox = filtered_pcd.get_axis_aligned_bounding_box()min_bound, max_bound = bbox.min_bound, bbox.max_boundground_points = []ground_colors = []# 网格划分（避免全局拟合失效）x_bins = np.arange(min_bound[0], max_bound[0], region_size)y_bins = np.arange(min_bound[1], max_bound[1], region_size)for i in range(len(x_bins)-1):for j in range(len(y_bins)-1):# 裁剪当前区域area_box = o3d.geometry.AxisAlignedBoundingBox(min_bound=[x_bins[i], y_bins[j], min_bound[2]],max_bound=[x_bins[i+1], y_bins[j+1], max_bound[2]])area_pcd = filtered_pcd.crop(area_box)if len(area_pcd.points) < 1500: continue# 局部平面拟合plane_model, inliers = area_pcd.segment_plane(distance_threshold, ransac_n=3, num_iterations=500)# 法向量验证（Z分量>0.8为地面）normal = np.array(plane_model[:3])if normal[2] > 0.9: selected = area_pcd.select_by_index(inliers)ground_points.append(selected.points)ground_colors.append(selected.colors)# 合并地面点ground_pcd = o3d.geometry.PointCloud()ground_pcd.points = o3d.utility.Vector3dVector(np.vstack(ground_points))ground_pcd.colors = o3d.utility.Vector3dVector(np.vstack(ground_colors))# 可视化o3d.visualization.draw_geometries([ground_pcd])return ground_pcddef _csf_filter(pcd, rigidness, cloth_resolution, height_threshold):"""布料模拟滤波（CSF）[6](@ref)"""# 将Open3D点云转为CSF所需格式points = np.asarray(pcd.points)# csf = CSF.CSF()csf=csf.csf(points)# csf.setPointCloud(points)# 参数设置csf.params.bSloopSmooth = True # 启用坡度平滑csf.params.rigidness = rigidness # 布料刚度（1-3：软，适合山地；>3：硬，适合城市）csf.params.resolution = cloth_resolution # 布料网格分辨率（米）csf.params.height_threshold = height_threshold # 地面点高度阈值# 执行滤波ground_indices = CSF.VecInt() # 表示计算后地面点索引的列表non_ground_indices = CSF.VecInt() # 表示计算后非地面点索引的列表csf.do_filtering(ground_indices, non_ground_indices) # 执行实际过滤。# ground_indices = csf.getGroundIndices()# 提取地面点ground_points = points[ground_indices]ground_pcd = o3d.geometry.PointCloud()ground_pcd.points = o3d.utility.Vector3dVector(ground_points)return ground_pcddef cluster_and_filter_ground(ground_pcd, min_cluster_points=100, eps=0.3, dynamic_threshold=False):labels = np.array(ground_pcd.cluster_dbscan(eps=0.5, # 邻域半径（单位：米）min_points=10, # 最小核心点数print_progress=True # 显示进度))# 解析结果max_label = labels.max()print(f"聚类数量：{max_label + 1}")print(f"噪声点数量：{np.sum(labels == -1)}")# 可视化：不同簇随机着色colors = np.random.rand(max_label + 1, 3) # 生成随机颜色pcd_colors = np.zeros((len(labels), 3))print(labels)for i in range(len(labels)):if labels[i] >= 0: # 非噪声点pcd_colors[i] = colors[labels[i]]# ground_pcd.colors = o3d.utility.Vector3dVector(pcd_colors)# o3d.visualization.draw_geometries([ground_pcd])# 统计非噪声簇的点数分布（噪声标签为-1）unique_labels, counts = np.unique(labels[labels >= 0], return_counts=True)if len(unique_labels) == 0:max_cluster_indices = np.array([]) # 无有效簇时返回空集else:# 找到点数最多的簇标签max_cluster_label = unique_labels[np.argmax(counts)]# 提取该簇所有点的索引max_cluster_indices = np.where(labels == max_cluster_label)[0]# 创建仅包含最大簇的点云max_cluster_pcd = ground_pcd.select_by_index(max_cluster_indices)# max_cluster_pcd.paint_uniform_color([0.5, 0.5, 0.5])o3d.visualization.draw_geometries([max_cluster_pcd])return labels# ====================== 3. 可视化与保存 ======================
def visualize_ground(original_pcd, ground_pcd):"""可视化原始点云与地面点云"""# 着色：地面=绿色，非地面=灰色# ground_pcd.paint_uniform_color([0.2, 0.8, 0.2])non_ground = original_pcd + ground_pcdnon_ground.paint_uniform_color([0.7, 0.7, 0.7])# 可视化vis = o3d.visualization.Visualizer()vis.create_window(window_name="最大地平面提取")vis.add_geometry(ground_pcd)# vis.add_geometry(non_ground)# 视角设置ctr = vis.get_view_control()ctr.set_front([0, -1, 0.5]) # 俯视30度角ctr.set_lookat(original_pcd.get_center())# 渲染配置opt = vis.get_render_option()opt.point_size = 1.5opt.background_color = np.array([0.1, 0.1, 0.1])print("按ESC退出可视化")vis.run()vis.destroy_window()# ====================== 主流程 ======================
if __name__ == "__main__":# 参数配置las_file = r"B:\360MoveData\Users\Administrator\Desktop\tmp\1026\las-detect\input\street.las"# 1. 读取点云pcd = read_las_to_open3d(las_file)# 2. 选择地面提取方法（二选一）# 方法A：区域化RANSAC（适合山地、丘陵）ground_pcd = extract_ground(pcd, method="region_ransac",voxel_size=0.15, # 下采样尺寸（米）distance_threshold=0.2, # 点到平面距离阈值region_size=6.0 # 网格大小)# # # 方法B：CSF滤波（适合城市道路、连续斜坡）# # ground_pcd = extract_ground(# # pcd,# # method="csf",# # rigidness=2, # 刚度（1软-5硬）# # cloth_resolution=0.5, # 布料网格分辨率# # height_threshold=0.15 # 地面点高度阈值# # )# 3. 可视化与保存# visualize_ground(pcd, ground_pcd)# 假设已通过地面分割获得 ground_pcdcluster_labels = cluster_and_filter_ground(ground_pcd,min_cluster_points=1500, # 最小点数阈值（低于此值视为噪声）eps=0.25, # 邻域半径基准值dynamic_threshold=True # 启用动态阈值（推荐远距离场景）)print('start save ground')o3d.io.write_point_cloud("ground.ply", ground_pcd) # 保存地面点云2. 提取车道线
import open3d as o3d
import numpy as np
from sklearn.decomposition import PCA# 1. 读取点云数据
pcd = o3d.io.read_point_cloud("ground.ply")# 2. 基于灰度值分割点云 (r=g=b且灰度>125)
def grayscale_segmentation(pcd, threshold=125):colors = np.asarray(pcd.colors)# 检测灰度点 (r=g=b) 且灰度值 > thresholdgray_mask = (colors[:, 0] == colors[:, 1]) & (colors[:, 1] == colors[:, 2]) & (colors[:, 0] > threshold/255.0)gray_indices = np.where(gray_mask)[0]return pcd.select_by_index(gray_indices), pcd.select_by_index(np.where(~gray_mask)[0])# 3. 迭代拟合多条直线 (RANSAC改进版)
def fit_multiple_lines(pcd, max_lines=5, distance_threshold=0.02):lines = []line_points_set = []remaining_pcd = pcdfor _ in range(max_lines):if len(remaining_pcd.points) < 5: # 点数不足时停止break# PCA拟合直线方向 [2,9](@ref)points = np.asarray(remaining_pcd.points)pca = PCA(n_components=3)pca.fit(points)direction = pca.components_[0]centroid = np.mean(points, axis=0)# 计算点到直线的距离 [1](@ref)vectors = points - centroidprojections = np.dot(vectors, direction)distances = np.linalg.norm(vectors - np.outer(projections, direction), axis=1)# 筛选内点 (距离小于阈值)inlier_mask = distances < distance_thresholdinlier_indices = np.where(inlier_mask)[0]if len(inlier_indices) == 0:break# 保存当前直线参数和内点line_params = {"centroid": centroid,"direction": direction,"inliers": remaining_pcd.select_by_index(inlier_indices)}lines.append(line_params)line_points_set.append(line_params["inliers"])# 移除已拟合的点 [3](@ref)outlier_indices = np.where(~inlier_mask)[0]remaining_pcd = remaining_pcd.select_by_index(outlier_indices)return lines, line_points_set, remaining_pcd# 4. 可视化结果
def visualize_results(original_pcd, line_points_set, remaining_pcd):# 原始点云设为灰色original_pcd.paint_uniform_color([0.5, 0.5, 0.5])# 为每条直线分配不同颜色colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1],[1, 1, 0], [1, 0, 1]]for i, line_pcd in enumerate(line_points_set):line_pcd.paint_uniform_color(colors[i % len(colors)])# 合并可视化对象# vis_objects = [original_pcd] + line_points_set + [remaining_pcd]vis_objects = line_points_set + [remaining_pcd]o3d.visualization.draw_geometries(vis_objects)# 主流程
if __name__ == "__main__":# 灰度分割gray_pcd,remaining_pcd = grayscale_segmentation(pcd, threshold=145)gray_pcd.paint_uniform_color([0.5, 0.5, 0.5])o3d.visualization.draw_geometries([gray_pcd])# 拟合多条直线lines, line_points_set, remaining_pcd = fit_multiple_lines(gray_pcd, max_lines=5, distance_threshold=5)# 输出结果print(f"拟合到 {len(lines)} 条直线")for i, line in enumerate(lines):print(f"直线 {i+1}: 中心点={line['centroid']}, 方向向量={line['direction']}")# 可视化visualize_results(pcd, line_points_set, remaining_pcd)