★ FEATURED ARTICLE 很多新手看视觉SLAM十四讲翻到坐标系变换那一章时都会有一种感觉每个公式单独看都认识拼在一起就晕。世界坐标系、相机坐标系、图像物理坐标系、像素坐标系这4个坐标系之间的变换可以说是整个视觉SLAM的地基。地基没打牢后面看三角化、PnP、BA优化全是糊涂账。我带的几个同学几乎无一例外是先在这种地方卡了两三周才逐渐绕出来。这篇内容不打算复述教科书而是把4个坐标系变换这件事彻底揉碎每个坐标系是什么为什么SLAM里非要用四个坐标系正向投影怎么从三维点换算到像素反向又要怎么从像素找空间点以及畸变模型、工程里常见的坑。看完你会觉得坐标系变换没有想象中那么难它本质就是一条流水线一个三维点经过三次坐标系切换最终落到照片的某个像素上。1. 四个坐标系是谁以及为什么你总把它们搞混1.1 四个坐标系的定义和分工先说定义。视觉SLAM里反复出现的四个坐标系分别是世界坐标系、相机坐标系、图像物理坐标系也叫归一化平面坐标系、像素坐标系。世界坐标系是用来描述整个场景里所有物体位置的参考系原点可以随便定。在实际SLAM系统中最常见的做法是把第一帧相机的位置当成世界坐标系原点这样最方便因为不用额外处理初始化。当然也可以定在地上某个点、二维码的角点、标定板的中心完全看算法需要。世界坐标系单位是米方向用右手系定义。相机坐标系是以相机光心为原点、以相机自身的朝向为坐标轴的坐标系。从相机看出去x轴指向右方y轴指向下方z轴指向前方这样z轴正好是相机光轴方向也是我们看的方向。它的单位同样是米。这个坐标系的意义在于它把相机当成宇宙中心描述相对于相机那个三维点在哪个方向、多远。图像物理坐标系和像素坐标系比较容易混。严格讲图像物理坐标系是相机成像平面上的坐标系单位是毫米原点在光轴与成像平面的交点上而像素坐标系是图像本身的坐标系原点在图像左上角单位是像素。视觉SLAM十四讲里经常把图像物理坐标系进一步归一化成归一化平面坐标也就是把成像平面放到z1的位置这时候坐标就没有量纲了。我把这四者的关系整理成一个表格方便对照记忆坐标系原点单位物理含义世界坐标系人为设定常为第一帧相机位置米全局固定参考系相机坐标系相机光心米以相机为参照的空间坐标图像物理坐标系/归一化平面光轴与成像平面的交点毫米或无量纲三维点投影到二维平面上像素坐标系图像左上角像素最终图像上的离散位置1.2 一个三维点的像素之旅为什么SLAM里要定义这么多坐标系不直接用世界坐标算到像素因为中间每一步都有独立的物理意义而且每个步骤对应的参数可以被标定、被优化。你可以把一个三维点想象成一个快递包裹世界坐标系是发货地址相机坐标系是物流分拣中心归一化平面是包裹经过的扫描仪像素坐标是最终送到你手上的签收单。包裹每经过一个环节就要换一个系统来记录它的位置。这里有一个特别容易让新手犯迷糊的点像素坐标系的原点在左上角u轴向右v轴向下。搞图像的人都知道图像坐标的y方向是朝下的而在三维坐标系里y通常朝上。这两种直觉打架很容易导致你在写代码时把v坐标取反。记住一点像素坐标系属于二维图像它的v轴向下是图像存储的天然属性不要试图用三维坐标系的右手定则去套它。2. 正向投影链路从世界系到像素系的三次变换2.1 外参世界系到相机系先看第一步把一个世界坐标系下的三维点变换到相机坐标系。这一步在数学上很简单P_c R * P_w tR是旋转矩阵t是平移向量两者合起来就是相机外参也叫相机位姿。外参描述了相机在世界坐标系里的位置和朝向。从几何直观上去理解R负责把世界坐标系的三个轴旋转到与相机坐标系的三个轴平行t负责把原点从世界坐标系移到相机光心。新手最容易在这里出问题的是R和t的方向。上面这个公式的含义是从世界到相机。如果你手上拿到的R、t是相机在世界坐标系下的位姿比如从SLAM前端直接拿到的估计值那这个R、t其实描述的是从相机到世界在使用前必须做一次逆变换。这个坑我在后文专门讲因为几乎所有初学者都会在这里翻车。用齐次坐标可以把加法和乘法统一成一次矩阵乘法[P_c; 1] [R, t; 0, 1] * [P_w; 1]其中[R, t; 0, 1]是一个4x4的变换矩阵T。这也是为什么视觉SLAM十四讲里到处能看到变换矩阵T的原因——矩阵形式便于把多个变换串联起来也便于后面做位姿优化时统一求导。2.2 归一化平面与内参矩阵三维到二维的压缩第二步把相机坐标系下的三维点投影到归一化平面。相机坐标系下的点是(P_x, P_y, P_z)归一化平面坐标就是x P_x / P_z y P_y / P_z这一步是整个变换里最需要理解的地方。它把三维点的坐标除以深度了相当于把所有点都压到z1这个平面上。在z1的平面上点的位置完全由x、y两个数决定这就是三维到二维的降维。为什么叫归一化因为不管三维点在多远处除完深度之后x、y的取值范围都被归一到同一个尺度。第三步从归一化平面坐标换算到像素坐标u fx * x cx v fy * y cy这里fx和fy是焦距与像素尺寸的比值cx和cy是光心在像素坐标系中的位置单位是像素。写成矩阵形式就是Z * [u; v; 1] K * P_c其中K [[fx, 0, cx], [0, fy, cy], [0, 0, 1]]这就是内参矩阵。注意左边乘了一个深度Z因为P_c的第三个分量就是Z_c等式要成立必须把归一化的比例关系补回来。这里有个容易误解的点fx和fy并不是相机的实际物理焦距而是焦距占多少个像素尺寸。如果传感器每个像素的物理宽度是d毫米那么fx f / d其中f是物理焦距。同一个相机在不同分辨率下采集图像内参也会随之变化所以标定内参时要和图像分辨率绑定。2.3 串起来看一条投影公式贯穿所有变换把三次变换串到一起就是视觉SLAM里最经典的那个投影公式也是我认为整个坐标系变换章节最值得反复抄写的一条式子Z * [u; v; 1] K * (R * P_w t) K * [R | t] * [X_w; Y_w; Z_w; 1]只要把这条公式吃透4个坐标系变换的核心就掌握了。左边是像素坐标乘以深度Z右边是内参矩阵乘以外参矩阵再乘以世界坐标的齐次形式。整条公式把世界坐标系中的三维点和像素坐标系中的二维点直接对应起来。写代码做投影的时候建议先逐行算不要一上来就拼大矩阵。一个比较简单的方式是这样的import numpy as np K np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1.0]]) P_w np.array([X, Y, Z]) # 世界坐标 P_c R P_w t # 相机坐标 p_norm P_c / P_c[2] # 归一化平面坐标 p_pix K p_norm # 像素坐标如果你发现投影出来的像素坐标和实际图像上的位置对不上先别急着怀疑参数按这个顺序逐帧打印P_c、p_norm、p_pix三个中间量看是在哪一步开始偏的。这个习惯比反复调整参数高效得多。3. 反向链路与深度谜题有像素坐标空间点在哪3.1 像素坐标回到归一化平面正推会了反推也要会。已知图像上一个像素点(u, v)我们想知道它对应的三维空间点在哪。第一步先利用内参把像素坐标变回归一化平面坐标x (u - cx) / fx y (v - cy) / fy这一步是纯数学逆运算没有不确定性。不管图像里这个点对应多远的三维点在归一化平面上它的x、y都已经确定了。很容易出错的是这里该用除法有些人写成u * cx或者(u cx) / fx结果全是乱的。记住内参矩阵正推时是乘fx、加cx反推时就是减cx、除fx。到这一步为止我们其实只能确定一条射线因为归一化平面坐标(x, y)只告诉了我们方向没有告诉深度。就像你站在一个路口看到远处有个红点你知道它在哪个方向但它离你10米还是100米从方向上看不出来。3.2 深度怎么补三种传感器不同的思路要想把二维像素点还原成三维相机坐标必须补上深度ZP_c_x x * Z P_c_y y * Z P_c_z Z那么深度Z从哪来不同传感器方案有不同做法这也是SLAM系统选型时最核心的区别之一。单目相机本身测不到深度只能通过多帧图像利用三角化来确定。假设同一个三维点在两帧图像中被观察到且这两帧之间的相机运动已知那么两条视线交汇的点就是三维点的位置这就是三角化的原理。单目SLAM在初始化阶段会花很多功夫就是为了确定一个可靠的深度初值。双目相机通过左右两个镜头看到的视差来计算深度视差越大距离越近。它的深度公式是Z f * b / d其中b是基线长度。双目的好处是单帧就能测深度坏处是计算量大而且基线有限远处点的视差太小深度精度会直线下降。RGB-D相机比如常见的结构光或ToF方案直接在硬件层面把深度测出来图像里每个像素都带着深度值。它的好处是简单直接坏处是室外强光下效果会变差反光、透光物体也会让深度数据出现黑洞。SLAM系统拿到深度之后用上面的公式就能直接从像素坐标恢复相机坐标系下的三维点。3.3 单目尺度不确定性单目SLAM最经典的坑就在这里。因为单目的深度是三角化估计出来的而三角化得到的深度和真实的物理深度之间差一个尺度因子。说白了如果整个场景被放大10倍相机运动轨迹也放大10倍单目相机看到的图像不会有任何区别。这就是为什么单目SLAM估计出来的轨迹和地图没有绝对尺度。你不知道一个路标点是距离你5米还是50米只知道它在一帧里移动了几像素。视觉SLAM十四讲里提到单目初始化必须要有平移运动道理就在这里如果相机原地旋转不产生位移两条视线几乎平行三角化方程退化深度就完全不可观测了。实际工程中想恢复尺度可以借助已知大小的物体。比如某个二维码的实际尺寸是10厘米它在图像里占了多少像素就能推算绝对深度。这是很多AR应用里恢复尺度的思路。所以看到别人做单目SLAM能给出米制轨迹要么是有尺度先验要么是做了其他约束不要以为纯单目能天然输出真实尺度。4. 畸变模型针孔相机之外的真实镜头4.1 径向畸变与切向畸变前面讲的内参模型是理想针孔模型但真实镜头不是完美的针孔。镜头畸变会让实际像素坐标偏离理想位置这个偏离主要体现在两种类型上。第一种是径向畸变图像边缘的点会向中心或者向外侧偏移。向外的叫枕形畸变边缘向外鼓向内的叫桶形畸变边缘向内收。径向畸变用三个系数k1、k2、k3来描述其中k3通常只在鱼眼镜头或畸变特别大的镜头上才需要。公式是x_dist x * (1 k1 * r^2 k2 * r^4 k3 * r^6) y_dist y * (1 k1 * r^2 k2 * r^4 k3 * r^6)其中r^2 x^2 y^2x、y是归一化平面坐标。可以看到离光心越远r越大畸变偏移越明显。第二种是切向畸变因为镜头和成像平面没有完全平行而产生用p1、p2两个系数描述x_dist x 2 * p1 * x * y p2 * (r^2 2 * x^2) y_dist y p1 * (r^2 2 * y^2) 2 * p2 * x * y实际标定得到的畸变系数经常是上面两类同时存在OpenCV的distCoeffs数组顺序一般是[k1, k2, p1, p2, k3]。这个顺序很多人会记反导致标定完了矫正出来画面更扭曲。4.2 畸变矫正的正确流程校正畸变的思路是先建立一个理想无畸变图像对于理想图像中的每个像素坐标反推出它在实际畸变图像上对应的像素然后去采样颜色。具体到公式层面最常见的工作流是对理想归一化坐标(x, y)先用畸变模型算出实际畸变归一化坐标(x_dist, y_dist)再用内参把它变成畸变图像的像素坐标(u_dist, v_dist)最后用双线性插值取出颜色。这个过程在图像处理库里有现成实现通常只要给它内参和畸变系数就能生成一张矫正后的图。但要注意一个很多人忽略的细节视觉SLAM里做特征匹配时不是每一帧都要先把整张图矫正很多系统直接用原图提特征再用undistortPoints对特征点的归一化坐标做矫正。这个流程更高效。也就是说你不需要看到矫正后的图只需要在用到归一化坐标的时候把畸变算进去。4.3 这里最常见的三个坑第一个坑是矫正顺序。如果你先做了内参逆变换把像素坐标变成归一化坐标再在这个归一化坐标上做畸变矫正那么注意方向标定公式表达的是从无畸变到有畸变的正向关系你要从有畸变的观测值反推无畸变坐标需要反过来求一般用迭代或者调用现成的矫正函数。自己写的时候容易把这个方向搞反矫正完越矫越弯。第二个坑是畸变模型和图像分辨率不匹配。内参和畸变系数是配套标定的换分辨率必须重新换算否则图像中心偏移和焦距比例全对不上。我用过一个项目标定和实际跑算法用的分辨率不一致结果特征点投影位置差了几十像素排查了很久才发现是这里的问题。第三个坑是忘掉了像素坐标系和归一化平面坐标系的转换。畸变公式只对归一化坐标成立很多人直接在像素坐标上套畸变公式那结果必然是错的。任何畸变有关计算都要先回到归一化平面坐标这个无边无界的坐标系。5. 工程中反复出现的坐标系列坑与验证方法5.1 右手系、坐标轴方向与库的约定坐标系变换本身不难难在“不同库的坐标系定义不一样”。OpenCV相机的坐标系是x右、y下、z前这是一种右手系而很多机器人框架里默认的相机坐标系也是x右、y下、z前和OpenCV一致但有些标定工具或3D引擎里y轴向上、z轴向后的约定也很常见。如果你从两个不同的库分别拿到外参拼在一起之前一定要检查它们的坐标系定义是否一致。检查方法很简单把一个已知三维点投影到图像上看看它落在左上还是右下。比如世界坐标系原点的投影应该在图像中心附近而z轴正方向上的点应该投影在图像中心偏上还是偏下如果和预期相反说明坐标系方向定义不一致需要翻转对应轴。右手系这一点也很关键。右手系意味着x叉乘y等于z。如果某个库给你的是左手系旋转矩阵的性质、叉乘符号、深度正负都会有问题。判断方法就是看三个坐标轴的朝向关系。视觉里我们讨论的相机坐标系基本都是右手系但不同表示下x、y、z的具体指向有差异拿到外参先验证不要上来就信。5.2 外参的逆世界到相机与相机位姿的区别这是我在实际带项目时发现出现频率最高的问题值得单独拎出来。solvePnP这类函数求解出来的R、t通常描述的是“世界坐标系到相机坐标系”的变换也就是公式P_c R * P_w t里的那个R和t。但SLAM系统里你经常需要的是“相机在世界坐标系下的位姿”也就是反过来的变换。反变换怎么做R的逆就是它的转置因为旋转矩阵是正交阵。平移部分需要通过t -R^T * t来算P_w R^T * (P_c - t) R^T * P_c - R^T * t所以在代码里区分这两个概念非常重要。我给每个变量命名都会加后缀比如R_cw表示“相机到世界”R_wc表示“世界到相机”避免自己都搞混。如果你发现某个点的投影位置完全不对先在纸上画出这两个坐标系想想你用的R、t到底是哪个方向这个习惯能省很多调试时间。5.3 验证变换写没写对的三个土办法写代码验证坐标系变换有几种很土但非常有效的方法。第一种是“已知点投影法”选一个你完全知道三维坐标的点比如标定板某个角点用你写的公式把它投影到图像上如果投影位置和图像里检测到的位置重合说明变换矩阵方向基本没问题。这个验证误差通常应该在一个像素量级。第二种是“往返重投影法”图像上检测一个特征点先用内参逆变换到归一化平面再在某个深度假设下反投影成三维点最后再把三维点用正投影公式投回像素坐标系。如果返回来的像素坐标和原始像素坐标一致说明正反变换的公式成对写对了。不一致的话多半是某一步的矩阵转置或平移方向写反了。第三种方法是“数量级检查法”打印变换后的中间量检查它们的数量级是否合理。比如世界坐标是米量级变换到相机坐标后Z_c不应该出现负数在相机前方的点应该是正深度归一化坐标x、y一般会在几倍的[-1,1]范围内像素坐标应该落在图像尺寸范围内。如果出现几千几万的数量级别怀疑一定是单位或者矩阵乘法写错了。我在实际项目里调试坐标系相关Bug时几乎每次都用到这三种验证方法。特别是当你新接到一套传感器标定数据不管对方说得多确定自己先跑一遍往返重投影基本上5分钟就能确认数据是否可用。这比盲目debug半天再怀疑标定数据要高效得多。说到底4个坐标系变换看似繁琐本质上只是一条流水线。把它拆成外参、归一化、内参、畸变四个环节后每个环节的输入输出都很清晰。视觉SLAM十四讲里后面所有的复杂内容——三角化、PnP、图优化——最终都是回到这条投影链路上来计算误差、调整状态。所以花一两天时间把这条链路彻底攻克绝对值回票价。等你熟练到能够不假思索地写出投影方程、能够在5分钟内检查出变换方向问题SLAM的地基就算是真正稳了。
阅读完成 · 觉得有帮助？
