Object Pose 6D Estimation English
← 返回知识体系
02 几何基础 3D 视觉 SLAM 机器人

Coordinate Frames & Rigid Transforms

坐标系与
刚体变换

位姿不是一个脱离语境的矩阵，而是两个坐标系之间的关系。只要先说清楚“从哪个系到哪个系”，复合、求逆、投影和机器人变换链都会变得可推导、可检查。

打开变换实验室 先统一记号

Mental Model

先区分：空间中的对象，和描述它的数字

坐标系是一套带原点和方向的测量基准；坐标只是某个几何对象在这套基准下的数值表达。换坐标系时，物理世界没有变化，变化的是数字。

POINT

一个点，多种坐标

桌面上的同一点，可以写成物体系中的 [0.1, 0, 0]，也可以写成相机系中的 [0.3, -0.2, 0.8]。两组数字描述的是同一个点。

FRAME

坐标系也有位姿

一个坐标系相对另一个坐标系的位置和朝向，可以用旋转和平移描述。所谓“物体位姿”，本质上是物体系相对参考系的位姿。

TRANSFORM

变换负责翻译

刚体变换不会弯曲或缩放空间。它只把点、方向和坐标轴从一种表达翻译成另一种表达，同时保持距离和夹角。

几何对象

空间中的点 P

独立于任何坐标表示

A 系表达

Ap = [xA, yA, zA]T

相对于 A 的原点和轴

B 系表达

Bp = [xB, yB, zB]T

相对于 B 的原点和轴

Notation Contract

本站采用的统一记号

不同论文和代码库会使用 T_AB、T_BA、pose、extrinsic 等含糊写法。本专题固定使用带上下标的 frame 记号，让每一步都能检查输入和输出。

Change of coordinates
Ap=ATB Bp,ATB=[ARBAtB0T1]{}^{A}\mathbf{p} = {}^{A}\mathbf{T}_{B}\,{}^{B}\mathbf{p}, \qquad {}^{A}\mathbf{T}_{B}=\begin{bmatrix}{}^{A}\mathbf{R}_{B}&{}^{A}\mathbf{t}_{B}\\\mathbf{0}^{\mathsf T}&1\end{bmatrix}
本站使用列向量。旋转先作用于 B 系坐标，再加上 B 原点在 A 系中的位置。齐次点写成 [x, y, z, 1]ᵀ。
Composition
ATC=ATB BTC{}^{A}\mathbf{T}_{C} = {}^{A}\mathbf{T}_{B}\,{}^{B}\mathbf{T}_{C}
中间的 B 首尾相接。矩阵从右往左作用：先 C→B，再 B→A。
Inverse
(ATB)−1=BTA=[RT−RTt0T1]({}^{A}\mathbf{T}_{B})^{-1} = {}^{B}\mathbf{T}_{A}=\begin{bmatrix}\mathbf{R}^{\mathsf T}&-\mathbf{R}^{\mathsf T}\mathbf{t}\\\mathbf{0}^{\mathsf T}&1\end{bmatrix}
逆变换的平移不是简单的 −t；必须先用 Rᵀ 把 t 转到新的坐标系。

Passive view · 本站默认

同一个点，改变坐标表达

物理点不动，用 ATB 将其 B 系坐标翻译成 A 系坐标。这是标定、PnP、SLAM 和 TF 中最常用的阅读方式。

Active view

在同一坐标系里移动物体

矩阵也可以被解释为对点进行旋转和平移。两种解释代数形式相似，但语义不同；一段推导中不要在未声明时切换。

Interactive Lab

亲手组成一条 SE(3) 变换链

实验室使用完整 3D 平移与 ZYX 欧拉角构造旋转。先编辑相机在世界中的位姿 WTC，再编辑物体在相机中的位姿 CTO，系统实时计算 WTO 及其逆。

Interactive · SE(3)

坐标变换链实验室

调整两个 6DoF 变换，观察坐标轴、复合矩阵和点坐标如何同步变化。
列向量 右手系 R = RzRyRx
Wp ← WTC Cp ← CTO Op 因此 WTO = WTCCTO
x y z W · world x y z C · camera x y z O · object 测试点
+X +Y +Z 固定观察视角 · 非透视投影

Composition result

复合与逆变换

[
0.0000.0000.0000.0000.0000.0000.0000.0000.0000.0000.0000.0000.0000.0000.0000.000
]
tx
ty
tz
roll
pitch
yaw

Point transform

同一个点，换一种坐标表达

输入物体坐标系中的点 Op，实验室会通过 WTO 得到它在世界坐标系中的表达 Wp。

Wp = WTO Op
x
y
z

注意：点本身没有移动；改变的是描述它所使用的坐标基准。

Frame Atlas

6D 位姿系统里最常见的坐标系

一个实际系统通常不是只有 object 和 camera 两个 frame。数据采集、SLAM、机器人和抓取会不断增加新的参考系，关键是为每条边定义清楚方向。

O

物体 / 模型坐标系

固定在 CAD 或扫描模型上的局部坐标系。模型点、3D 包围盒和对称轴都以它为基准。

  • 原点由数据集或建模流程定义
  • 标准朝向不一定等于当前观测朝向
  • 模型单位可能是 m、cm 或 mm
C

相机 / 光学坐标系

固定在相机光心上的三维坐标系。6D 物体姿态通常先表达为物体相对相机的变换。

  • OpenCV：x 向右、y 向下、z 向前
  • 深度值常对应 Z，而非到光心的欧氏距离
  • RGB 与深度相机可能各有独立坐标系
W

世界 / 地图坐标系

用于跨帧、跨相机或跨设备保持一致的全局参考系，在 SLAM 中通常对应 map 或 world。

  • 原点和朝向是系统设计选择
  • 相机轨迹常表示为相机在世界中的位姿
  • 动态场景中需要区分静态世界和运动物体
B/E/G

机器人基座、末端与夹爪

机器人链路常包含 base、end-effector、tool center point 和 grasp frame 等多个参考系。

  • TF 树描述父子 frame 的时变关系
  • Eye-in-hand 相机随末端运动
  • 抓取目标最终必须转换到基座或规划坐标系

图像坐标与像素坐标不是普通三维刚体坐标系

图像中的 (u, v) 通常从左上角开始，u 向右、v 向下。三维相机点需要先除以深度，再经过内参 K 才能得到像素；这个透视投影不能只靠一个 SE(3) 刚体变换完成。

Convention Atlas

同样叫 camera frame，方向可能完全不同

坐标系统没有天然唯一的选法。OpenCV、ROS、BOP、Blender 和图形学管线各自有合理约定；出错通常发生在约定交界处。

生态 / 软件典型 frame轴方向常见变换语义单位
OpenCV 相机 Camera / optical +x 向图像右，+y 向图像下，+z 沿光轴向前 rvec、tvec 通常把物体或世界点变到相机系 由输入数据决定
BOP Model / camera / world 全部右手系；相机遵循 OpenCV；模型通常 +z 向上 cam_R_m2c、cam_t_m2c：model → camera 模型与平移为 mm
ROS body base_link 等 +x 向前，+y 向左，+z 向上 TF 表达 frame 之间的父子关系，必须看 header / child m、rad（SI）
ROS optical *_optical +x 向右，+y 向下，+z 向前 专门与常见相机成像约定对齐 m、rad（SI）
Blender World / object / camera local 世界 +z 向上；相机通常沿局部 -z 看，局部 +y 向上 matrix_world 常表示 local → world；导入导出可重映射轴 Blender Unit，可配置
OpenGL 风格视图 Eye / view +x 向右，+y 向上，镜头通常看向 -z view matrix 通常是 camera pose 的逆 由应用决定

不要靠记忆猜符号

把轴映射写成显式矩阵

例如，从 OpenCV optical frame 转向一种 x-right、y-up、z-backward 的图形学 view frame，常见轴翻转是 diag(1, -1, -1)。但实际项目仍应根据源和目标定义逐轴验证，而不是复制一段不带语境的转换代码。

Task Chains

这些变换在真实任务中怎样连接

同一套复合与求逆规则贯穿 PnP、相机定位、SLAM、手眼标定和机器人抓取。下面用 frame 记号把常见语义彻底展开。

3D Vision

PnP 与 6D 物体位姿

OpenCV 风格 PnP 通常返回 CTO：它把物体模型点从 O 系变到 C 系，然后通过相机内参投影。

Cp = CTO Op → pixel = π(K, Cp)
  • t 是物体原点在相机系中的位置，不是相机在物体系中的位置。
  • 要得到相机相对物体的位姿，需要计算 OTC = (CTO)−1。
SLAM

外参与相机位姿

视觉算法常用外参 CTW 把世界点送入当前相机；轨迹可视化通常想要的是相机在世界中的位姿 WTC。

WTC = (CTW)−1
  • 如果外参写成 [R | t]，相机中心不是 t。
  • 相机中心在世界系中为 CW = −RTt。
Robotics

Eye-in-hand 观察目标

设 B 为机器人基座、E 为末端、C 为手上相机、O 为物体，则物体在基座中的位姿由三段已知变换组成。

BTO = BTE ETC CTO
  • BTE 来自机器人正运动学。
  • ETC 来自手眼标定；CTO 来自视觉估计。
Grasping

从物体位姿到抓取目标

若抓取模板给出夹爪目标 G 在物体系中的位姿 OTG，则可以继续接到机器人基座中。

BTG = BTC CTO OTG
  • 每个相邻 frame 都能首尾相消，是链路方向正确的快速检查。
  • 真正执行前仍需碰撞检测、逆运动学与工具中心点修正。

Worked Example

一个最小数值例子

相机系 C 相对世界系 W 绕 z 轴旋转 90°，相机原点位于世界坐标 [1, 0, 0]。一个点在相机系中的坐标为 [0.5, 0, 1]。

Numerical example
WTC=[0−101100000100001],Cp=[0.5011]⇒Wp=[10.511]{}^{W}\mathbf{T}_{C}=\begin{bmatrix}0&-1&0&1\\1&0&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix},\quad {}^{C}\mathbf{p}=\begin{bmatrix}0.5\\0\\1\\1\end{bmatrix}\Rightarrow{}^{W}\mathbf{p}=\begin{bmatrix}1\\0.5\\1\\1\end{bmatrix}
相机系 +x 方向在世界系中变成 +y，因此 0.5 m 的相机 x 分量最终出现在世界 y 坐标上；相机原点的世界 x=1 作为平移被加上。

Debug Protocol

坐标错了，不要盯着矩阵猜

最有效的排错方法，是把隐含语义重新写出来，并用可视化与不变量逐层缩小范围。

  1. 01

    给每个量标 frame

    把 p、R、t、T 暂时全部改写成 Ap、ATB。如果无法标注，说明变量语义本身就不清楚。

  2. 02

    检查变换链是否首尾衔接

    ATBCTD 不能直接相乘，因为中间的 B 与 C 不一致。

  3. 03

    用原点和单位轴做测试

    把 [0,0,0,1] 和三个单位轴分别送入变换，比随机点更容易解释平移、轴翻转和旋转方向。

  4. 04

    检查刚体不变量

    应满足 RTR≈I、det(R)≈+1，齐次矩阵最后一行为 [0,0,0,1]。

  5. 05

    投影坐标轴与模型轮廓

    把物体系 xyz 轴、3D 框和 CAD 轮廓投影回图像，方向错误通常会立刻暴露。

  6. 06

    最后才怀疑模型

    先排除 mm/m、度/弧度、行/列向量、变换方向和 OpenCV/OpenGL 轴差异，再分析网络预测质量。

约定与公式来源展开查看官方文档与格式规范

Next topic · 03

旋转与姿态表示

下一步将系统比较旋转矩阵、欧拉角、轴角、四元数、6D 连续表示，以及 Egocentric / Allocentric 和 in-plane / out-of-plane rotation。
返回地图查看位置