YOLO11 + 深度相机 6DoF 位姿估计 YOLO11 + Depth Camera 6DoF Pose Estimation

目标：从 YOLO11 检测结果 + 深度相机，获取物体完整 6DoF 位姿（位置 + 姿态）。 Goal: Recover the complete 6DoF pose (position + orientation) of an object from YOLO11 detections combined with a depth camera.

本教程以 GB/T 20234.3 直流充电口为例，使用三个检测类别：charging_port / DC_hole / PE。 This tutorial uses a GB/T 20234.3 DC charging port as the example object with three detection classes: charging_port / DC_hole / PE.

最终输出：T_cam2port（4×4 变换矩阵），包含相机坐标系下充电口的完整位置和朝向。 Final output: T_cam2port (4×4 transformation matrix) encoding the full position and orientation of the charging port in the camera coordinate frame.

YOLO + 深度读数只能给出「物体在多远」——Z 方向的标量距离。插接任务需要完整的 6 个自由度：X/Y/Z 三维位置 + Roll/Pitch/Yaw 三个朝向角（共 6 个自由度）。 YOLO plus a depth reading only tells you how far away the object is — a scalar Z distance. A plug-insertion task requires all 6 degrees of freedom: X/Y/Z translation plus Roll/Pitch/Yaw orientation (6 DOF total).

三步原理： Three-step principle:

1. YOLO 检测 charging_port bbox → 区域深度点云 → RANSAC 平面拟合 → 法向量（Z 轴方向） YOLO detects charging_port bbox → extract region depth point cloud → RANSAC plane fitting → surface normal (Z-axis direction)
2. DC_hole（两个孔）+ PE 的 bbox 中心像素 + 深度 → 相机内参反投影 → 三个 3D 关键点 DC_hole (two holes) + PE bbox center pixels + depth → back-project via camera intrinsics → three 3D keypoints
3. DC+/DC- 连线 → X 轴；DC 中点 → PE 方向 → Y 轴；法向量校验 Z 轴 → 完整旋转矩阵 R DC+ → DC- vector defines X-axis; DC midpoint → PE direction defines Y-axis; normal validates Z-axis → full rotation matrix R

数据流示意Data-flow diagram

• 已完成 YOLO11 自定义模型训练（参见训练教程 LEARN — 002） YOLO11 custom model training completed (see training tutorial LEARN — 002)
• 已安装 pyorbbecsdk 并可正常驱动 Orbbec DaBai DCW2 pyorbbecsdk installed and Orbbec DaBai DCW2 working
• 检测类别包含 charging_port、DC_hole、PE Detection classes include charging_port, DC_hole, PE
• Python 环境已安装：ultralytics、opencv-python、numpy Python environment with: ultralytics, opencv-python, numpy

相机内参（fx, fy, cx, cy）是将像素坐标反投影为 3D 点的必要参数。在 pipeline.start(config) 之后调用： Camera intrinsics (fx, fy, cx, cy) are required to back-project pixel coordinates into 3D space. Call after pipeline.start(config):

预先量好的充电口物理尺寸，不是实时测量。以充电口圆心为原点，单位米。数据来源 GB/T 20234.3-2023： Pre-measured physical dimensions of the charging port — not measured at runtime. Origin at the charging port center, unit: meters. Source: GB/T 20234.3-2023:

将像素坐标 (u, v) 加上深度值，利用相机内参反投影为相机坐标系下的 3D 点： Convert a pixel coordinate (u, v) plus a depth value into a 3D point in camera space using the intrinsic parameters:

对每个关键点（DC+、DC-、PE），取 bbox 中心像素作为 (u, v)，在深度图中采样对应深度值，再调用 backproject() 得到 3D 坐标。 For each keypoint (DC+, DC-, PE), use the bbox center pixel as (u, v), sample the corresponding depth from the depth map, then call backproject() to get the 3D coordinate.

从 charging_port bbox 区域的深度点云中，用 RANSAC 鲁棒地拟合充电口平面，得到法向量（即 Z 轴方向）： From the depth point cloud within the charging_port bbox region, robustly fit the charging port plane using RANSAC to obtain the surface normal (Z-axis direction):

为什么用三点而不是两点：PE 提供 Y 轴约束，三点互相冗余校验比两点更鲁棒。若只用 DC+/DC-，则 Y 轴方向完全依赖法向量，无额外校验；加入 PE 后，两路信息可以交叉验证，减少单帧噪声影响。 Why three points instead of two: PE provides the Y-axis constraint, giving three-point redundancy that is more robust than two points alone. With only DC+/DC-, the Y-axis depends entirely on the surface normal with no cross-check. Adding PE creates two independent estimates that validate each other, reducing the effect of single-frame noise.

YOLO 无法区分 DC+ 和 DC-（两者属于同一类别 DC_hole，外观完全相同）。代码中按图像 X 坐标排序：X 较小（左边）= DC+，X 较大（右边）= DC-。 YOLO cannot distinguish DC+ from DC- — both belong to class DC_hole and look identical. The code sorts by image X coordinate: smaller X (left) = DC+, larger X (right) = DC-.

每帧执行以下步骤： Execute the following steps every frame:

1. YOLO 推理，按类别收集 charging_port / DC_hole / PE 检测框 Run YOLO inference, collect detected bounding boxes by class: charging_port, DC_hole, PE
2. 检查三类是否都存在（charging_port ≥1，DC_hole ≥2，PE ≥1），否则显示缺失提示并跳过本帧 Verify all three classes are present (charging_port ≥1, DC_hole ≥2, PE ≥1); if not, display a missing-detection warning and skip this frame
3. 取 charging_port bbox 区域深度点云，调用 fit_plane_ransac() 拟合平面法向量 Extract the depth point cloud within the charging_port bbox region, call fit_plane_ransac() to fit the surface normal
4. 取两个 DC_hole bbox 中心 + 深度 → 调用 backproject() → 得到 3D 坐标 p_dc_plus、p_dc_minus Get center + depth of two DC_hole bboxes → call backproject() → obtain 3D coordinates p_dc_plus, p_dc_minus
5. 取 PE bbox 中心 + 深度 → 调用 backproject() → 得到 3D 坐标 p_pe Get center + depth of PE bbox → call backproject() → obtain 3D coordinate p_pe
6. 调用 build_pose(p_dc_plus, p_dc_minus, p_pe, normal_ransac) 构建 T_cam2port Call build_pose(p_dc_plus, p_dc_minus, p_pe, normal_ransac) to construct T_cam2port
7. 调用 draw_axis() 在图像上画坐标轴，打印变换矩阵到终端 Call draw_axis() to overlay coordinate axes on the image, print the transformation matrix to the terminal

在图像上画出充电口坐标系三轴，便于调试和验证位姿结果： Draw the three axes of the charging port coordinate frame onto the image for debugging and visual validation:

length=0.03 表示轴长 30 mm，适合充电口尺寸，可根据相机距离调整。 length=0.03 draws 30 mm arrows, appropriate for the charging port size. Adjust based on the camera-to-port distance.

T_cam2port 的结构含义： Structure of T_cam2port:

T_cam2port 是相机坐标系下的位姿。要让机械臂知道末端去哪，还需手眼标定得到 T_cam2gripper： T_cam2port is expressed in the camera frame. For the robot arm to know where to move its end effector, eye-hand calibration is needed to obtain T_cam2gripper: