|
|
|
|
|
import numpy as np
|
|
|
|
|
|
import config as con
|
|
|
|
|
|
|
|
|
|
|
|
def clip_and_rotate_point_cloud(points, config):
|
|
|
|
|
|
"""
|
|
|
|
|
|
对点云先旋转，再裁剪
|
|
|
|
|
|
|
|
|
|
|
|
:param points: list of [x, y, z], 原始点云数据
|
|
|
|
|
|
:param config: dict, 包含 min_pt, max_pt, rotation 的配置
|
|
|
|
|
|
:return: list of [x, y, z], 旋转并裁剪后的点云
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
|
|
|
|
# 提取配置参数
|
|
|
|
|
|
min_pt = np.array(config.get("min_pt", [-np.inf] * 3))
|
|
|
|
|
|
max_pt = np.array(config.get("max_pt", [np.inf] * 3))
|
|
|
|
|
|
rotation = np.array(config.get("rotation", [1, 0, 0, 0, 1, 0, 0, 0, 1])).reshape(3, 3)
|
|
|
|
|
|
|
|
|
|
|
|
# 转换为 NumPy 数组
|
|
|
|
|
|
points = np.array(points, dtype=np.float64)
|
|
|
|
|
|
|
|
|
|
|
|
# 1. 先旋转：应用旋转矩阵
|
|
|
|
|
|
rotated_points = (rotation @ points.T).T
|
|
|
|
|
|
|
|
|
|
|
|
# 2. 再裁剪：保留落在 min_pt ~ max_pt 范围内的点
|
|
|
|
|
|
mask = np.all((rotated_points >= min_pt) & (rotated_points <= max_pt), axis=1)
|
|
|
|
|
|
clipped_points = rotated_points[mask]
|
|
|
|
|
|
|
|
|
|
|
|
return clipped_points.tolist()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def merge_point_clouds(clouds, config,sn):
|
|
|
|
|
|
"""
|
|
|
|
|
|
将多个点云沿 x 轴依次拼接在一起
|
|
|
|
|
|
|
|
|
|
|
|
:param clouds: list of (list of [x, y, z]), 多个点云
|
|
|
|
|
|
:return: list of [x, y, z], 合并后的点云
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
|
|
|
|
min_pt = config.get("min_pt", [0, 0, 0])
|
|
|
|
|
|
max_pt = config.get("max_pt", [0, 0, 0])
|
|
|
|
|
|
# 返回 x 方向的区域宽度
|
|
|
|
|
|
|
|
|
|
|
|
merged = []
|
|
|
|
|
|
current_x_offset = 0
|
|
|
|
|
|
|
|
|
|
|
|
reverse_order = con.CAMERA_CONFIG_MAP[sn].get("reverse_order", False)
|
|
|
|
|
|
for cloud in reversed(clouds) if reverse_order else clouds:
|
|
|
|
|
|
if not cloud:
|
|
|
|
|
|
continue
|
|
|
|
|
|
# 添加偏移后写入结果
|
|
|
|
|
|
offset_cloud = [[p[0] + current_x_offset, p[1], p[2]] for p in cloud]
|
|
|
|
|
|
merged.extend(offset_cloud)
|
|
|
|
|
|
|
|
|
|
|
|
# 获取当前相机采集区域的 x 宽度
|
|
|
|
|
|
x_width = float(max_pt[0] - min_pt[0])
|
|
|
|
|
|
|
|
|
|
|
|
# 更新下一个点云的偏移量（当前偏移 + 当前区域宽度）
|
|
|
|
|
|
# 在下一次删除
|
|
|
|
|
|
current_x_offset += x_width
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return merged,current_x_offset+min_pt[0]
|
|
|
|
|
|
|
|
|
|
|
|
|
