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")