|
|
import numpy as np
|
|
|
from PIL import Image
|
|
|
import math
|
|
|
|
|
|
import config
|
|
|
from cat import clip_and_rotate_point_cloud, merge_point_clouds
|
|
|
from image import detect_large_holes
|
|
|
|
|
|
|
|
|
def tiff_depth_to_point_clouds(tiff_paths, sn, dedup=True):
|
|
|
"""
|
|
|
将多个 TIFF 深度图转换为点云，并拼接在一起后生成 PCD 文件
|
|
|
|
|
|
:param tiff_paths: list of str, TIFF 深度图路径列表
|
|
|
:param sn: str, 设备序列号，用于查找配置参数
|
|
|
:param dedup: bool, 是否根据 (x, y) 去重，默认开启
|
|
|
:return: list of [x, y, z], 合并后的点云数据
|
|
|
"""
|
|
|
merged_points = []
|
|
|
|
|
|
# Step 1: 遍历每个 TIFF 文件，生成对应的点云
|
|
|
for tiff_path in tiff_paths:
|
|
|
points = tiff_depth_to_point_cloud(tiff_path+".tiff", sn, dedup)
|
|
|
merged_points.extend([points])
|
|
|
|
|
|
# Step 2: 可选：使用 merge_point_clouds 对点云进行 x 轴拼接（若需要）
|
|
|
# 注意：如果每个点云已经带有偏移，则不需要再次拼接
|
|
|
has_voids = True
|
|
|
if hasattr(config, 'CUT_CONFIG_MAP') and isinstance(config.CUT_CONFIG_MAP, dict) and (sn in config.CUT_CONFIG_MAP):
|
|
|
merged_points, xrange = merge_point_clouds(merged_points, config.CUT_CONFIG_MAP[sn],sn)
|
|
|
|
|
|
real_holes = detect_large_holes(merged_points, sn, xrange)
|
|
|
if real_holes.__len__()==0:
|
|
|
has_voids=False
|
|
|
# Step 3: 写入最终的 PCD 文件
|
|
|
output_pcd_path = config.save_path("pcd", f"{sn}_merged.pcd")
|
|
|
write_pcd(output_pcd_path, merged_points)
|
|
|
# print(f"Merged and saved to {output_pcd_path}")
|
|
|
return has_voids
|
|
|
|
|
|
|
|
|
def tiff_depth_to_point_cloud(tiff_path,sn, dedup=True):
|
|
|
"""
|
|
|
将 TIFF 深度图转换为点云，基于给定的视场角计算相机内参
|
|
|
|
|
|
:param tiff_path: str, TIFF 深度图路径
|
|
|
:param hfov_degrees: float or None, 水平视场角（单位：度）
|
|
|
:param vfov_degrees: float or None, 垂直视场角（单位：度）
|
|
|
:param output_ply_path: str or None, 输出 Pcd 文件路径（可选）
|
|
|
:param dedup: bool, 是否根据 (x, y) 去重，默认开启
|
|
|
:return: list of [x, y, z] 点云数据
|
|
|
"""
|
|
|
hfov_degrees = config.CAMERA_CONFIG_MAP[sn].get("x_angle")
|
|
|
max_z = config.CAMERA_CONFIG_MAP[sn].get("max_z")
|
|
|
vfov_degrees = config.CAMERA_CONFIG_MAP[sn].get("y_angle")
|
|
|
# plane_scaling_ratio = config.CAMERA_CONFIG_MAP[sn].get("plane_scaling_ratio")
|
|
|
# 加载 TIFF 图像
|
|
|
depth_image = Image.open(tiff_path)
|
|
|
depth_array = np.array(depth_image, dtype=np.float32)
|
|
|
|
|
|
height, width = depth_array.shape
|
|
|
|
|
|
# 根据提供的 FOV 计算 fx/fy
|
|
|
if hfov_degrees is not None:
|
|
|
hfov_rad = math.radians(hfov_degrees)
|
|
|
fx = width / (2 * math.tan(hfov_rad / 2))
|
|
|
fy = fx * height / width # 保持像素方形比例
|
|
|
else:
|
|
|
vfov_rad = math.radians(vfov_degrees)
|
|
|
fy = height / (2 * math.tan(vfov_rad / 2))
|
|
|
fx = fy * width / height # 保持像素方形比例
|
|
|
|
|
|
cx = width / 2
|
|
|
cy = height / 2
|
|
|
|
|
|
print(f"Calculated intrinsics: fx={fx:.2f}, fy={fy:.2f}, cx={cx:.2f}, cy={cy:.2f}")
|
|
|
|
|
|
points = []
|
|
|
seen_xy = set() # 用于记录已添加的 (x, y)
|
|
|
|
|
|
# 获取裁剪配置
|
|
|
clip_config = config.CUT_CONFIG_MAP.get(sn)
|
|
|
if clip_config:
|
|
|
min_pt = np.array(clip_config.get("min_pt", [-np.inf] * 3))
|
|
|
max_pt = np.array(clip_config.get("max_pt", [np.inf] * 3))
|
|
|
rotation = np.array(clip_config.get("rotation", [1,0,0,0,1,0,0,0,1])).reshape(3, 3)
|
|
|
else:
|
|
|
min_pt = max_pt = rotation = None
|
|
|
|
|
|
for v in range(height):
|
|
|
for u in range(width):
|
|
|
z = depth_array[v, u] # mm -> m
|
|
|
if z <= 0 or z > max_z:
|
|
|
continue
|
|
|
|
|
|
x = (u - cx) * z / fx
|
|
|
y = (v - cy) * z / fy
|
|
|
|
|
|
# 构造点并旋转（保留浮点精度）
|
|
|
point = np.array([x, y, z])
|
|
|
if clip_config:
|
|
|
rotated_point = (rotation @ point.T).T
|
|
|
if not np.all(rotated_point >= min_pt) or not np.all(rotated_point <= max_pt):
|
|
|
continue
|
|
|
x_final, y_final, z_final = rotated_point
|
|
|
else:
|
|
|
x_final, y_final, z_final = point
|
|
|
|
|
|
# 到这里才进行去重判断
|
|
|
if dedup:
|
|
|
x_int = int(round(x_final))
|
|
|
y_int = int(round(y_final))
|
|
|
z_int = int(round(z_final))
|
|
|
if (x_int, y_int) in seen_xy:
|
|
|
continue
|
|
|
seen_xy.add((x_int, y_int))
|
|
|
# 保留浮点精度写入结果
|
|
|
points.append([x_int, y_int, z_int])
|
|
|
|
|
|
|
|
|
# 可选输出PLY文件用于可视化
|
|
|
if config.CAMERA_CONFIG_MAP[sn].get("save_pcd"):
|
|
|
output_ply_path = config.save_path("pcd",sn+".pcd")
|
|
|
write_pcd(output_ply_path, points)
|
|
|
|
|
|
return points
|
|
|
|
|
|
|
|
|
# 新增点云数据转换函数
|
|
|
def convert_sdk_points(sValue, width, height):
|
|
|
"""
|
|
|
将 SDK 原始点云数据转换为 NumPy 点云数组
|
|
|
|
|
|
:param sValue: tuple(float), SDK 输出的扁平化点云数据
|
|
|
:param width: int, 点云宽度
|
|
|
:param height: int, 点云高度
|
|
|
:return: np.ndarray(shape=(N,3)), 三维点云数组
|
|
|
"""
|
|
|
points = np.array(sValue).reshape(-1, 3) # 形状转换 (N,3)
|
|
|
# 剔除无效点（X/Y/Z 任一为 NaN 或 Inf）
|
|
|
mask = np.all(np.isfinite(points), axis=1)
|
|
|
return points[mask]
|
|
|
|
|
|
|
|
|
def process_point_cloud(points, sn, dedup=True):
|
|
|
"""
|
|
|
对原始点云应用旋转、裁剪和去重
|
|
|
|
|
|
:param points: np.ndarray(shape=(N,3)), 原始点云数据
|
|
|
:param sn: str, 设备序列号（用于加载配置）
|
|
|
:param dedup: bool, 是否启用去重
|
|
|
:return: list of [x, y, z], 处理后的点云列表
|
|
|
"""
|
|
|
# 加载配置参数
|
|
|
clip_config = config.CUT_CONFIG_MAP.get(sn)
|
|
|
if clip_config:
|
|
|
min_pt = np.array(clip_config.get("min_pt", [-np.inf] * 3))
|
|
|
max_pt = np.array(clip_config.get("max_pt", [np.inf] * 3))
|
|
|
rotation = np.array(clip_config.get("rotation", [1, 0, 0, 0, 1, 0, 0, 0, 1])).reshape(3, 3)
|
|
|
else:
|
|
|
min_pt = max_pt = rotation = None
|
|
|
|
|
|
processed_points = []
|
|
|
seen_xy = set()
|
|
|
|
|
|
for point in points:
|
|
|
x, y, z = point
|
|
|
|
|
|
# 无效点过滤（Z ≤ 0 或超出最大距离）
|
|
|
if z <= 0 or z > config.CAMERA_CONFIG_MAP[sn].get("max_z", np.inf):
|
|
|
continue
|
|
|
|
|
|
# 应用旋转矩阵
|
|
|
if clip_config:
|
|
|
rotated = rotation @ point
|
|
|
if not np.all(rotated >= min_pt) or not np.all(rotated <= max_pt):
|
|
|
continue
|
|
|
x_final, y_final, z_final = rotated
|
|
|
else:
|
|
|
x_final, y_final, z_final = x, y, z
|
|
|
|
|
|
# 去重逻辑（保留浮点精度）
|
|
|
if dedup:
|
|
|
# 使用浮点哈希避免离散化损失
|
|
|
key = (round(x_final, 3), round(y_final, 3))
|
|
|
if key in seen_xy:
|
|
|
continue
|
|
|
seen_xy.add(key)
|
|
|
|
|
|
processed_points.append([x_final, y_final, z_final])
|
|
|
|
|
|
return processed_points
|
|
|
|
|
|
|
|
|
def sValue_to_pcd(sValue,stPointCloudImage,sn):
|
|
|
# 数据格式转换
|
|
|
points = convert_sdk_points(sValue, stPointCloudImage.nWidth, stPointCloudImage.nHeight)
|
|
|
|
|
|
# 应用旋转、裁剪、去重（假设设备序列号已知）
|
|
|
processed_points = process_point_cloud(points, sn, dedup=True)
|
|
|
|
|
|
output_ply_path = config.save_path("pcd", sn + ".pcd")
|
|
|
# 保存结果
|
|
|
write_pcd(output_ply_path, processed_points)
|
|
|
|
|
|
|
|
|
def write_pcd(filename, points):
|
|
|
"""
|
|
|
将点云写入 PCD 文件
|
|
|
|
|
|
:param filename: str, 输出 PCD 文件路径
|
|
|
:param points: list of [x, y, z], 点云数据
|
|
|
"""
|
|
|
with open(filename, 'w') as f:
|
|
|
# 写入 PCD 文件头
|
|
|
f.write("# .PCD v0.7 - Point Cloud Data file format\n")
|
|
|
f.write(f"VERSION 0.7\n")
|
|
|
f.write("FIELDS x y z\n")
|
|
|
f.write("SIZE 4 4 4\n") # float 类型的大小是 4 字节
|
|
|
f.write("TYPE F F F\n") # 每个字段的数据类型
|
|
|
f.write(f"COUNT 1 1 1\n") # 每个字段的数量
|
|
|
f.write(f"WIDTH {len(points)}\n") # 点数量
|
|
|
f.write("HEIGHT 1\n") # 单行表示无结构点云
|
|
|
f.write("VIEWPOINT 0 0 0 1 0 0 0\n") # 默认视角参数
|
|
|
f.write(f"POINTS {len(points)}\n") # 总点数
|
|
|
f.write("DATA ascii\n") # 数据部分以 ASCII 形式存储
|
|
|
|
|
|
# 写入点数据
|
|
|
for point in points:
|
|
|
f.write(f"{point[0]} {point[1]} {point[2]}\n")
|
|
|
|
|
|
print(f"Saved point cloud to {filename}")
|
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
tiff_depth_to_point_cloud("D:/git/test/hik3d-python/image/2025-06-26/depth/191330147_-Depth.tiff", "00DA6823936")
|