简介：这份资源是面向计算机视觉研究者与工程开发者的 Matlab PnP 算法代码合集，聚焦相机姿态估计这一核心问题，适用于机器人定位、增强现实、三维重建等场景。包内整合了 SRPnP 及 P3P、EPnP、DLS 等多种透视 n 点求解实现，并配套特征匹配、几何验证与后处理优化的完整流程，方便读者对比不同算法在精度与鲁棒性上的差异。资源共 116 个文件，以 100 个 m 脚本为主体，另含 bmp 示例图像、mat 数据、txt 说明及 readme、md 文档，压缩包约 8.84MB，目录结构清晰，便于按模块检索与二次开发。目前已有 1094 人学习下载，读者可借助源码深入理解 PnP 原理与相机模型，快速搭建实验环境、复现算法结果，并在此基础上进行改进与工程落地。
1. 从一组 PnP 代码包说起：SRPnP 到底解决了谁的痛点
做视觉位姿估计的同行大概率都经历过这个场景：相机标定完了，2D-3D 对应点也拿到了，结果一跑solvePnP出来的旋转矩阵抖得没法看，重投影误差忽大忽小，换个视角直接翻车。问题往往不在标定，而在 PnP 求解器本身对噪声、外点、平面退化场景的鲁棒性差异。这次拆的这个 Matlab 资源包，把包括 SRPnP 在内的多种 PnP 算法集中到了一起，用同一套接口跑对比实验，省去了一个个去 GitHub 翻仓库、改接口、对齐数据格式的麻烦。它适合做 SLAM 前端、AR 注册、机器人抓取位姿估计的工程师，也适合正在写论文需要横向对比 PnP 求解精度的研究生。Matlab 环境下直接能跑，不需要编译 C++ 扩展，对算法验证阶段来说省事不少。
2. PnP 求解器的选型逻辑与资源包结构拆解
2.1 为什么 PnP 不是调一个函数就完事
PnP（Perspective-n-Point）要解决的问题很明确：已知 n 个 3D 空间点和它们在图像上的 2D 投影，求相机的旋转 R 和平移 t。听起来是个纯数学问题，但实际工程里麻烦在于——不同算法对噪声模型、点对数量、空间构型的假设完全不同。
常见做法是分两派。解析解派以 P3P、EPnP 为代表，P3P 用 3 个点就能解出最多四个候选位姿，速度快但对外点零容忍；EPnP 把问题转化到四个控制点的加权和上，O(n) 复杂度，n 大时效率优势明显，但对平面构型会退化。迭代优化派以 DLT 加 LM 精化、正交迭代（OI）为代表，精度高但依赖初值，初值不好直接收敛到局部极小。
SRPnP 走的是另一条路。它的核心思路是在求解过程中引入尺度不变性和重投影误差的鲁棒核函数，对噪声和部分外点有更好的抑制能力。具体来说，它不像 EPnP 那样先线性求解再精化，而是在迭代框架里直接优化一个带鲁棒权重的目标函数，权重根据当前残差自适应调整。这意味着在点对里有少量误匹配时，SRPnP 不会像 EPnP 那样被带偏。
这个资源包的价值就在于：它把 SRPnP、EPnP、P3P、DLT、OI 等算法统一封装成PnP_Solver类，输入都是points3D（n×3）、points2D（n×2）、相机内参K（3×3），输出都是R（3×3）、t（3×1）。你不需要为每个算法单独写数据适配层，换算法只改一个字符串参数。
2.2 资源包目录结构与核心文件说明
下载解压后，目录结构大致如下（不同版本可能略有差异，以实际为准）：
PnP_Collection/ ├── algorithms/ │ ├── SRPnP.m │ ├── EPnP.m │ ├── P3P.m │ ├── DLT.m │ └── OrthogonalIteration.m ├── utils/ │ ├── skew.m │ ├── reprojectionError.m │ └── generateSyntheticData.m ├── demo/ │ ├── demo_compare.m │ └── demo_noise.m └── README.md核心文件是algorithms/下的各个求解器。utils/generateSyntheticData.m是个实用工具，能生成带高斯噪声和随机外点的合成数据，方便你在没有真实数据时先跑通流程。demo/demo_compare.m是入口脚本，跑一遍就能看到各算法在同一组数据上的重投影误差对比。
提示：如果你的 Matlab 版本较老（比如 R2016b 之前），类定义文件可能报语法错误，建议用 R2018a 及以上版本。
2.3 统一接口的调用方式与参数含义
以 SRPnP 为例，调用方式如下：
% 假设已有 3D 点 points3D (n×3), 2D 点 points2D (n×2), 内参 K (3×3) addpath(genpath('PnP_Collection')); % 调用 SRPnP 求解 [R, t, info] = SRPnP(points3D, points2D, K); % info 结构体包含迭代次数、最终重投影误差、收敛标志 fprintf('迭代次数: %d\n', info.iterations); fprintf('重投影误差: %.6f\n', info.reprojError);参数说明：points3D是 double 类型的 n×3 矩阵，单位与 t 一致；points2D是 n×2 矩阵，像素坐标，原点在图像左上角；K是 3×3 内参矩阵，格式为[fx 0 cx; 0 fy cy; 0 0 1]。返回值R是 3×3 旋转矩阵，t是 3×1 平移向量。info里的reprojError是均方根重投影误差，单位像素，这个值直接反映求解质量。
如果你要换 EPnP，只需把函数名改成EPnP，输入输出完全一致。这种统一接口设计对做对比实验特别友好，写个循环就能批量跑。
3. 动手复现：从合成数据到真实图像的完整流程
3.1 用合成数据验证各算法的基础精度
在拿真实数据跑之前，先用合成数据摸清各算法的脾气。generateSyntheticData.m的调用方式：
% 生成 20 个 3D 点，相机内参 fx=fy=800, cx=320, cy=240 n = 20; K = [800 0 320; 0 800 240; 0 0 1]; [points3D, points2D, R_gt, t_gt] = generateSyntheticData(n, K, 'noise', 0.5, 'outlierRatio', 0); % 分别用三种算法求解 algorithms = {'EPnP', 'SRPnP', 'OrthogonalIteration'}; for i = 1:length(algorithms) func = str2func(algorithms{i}); [R, t, info] = func(points3D, points2D, K); rotErr = acos((trace(R_gt' * R) - 1) / 2) * 180 / pi; transErr = norm(t - t_gt) / norm(t_gt) * 100; fprintf('%s: 旋转误差 %.4f 度, 平移误差 %.2f%%, 重投影 %.4f px\n', ... algorithms{i}, rotErr, transErr, info.reprojError); end这段代码的逻辑是：先生成一组已知真值的合成数据，噪声标准差 0.5 像素，无外点。然后循环调用三个算法，分别计算旋转误差（用旋转矩阵的迹反推角度）、平移相对误差和重投影误差。str2func把字符串转成函数句柄，避免写三遍重复代码。
参数怎么改：noise控制高斯噪声标准差，单位像素，0.5 是比较温和的条件，调到 2.0 就能看出算法差异；outlierRatio是外点比例，0 到 1 之间，设成 0.2 表示 20% 的点是误匹配。建议先跑noise=0.5, outlierRatio=0建立基线，再逐步加噪声和外点，观察哪个算法先崩。
我一般会跑三组：干净数据、高噪声数据、带外点数据。干净数据下 EPnP 和 SRPnP 差距不大，都在 0.1 度以内；噪声到 2.0 像素时 EPnP 开始抖，SRPnP 还能稳住；加到 20% 外点时 EPnP 直接偏出几度，SRPnP 因为鲁棒核函数的存在，误差控制在 1 度以内。这就是选型的依据——你的场景有没有外点，决定了值不值得上 SRPnP。
3.2 真实图像数据的准备与格式对齐
合成数据跑通后，换真实数据。你需要准备三样东西：3D 点坐标、对应的 2D 像素坐标、相机内参。3D 点可以来自标定板、已知尺寸的物体模型、或者 RGB-D 相机反投影。2D 点用角点检测或手动标注。
常见做法是用棋盘格标定板，Matlab 的detectCheckerboardPoints能直接输出角点像素坐标，同时estimateCameraParameters给出内参。3D 点按棋盘格的实际物理尺寸构造，比如方格边长 30mm，那 3D 点就是(i*30, j*30, 0)。
% 假设已通过标定得到 K，棋盘格角点像素坐标 imagePoints (n×2) % 棋盘格方格尺寸 30mm，构造对应的 3D 点 boardSize = [6, 9]; % 内角点数 squareSize = 30; % mm [X, Y] = meshgrid(0:boardSize(2)-1, 0:boardSize(1)-1); points3D = [X(:) * squareSize, Y(:) * squareSize, zeros(numel(X), 1)]; points2D = imagePoints; % 注意：points3D 和 points2D 的顺序必须一一对应 % 如果角点检测的顺序和 meshgrid 展开顺序不一致，需要手动调整这里最容易翻车的地方是点对顺序。detectCheckerboardPoints返回的角点顺序是从左上到右下逐行排列，而meshgrid展开的顺序也是逐行，但 X 和 Y 的方向可能相反。建议先画个散点图确认对应关系：把points3D投影到图像上，看是否和points2D重合。如果不重合，交换 X 或 Y 的顺序再试。
参数说明：squareSize的单位决定了t的单位，用 mm 则 t 是 mm，用 m 则 t 是 m。boardSize是内角点数，不是方格数，6×9 的棋盘格内角点是 5×8。这些细节搞错，后面误差会大得离谱。
3.3 批量对比实验的脚本组织与结果可视化
单次跑通不够，要做统计才有说服力。我一般会写一个批量脚本，对同一组数据加不同随机种子，跑 50 次取均值和标准差。
numTrials = 50; results = struct(); algorithms = {'EPnP', 'SRPnP', 'P3P', 'DLT', 'OrthogonalIteration'}; for a = 1:length(algorithms) rotErrors = zeros(numTrials, 1); transErrors = zeros(numTrials, 1); for trial = 1:numTrials rng(trial); % 固定随机种子，保证可复现 [points3D, points2D, R_gt, t_gt] = generateSyntheticData(20, K, ... 'noise', 1.0, 'outlierRatio', 0.1); func = str2func(algorithms{a}); [R, t, ~] = func(points3D, points2D, K); rotErrors(trial) = acos((trace(R_gt' * R) - 1) / 2) * 180 / pi; transErrors(trial) = norm(t - t_gt) / norm(t_gt) * 100; end results.(algorithms{a}).rotMean = mean(rotErrors); results.(algorithms{a}).rotStd = std(rotErrors); results.(algorithms{a}).transMean = mean(transErrors); results.(algorithms{a}).transStd = std(transErrors); end % 打印对比表格 fprintf('%-25s %-15s %-15s\n', '算法', '旋转误差(度)', '平移误差(%)'); for a = 1:length(algorithms) r = results.(algorithms{a}); fprintf('%-25s %.4f ± %.4f %.2f ± %.2f\n', ... algorithms{a}, r.rotMean, r.rotStd, r.transMean, r.transStd); end这段脚本的关键点是rng(trial)固定随机种子，保证每次实验的数据可复现。noise=1.0, outlierRatio=0.1是中等难度条件，能拉开算法差距又不至于全崩。输出用±格式同时给出均值和标准差，标准差大的算法说明稳定性差，哪怕均值好看也不可靠。
跑完这个脚本，你手里就有一张各算法在统一条件下的性能对比表。这张表就是你选型的依据，也是论文里能直接用的实验数据。
4. 避坑与排查：PnP 求解中最容易翻车的五个地方
4.1 重投影误差很小但位姿完全不对
现象：info.reprojError只有 0.3 像素，看起来完美，但把 R 和 t 拿去用，发现物体位置偏了几十厘米。
原因：平面退化。当所有 3D 点共面时，PnP 问题存在二义性，有两组位姿都能让重投影误差很小，算法可能收敛到了错误的那组。EPnP 和 DLT 在平面场景下尤其容易出这个问题。
解决：检查 3D 点的空间分布，用rank(points3D - mean(points3D))看秩，如果是 2 就是共面。共面时优先用 P3P 加随机采样验证，或者引入额外的非共面点。SRPnP 因为迭代框架里带了先验约束，平面退化时比 EPnP 稳，但也不是万能的，最好还是从数据源头解决。
4.2 点对数量少于 6 个时 EPnP 直接报错
现象：调用EPnP时提示Not enough points，但明明有 4 个点。
原因：EPnP 需要至少 6 个点对才能稳定求解控制点权重，少于 6 个时方程欠定。P3P 理论上 3 个点就够，但实际中 3 个点对噪声极其敏感，稍微抖一点就解偏。
解决：如果只有 3 到 5 个点，用 P3P 加 RANSAC 多试几组，或者用 DLT 加 LM 精化。但更根本的建议是：尽量保证 6 个以上的点对，且点不要集中在一小块区域。点分布越分散，求解越稳。
4.3 内参矩阵 K 的格式写错导致全盘皆输
现象：所有算法输出的 t 都大得离谱，或者 R 接近单位矩阵但明显不对。
原因：K 矩阵写成了[fx 0 cx; 0 fy cy; 0 0 1]之外的格式，比如把cx, cy写成了归一化坐标，或者 fx 和 fy 写反了。还有一种常见错误是 K 转置了。
解决：在调用 PnP 之前，先手动验证 K。拿一个已知 3D 点，用K * [R_gt t_gt] * [X; 1]投影，看像素坐标是否合理。如果投影结果和预期差很远，先查 K。另外注意 Matlab 的矩阵索引是列优先，构造 K 时别把行列搞混。
4.4 外点比例超过 30% 时所有算法都崩
现象：加了 40% 的外点后，SRPnP 的旋转误差也超过了 5 度。
原因：鲁棒核函数能抑制外点，但有个上限。当外点比例太高时，内点变成了少数派，鲁棒权重反而会把内点压下去。这是所有鲁棒方法的共同局限，不是 SRPnP 独有的问题。
解决：先上 RANSAC 做粗筛，把外点比例降到 20% 以下，再用 SRPnP 精化。RANSAC 的阈值根据重投影误差设，一般 2 到 3 像素。如果外点实在太多，说明前端匹配出了问题，得回去查特征描述子或光流跟踪，PnP 层面救不回来。
4.5 迭代不收敛但没有任何报错
现象：info.iterations达到了最大迭代次数，info.converged是 false，但函数正常返回了 R 和 t，不报错。
原因：SRPnP 和 OI 都是迭代算法，初值不好或者目标函数非凸时可能不收敛。默认最大迭代次数一般是 50 或 100，到了就停，不报错。
解决：检查info.converged标志，如果是 false，先换 EPnP 求个初值，再把初值传给 SRPnP。资源包里应该支持传入初值参数，看 README 确认。另外可以把最大迭代次数调大，但更有效的是换初值。我一般会先用 EPnP 跑一遍，把结果作为 SRPnP 的初值，收敛率和精度都明显提升。
5. 进阶技巧：用 SRPnP 做滑动窗口位姿跟踪的验证方法
把 PnP 用在视频序列里做位姿跟踪时，单帧求解不够，得考虑时序一致性。一个实用技巧是：用上一帧的位姿作为当前帧 SRPnP 的初值，同时把上一帧的重投影误差作为当前帧的参考阈值，超过阈值就触发重检测。
% 滑动窗口跟踪示例 prev_R = eye(3); prev_t = zeros(3, 1); maxReprojThreshold = 3.0; % 像素 for frame = 1:numFrames [points3D, points2D] = getFrameData(frame); % 自定义函数 if frame == 1 [R, t, info] = SRPnP(points3D, points2D, K); else % 用上一帧位姿作为初值 [R, t, info] = SRPnP(points3D, points2D, K, ... 'initR', prev_R, 'initt', prev_t); if info.reprojError > maxReprojThreshold % 误差突增，可能跟丢了，重新初始化 [R, t, info] = SRPnP(points3D, points2D, K); end end prev_R = R; prev_t = t; % 存储或使用 R, t end这段代码的逻辑是：第一帧正常求解，之后每帧把上一帧的 R 和 t 作为初值传给 SRPnP。如果重投影误差超过 3 像素，说明跟踪可能断了，就丢掉初值重新求解。initR和initt是资源包里 SRPnP 支持的命名参数，具体名称以 README 为准。
验证方法：跑完整个序列后，把每帧的info.reprojError画出来。正常情况下应该在 1 到 2 像素之间小幅波动，如果出现尖峰，说明那一帧跟丢了或者数据有问题。另外可以把求解出的 R 转成欧拉角，看角度变化是否平滑，突跳就是异常。
我自己的习惯是：每次拿到一个新的 PnP 代码包，先跑合成数据看基线精度，再跑真实数据看鲁棒性，最后跑视频序列看时序稳定性。这三步走完，这个包能不能用、在什么条件下用、边界在哪，心里就有数了。从那以后我每次换 PnP 求解器都强制走一遍这个流程，省得后面在项目里翻车再回头查。希望帮到你。
本文还有配套的精品资源，点击获取
