|
|
from datetime import time
|
|
|
|
|
|
import numpy as np
|
|
|
from PIL import Image
|
|
|
import math
|
|
|
|
|
|
import config
|
|
|
import asyncio
|
|
|
|
|
|
|
|
|
import numpy as np
|
|
|
from scipy.spatial.transform import Rotation as R
|
|
|
|
|
|
from logConfig import get_logger
|
|
|
|
|
|
# 使用示例
|
|
|
logger = get_logger()
|
|
|
|
|
|
def apply_rotation(matrix, angle_degrees, axis='x'):
|
|
|
"""
|
|
|
对输入的 3x3 旋转矩阵 应用额外的旋转
|
|
|
|
|
|
参数:
|
|
|
matrix (np.ndarray): 原始的 3x3 旋转矩阵
|
|
|
angle_degrees (float): 旋转角度 (单位: 度)
|
|
|
axis (str): 旋转轴，支持 'x', 'y', 'z'
|
|
|
|
|
|
返回:
|
|
|
np.ndarray: 新的 3x3 旋转矩阵
|
|
|
"""
|
|
|
# 创建绕指定轴旋转的旋转对象
|
|
|
rot = R.from_euler(axis, angle_degrees, degrees=True)
|
|
|
|
|
|
# 将当前旋转转换为矩阵
|
|
|
rotation_matrix = rot.as_matrix()
|
|
|
|
|
|
# 合并旋转：新旋转 × 原始矩阵
|
|
|
combined_rotation = np.dot(rotation_matrix, matrix)
|
|
|
|
|
|
return combined_rotation
|
|
|
|
|
|
#
|
|
|
# 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_fast(points, sn, type):
|
|
|
# points = np.array(points, dtype=np.float32)
|
|
|
# # 获取目标高度和容差（来自模板配置）
|
|
|
# template_config = config.TEMPLATE_CONFIG_MAP.get(type, {})
|
|
|
# target_z = template_config.get("height", 0) # 假设 height 是期望的中心高度
|
|
|
# tolerance = template_config.get("tolerance", 50) # 默认容差为 50
|
|
|
#
|
|
|
# # 计算上下限
|
|
|
# z_min = target_z - tolerance
|
|
|
# z_max = target_z + tolerance # 不超过 200
|
|
|
# # Z 值过滤
|
|
|
# valid_mask = ((points[:, 2] > 0)
|
|
|
# & (points[:, 2] <= config.CAMERA_CONFIG_MAP[sn].get("max_z", np.inf))
|
|
|
# )
|
|
|
#
|
|
|
# points = points[valid_mask]
|
|
|
#
|
|
|
# if clip_config := config.CUT_CONFIG_MAP.get(sn):
|
|
|
# rotation = np.array(clip_config["rotation"], dtype=np.float32).reshape(3, 3)
|
|
|
# min_pt = np.array(clip_config["min_pt"], dtype=np.float32)
|
|
|
# max_pt = np.array(clip_config["max_pt"], dtype=np.float32)
|
|
|
# floorHeight = clip_config.get("floorHeight", 0)
|
|
|
#
|
|
|
# # 批量旋转
|
|
|
# rotated = (rotation @ points.T).T
|
|
|
#
|
|
|
# # 裁剪范围过滤
|
|
|
# bounds_mask = np.all((rotated >= min_pt) & (rotated <= max_pt), axis=1)
|
|
|
# rotated = rotated[bounds_mask]
|
|
|
# x_trimmed = np.round(rotated[:, 0], 3)
|
|
|
# y_trimmed = np.round(rotated[:, 1], 3)
|
|
|
# z_trimmed = np.round(floorHeight - rotated[:, 2], 3)
|
|
|
# else:
|
|
|
# x_trimmed = np.round(points[:, 0], 3)
|
|
|
# y_trimmed = np.round(points[:, 1], 3)
|
|
|
# z_trimmed = np.round(points[:, 2], 3)
|
|
|
#
|
|
|
# # 合并为一个数组
|
|
|
# trimmed_points = np.column_stack((x_trimmed, y_trimmed, z_trimmed))
|
|
|
# # 应用 z 值过滤
|
|
|
# final_mask = (
|
|
|
# (trimmed_points[:, 2] >= z_min) &
|
|
|
# (trimmed_points[:, 2] <= z_max)
|
|
|
# )
|
|
|
# filtered_points = trimmed_points[final_mask]
|
|
|
#
|
|
|
# # 去重
|
|
|
# keys = np.column_stack((filtered_points[:, 0], filtered_points[:, 1]))
|
|
|
# _, unique_indices = np.unique(keys, axis=0, return_index=True)
|
|
|
#
|
|
|
# # 返回 list of [x, y, z]
|
|
|
# return filtered_points[unique_indices].tolist()
|
|
|
def process_point_cloud_fast(points, sn, type):
|
|
|
points = np.array(points, dtype=np.float32)
|
|
|
if not len(points):
|
|
|
return []
|
|
|
|
|
|
# 获取配置
|
|
|
template_config = config.TEMPLATE_CONFIG_MAP.get(type, {})
|
|
|
target_z = template_config.get("height", 0)
|
|
|
tolerance = template_config.get("tolerance", 25)
|
|
|
z_min = target_z - tolerance
|
|
|
z_max = target_z + tolerance
|
|
|
|
|
|
# 初始 Z 过滤
|
|
|
valid_mask = (points[:, 2] > 0) & (points[:, 2] <= config.CAMERA_CONFIG_MAP[sn].get("max_z", np.inf))
|
|
|
points = points[valid_mask]
|
|
|
|
|
|
# 获取裁剪配置
|
|
|
clip_config = config.CUT_CONFIG_MAP.get(sn + "_" + type, None) or config.CUT_CONFIG_MAP.get(sn)
|
|
|
|
|
|
if clip_config:
|
|
|
rotation = np.array(clip_config["rotation"], dtype=np.float32).reshape(3, 3)
|
|
|
min_pt = np.array(clip_config["min_pt"], dtype=np.float32)
|
|
|
max_pt = np.array(clip_config["max_pt"], dtype=np.float32)
|
|
|
floorHeight = clip_config.get("floorHeight", 0)
|
|
|
|
|
|
# 批量旋转
|
|
|
rotated = (rotation @ points.T).T
|
|
|
|
|
|
# 裁剪范围过滤
|
|
|
bounds_mask = np.all((rotated >= min_pt) & (rotated <= max_pt), axis=1)
|
|
|
rotated = rotated[bounds_mask]
|
|
|
|
|
|
# 计算坐标并应用 floorHeight
|
|
|
x_trimmed = rotated[:, 0]
|
|
|
y_trimmed = rotated[:, 1]
|
|
|
z_trimmed = floorHeight - rotated[:, 2]
|
|
|
else:
|
|
|
x_trimmed = points[:, 0]
|
|
|
y_trimmed = points[:, 1]
|
|
|
z_trimmed = points[:, 2]
|
|
|
|
|
|
# 合并为一个数组
|
|
|
trimmed_points = np.column_stack((
|
|
|
np.round(x_trimmed, 3),
|
|
|
np.round(y_trimmed, 3),
|
|
|
np.round(z_trimmed, 3)
|
|
|
))
|
|
|
|
|
|
# 应用 z 值过滤
|
|
|
final_mask = (trimmed_points[:, 2] >= z_min) & (trimmed_points[:, 2] <= z_max)
|
|
|
filtered_points = trimmed_points[final_mask]
|
|
|
|
|
|
# 去重
|
|
|
keys = filtered_points[:, :2] # 只取 x,y
|
|
|
_, unique_indices = np.unique(keys, axis=0, return_index=True)
|
|
|
|
|
|
# 返回 list of [x, y, z]
|
|
|
return filtered_points[unique_indices].tolist()
|
|
|
|
|
|
|
|
|
def sValue_to_pcd(sValue,stPointCloudImage,sn,type):
|
|
|
# 数据格式转换
|
|
|
points = convert_sdk_points(sValue, stPointCloudImage.nWidth, stPointCloudImage.nHeight)
|
|
|
|
|
|
# 应用旋转、裁剪、去重（假设设备序列号已知）
|
|
|
processed_points = process_point_cloud_fast(points, sn,type)
|
|
|
|
|
|
output_ply_path = config.save_path("pcd", sn + ".pcd")
|
|
|
# 保存结果
|
|
|
# Point.py 修改部分
|
|
|
from thread_pool import submit_task
|
|
|
|
|
|
if config.CAMERA_CONFIG_MAP[sn].get("save_pcd"):
|
|
|
output_ply_path = config.save_path("pcd", sn + ".pcd")
|
|
|
# 使用线程池异步执行点云文件写入
|
|
|
submit_task(write_pcd, output_ply_path, processed_points)
|
|
|
if config.CAMERA_CONFIG_MAP[sn].get("save_not_cut", True):
|
|
|
output_ply_path = config.save_path("pcd", sn + "_original.pcd")
|
|
|
submit_task(write_pcd, output_ply_path, points)
|
|
|
|
|
|
return 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("VERSION 0.7\n")
|
|
|
f.write("FIELDS x y z\n")
|
|
|
f.write("SIZE 4 4 4\n")
|
|
|
f.write("TYPE F F F\n")
|
|
|
f.write("COUNT 1 1 1\n")
|
|
|
f.write(f"WIDTH {len(points)}\n")
|
|
|
f.write(f"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")
|
|
|
|
|
|
|
|
|
# 写入点数据
|
|
|
for point in points:
|
|
|
f.write(f"{point[0]} {point[1]} {point[2]}\n")
|
|
|
|
|
|
logger.info(f"Saved point cloud to {filename}")
|
|
|
|