import open3d as o3d
import numpy as np
import os
def read_pointcloud(file_path):
"""读取点云文件"""
try:
pcd = o3d.io.read_point_cloud(file_path)
print(f"成功读取点云: {file_path}")
print(f"点云点数: {len(pcd.points)}")
return pcd
except Exception as e:
print(f"读取点云失败: {e}")
return None
def get_pointcloud_bounds(pcd):
"""获取点云各轴的范围"""
points = np.asarray(pcd.points)
x_min, y_min, z_min = np.min(points, axis=0)
x_max, y_max, z_max = np.max(points, axis=0)
return {
'x': (x_min, x_max),
'y': (y_min, y_max),
'z': (z_min, z_max)
}
def segment_by_height(pcd, y_interval=0.1):
"""
按高度(Y轴)分割点云
Args:
pcd: 点云对象
y_interval: 高度区间大小(米)
Returns:
分层结果字典: {高度下限: 点云对象}
"""
points = np.asarray(pcd.points)
y_min, y_max = np.min(points[:, 1]), np.max(points[:, 1])
# 计算层数
num_layers = int(np.ceil((y_max - y_min) / y_interval))
layers = {}
for i in range(num_layers):
lower = y_min + i * y_interval
upper = lower + y_interval
# 创建掩码，筛选当前层的点
mask = (points[:, 1] >= lower) & (points[:, 1] < upper)
layer_points = points[mask]
if len(layer_points) > 0:
layer_pcd = o3d.geometry.PointCloud()
layer_pcd.points = o3d.utility.Vector3dVector(layer_points)
layers[lower] = layer_pcd
print(f"按高度分割完成，共{len(layers)}层")
return layers
def process_layers(layers, z_interval=0.2, x_interval=0.2):
"""
处理每层点云，按Z轴分块，每块内按X轴排序，并计算块的平均坐标
Args:
layers: 分层结果字典
z_interval: Z轴(深度)分块大小(米)
x_interval: X轴(水平)分块大小(米)
Returns:
处理结果字典: {(高度下限, 深度下限, 水平下限): {
"point_count": 点数,
"avg_coords": 平均坐标(x,y,z)
}}
"""
processed_blocks = {}
# 按高度从高到低处理
sorted_layers = sorted(layers.keys(), reverse=True)
for y_lower in sorted_layers:
layer_pcd = layers[y_lower]
layer_points = np.asarray(layer_pcd.points)
# 获取当前层Z轴范围
z_min, z_max = np.min(layer_points[:, 2]), np.max(layer_points[:, 2])
# 按Z轴(深度)从远到近分块
num_z_blocks = int(np.ceil((z_max - z_min) / z_interval))
for i in range(num_z_blocks):
z_lower = z_min + i * z_interval
z_upper = z_lower + z_interval
# 创建Z轴掩码
z_mask = (layer_points[:, 2] >= z_lower) & (layer_points[:, 2] < z_upper)
z_block_points = layer_points[z_mask]
if len(z_block_points) > 0:
# 获取当前块X轴范围
x_min, x_max = np.min(z_block_points[:, 0]), np.max(z_block_points[:, 0])
# 按X轴(水平)从右到左分块
num_x_blocks = int(np.ceil((x_max - x_min) / x_interval))
for j in range(num_x_blocks):
x_lower = x_min + j * x_interval
x_upper = x_lower + x_interval
# 创建X轴掩码
x_mask = (z_block_points[:, 0] >= x_lower) & (z_block_points[:, 0] < x_upper)
x_block_points = z_block_points[x_mask]
if len(x_block_points) > 0:
# 计算块的平均坐标
avg_coords = np.mean(x_block_points, axis=0)
# 保存块信息，包含点数和平均坐标
block_key = (y_lower, z_lower, x_lower)
processed_blocks[block_key] = {
"point_count": len(x_block_points),
"avg_coords": avg_coords
}
print(f"处理完成，共{len(processed_blocks)}个块")
return processed_blocks
def print_block_statistics(processed_blocks):
"""打印每个块的统计信息，包含平均坐标"""
if not processed_blocks:
print("没有可统计的块数据")
return
print("\n=== 块统计结果 ===")
print("-" * 70)
print("高度下限(m)\t深度下限(m)\t水平下限(m)\t点数\t\t平均坐标(x,y,z)")
print("-" * 70)
# 按高度从高到低、深度从远到近、水平从右到左排序
sorted_blocks = sorted(
processed_blocks.items(),
key=lambda x: (-x[0][0], x[0][1], x[0][2]) # 负号表示降序
)
for i, (block_key, block_data) in enumerate(sorted_blocks):
y_lower, z_lower, x_lower = block_key
point_count = block_data["point_count"]
avg_x, avg_y, avg_z = block_data["avg_coords"]
print(
f"{y_lower:.2f}\t\t{z_lower:.2f}\t\t{x_lower:.2f}\t\t{point_count}\t\t({avg_x:.2f}, {avg_y:.2f}, {avg_z:.2f})")
print("-" * 70)
total_blocks = len(processed_blocks)
total_points = sum(block["point_count"] for block in processed_blocks.values())
print(f"总计块数: {total_blocks}")
print(f"总计点数: {total_points}")
def save_block_statistics(processed_blocks, output_file="block_statistics.txt"):
"""将块统计信息保存到文件，包含平均坐标"""
if not processed_blocks:
print("没有可保存的块数据")
return
try:
with open(output_file, 'w') as f:
f.write("=== 块统计结果 ===\n")
f.write("-" * 70 + "\n")
f.write("高度下限(m)\t深度下限(m)\t水平下限(m)\t点数\t\t平均坐标(x,y,z)\n")
f.write("-" * 70 + "\n")
# 按高度从高到低、深度从远到近、水平从右到左排序
sorted_blocks = sorted(
processed_blocks.items(),
key=lambda x: (-x[0][0], x[0][1], x[0][2])
)
for (block_key, block_data) in sorted_blocks:
y_lower, z_lower, x_lower = block_key
point_count = block_data["point_count"]
avg_x, avg_y, avg_z = block_data["avg_coords"]
f.write(
f"{y_lower:.2f}\t\t{z_lower:.2f}\t\t{x_lower:.2f}\t\t{point_count}\t\t({avg_x:.2f}, {avg_y:.2f}, {avg_z:.2f})\n")
f.write("-" * 70 + "\n")
f.write(f"总计块数: {len(processed_blocks)}\n")
f.write(f"总计点数: {sum(block['point_count'] for block in processed_blocks.values())}\n")
print(f"统计结果已保存到: {output_file}")
except Exception as e:
print(f"保存统计结果失败: {e}")
def main():
"""主函数"""
# 设置参数
FILE_PATH = "pointcloud_data/pointcloud_20250703_144541.pcd" # 修改为实际文件路径
Y_INTERVAL = 1 # 高度(Y轴)分层间隔(米)
Z_INTERVAL = 1 # 深度(Z轴)分块间隔(米)
X_INTERVAL = 1 # 水平(X轴)分块间隔(米)
OUTPUT_FILE = "block_statistics.txt" # 统计结果输出文件
print("\n=== 点云码垛处理工具 ===")
# 读取点云
pcd = read_pointcloud(FILE_PATH)
if not pcd:
return
# 获取点云范围
bounds = get_pointcloud_bounds(pcd)
print("\n点云范围:")
print(f"X轴(水平): {bounds['x'][0]:.2f}m ~ {bounds['x'][1]:.2f}m")
print(f"Y轴(高度): {bounds['y'][0]:.2f}m ~ {bounds['y'][1]:.2f}m")
print(f"Z轴(深度): {bounds['z'][0]:.2f}m ~ {bounds['z'][1]:.2f}m")
# 按高度分层
print(f"\n正在按高度分层 (间隔: {Y_INTERVAL}m)...")
layers = segment_by_height(pcd, Y_INTERVAL)
# 处理每层，按深度和水平位置分块，并计算平均坐标
print(f"\n正在按深度和水平位置分块 (深度间隔: {Z_INTERVAL}m, 水平间隔: {X_INTERVAL}m)...")
processed_blocks = process_layers(layers, Z_INTERVAL, X_INTERVAL)
# 打印块统计信息
print_block_statistics(processed_blocks)
# 保存块统计信息到文件
save_block_statistics(processed_blocks, OUTPUT_FILE)
print("\n=== 处理完成 ===")
if __name__ == "__main__":
main()基于这个代码写一个过滤点云块的代码，要求距离大于0.55的和小于0.3的过滤
本文探讨了在计算机图形学和计算机视觉中，如何进行空间坐标转换，特别是布尔沙参数到点云的转换。布尔沙参数常用于表示三维旋转，避免万向节死锁，而在点云数据表示中，需要对位置和方向进行转换。文中提供了一个函数，通过该函数可以将布尔沙参数应用于点云，改变点云在不同坐标系的位置和方向。
订阅专栏 解锁全文
扫一扫
797
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
