简介一套基于MATLAB的三维重建辅助代码包聚焦两视图间本质矩阵的求解适合正在学习三维重建、对极几何或相机相对位姿估计的研究者与相关专业学生。内嵌五点算法实现只需五对匹配点即可估计本质矩阵相比传统九点法计算复杂度更低同时引入期望最大化思想处理噪声数据并借助秩2截断修正矩阵估计帮助使用者在实际测量不完备的情况下恢复相对旋转和平移进而衔接特征匹配与三角测量流程。压缩包共5个文件含3个m脚本作为核心算法实现2个asv文件为自动保存副本整体约10KB体量小、便于逐行对照研究。目前已有170人学习下载通过动手运行代码可以直观理解五点法、本质矩阵优化与三维点云重建间的关键衔接对入门或调试相关系统都具备直接参考价值。1. 为什么三维重建绕不开 5 点本质矩阵拿到一个叫lhd_5ptEssentialfordistribution.zip的 MATLAB 包读文件名基本就能猜到它在干什么5pt指 5 点法Five-Point AlgorithmEssential指本质矩阵Essential Matrixfordistribution说明作者把代码整理成适合分发集成的版本。在双视三维重建里本质矩阵用一组经过相机内参校准过的对应点把两帧之间的旋转和平移约束到一条二次曲面上5 点法正是这个环节里样本量最少的解法。这套代码适合两种人一是做三维重建但不想依赖 OpenCV 内部封装的算法工程师二是需要把本质矩阵估计嵌入 MATLAB 图像处理流程的科研或工程开发。它能直接解决重建中最容易出错的「相对位姿怎么从极线约束里求出来」这一步。2. 5 点本质矩阵与 8 点法的区别什么时候该用 5ptEssential2.1 对极约束与本质矩阵的数学基础做三维重建第一步几乎都是估计两帧相机之间的相对位姿。假设左相机坐标系作为世界系右相机相对于左相机有旋转 R 和平移 t。对空间点 P它在两帧归一化平面上的投影点为 x1 和 x2两者满足对极约束 x2^T E x1 0其中 E [t]× R[t]× 是 t 的反对称矩阵。本质矩阵的数学性质有两条秩为 2且两个非零奇异值相等。这两条在后面分解 R、t 时会反复用到也是 5 点法能成立的根本依据。还有一个自由度层面的直觉在对极约束里E 的自由度是 5旋转 3 个自由度 平移方向 2 个自由度尺度不可观所以 5 对点恰好是理论下界。用少于 5 对点时本质矩阵欠约束用多于 5 对点则是希望用冗余观测对抗噪声。这也是5ptEssential这个文件名里最值得注意的信息——作者把5pt放在最前面就是在提醒使用者这套估计是围绕最小样本量设计的。MATLAB 做三维重建很多人第一反应是直接调用estimateEssentialMatrix但当你需要判断算法在某个场景下是否准确时还是得回到 5 点法本身理解它要解一个 10 次多项式、产生最多 10 个候选解然后用校验点筛选。这个过程不会出现在高层的封装函数里但会影响内点判定和最终点云质量。2.2 5 点法相对 8 点法的优势与代价8 点法是最直观的二视图几何算法把 x2^T E x1 0 展开成关于 E 的九个元素的线性方程用 8 对以上对应点组成线性方程组经 SVD 得到最小二乘解。优点是实现简单一句svd就能跑通缺点是线性解没有自动满足 E 的奇异值约束通常需要事后投影回流形而且在 RANSAC 里一次要抽 8 个样本迭代次数明显更多。5 点法用 5 对对应点即可构造约束再加上 E 的约束条件能得到一个 10 次多项式用隐式特征值法或 Gröbner 基解出候选本质矩阵。它的优势在于样本最少、RANSAC 迭代轮数少而且约束更强抗噪能力好。代价则体现在工程上求解过程必须用归一化坐标避免数值病态候选解需要用至少一个额外校验点来筛选。下表是两者在工程选择上的对照对比项8 点法5 点法最小样本数85求解方式线性最小二乘多项式求解奇异值约束事后投影内建约束RANSAC 迭代理论次数多少对点噪声敏感度高低工程实现复杂度低高实际选型时我一般先用 8 点法做快速粗筛把明显错误的匹配对去掉再用 5 点法进 RANSAC 主循环。两种方法在同一套特征匹配数据上结果差距通常来自野点比例而不是数值精度。这张表还有一个容易被忽略的维度5 点法的多项式解会产生 10 个候选如何在 RANSAC 循环里对每个样本做 10 次模型验证会影响整组耗时。开源实现通常会用“solve for minimal cases verify with one extra point”所以实际 cost 不是纯 5 点求解而是 51。如果你在自己的工程里实现优先选择带预筛选的变体先以 8 点法快速剔除不可能的解再进 5 点主循环性能会好很多。2.3 分发包里常见文件与调用约定像lhd_5ptEssentialfordistribution.zip这类分发包通常不会把所有算法塞进一个文件而是拆成「主求解函数 辅助滤波 示例脚本」三层。常见名称是fivepoint_essential.m、select_solution.m、run_demo.m和README.md。拿到包后先不要急着跑示例先确认它的函数签名是像素坐标还是归一化坐标这直接决定后面要不要内参参与。下面是最常见的调用结构% 典型的 5ptEssential 包接口 points1 [...]; % Mx2 左图特征点像素坐标 points2 [...]; % Mx2 右图特征点像素坐标 K [fx 0 cx; 0 fy cy; 0 0 1]; % 相机内参 % 归一化到归一化坐标先减主点再除以焦距 n1 (points1 - K(1:2,3)) ./ diag(K(1:2,1:2)); n2 (points2 - K(1:2,3)) ./ diag(K(1:2,1:2)); % 主函数返回 3x3 本质矩阵 E fivepoint_essential(n1, n2);这里的points1、points2行数必须一致顺序要一一对应否则整组约束方程都是错的。fx、fy是焦距单位是像素cx、cy是主点。为什么要先减主点再除焦距因为对极约束里的点坐标是无量纲的归一化坐标如果跳过这一步得到的矩阵是基本矩阵 F而不是本质矩阵 E后面分解 R、t 会多出内参耦合三维重建质量直接劣化。如果分发包里没有提供归一化函数不要自己写一个复杂的矩阵求逆版本直接用上面的两行减法除法即可留在基准线上的简单实现比参数花哨但引入缓存错误的实现更可靠。还有一类包会把内参 K 作为输入参数那它内部就会帮你做归一化这时候主函数接收的仍是像素坐标。判断方式很简单看主函数的第一行有没有出现inv(K)或K \ x这类操作。3. 在 MATLAB 里跑通 5 点算法的最小代码与参数矩阵3.1 输入数据组织对应点、相机内参、坐标归一化5 点算法的输入必须是“同一时刻两帧图像的同名点”不是随便找的特征点。常见做法是先detectSURFFeatures或detectORBFeatures再用matchFeatures匹配最后用estimateFundamentalMatrix的Method选项做粗剔除。这里有一个很容易被忽略的前提本质矩阵用的是归一化坐标也就是像素坐标要左乘 K 的逆。如果直接把像素坐标丢给 5 点求解器得到的矩阵不是本质矩阵而是基本矩阵分解出来的 t 会带上内参畸变重建出来全是扭曲的。% 读入两帧图像 I1 imread(left.png); I2 imread(right.png); % 提取并匹配特征 pts1 detectSURFFeatures(rgb2gray(I1)); pts2 detectSURFFeatures(rgb2gray(I2)); [f1, v1] extractFeatures(rgb2gray(I1), pts1); [f2, v2] extractFeatures(rgb2gray(I2), pts2); pairs matchFeatures(f1, f2); % 匹配后的像素坐标 p1 v1(pairs(:,1)).Location; % 左图 p2 v2(pairs(:,2)).Location; % 右图 % 相机内参使用 KITTI 风格标定值 K [718.8560 0 607.1928; 0 718.8560 185.2157; 0 0 1]; % 像素坐标转归一化坐标 n1 (p1 - K(1:2,3)) ./ diag(K(1:2,1:2)); n2 (p2 - K(1:2,3)) ./ diag(K(1:2,1:2));detectSURFFeatures在这里只是示例如果你自己用深度学习匹配器只要最后输出两列坐标即可。注意K(1:2,3)是主点diag(K(1:2,1:2))是两个焦距。这套归一化必须先减主点再除焦距顺序反了会导致 xy 方向尺度不一致解出来的 E 会在两个方向上有缩放。另一个细节是matchFeatures默认使用欧氏距离和唯一匹配但低纹理区域容易产生错误匹配最好在进入 5 点法之前用一个宽松阈值的estimateFundamentalMatrix做 RANSAC 粗筛把明显外点剔除。提取特征时SURF 的参数MetricThreshold默认是 1000但在低纹理户外图像上很容易把关键点数量压到几十个。我一般调到 200先让匹配数量充分一点再用 RANSAC 把内点筛出来。这样做虽然初始误匹配更多但相比一开始就苛求特征检测器整套流程的召回率更高。若你的图像已经去除畸变可以直接用这里的归一化若图像有镜头畸变而没去畸变需要先用undistortImage或undistortPoints处理再进 5 点法。很多三维重建包不会提醒这一步导致边缘区域的点会造成一片噪声。3.2 5 点算法求解 E 矩阵的核心步骤function E fivepoint_essential_linear(x1, x2) % x1, x2: Nx2 归一化坐标 N size(x1, 1); % 构造 9 维对极约束矩阵 A每行对应一个点对 A [x2(:,1).*x1(:,1), x2(:,1).*x1(:,2), x2(:,1), ... x2(:,2).*x1(:,1), x2(:,2).*x1(:,2), x2(:,2), ... x1(:,1), x1(:,2), ones(N,1)]; % 解线性方程 A * e 0取最小奇异值对应的右奇异向量 [~, ~, V] svd(A); E reshape(V(:, end), 3, 3); % 强制本质矩阵的奇异值约束 [U, S, V2] svd(E); E U * diag([1 1 0]) * V2; end这段代码是线性近似版 5 点法真正意义上的 5 点法还要在这个基础上解 10 次多项式但把它当作入门骨架是够用的。构造 A 时每一行对应一对匹配点展开式来自 x2^T E x1 0如果 E 的元素按列展开x1 [x,y,1]x2 [u,v,1]那么 uxE11 uyE12 uE13 vxE21 vyE22 vE23 xE31 yE32 E33 0和上面 A 的列顺序完全一致。V(:,end)是零空间的最小二乘解把它 reshape 成 3x3 时注意 MATLAB 是列优先存储所以要取转置才能得到数学上约定排列的 E。强制奇异值约束那两步很容易被当成多余操作但在实际匹配含噪时不能跳过。diag([1 1 0])保证秩为 2奇异值两个为 1这样分解 R、t 才会有稳定结果。分发包里若提供的是完整fivepoint_essential你还会看到它对候选解做复数滤除和 Cheirality 检查这些都属于后处理不改变核心步骤。如果直接用这套线性近似求解后续分解出的 R,t 可能不唯一它只能作为调试桩或初值不要把它当成生产级实现。3.3 从 E 矩阵分解出 R 和 t 的陷阱拿到 E 以后下一个坑是分解。E 的 SVD 分解是 U*S*V约束 S 必须为 diag(1,1,0)。但实测的 E 可能因为噪声、野点和数值误差不满足这个约束常见处理是先对 E 做一次强制修正[U,S,V]svd(E); Sdiag([1 1 0]); EU*S*V;。然后有两种 R,t 候选组合。MATLAB 的cameraPose或relativeCameraPose会返回四种可能需要通过三角化点是否在相机前方来筛选。这里不要直接用[U,~,V]的教科书公式因为 MATLAB 的svd返回的 U、V 可能带反射导致右手、左手坐标系问题稳妥做法是检查 det(U*V)如果为 -1 就把第三列变号。% 从 E 中恢复四种 R,t 组合 [U, ~, V] svd(E); if det(U * V) 0 V(:, 3) -V(:, 3); end W [0 -1 0; 1 0 0; 0 0 1]; R1 U * W * V; R2 U * W * V; t1 U(:, 3); t2 -U(:, 3); % 对四个组合逐一做三角化深度检查选出最优 candidates {R1, R1, R2, R2}; t_candidates {t1, t2, t1, t2};det(U*V)这个检查经常被忽略。SVD 不保证旋转矩阵方向加了这一步才保证 R1、R2 的行列式接近 1也就是真旋转。对四个候选组合做深度检查时只需要随机抽 20 个内点做三角化统计每个组合下 3D 点在两相机系下的 z 值都为正的数量取数量最多者为正确解。这个步骤如果省掉点云会出现镜像翻转三维重建看起来像透过哈哈镜看场景。深度检查的本质是验证设左相机坐标系下 3D 点坐标为 X则它在左相机的深度是 X 的 z 分量在右相机深度是 (R*Xt) 的 z 分量。只有两个 z 分量都为正这个 3D 点才位于两个相机的前方。实际中由于噪声往往要保留一定容差比如 z 1e-6。如果四个组合里没有一个组合能覆盖超过一半的内点说明 E 本身不可靠应该回到 RANSAC 阶段而不是继续调分解代码。4. RANSAC 与三维重建把本质矩阵变成稀疏点云4.1 RANSAC 参数阈值、迭代次数、置信度怎么设用 5 点法做 RANSAC 时每轮迭代只选 5 对点估计 E然后用所有匹配点算 Sampson 距离。Sampson 距离是对极误差的一阶近似比点到极线距离更稳定。阈值通常设置在 0.01 到 1 像素之间归一化坐标下要换算成像素阈值除以焦距。比如焦距 700像素阈值 0.5归一化阈值就是 0.5/700 7.14e-4。参数取值范围影响归一化阈值1e-4 到 1e-2太小则内点少太大则把野点放进来置信度0.99 或 0.999越高迭代次数越多最大迭代次数500-2000终止条件之一最小内点数匹配数的 30%-70%低于这个值可以提前停止迭代次数的理论公式是log(1-P)/log(1-w^5)其中w是内点比例。例内点比例 0.6置信度 0.99套进去是 log(0.01)/log(1-0.6^5)约等于 23 次但如果内点比例降到 0.3需要 314 次差异非常明显。所以如果内点比例太低我会先用estimateFundamentalMatrix做一次宽松 RANSAC 粗筛通常能把w提升到 0.5 以上再进 5 点法主循环。Sampson 距离的计算公式为 (x2^T E x1)^2 / ( (E x1)_1^2 (E x1)_2^2 (E^T x2)_1^2 (E^T x2)_2^2 )分子是残差的平方分母是对应梯度长度MATLAB 一行就能完成向量化。不要在 RANSAC 里用重投影误差作为判定因为此时还没有 3D 点必须用对极距离。还有一个容易被忽略的细节对每个候选 E 都要先做奇异值修正再算 Sampson 距离否则阈值等价关系不成立。4.2 三角测量与空间点验证% 使用 MATLAB 内置函数进行三角化 P1 K * eye(3,4); % 左相机投影矩阵世界系对齐左相机 P2 K * [R t]; % 右相机投影矩阵R,t 来自第 3 章 [points3D, reprojErr] triangulate(p1, p2, P1, P2); % 剔除重投影误差过大的点 valid reprojErr 2; points3D points3D(valid, :);triangulate接受两帧像素坐标和投影矩阵返回 Nx3 的 3D 坐标和 Nx1 的重投影误差。这里eye(3,4)为什么不是[R t]因为左相机坐标系被定义为世界系它的外参就是单位旋转加零平移。P2 里的 R,t 必须来自上一节深度检查后的最优组合否则点云会变成前后镜像。valid reprojErr 2;是以像素为单位的硬阈值在标清图像上一般用 1~2高清图上可以放宽到 3。不要把这个阈值放到 RANSAC 的 Sampson 阈值之上两者度量方式不同。三角化并不只依赖一对 2D 点。triangulate内部使用最小二乘对像素坐标的噪声有抑制作用如果你有两帧以上的图像可以继续用多视图三角化或者在 BA 阶段统一优化。MATLAB 内置的triangulateMultiview可以处理多帧但需要传入相机位姿集合和对应的点索引适合后续把稀疏重建扩展成更多帧。当前阶段先用两帧跑通再逐步向多帧扩展是比较稳的路线。4.3 针对这个 zip 包的典型错误检查拿到lhd_5ptEssentialfordistribution.zip里的函数后常见错误有三种。第一种是直接把像素点传给函数没有做归一化导致返回的矩阵秩不对第二种是内参矩阵 K 的单位不对像素、毫米混用第三种是把 5 点法估计出来的 E 当基本矩阵直接用estimateFundamentalMatrix的输出做三角化导致尺度错误。通常我会先写一个单元测试用自己生成的数据验证内参已知设置一个 R,t随机生成 100 个 3D 点投影到两帧再用这个包恢复 R,t比较误差是否在 1e-3 量级。如果不行逐行打印中间变量特别是对极约束的残差。检查残差有一个快速方法对每个内点算 x2 * E * x1如果大量结果大于 1e-3说明 E 的解算有问题。再检查 E 的奇异值理想状态是 [1, 1, 0]实际接近但不完全是如果第三个奇异值明显不为零说明输入坐标没有归一化干净或者主点偏移太多。还有一个身份误区estimateFundamentalMatrix输出的是 3x3 矩阵很多教程把它直接作为 E 用它在像素坐标下工作时并不是本质矩阵强行分解会得到病态的 R,t。这是初学者在 MATLAB 三维重建里最容易踩的坑。另一个容易踩的坑是索引错位。matchFeatures返回的 pairs 顺序是左图索引在前、右图索引在后很多人在取坐标时把pairs(:,1)和pairs(:,2)对调了导致对极约束残差巨大。出现这种错误时E 的奇异值仍然像模像样但三角化的点云很均匀地散布在视锥里。遇到这个现象先检查匹配顺序再检查归一化。5. 验证本质矩阵的实用技巧重投影误差与尺度对齐5.1 重投影误差的快速算法不要用循环逐点计算重投影MATLAB 里用矩阵运算一次性算。把 3D 点齐次化然后proj P * points3D_hom再除齐次分量算sqrt(sum((proj(1:2,:) - points2).^2, 1))。这个写法避免了triangulate内部函数调用带来的重复计算适合批量 Debug。注意points2是第 2 帧的像素坐标用转置是为了和proj的列维度对齐。% 快速重投影误差 pH [points3D, ones(size(points3D,1),1)]; proj K * [R t] * pH; proj proj ./ proj(3,:); err sqrt(sum((proj(1:2,:) - p2).^2, 1)); median(err) % 稳健统计用中位数而非均值proj(3,:)是齐次坐标的深度除完以后才是像素坐标用中位数而不是均值是因为少数野点会对均值产生过大扰动十个点的 100 像素误差会把整体均值拉高中位数则能直接反映大部分点的真实水平。5.2 尺度对齐与后续优化/建图的衔接本质矩阵恢复的 t 只有方向没有尺度所以 3D 点云处在任意尺度下。如果只做显示把点云归一化到单位尺度即可如果要做融合需要用真实距离或已知物体尺寸做相似变换对齐。常见的做法是计算两帧各自点云的尺度因子利用极线约束的对称性用procrustes进行相似变换对齐。procrustes在 MATLAB 的 Statistics Toolbox 里输入两组同维点返回旋转、平移、缩放。注意不要直接用它的输出当绝对尺度一般我先跑一次estimateWorldCameraPose对比结果再去决定要不要手动对齐。尺度对齐在实际重建里的复杂度往往不亚于位姿估计本身。一个比较稳妥的做法是先用一对已知实际距离的点计算比例因子再对整片点云做整体缩放。如果没有这样的先验就用手持设备测一段实际距离比纯视觉猜测靠谱得多。把本质矩阵 t 归一化到单位长度后点云尺度和实际尺度可能相差几十倍直接做纹理映射会出现严重偏移。5.3 一个可复用的调试验证脚本实际项目里我会把这几件事串成一个脚本读图、提特征、匹配、RANSAC 5 点、三角化、重投影检查。重点打印两个数一是内点比例二是重投影误差的中位数。如果中位数超过 0.5 像素就去检查特征匹配而不是继续调 RANSAC 阈值。下个阶段接入 Bundle Adjustment 时这个脚本可以直接改成验证模块把本质矩阵、内点和 3D 点作为初始值交给lsqnonlin或 GTSAM 优化。整套流程在 MATLAB 里跑通后再移植到 C 时只需要保留 5 点求解和 RANSAC 骨架函数名基本不用改。最后附一个简易检查清单确认p1、p2的索引没有跨帧交换确认已做去畸变确认内参主点和焦距单位正确确认 RANSAC 阈值用了归一化坐标确认 E 的奇异值在投影修正前接近 [1 1 0]确认分解后至少一组 R,t 的 Cheirality 内点超过 50%。清单里任何一条不满足都说明刚跑出来的点云不值得继续做 BA。本文还有配套的精品资源点击获取