樱花客栈

从分割掩码到相机系 3D 位姿：关键点定位与朝向角 (x, y, z, yaw) 的实现

作者：

大嘴巴小牙齿

在

核心问题：如何从一张 RGB 图像出发，输出目标物体上某个关键点在【相机坐标系】下的 {x, y, z, yaw} 四元组——其中 (x, y, z) 是关键点相对相机的三维位置，yaw 是目标在该点的水平朝向角（主轴方向）。

整套方案基于「分割掩码 + 深度图 + 相机内参」：先用 YOLO 分割出目标区域，再从掩码中提取关键点像素坐标，结合深度与内参投影到相机系，最后用两点法（底部→顶部）估算朝向。目前深度相机尚未到手，当前为模拟深度阶段的预研代码。本文记录完整链路、实现细节、踩过的坑与真机改造点，可复用到各类「视觉引导定位 / 视觉伺服」任务。

一、背景与目标

  • 输入：RGB 图像（已能用 YOLO 分割出目标物体及其相邻参考区域）
  • 目标：输出目标关键点在【相机坐标系】下的 {x, y, z, yaw} 四元组
  • 下游：机械臂 / 执行机构据此移动到位，并按朝向角旋转工具对准目标
  • 相机：深度相机（RGB-D）

二、完整链路

RGB 图
  → ① YOLO 分割（目标 / 参考区域 mask）
  → ② 从掩码提取目标关键点 (u, v)
  → ③ 取该像素深度 d
  → ④ 像素 + 深度 + 内参 → 相机系 3D 点 (x, y, z)
  → ⑤ 两点法（底部→顶部）算 yaw
  → ⑥ 输出 JSON {x, y, z, yaw} + 渲染图

三、各步骤详解

1、YOLO 分割

  • 模型：自训练分割权重 best.pt
  • 置信度：CONF_THRESH = 0.3
  • 类别：目标物体(0) / 参考区域A(1) / 参考区域B(2)（按任务自行定义）
  • 推理：mode「代码结构」（原项目目录树）→ 整体替换为「核心流程（伪代码）」：单帧主流程 1~6 步l.predict(orig_frame, conf=conf)（默认 imgsz=640）

2、提取目标关键点（三重交叉区域法）

核心思路：在「目标 × 参考区域A × 参考区域B」的三重交叉区域中，取离目标质心最近的点作为底部关键点（anchor）。

伪代码：提取底部关键点
for 每个目标掩码 T:
    候选 = []
    for 参考区域A × 参考区域B 的每个组合:
        overlap = 交集(T, A, B)          # 三重交叉区域
        if overlap 非空:
            p = overlap 中离 T 质心最近的点
            候选.append(p)
    anchor = 候选里离质心最近的点
    if anchor 为空:                       # fallback
        anchor = T 底部 20% 区域的质心

fallback：无三重交叉 anchor 时，用目标掩码底部 20% 区域质心作为关键点（保证链路可验证）。

顶部点：目标掩码顶部 20% 区域质心（与底部对称），用于 yaw 计算。

关键坑（已修复）：mask 必须用 result.masks.xy（ultralytics 已映射回原图的多边形）经 fillPoly 生成。不要用 cv2.resize(mask, (w,h))——imgsz=640 时 mask 是 640×480，原图 3072×4096，非等比拉伸（宽4.8x / 高8.5x）导致 mask 形状严重变形，描线/标记偏移真实物体。

补充说明 1：为什么用「区域质心」而不是极值点？

若取”最远点/极值点”：容易被掩码边角的尖刺、缺口带偏；区域质心 = 对该区域内所有像素取平均，天然抗噪、稳定。

3、取像素深度 d（真机改造点）

  • 模拟版：simulate_depth() 生成假深度图（uint16 毫米）
    • 目标区域基准深度 SIM_BASE_DEPTH_MM = 800.0
    • 沿目标底部→顶部方向线性渐变（顶部近 SIM_SLOPE=5%）
    • 渐变让两点深度不同 → yaw 计算有物理意义（见补充说明 2）
  • 真机版：depth_frame.get_data()[v,u] * get_depth_scale()
    • 需 D2C 对齐（彩色图与深度图逐像素对应）
    • 深度空洞处理（d<=0 时取邻域均值 / 标记失败）

补充说明 2：为什么模拟深度需要渐变？

情况1：深度恒定（无渐变）
  底部点与顶部点 z 相同 → Δz = 0
  yaw = atan2(Δx, 0) 退化为只看水平像素偏移，
  且与深度无关，无法验证"像素+深度→3D"这条链路。

情况2：深度线性渐变（本实现）
  底部 z_A ≠ 顶部 z_B → Δz ≠ 0
  yaw 同时综合了水平偏移与深度差，
  更接近真机（物体表面本来就有深度变化），链路更有验证价值。

4、像素 + 深度 + 内参 → 3D 点（手写投影）

相机内参（★真机用 SDK 出厂标定）：

fx = (W/2) / tan(FOV_x/2)     # 水平视场角 FOV_x
fy = (H/2) / tan(FOV_y/2)     # 垂直视场角 FOV_y，模拟阶段取图片实际尺寸
cx = W/2, cy = H/2            # 主点居中

投影公式：

x = (u - cx) * d / fx
y = (v - cy) * d / fy
z = d

单位：毫米。直观理解：像素相对主点的偏移量（像素）× 深度 ÷ 焦距 = 物理偏移量（毫米）。

补充说明 3：相机坐标系约定（重要）


约定来源：图像坐标系原点在左上角、行号 v 向下增长
取 Y 朝下可让 3D 坐标与像素行方向一致，避免符号翻转混乱。
（该约定与 OpenCV / ROS 相机模型一致）

5、两点法算 yaw

特征点 A = 目标底部关键点（anchor）
特征点 B = 目标顶部点（掩码顶部 20% 质心）

yaw = atan2(x_B - x_A, z_B - z_A)    # 度，范围 -180°~180°

yaw = 目标主轴（底部→顶部方向）在 XZ 平面的水平夹角，下游执行机构据此旋转工具对准目标朝向。

补充说明 4：yaw 的几何含义（俯视图）

俯视 XZ 平面（从 Y 轴正上方往下看）：

yaw = atan2(Δx, Δz)，其中 Δx = x_B - x_A，Δz = z_B - z_A
  若目标偏向右侧：yaw > 0；偏向左侧：yaw < 0。

  说明：这里只取 XZ 平面算水平朝向角（通常最关心的一项）；
  若需要完整姿态（俯仰/翻滚角），可扩展为三点法，
  或对掩码点云做 PCA 取主轴向量。

6、输出

{
  "image": "frame_0001.jpg",
  "targets": [
    {"u": 1666, "v": 495, "x_mm": 63.14, "y_mm": -315.8, "z_mm": 800.0, "yaw_deg": 132.0}
  ]
}

渲染图标注：黄十字 = 底部关键点，蓝十字 = 顶部点，绿线 = 目标主轴方向，文本 = xyz/yaw 信息。

四、坐标系与输出字段

字段含义符号约定
x_mm左右偏移光心右正左负
y_mm上下偏移光心下正上负（Y 朝下）
z_mm深度距离恒正（沿 Z 轴）
yaw_deg水平朝向角-180° ~ 180°

五、核心流程（伪代码）

伪代码：单帧主流程
1. RGB 帧 → YOLO 分割 → 掩码列表（目标 + 参考区域）
2. 提取关键点：
   anchor = 三重交叉区域中离目标质心最近的点（无则取底部 20% 质心）
   top    = 目标掩码顶部 20% 区域的质心
3. 取深度：
   d_bottom = 深度图[anchor]      # 模拟版：读模拟深度图
   d_top    = 深度图[top]         # 真机版：SDK 深度帧（注意 D2C 对齐与空洞处理）
4. 投影到相机系：
   A = pixel_to_xyz(anchor, d_bottom)
   B = pixel_to_xyz(top, d_top)
5. 算朝向：yaw = atan2(x_B - x_A, z_B - z_A)
6. 输出 {x, y, z, yaw} + 渲染标注图

六、真机改造点汇总（★）

位置模拟版真机版
simulate_depth假深度（800mm+渐变）SDK depth_frame.get_data()
get_depth_at查模拟深度图depth[v,u] * get_depth_scale()
fx/fy/cx/cyFOV 反推 + 主点居中SDK 出厂标定内参
图片分辨率实际图片尺寸相机 RGB 流分辨率
输出JSON 打印封装函数供下游调用

七、关键经验（踩过的坑）

  1. mask 非等比拉伸：cv2.resize 到原图会变形，必须用 result.masks.xy（原图坐标多边形）
  2. 顶部点不用最远点：最远点会跑到掩码角落尖角，应用顶部区域质心
  3. 模拟深度需渐变：让两点深度不同，yaw 才有物理意义
  4. fallback 保证链路：无三重交叉 anchor 时用目标底部中心，确保模拟阶段可验证
  5. 坐标系约定要统一：Y 朝下与像素 v 方向一致，避免后续执行机构换算时符号出错

八、工作逻辑

# 处理单张图片或文件夹（模拟阶段可先跑少量样本）
python <入口脚本> <图片路径/文件夹>

# 结果输出到自动递增的目录：predict → predict2 → predict3 ...

九、验收标准（对应方案文档）

  1. 输出 (x, y, z, yaw) 连续 100 帧 yaw 波动 < 5°
  2. 深度误差 < 2%（1m 处 ±2cm）
  3. 单帧处理满足下游需求（30fps 或按需触发）

更多文章