目录
安装依赖项：
pip install CSF
windows
pip install csf
import csf
csf=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-1
if max_val > min_val:
# 归一化到0-255
gray_values = (intensity - min_val) * 255.0 / (max_val - min_val)
else:
gray_values = np.zeros_like(intensity)
# 然后除以255，得到0-1
gray_values = (gray_values)+125
normalized_gray = gray_values / 255.0
rgb_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_bound
ground_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_pcd
def _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_pcd
def 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_pcd
non_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.5
opt.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_pcd
cluster_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) 且灰度值 > threshold
gray_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 = pcd
for _ 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 - centroid
projections = np.dot(vectors, direction)
distances = np.linalg.norm(vectors - np.outer(projections, direction), axis=1)
# 筛选内点 (距离小于阈值)
inlier_mask = distances < distance_threshold
inlier_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)
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
