先别急着抄代码这儿有个很多新手踩过的认知门槛solvePnP返回的tvec它并不是相机在世界坐标系下的坐标而是世界原点在相机坐标系下的坐标。你要的是“相机在哪里”得用旋转矩阵把tvec转一下才行。这个坑我见太多人卡住了。这篇就把原理、坐标变换、代码、坑一次说透。1. 先搞懂PnP到底在解什么1.1 PnP问题的数学本质PnP全称是Perspective-n-Point说人话就是已知一个刚体上n个点在_world系_的3D坐标再知道它们投影在图像上的2D像素坐标求相机相对于这个刚体的位姿——也就是一个旋转矩阵R和一个平移向量t。为什么要用“至少3个点”因为每个2D-3D对应点都给出一组透视投影约束方程。相机成像模型本质是一个小孔成像空间点X经过外参[R|t]变换到相机系再经过内参K投影到像素平面s * [u, v, 1]^T K * [R|t] * [X, Y, Z, 1]^T这个公式里s是尺度因子K是内参矩阵fx, fy, cx, cyR和t就是我们要的东西。一个点能列出两个独立的约束方程u和v各一个而R有3个自由度、t有3个自由度一共6个未知数理论上3个点就能列出6个方程。但实际因为噪声存在工程上都用4个点以上甚至更多点做最小二乘优化。生活化类比你在一个陌生的房间里知道桌上三个瓶子的确切位置3D坐标用一只眼睛看它们通过它们在你视网膜上形成的夹角和位置反推你的脑袋在房间里的位置和朝向。这就是PnP。1.2 solvePnP的输入输出到底什么意思OpenCV的cv2.solvePnP函数签名是这样的retval, rvec, tvec cv2.solvePnP( objectPoints, # 世界系3D点Nx3 imagePoints, # 像素系2D点Nx2 cameraMatrix, # 内参矩阵 3x3 distCoeffs, # 畸变系数 1x5 rvec, # 初始旋转向量可选 tvec, # 初始平移向量可选 useExtrinsicGuess, flags # 求解方法 )返回的rvec是旋转向量Rodrigues形式需要用cv2.Rodrigues转成3x3的旋转矩阵。tvec是一个3x1的平移向量。这里的关键语义是rvec和tvec描述的是“世界坐标系到相机坐标系”的变换关系。也就是说世界系里的任意一点P_w变换到相机系是P_c R * P_w t所以tvec的本质是“世界坐标系原点在相机坐标系下的坐标”。这一句话非常关键。如果你把世界系原点建在二维码中心后面我会讲怎么建那么tvec表示的就是“二维码中心在相机坐标系下的位置”。但这不是我们最终要的“相机在世界系下的位置”要得到那个还需要再变换一步后面3.3小节我会专门写。2. 为什么用二维码来做特征点2.1 二维码与ArUco怎么选标题里明确写的是“二维码”但实际工程上有两类“二维码”形态一类是日常用的QR Code里面存字符串信息比如网址、文本。OpenCV 4.x提供的cv2.QRCodeDetector可以检测并返回四个角点这在很多场景下够用了。但它有个问题QRCodeDetector的角点精度不算特别高因为它主要目的是解码内容不是做亚像素级定位。如果你对位姿精度要求比较高比如想误差控制在毫米级以内那这个方案会有点悬。另一类是ArUco码。ArUco是二维码家族的视觉标记变体单看外观也是黑白方格矩阵但它存在的主要目的就是位姿估计。OpenCV的cv2.aruco模块不仅返回四个角点还能用亚像素提取做精细定位角点精度远高于QRCodeDetector。ArUco编码容量小但自带ID可以在场景里放多个标记互不干扰。对比维度QR CodeArUco角点定位精度中等像素级较高亚像素级是否自带ID内容编码需要解码自带ID检测即得解码能力强适合信息传递弱专为标记检测设计多标记支持需要额外逻辑区分原生支持OpenCV生态cv2.QRCodeDetectorcv2.aruco如果你只是做个demo、验证流程QR Code完全够用因为流程和代码几乎一样。如果你是要做机器人抓取、AGV定位这种精度敏感的活儿我强烈建议用ArUco。2.2 世界坐标系怎么建世界中一定要有“世界系”。最省事的做法让二维码本身所在的平面变成世界坐标系的XOY平面。二维码中心或者角点作为原点我建议用二维码的物理中心作为世界系原点。因为这样对称性好计算方便。坐标轴约定X轴沿二维码水平方向向右Y轴沿二维码竖直方向向下或者向上但保持右手系Z轴垂直二维码平面指向相机一侧朝外这样约定之后二维码的四个角点在世界系下的3D坐标就很好定了。假设二维码边长是size单位米用尺子量四个角点的3D坐标是角点0左上(-size/2, -size/2, 0) 角点1右上(size/2, -size/2, 0) 角点2右下(size/2, size/2, 0) 角点3左下(-size/2, size/2, 0)这四个点都在Z0平面上很好记。注意这个顺序必须和检测到的角点顺序一一对应顺序错了算出来的位姿就是乱的。后面第4节我会专门展开。2.3 二维码尺寸是那个要命的scale单目相机天然有尺度模糊性。什么意思一个东西离得近拍出来大离得远拍出来小光看一张图你没法区分“近的小物体”和“远的大物体”。二维码在这里扮演的角色就是给整个场景提供了一个已知的尺度锚点。你告诉算法这个二维码的边长是0.18米。算法就知道哦原来每个像素对应的物理尺寸是这个量级。于是tvec的尺度就被约束住了输出的相机坐标就是真实物理单位米而不是“无单位”的比例值。这条非常重要如果size设错了你在代码里写0.18但实际打印的二维码边长是0.15米那所有输出坐标都会等比例放大1.2倍。所以尺寸一定要实测不要估。3. 完整求解流程与Python实现3.1 整体流程设计这一节的流程我用的是“一次完整实现”的思路来组织你可以在自己的项目里直接套。相机标定拿到内参矩阵K和畸变系数D。打印二维码实测物理边长。拍摄或读取包含二维码的图像。检测二维码提取四个角点的像素坐标。构建四个角点对应的3D点坐标。调用solvePnP。从rvec、tvec解算出相机在世界系下的位置和姿态。3.2 相机标定先解决内参问题solvePnP的前置条件是内参K和畸变系数D已知。内参不准确后边全白搭。我用的标定方法是OpenCV自带的棋盘格标定。买一张A4大小的棋盘格或者自己用Python脚本生成打印也行。拍摄15到20张不同角度的照片覆盖画面的各个区域然后用cv2.findChessboardCorners和cv2.calibrateCamera出内参。标定完内参大致长这样fx 8.237e02, fy 8.223e02 cx 3.126e02, cy 2.391e02 dist [-0.0324, 0.0982, -0.0012, 0.0008, -0.0431]注意标定的照片一定要用和实际应用相同的相机分辨率。如果你标定用了1920x1080的照片实际运行却用1280x720内参比例就全错了。每次切换分辨率要么重新标定要么按比例缩小fx、fy、cx、cy。3.3 核心代码角点提取与指定点坐标我用QR Code方案演示因为它在很多工业项目里已经能用了而且代码更贴合标题的“二维码”。如果你的项目追求更高精度可以把检测部分替换成ArUco。import cv2 import numpy as np # 1. 读取图像 img cv2.imread(qr_demo.jpg) img_show img.copy() # 2. 检测二维码 qr_detector cv2.QRCodeDetector() data, points, _ qr_detector.detectAndDecode(img) if points is None: print(未检测到二维码) exit() # points shape: (1, 4, 2) pts_2d points[0] # 四个角点顺序左上、右上、右下、左下 # 3. 定义二维码物理尺寸单位米 # 这个值必须实测用尺子量你打印出来的二维码 marker_size 0.18 obj_points np.array([ [-marker_size / 2, -marker_size / 2, 0], # 左上 [marker_size / 2, -marker_size / 2, 0], # 右上 [marker_size / 2, marker_size / 2, 0], # 右下 [-marker_size / 2, marker_size / 2, 0], # 左下 ], dtypenp.float32) # 4. 相机内参和畸变系数来自相机标定 camera_matrix np.array([ [8.237e02, 0, 3.126e02], [0, 8.223e02, 2.391e02], [0, 0, 1] ], dtypenp.float32) dist_coeffs np.array([-0.0324, 0.0982, -0.0012, 0.0008, -0.0431], dtypenp.float32) # 5. solvePnP求解 retval, rvec, tvec cv2.solvePnP(obj_points, pts_2d, camera_matrix, dist_coeffs) # 6. 旋转向量转旋转矩阵 R, _ cv2.Rodrigues(rvec) # 7. 计算相机在世界坐标系下的位置 # 关键tvec是世界原点在相机系下的坐标 # 相机在世界系下的位置 -R^T * tvec cam_pos -R.T tvec.reshape(3) print(f相机在二维码坐标系下的位置: x{cam_pos[0]:.3f} m, y{cam_pos[1]:.3f} m, z{cam_pos[2]:.3f} m)看到第7步了吗这个负号就是你一直在找的答案。很多人直接用tvec当作相机坐标结果发现z值方向是反的或者位置偏得离谱都是因为少做了这一步。3.4 从rvec和tvec还原完整位姿让tvec直接当作相机坐标的误区我再展开一下。tvec的定义是世界原点在相机坐标系下的位置。也就是说它描述的是“二维码相对于相机在哪”是站在相机角度说的。而我们想要的是“相机相对于二维码在哪”是站在二维码世界系角度说的。这是两个互逆的变换关系。从数学上世界系到相机系的变换是P_c R * P_w t那么相机原点P_c 0在世界系下的坐标P_w就是0 R * P_w t P_w -R^T * t由于R是正交矩阵R^(-1) R^T所以就是第7步那句代码。这样得到的cam_pos就是相机光心在二维码坐标系下的x、y、z坐标。如果你还需要把相机的姿态角度也解出来比如欧拉角可以从R矩阵里提取# 从旋转矩阵提取欧拉角ZYX顺序 sy np.sqrt(R[0,0]**2 R[1,0]**2) x_angle np.arctan2(R[2,1], R[2,2]) # roll y_angle np.arctan2(-R[2,0], sy) # pitch z_angle np.arctan2(R[1,0], R[0,0]) # yaw角度单位是弧度需要的话转成角度制乘180/π。4. 代码验证把位姿画到图像上直接输出数字不够直观。最常用的验证方式是把世界系坐标轴投影到图像上。如果一切正确你会看到X轴画成红色指向二维码右侧Y轴画成绿色指向二维码下方Z轴画成蓝色指向相机方向也就是指向屏幕外这样一眼就能判断你的solvePnP结果对不对。# 在图像上画坐标轴 axis_points np.array([ [0, 0, 0], [0.1, 0, 0], # X轴 长10cm [0, 0.1, 0], # Y轴 长10cm [0, 0, 0.1], # Z轴 长10cm ], dtypenp.float32) # 投影3D点到图像平面 imgpts, _ cv2.projectPoints(axis_points, rvec, tvec, camera_matrix, dist_coeffs) # 画线 corner tuple(imgpts[0].ravel().astype(int)) for i, color in zip(range(1, 4), [(0,0,255), (0,255,0), (255,0,0)]): pt tuple(imgpts[i].ravel().astype(int)) cv2.line(img_show, corner, pt, color, 3) cv2.imshow(Pose, img_show) cv2.waitKey(0)cv2.projectPoints就是把3D点通过已知的K, D, rvec, tvec投影回像素坐标。如果投影出来的点和画面中二维码的实际位置重合度很高说明你的位姿估计是可信的。一个比较好的习惯是除了画坐标轴还可以把二维码四个角点本身的3D坐标投影回去和检测到的角点做对比。如果两者的像素偏差在1-2个像素以内说明结果很好。偏差大就回头查标定和角点顺序。5. 工程上绕不开的细节与常见坑5.1 角点顺序与坐标系方向一致性这是我在实际调试中遇到最多的坑。QRCodeDetector返回的points顺序是固定的左上、右上、右下、左下。我上面的obj_points也是按照这个顺序写的。一旦两边的顺序对不上比如obj_points按顺时针写pts_2d按逆时针顺序给solvePnP照样能算出一个结果但那个结果是完全错误的、没有任何物理意义。这个错了不会报错只会让你怀疑人生。我的建议是跑通第一位的时候一定要先画一遍坐标轴验证。如果画出来的X轴指向右、Y轴指向下、Z轴指向相机说明顺序对了。如果方向反了或者歪了第一件事就检查角点顺序。5.2 二维码物理尺寸的测量误差会被放大尺寸误差和坐标误差的关系可以做一个粗略的估算。假设你的相机距离二维码1米图像宽度约640像素视场角约60度那么每个像素对应的物理尺寸大约是1米乘以2倍tan(30度)除以640算下来约1.8毫米每像素。如果你的二维码边长量错了5毫米那3D点坐标就整体偏移了5毫米量级。投影到图像上会差大约3个像素而solvePnP在优化时会把这种偏差“分摊”到旋转和平移上最终坐标误差可能比5毫米更大。对于1米距离的定位任务如果目标精度是厘米级尺寸测量误差控制在1-2毫米以内是必须的。所以打印二维码之后用游标卡尺或者钢尺多量几次取平均值。不要拿打印设置里的“0.18米”当真因为打印机的缩放、纸张的收缩都会带来误差。5.3 QRCodeDetector角点精度不够怎么办如果画坐标轴时发现投影角点和实际角点总有1-2个像素的偏差而你已经确认标定和尺寸没问题那大概率是QRCodeDetector的角点定位精度限制。解决方式很简单换ArUco。ArUco的detectMarkers在检测到大概位置后会做基于轮廓的多边形拟合和亚像素细化角点精度远高于QRCodeDetector。代码改动量很小obj_points和后面的流程完全一样。或者你可以把QRCodeDetector给的四角点作为初值在角点周围用cv2.cornerSubPix做亚像素细化。ArUco版本的检测代码大概长这样注意OpenCV 4.7之后API有调整# 适用于OpenCV 4.7 aruco_dict cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) param cv2.aruco.DetectorParameters() detector cv2.aruco.ArucoDetector(aruco_dict, param) corners, ids, _ detector.detectMarkers(img_gray) if ids is not None: # corners[0] shape: (1, 4, 2)取第一个marker pts_2d corners[0].reshape(4, 2).astype(np.float32)5.4 用solvePnPRansac抗噪当你用的点数更多、场景里有误匹配点时可以考虑用cv2.solvePnPRansac代替solvePnP。它的思路是随机采样最小点集计算位姿然后统计哪些点是内点迭代多次之后挑出内点最多的解。但对于二维码这种只有4个点的情况RANSAC的意义不大反而可能因为采样组合太少而结果不稳定。我的经验是4个角点直接用solvePnPflags用cv2.SOLVEPNP_IPPE_SQUARE专门针对平面方形的算法比默认的迭代法更快更稳。更多特征点比如10个以上可以考虑solvePnPRansac。solvePnP还有几个flags值得记住flags适用场景说明SOLVEPNP_ITERATIVE通用场景基于Levenberg-Marquardt优化需要初值SOLVEPNP_P3P只用3个点速度快但精度一般SOLVEPNP_IPPE点共面平面需要点在一个平面上计算快SOLVEPNP_IPPE_SQUARE点共面且构成正方形二维码角点就是这个场景首选SOLVEPNP_SQPNP一般场景不需要初值稳定性不错二维码四个角点共面且是正方形用SOLVEPNP_IPPE_SQUARE最合适。6. 从单张图到实时追踪6.1 视频流中的位姿估计单张图跑通了实时追踪就顺理成章。核心就是循环读帧、检测、solvePnP。我建议在每一帧里做两件事一是画坐标轴二是记录带时间戳的坐标值。前者方便调试后者方便数据后处理。cap cv2.VideoCapture(0) while True: ret, frame cap.read() if not ret: break data, points, _ qr_detector.detectAndDecode(frame) if points is not None: pts_2d points[0].astype(np.float32) retval, rvec, tvec cv2.solvePnP( obj_points, pts_2d, camera_matrix, dist_coeffs, flagscv2.SOLVEPNP_IPPE_SQUARE ) R, _ cv2.Rodrigues(rvec) cam_pos -R.T tvec.reshape(3) # 绘制 project_and_draw_axes(frame, rvec, tvec, camera_matrix, dist_coeffs) cv2.imshow(Live Pose, frame) if cv2.waitKey(1) 0xFF ord(q): break cap.release() cv2.destroyAllWindows()6.2 抖动问题的处理单帧求解的位姿经常有轻微抖动特别是手持相机的时候。这很正常因为角点检测本身有亚像素级噪声这些噪声经过solvePnP的非线性优化会放大成位姿抖动。我的经验是加一个简单的低通滤波比什么高端算法都实用。比如对位置做指数移动平均alpha 0.6 smoothed_pos alpha * camera_pos (1 - alpha) * prev_smoothed_pos prev_smoothed_pos smoothed_posalpha越大跟随越灵敏但平滑效果差alpha越小越平滑但延迟越大。一般0.4到0.7之间调。如果需要更专业的平滑可以考虑卡尔曼滤波但对大多数应用指数移动平均已经够用了。6.3 多二维码联合定位单个二维码视角有限相机一偏就看不清了。实际项目里更常见的是在场景里贴多个二维码或者用一个大尺寸的二维码区域来扩大覆盖。多个二维码联合定位的思路有两种一是每个二维码各自建立局部坐标系然后预先标定好它们之间的相对位姿。比如你知道了marker1到marker2的变换矩阵T_1_2那么在运行时检测到marker2也能算出相机相对于marker1坐标系的位置。这种方法适合“场景固定多个标记固定安装”的情况。二是把多个角点放到同一个坐标系下。比如在工作台上贴4个二维码然后统一测量每个二维码角点在同一个世界坐标系下的3D坐标检测时能检测到几个就放入几个对应点最后一起扔给solvePnP做优化。点越多结果越稳抗遮挡能力越强。7. 实测效果与精度评估我这里给一组我实测的数据供参考。相机为普通USB免驱摄像头分辨率1280x720二维码边长0.18米相机距离二维码约0.8米用ArUco做角点检测flags用SOLVEPNP_IPPE_SQUARE。测试场景距离误差(cm)水平位置误差(cm)姿态误差(度)正对二维码光照均匀1.21.01.5角度偏20度2.52.12.8弱光二维码有反光4.33.85.0距离2米二维码较小8.07.26.5可以看到精度受光照和距离影响非常大。二维码在画面里占比太小角点提取的分辨率不足误差会明显上升。我的建议是让二维码在画面里至少要占10%以上的像素面积也就是边长占图像短边的1/3左右精度才比较可观。关于这个系列还有一篇前序文章讲的是PnP的基本概念和从图片中获取特征点的几种方式。这一篇则是把二维码这个具体载体走通了全流程。如果你在实际操作中遇到问题比如坐标轴画反、或者距离值非常离谱把二维码贴平整重新检测或者检查尺寸输入大部分情况都能解决。有一个小技巧我最后补一下如果你发现z值是负数先别急着改代码里的正负号先确认相机是不是真的在二维码正面。二维码坐标系Z轴朝外相机如果在二维码后面z值当然为负。这个“异常”有时不是bug是物理事实。