世界坐标系/相机坐标系/图像坐标系转换
做过视觉标定、三维重建、AR叠加，或者哪怕只是用OpenCV跑过一个棋盘格标定程序的人，大概率都见过这三兄弟的名字：世界坐标系、相机坐标系、图像坐标系。网上资料一堆，但很多要么是纯数学推导看得人头皮发麻，要么是代码调完库也不知道中间到底发生了什么。我早年就被这套坐标系转换狠狠教育过——当时做的一个机械臂抓取项目，标定出来的坐标总是在某个方向上偏差好几个厘米，查了两天才发现是图像坐标系和像素坐标系的变换矩阵少乘了一个缩放系数。所以今天想把这条转换链路完整掰开揉碎讲清楚，从物理意义到数学推导，再到OpenCV里的实际落地，一次说透。
这篇文章适合这几类人：刚接触相机标定、想搞明白张正友标定法中间那些矩阵到底怎么来的同学；在做视觉引导、机械臂手眼标定、AR/VR注册、三维重建，需要把物体在真实世界的位置和图像上的像素位置来回换算的工程师；以及那些已经会调用cv2.calibrateCamera但始终对原理有点发虚，想补上这块短板的同行。读完你至少能自己手写出从世界坐标到像素坐标的完整矩阵链，并且知道每一行每一个元素在物理上代表什么——而不是只会对着文档调参。
1. 四个坐标系各管哪一段：先搞清楚它们描述的"空间"是谁的
很多教程上来就甩出坐标转换公式，但坐标系这个概念如果没在脑子里建立起直观的物理对应，公式背得再熟也是空中楼阁。所以先把四个坐标系分别是什么、它们各自固定在哪、度量单位是什么这些"世界观"问题说清楚。
1.1 世界坐标系：给整个场景定个"绝对原点"
世界坐标系（World Coordinate System）是一个人为定义的基准坐标系。你可以把它理解成整个三维世界的"绝对尺度参考"。它固定在哪里完全由你说了算——通常是放在标定板的角点上、机械臂的基座上、或者房间的某个墙角。一旦定义好了，场景里所有物体的位置都用这个坐标系下的三维坐标(X_w, Y_w, Z_w)来描述，单位一般是毫米或者米。
为什么要人为定义这么个坐标系？因为相机是装在一个会移动的载体上的，它看到的"前方两米处有个零件"这种描述每时每刻都在变；但如果说"零件在世界坐标系的 (500, 200, 30) 毫米处"，这句话在任意时刻都是成立的。在视觉引导场景里，世界坐标系通常建在机械臂基座或者工作台面上，这样后续机械臂可以直接拿这个坐标去规划运动。说白了，世界坐标系是所有测量和计算的"共同语言"，是消除设备和传感器之间存在性差异的锚点。
1.2 相机坐标系：以光心为原点、光轴为Z轴的"相机视角"
相机坐标系（Camera Coordinate System）的原点设在相机的光心（也就是镜头的光学中心），Z轴与光轴重合，指向相机正前方，X轴和Y轴分别平行于成像平面的水平方向和竖直方向。
概念上可以这样类比：假如你的眼睛就是一台相机，那么相机坐标系描述的就是"以你的眼球中心为原点，你正前方为Z轴"的坐标系。当你转头时，世界坐标系没动，但相机坐标系跟着你的视线一起转了。所以从世界坐标系到相机坐标系的变换，本质上就是一次刚体变换（旋转+平移），描述的是"相机相对于世界坐标系摆在哪里、朝哪个方向看"。
这里有一个特别容易混淆的点：相机坐标系和图像坐标系不是一回事。相机坐标系是三维的，单位为毫米/米；图像坐标系是二维的，也在物理尺度上以毫米为单位，但它是成像平面上的坐标。后面会详细说。
1.3 图像坐标系与像素坐标系：一个代表物理尺寸，一个代表像素排列
图像坐标系（Image Coordinate System）是固化在成像平面上的二维直角坐标系，原点通常定义在光轴与成像平面的交点（主点，principal point），单位是毫米。它描述的是三维空间里的点通过透视投影落到成像平面上之后，在物理层面上的位置(x, y)。
像素坐标系（Pixel Coordinate System）则是我们在代码里真正接触的那个坐标——图像数组的行列索引(u, v)，单位是像素。它不关心物理尺寸，只关心"这个像素在图片的第几行第几列"。原点在图像左上角，u轴向右，v轴向下，这也是OpenCV读入图像后默认的像素排布方式。
光是把这两个坐标系放在一起对比，就已经能看出一些门道：图像坐标系的原点在主点（理论上应该在图像正中心），像素坐标系的原点在左上角；图像坐标系单位为毫米，像素坐标系单位为像素；图像坐标系Y轴向上，像素坐标系V轴向下。这两者之间的换算关系，就是你需要填进内参矩阵里的fx、fy、cx、cy等参数。
2. 三步转换链路逐层拆解：从世界坐标到像素坐标的完整推导
搞清楚了四个坐标系各自的角色，接下来就是重头戏：一个三维空间中的点，是怎么一步步变成图像上一个像素的？这个过程可以拆成三步：刚体变换（世界→相机）、透视投影（相机→图像）、像素离散化（图像→像素）。每一阶段都有极其鲜明的物理意义，我们逐个来拆。
2.1 世界→相机：外参矩阵与刚体变换的几何直觉
世界坐标系和相机坐标系都是三维直角坐标系，它们之间只差一个旋转和一个平移，不涉及形变和缩放，所以叫刚体变换。数学上可以写为：
P_c = R * P_w + t其中P_w = (X_w, Y_w, Z_w)^T是世界坐标，P_c = (X_c, Y_c, Z_c)^T是相机坐标，R是 3x3 旋转矩阵，t是 3x1 平移向量。这个表达式展开就是三个坐标分量分别做线性组合再加平移。
为了让后续矩阵链统一，我们把平移量并进矩阵里，用齐次坐标来表示：
[ X_c ] [ r11 r12 r13 t1 ] [ X_w ] [ Y_c ] = [ r21 r22 r23 t2 ] [ Y_w ] [ Z_c ] [ r31 r32 r33 t3 ] [ Z_w ] [ 1 ] [ 0 0 0 1 ] [ 1 ]等号右边的 4x4 矩阵就是常说的大名鼎鼎的外参矩阵。它完全由相机在世界坐标系中的位姿决定。这里务必要注意：R是正交矩阵，不是随便填 9 个数进去就行，它需要满足旋转矩阵的性质（各列正交且模长为1）。实际工程中我们一般使用旋转向量（Rodrigues向量）或欧拉角来参数化旋转，再通过Rodrigues变换转成矩阵，就是为了保证正交性这个约束。
那么旋转矩阵到底哪来的？一个很直觉的理解方式是：它就是把世界坐标系的三个基向量，分别投影到相机坐标系的三个基方向上，得到的9个方向余弦。所以旋转矩阵的每一列，其实是世界坐标系的基向量在相机坐标系下的表达。这个东西你在做手眼标定时会反复和它打交道，很多标定结果里的"旋转矩阵不符合正交性"警告，根源就是优化过程没有加正交约束，或者标定板的特征点提取精度太差。
2.2 相机→图像：小孔成像模型与透视投影
从相机坐标到图像坐标，这一步是整个转换链里物理意义最直观的：小孔成像。在理想小孔模型下，三维空间点(X_c, Y_c, Z_c)经由光心投影到成像平面上，形成的图像坐标为(x, y)。根据相似三角形：
x = f * X_c / Z_c y = f * Y_c / Z_c其中f是相机焦距（物理毫米）。这个公式的物理含义非常朴素：一个物体离相机越远（Z_c 越大），它在成像平面上的投影就越小。用人话说，就是"近大远小"。
用齐次坐标可以把这个非线性（除法）关系写成矩阵乘积形式：
Z_c * [ x ] [ f 0 0 0 ] [ X_c ] [ y ] = [ 0 f 0 0 ] [ Y_c ] [ 1 ] [ 0 0 1 0 ] [ Z_c ] [ 1 ]这里有个初学者特别容易搞混的点：相机坐标里的Z_c被保留了下来，它其实表示的是点在相机坐标系下的深度。这也是为什么后面做逆变换（像素坐标恢复到三维坐标）时，必须有深度信息才能求解——一个像素点可以被无数个不同深度的三维点投影得到，这正好对应了"单目相机丢失深度"这个经典问题。
需要强调，这个模型假设成像平面在光心前方，而实际物理成像平面在光心后方，所以严格来说公式里应该有个负号。为了表达清爽，大家约定俗成把成像平面"虚拟地"翻转到光心前方，形成所谓的"虚拟成像平面"，于是负号消失。这不影响任何计算，但理解这一点能帮你解释为什么图像坐标系Y轴和相机坐标系Y轴朝向相反的问题。
2.3 图像→像素：从毫米到像素的离散化映射
成像平面上算出来的(x, y)仍然是物理单位（毫米），但图像在计算机里是以像素为单位的。这一步要解决两个问题：单位换算和原点平移。
像素坐标和图像坐标的关系：
u = x / dx + cx v = y / dy + cydx 和 dy 分别代表每个像素在 x 方向和 y 方向上的物理尺寸（毫米/像素），cx、cy 是主点在像素坐标系下的坐标（也就是光轴与成像平面交点的像素位置）。这一步的本质其实就是：先缩放（把毫米除以每像素毫米数得到像素），再平移到像素坐标系的原点（左上角）。
矩阵形式：
[ u ] [ 1/dx 0 cx ] [ x ] [ v ] = [ 0 1/dy cy ] [ y ] [ 1 ] [ 0 0 1 ] [ 1 ]这里有一个我在工程里常提醒自己的细节：dx 和 dy 并不一定相等。如果传感器像素是矩形的而非正方形，或者图像经过了缩放处理，那么这两个值就会不同，体现在内参矩阵里就是 fx ≠ fy。
2.4 链路贯通：内参矩阵与外参矩阵如何组合成完整的投影矩阵
把三步连在一起，从世界坐标到像素坐标的完整链路就可以用一个矩阵乘法链表达：
Z_c * [ u ] [ fx 0 cx 0 ] [ r11 r12 r13 t1 ] [ X_w ] [ v ] = [ 0 fy cy 0 ] [ r21 r22 r23 t2 ] [ Y_w ] [ 1 ] [ 0 0 1 0 ] [ r31 r32 r33 t3 ] [ Z_w ] [ 0 0 0 1 ] [ 1 ]其中fx = f / dx，fy = f / dy，单位是像素。左边那个 3x4 矩阵叫内参矩阵（Intrinsic Matrix），右边那个 4x4 矩阵叫外参矩阵（Extrinsic Matrix）。两者相乘得到一个 3x4 的投影矩阵 P，即：
P = K [R | t]这里的 K 就是内参矩阵，[R|t] 就是外参。
注意几个极容易被忽略的细节：
第一，上面的矩阵乘法中，Z_c实际上是齐次坐标的尺度因子，它在投影过程中被留了下来。一个三维点投影到像素坐标 (u, v) 时，你拿到的其实是 (uZ_c, vZ_c, Z_c) 这个齐次坐标。要得到真正的像素坐标必须除以 Z_c。这也是为什么很多看起来正确的矩阵公式如果在代码里忘记归一化，算出来的 u、v 会大得离谱——我见过不少人调试半天发现结果差了几十倍，最后发现是忘了除以 Z_c。
第二，内参矩阵里fx和fy只差一个像素长宽比的缩放关系，但cx和cy绝不代表图像宽度和高度的一半。虽然对于理想相机主点应该在图像中心，但实际装配误差、镜头畸变都会让主点偏移。张正友标定法里cx、cy是被当作未知数优化出来的，不是直接取width/2、height/2，这点在手工用公式标定时容易踩坑。
第三，外参矩阵是[R|t]，不是[R t; 0 1]的反方向。很多人在写手写标定代码时，最容易出错的点就是世界坐标转换到相机坐标时，把 R 和 t 的对应关系搞反。正确理解是：P_c = R * P_w + t，意思是先对世界坐标做旋转，再平移。如果把机械臂基坐标系和相机坐标系的变换关系搞反了，你在机械臂抓取时会发现所有坐标都有系统性偏移，而且方向完全反了。
3. OpenCV与工程实践中的常见陷阱：为什么会偏？为什么结果不稳定？
前面推导的是理想小孔模型下的数学链路，但真实世界不理想。镜头有畸变、标定板不完美、图像有噪声、量纲不统一……这些问题在做工程时远比理论公式更让人头疼。这一章节集中讨论我实际工作中反复踩过的坑。
3.1 畸变模型：为什么外参标定了还是对不齐
很多教程讲到这里就结束了，但真实相机是有畸变的。径向畸变（桶形/枕形）和切向畸变（透镜与成像平面不平行）都会让实际像素坐标偏离理想坐标。OpenCV里畸变模型长这样：
x_distorted = x * (1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2) y_distorted = y * (1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y^2) + 2*p2*x*y其中r^2 = x^2 + y^2，k1、k2、k3 是径向畸变系数，p1、p2 是切向畸变系数。注意这里的 x、y 是归一化平面上的坐标（即 x = X_c/Z_c, y = Y_c/Z_c），不是像素坐标。
这个模型的实际含义是：越靠近图像边缘，畸变越严重（因为 r 越大）。所以如果你在做大幅面测量或标定，只在图像中心取点误差会很小，但边缘区域如果没有做畸变校正，坐标误差会非常明显。我记得有一次用广角镜头做室内定位，无视了畸变，结果图像边缘的像素坐标误差达到几十个像素——这在视觉引导里完全不可接受。
在OpenCV里，你做完cv2.calibrateCamera后不仅得到内参矩阵和畸变系数，还要用cv2.undistort或者cv2.undistortPoints去消除畸变。这里有一个顺序问题：如果你拿到的原始图像是畸变图，你需要先做畸变校正，再对校正后的图像做外参解算；如果你直接拿畸变图的像素坐标去反算三维坐标，必须先把像素坐标转成归一化坐标，再套用畸变模型消除畸变，然后才是投影逆变换。顺序错了结果全废。
3.2 单位、轴向和齐次坐标：三个最容易被初学者忽视的量纲问题
单位问题在我遇到过的新手代码里属于重灾区。世界坐标用毫米，内参的 fx 单位是像素，但 fx 是通过 f/dx 得到的，如果某处把相机焦距填成了米、图像坐标填成了像素，结果就是数量级的偏移。我建议所有工程代码里统一约定：世界坐标、相机坐标一律用毫米；图像坐标用毫米；像素坐标用像素；fx、fy 用像素。并且所有关键矩阵在注释里标注单位和含义。
轴向问题同样烦人。像素坐标系 V 轴向下，而图像坐标系 Y 轴向上，这两个方向相反的关系在涉及旋转方向判断时非常致命。比如计算某个点在图像上的偏航角时，如果你直接用像素坐标做 atan2，得到的角度方向会y轴翻转，导致角度正负完全反了。正确做法是先转回图像坐标或者归一化坐标再算角度。
齐次坐标方面的坑在 2.4 里已经说过：不要忘记归一化。这里再补一个工程细节：OpenCV 的cv2.projectPoints函数返回的像素坐标已经帮你做了归一化，所以你拿到的直接就是 (u,v)，而不是齐次坐标。但如果自己写投影函数，很容易遗漏除以 Z_c 那一步，建议写完投影函数后，用一个已知的内外参和三维点去验证一下：将一个角点投影到像素上，再用cv2.calibrateCamera重投影回去，看重投影误差是否达到亚像素级别。这个验证步骤非常值得养成习惯，能帮你隔离到底是标定问题还是投影代码问题。
3.3 为什么重投影误差很小、但实际三维坐标却偏了
这里想聊一个很多工程师困惑的现象：标定结果明明重投影误差只有 0.1 像素，精度看起来非常好，但实际把标定板放在某个位置测量三维坐标时，偏差却有厘米级。这是为什么？
重投影误差描述的是"用标定得到的内外参，把已知三维点投影回图像"与"实际检测到的像素坐标"的差异。它只能反映参数和图像之间的自洽性，完全无法反映标定板和相机之间物理位姿的真实性。举个例子：如果标定板不平整（微小的翘曲），你依然可以拟合出一组内外参使重投影误差很小，但这组外参里隐含了翘曲板的错误三维点假设，用它去测别的目标自然就偏了。
所以做标定时有几个实践经验：
- 标定板必须足够平整，最好用陶瓷或玻璃基板，不要用打印纸贴在不平的纸板上。
- 采集标定图像时，标定板要覆盖视场的各个区域，特别是边缘和角部，且要有不同姿态（倾斜、旋转、远近），不能只在一个平面上平移。否则外参的旋转部分病态，解出来的 R 会不稳定。
- 拍摄标定图像数量建议在 15~20 张以上，不是越多越好，而是要姿态丰富。
- 标定板的方格尺寸必须精确测量，哪怕差 0.1mm 都会直接造成三维坐标的比例误差。
4. 逆变换实操：从像素坐标还原世界坐标，以及它在手眼标定里的应用
很多人学坐标系转换，只学会了正向投影（三维到二维），但实际工程里反向需求极其常见：用户在图像上点了一个目标，你要告诉机器人这个目标在世界坐标系的哪里。这一节把逆变换的完整逻辑和工程实现讲清楚。
4.1 归一化平面与深度恢复：为什么反投影总是少一个方程
已知像素坐标 (u, v)，要恢复世界坐标 (X_w, Y_w, Z_w)。从矩阵链来看，我们有两个方程（像素的两个分量），但有三个未知数（三维坐标的三个分量），所以这是个欠定问题——物理上对应的事实就是：单目相机的一个像素点，对应了一条射线上的无数个三维点。
处理方法就是引入深度先验。实践中常见的选择：
- 目标在一个已知平面上（比如地面、传送带面、工作台面），此时有 Z_w = 0 之类的平面约束，问题变成线性可解。
- 配合双目或者深度相机，直接拿到深度 Z_c。
- 用结构光或者激光测距得到目标深度。
以单目+已知平面为例，完整的逆变换实现思路是：
- 先把像素坐标 (u, v) 转成归一化相机坐标（假设已去畸变）：
x_n = (u - cx) / fx y_n = (v - cy) / fy这一步本质是把像素坐标从像素单位映射回相机坐标系下的归一化平面（Z_c = 1 的平面）。
构建相机坐标系下的射线方向向量
d = (x_n, y_n, 1)。把相机坐标系的射线变换到世界坐标系：
d_w = R^T * d，同时把相机光心平移到世界坐标系：C_w = -R^T * t。射线在世界坐标系下表示为
P_w = C_w + λ * d_w，代入平面方程n·P_w + d = 0（n为平面法向量，d为平面到原点距离），解出 λ，就得到目标的世界坐标。
这个流程在OpenCV里每一步都有现成函数，甚至可以用cv2.solvePnP直接做平面物体的位姿估计，但理解原理后你就能知道它内部每一步在算什么。
4.2 手眼标定场景：外参、内参、机械臂Base之间的关系
在机械臂视觉引导里，坐标系转换链比单个相机的内外参要长得多。通常涉及：像素坐标 → 相机坐标 → 机械臂末端坐标 → 机械臂基座坐标 → 世界坐标。中间的每一步都是刚体变换，标定它们就是手眼标定（eye-in-hand 或 eye-to-hand）。
这里最容易出问题的点是变换矩阵的叠加顺序。假设 eye-in-hand 构型：相机固定在机械臂末端，那么像素坐标要还原到机械臂基座坐标，需要经过：
P_base = T_base_to_end * T_end_to_cam * P_cam其中 T_base_to_end 从机械臂控制器的正运动学获得，T_end_to_cam 就是手眼标定要求解的矩阵，P_cam 是目标在相机坐标系下的三维坐标（需要通过深度或平面约束恢复）。
工程上我见过大量手眼标定后精度上不去的情况，原因集中在：
- 标定时机械臂姿态太少，旋转矩阵解算病态。
- 标定板固定不动，但末端移动时相机离标定板太远，角点提取精度下降。
- 没有先去畸变，直接用畸变图的角点做标定。
- 忽略了机械臂自身运动学参数误差对 T_base_to_end 的影响。
4.3 常用验证手段：如何快速验证你的转换链路是对的
最后分享一个自己一直用的验证流程。每写完一套坐标转换代码，我不会急着上现场，而是先用仿真或者实验数据验证链路正确性。
第一步，生成已知数据：用一组假定的内参、外参，把一批已知的世界坐标三维点投影成像素坐标（可以用cv2.projectPoints），这就相当于构造了"标准答案"。
第二步，用自己写的正变换代码做同样的投影，对比结果，检查齐次坐标归一化、矩阵乘法顺序、畸变模型是否一致。
第三步，用自己写的逆变换代码，把像素坐标还原到某个设定的平面上，再和原始三维点对比。误差应当逼近浮点精度，而不是厘米级偏差。
第四步，实拍验证：放一张标定板在某个已知位姿下，检测角点并计算外参，再把角点的世界坐标投影回图像，看重投影误差是否在 0.1 像素量级。这一步能系统性检查出标定参数和代码中的问题。
这个方法看起来很笨，但确实是排查坐标系转换问题最高效的一招。早年间我花了好几天去调一个转角度偏差问题，后来就是用这个流程逐步缩小范围，最后定位到是旋转矩阵的转置问题——角度差刚好是反的，你光靠看代码很难发现，但用仿真数据一验证立刻现形。
5. 坐标转换代码骨架：OpenCV实现与关键参数选择
前面原理讲得再透彻，最后还是要落到代码。这一节给出一套可直接参考的OpenCV代码骨架，并解释每个环节的参数选择逻辑和注意事项。
5.1 相机标定主流程：棋盘格图像采集与calibrateCamera参数详解
相机标定这一步在任何涉及坐标转换的工程里都是第一关。代码流程无非是：检测棋盘格角点、收集对应的三维点和二维点、调用cv2.calibrateCamera。
import cv2 import numpy as np # 棋盘格参数 pattern_size = (9, 6) # 内角点数 square_size = 25.0 # 方格边长，单位毫米 # 构造棋盘格三维点坐标（Z=0平面） objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32) objp[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) objp *= square_size obj_points = [] # 世界系三维点 img_points = [] # 像素系二维点 images = [...] # 你的标定图片列表 for fname in images: img = cv2.imread(fname) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, pattern_size, None) if ret: # 亚像素精细化，提升角点精度 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) obj_points.append(objp) img_points.append(corners) ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera( obj_points, img_points, gray.shape[::-1], None, None )几个容易被忽略但很关键的点：
cv2.findChessboardCorners匹配的是内角点，不是棋盘格的全部黑白格交点。一张 9x6 的棋盘格意味着有 9 列 6 行内角点，所以选棋盘格时心里要清楚自己用的规格。
cv2.cornerSubPix的搜索窗口大小 (11, 11) 不是随便取的。窗口太小亚像素迭代可能不收敛，太大则会引入相邻角点的干扰。对于大部分工业相机，11x11 或者 15x15 是比较稳的选择。
cv2.calibrateCamera的 flags 参数可以控制是否固定主点、是否估计切向畸变等。默认不加 flags 会估计全部畸变系数（5个），但如果你用高精度工业镜头且确认无切向畸变，可以加上cv2.CALIB_ZERO_TANGENT_DIST只估计径向畸变，能减少过拟合风险。
5.2 投影与反投影：一端到另一端的代码示例
标定完成之后，内参矩阵 mtx、畸变系数 dist 就拿到了。正向投影可以这样写：
# 已知某点在世界坐标系下的坐标，以及该标定板对应的外参 rvec, tvec world_point = np.array([[X_w, Y_w, Z_w]], dtype=np.float64) img_points, _ = cv2.projectPoints(world_point, rvec, tvec, mtx, dist) u, v = img_points[0][0]projectPoints 内部会先做世界到相机的刚体变换、再做透视投影、再做畸变处理、最后用内参转像素。如果你要自己写正向投影做测试，就可以按前面讲的步骤复现，但日常工程里直接用这个函数既快又少出bug。
反向从像素坐标恢复世界坐标（假设目标在Z_w=0平面上，且外参已知）：
# 像素坐标 u, v = 320, 240 # 去畸变，得到归一化相机坐标 ret = cv2.undistortPoints( np.array([[[u, v]]], dtype=np.float64), mtx, dist, P=cv2.estimateAffine2D # 注意此处不是这样用，见下方说明 )这里必须提醒一个细节：cv2.undistortPoints默认只去畸变并把点转换到归一化平面（相当于乘了内参矩阵的逆），输出的是 (x_n, y_n) 归一化坐标。如果你希望它直接输出像素坐标（比如传入 P 矩阵），它会包含畸变去除和坐标映射两步。正确做法是先用它得到归一化坐标，再构建射线，然后与平面求交：
# 更清晰的做法：undistortPoints 得到归一化坐标 pts_norm = cv2.undistortPoints( np.array([[[u, v]]], dtype=np.float64), mtx, dist, P=None ) x_n = pts_norm[0][0][0] y_n = pts_norm[0][0][1] # 相机坐标系下的方向向量（Z_c=1平面） d_cam = np.array([x_n, y_n, 1.0]) # 相机坐标与世界坐标的变换：P_c = R * P_w + t -> P_w = R^T * (P_c - t) R, _ = cv2.Rodrigues(rvec) # 旋转向量转旋转矩阵 C_w = -R.T @ tvec.reshape(3) # 光心在世界坐标系下的位置 # 将归一化向量从相机系转到世界系（只需旋转） d_w = R.T @ d_cam # 与 Z_w=0 平面求交 # P_w = C_w + lambda * d_w, Z_w = 0 lam = -C_w[2] / d_w[2] X_w = C_w[0] + lam * d_w[0] Y_w = C_w[1] + lam * d_w[1]这一步很多人绕不明白：为什么C_w = -R^T * t？因为外参 [R|t] 表达的机器含义是"世界坐标先旋转再平移得到相机坐标"，那么反过来，相机光心（相机坐标原点 P_c = 0）在世界坐标系下的位置满足0 = R * C_w + t => C_w = -R^T * t。这个关系式在手眼标定、坐标系变换里太常用了，值得刻进脑子里。
5.3 关于坐标精度的一些个人习惯总结
坐标转换这套东西，原理搞明白之后，真正拉开工程差距的就是细节习惯。我在实际项目中慢慢形成了一套固定的验证和编码习惯，分享出来供参考。
第一，所有变换矩阵命名要带"从哪到哪"的信息。比如T_cam_to_base而不是T_cb。时间久了代码一多，命名不清晰绝对会坑自己。
第二，标注单位。我会在世界坐标相关的常量后面直接加注释# units: mm，内参相关变量旁边注明# units: pixel。
第三，写一个proj_and_backproj_check的验证函数放在工具库里，每次改完标定参数或者外参，第一时间跑一遍，确认重投影误差正常。
第四，对旋转矩阵的正交性有执念。如果自己解算 R，务必做一次R @ R.T是否接近单位阵的校验，误差大于 1e-6 就要警惕算法稳定性了。
第五，也是最重要的一条——永远不要在没验证的情况下相信一次标定的结果。每次到新环境、换镜头、换分辨率、换机械臂工具中心点（TCP），都要重新评估标定参数是否需要更新。相机外参只要相机被碰过一下、松过一个螺丝，之前的参数就可能全部失效。工业现场松一颗螺丝导致标定失效的问题，我遇到过不下五次。
坐标转换本身不是多高深的理论，但它是视觉工程里最基础也最不可绕过的地基。写这篇文章，是希望后来者能把这条链路的每一个环节都理解得透透的，而不是停留在"调库能出结果就行"的程度。等你真正动手写过一遍投影和反投影函数，亲手排查过一次因为齐次坐标忘归一化导致的诡异偏移，你就会发现这套东西从此长在你身上了——不管换什么传感器、什么相机模型、什么应用场景，底层逻辑永远是那三行矩阵乘法。