实例分割聊完了，这篇讲6D位姿估计。机器人抓取不仅要找到物体在哪里（3D位置），还要知道物体怎么旋转的（3D旋转）。位置和旋转加起来就是6个自由度——6D位姿。

6D位姿估计是机器人抓取、装配、操作的核心感知任务。不知道物体的精确位姿，机械臂就不知道该以什么角度去抓、该往哪个方向拧。工业场景里，零件在传送带上可能是任意姿态，机器人必须实时估计位姿才能正确抓取。

面试中6D位姿估计的追问通常围绕：位姿怎么表示的、主流方法有哪些、精度怎么评估。把这些概念搞清楚，机器人抓取的感知部分就理解了。

一、位姿表示

6D位姿由旋转R（3x3旋转矩阵）和平移t（3维向量）组成。合在一起写成4x4的齐次变换矩阵T。

旋转的表示方式有多种：旋转矩阵（3x3正交矩阵，9个参数但有6个约束）、欧拉角（roll/pitch/yaw，3个参数但有万向锁问题）、四元数（4个参数，没有奇异性）、轴角（旋转轴+旋转角度，3个参数）。

import numpy as np
# 齐次变换矩阵
T = np.array([
    [r11, r12, r13, tx],
    [r21, r22, r23, ty],
    [r31, r32, r33, tz],
    [0,   0,   0,   1 ]
])
# R: 3x3旋转矩阵, t: [tx, ty, tz]平移向量

面试常问：为什么不用欧拉角？因为万向锁（gimbal lock）——当某个轴旋转90度时，另外两个轴重合，自由度退化为2。四元数没有这个问题，所以机器人学里通常用四元数表示旋转。

二、主流方法

6D位姿估计的方法大致分三类：基于模板匹配、基于关键点、基于回归。

基于模板匹配：准备物体的3D模型，用ICP（Iterative Closest Point）算法把模型和观测的点云对齐。ICP迭代地找最近点对，计算最优变换，反复迭代直到收敛。优点是精度高（毫米级），缺点是需要精确的3D模型、对初始位姿敏感（容易陷入局部最优）、速度较慢。通常用网络预测的位姿作为ICP的初始值，两者配合效果最好。

基于关键点：在物体上定义一组关键点（比如8个角点或者密集采样的控制点），网络预测每个关键点在图像中的2D位置，然后用PnP（Perspective-n-Point）算法从2D-3D对应关系求解位姿。代表工作：PVNet、DenseFusion。PVNet用像素投票的方式预测关键点位置，比直接回归更鲁棒。DenseFusion融合RGB图像和点云信息，在遮挡场景下表现更好。

关键点方法的好处是中间结果可解释——你能看到网络预测的关键点位置对不对。坏处是PnP求解对关键点精度很敏感，一个关键点偏了位姿就可能差很多。实际使用中通常用RANSAC来增强鲁棒性——随机选4个点算位姿，看有多少点支持这个位姿，选支持最多的那个。

# PnP求解位姿
import cv2
# 2D关键点（图像坐标）和3D关键点（物体坐标）
success, rvec, tvec = cv2.solvePnP(
    points_3d, points_2d, camera_matrix, dist_coeffs
)
R, _ = cv2.Rodrigues(rvec)  # 轴角→旋转矩阵

基于回归：网络直接从图像回归位姿参数。端到端训练，不需要中间步骤。代表工作：PoseCNN、GDR-Net。GDR-Net用几何感知网络，同时回归位置和旋转，精度在LINEMOD数据集上达到SOTA。

回归方法的关键挑战是旋转的表示。直接回归旋转矩阵的9个参数不保证正交性。用四元数回归有双重覆盖问题（q和-q表示同一个旋转）。用6D连续旋转表示（two consecutive columns of rotation matrix）是目前比较好的方案——既保证了连续性，又没有奇异性。

基于投票：网络预测每个像素投票的位姿，然后聚类得到最终结果。代表工作：FFB6D、PVN3D。这类方法对遮挡和噪声有较好的鲁棒性。每个像素独立投票，即使部分像素被遮挡，其他像素的投票仍然能给出正确结果。

三、位姿评估指标

6D位姿估计的评估指标和检测不同。常用的有两个。

ADD（Average Distance）：计算预测位姿变换后的模型点和GT位姿变换后的模型点之间的平均距离。ADD小于物体直径的10%就算正确。这个指标对对称物体不友好——对称物体有多个正确位姿。

ADI（Average Distance of Inverse model points）：ADD的改进版，对对称物体友好。计算时找每个点到最近GT点的距离（而不是对应点的距离）。

# ADD指标计算
def compute_add(pred_R, pred_t, gt_R, gt_t, model_points):
    pred_pts = (pred_R @ model_points.T).T + pred_t
    gt_pts = (gt_R @ model_points.T).T + gt_t
    mean_dist = np.mean(np.linalg.norm(pred_pts - gt_pts, axis=1))
    diameter = compute_diameter(model_points)
    return mean_dist < 0.1 * diameter  # 是否通过10%直径阈值

5cm5°准确率：位置误差小于5cm且旋转误差小于5°的比例。这是工业场景常用的指标——直接对应实际任务的精度要求。

四、数据集与训练

6D位姿估计常用的数据集：

LINEMOD：经典数据集，包含15个日常物体。每个物体约1000张图像，带位姿标注。数据量小，适合验证算法。

YCB-Video：21个YCB标准物体的视频序列。数据量大，场景复杂，有遮挡。目前最主流的benchmark之一。

BOP Challenge：年度竞赛，包含多个数据集（LINEMOD、YCB-Video、T-LESS等）。推动了很多新方法的发展。

训练数据的获取是个大问题。真实数据标注成本高——每张图都要精确标注6D位姿。常用方案是仿真数据+域适应——在仿真引擎里渲染物体，自动生成大量带标注的数据，然后迁移到真实场景。但仿真和真实之间有差距（sim-to-real gap），需要用域随机化（Domain Randomization）来缩小差距——训练时随机化光照、纹理、背景，让模型见过足够多的变化。

五、面试高频追问

Q：6D位姿估计的难点在哪里？ A：三个难点。遮挡——物体被其他东西挡住了，能看到的信息不完整。对称——对称物体有多个正确位姿，评估和训练都要特殊处理。精度——抓取任务通常要求毫米级精度，深度估计本身的误差就会影响位姿精度。

Q：怎么处理对称物体的位姿估计？ A：训练时用多标签——对称物体的多个正确位姿都作为正样本。评估时用ADI指标而不是ADD。有些方法直接预测对称物体的对称轴方向，避免歧义。

Q：工业场景中位姿估计的pipeline是什么？ A：RGB-D相机采集图像→目标检测定位物体→实例分割获取mask→6D位姿估计（用mask裁剪后的区域做位姿回归）→位姿精修（用ICP在点云上微调）。整个pipeline的延迟通常要控制在100ms以内。实际部署中，位姿精修这一步很关键——网络预测的位姿通常不够精确，用ICP在局部点云上做一次精修能把精度提升一个量级。

Q：深度信息对位姿估计有多重要？ A：非常重要。只用RGB图像的位姿估计（如GDR-Net）精度有限，因为单目图像的深度信息是模糊的。加入深度信息（RGB-D）后，位姿精度通常能提升30%-50%。但深度相机也有问题——深度噪声大、边缘不准确、反光表面深度缺失。实际使用中要对深度数据做预处理（去噪、填充空洞）。

6D位姿估计是机器人抓取的核心感知任务。位姿表示方法、主流算法、评估指标、数据集与训练、实际应用，这五个方面理解了，机器人抓取的感知部分就搞定了。位姿估计是连接感知和执行的桥梁，值得深入学习。下一篇我们聊PointNet系列。

---

6D位姿估计是机器人操作的核心感知技术。位姿表示方法、主流估计算法、评估指标体系、工业应用实践，这四个维度覆盖了6D位姿估计的核心内容。

上一篇：第293篇 实例分割Mask R-CNN 

下一篇聊PointNet系列。

如果这篇文章对你有帮助，欢迎点赞支持一下，你的鼓励是我持续更新的动力！