Python手写SFM三维重建：从特征匹配到光束法平差完整指南

简介：三维重建是计算机视觉的热点方向，这份项目实践包专门讲解如何用Python实现SFM（运动恢复结构）算法，适合具备一定Python与图像处理基础、希望从零跑通三维重建流程的开发者或研究者。包体非常精简，共3个文件，包括2个Python脚本和1个Markdown说明文档，压缩包仅5KB，便于快速阅读和复用关键代码。资源围绕图像采集、预处理、特征检测与匹配、运动估计、三维点云处理等核心环节展开，结合OpenCV、NumPy等常用库，给出可运行的实现思路与排错方向，能够帮助读者理解从二维图像序列到三维点云生成的完整链路。目前已有243人学习下载，对于想快速入门SFM算法实践、避开常见坑位的学习者来说，是一份轻量而实用的参考。

1. SFM 三维重建是什么：用 Python 手写一遍运动恢复结构，值在哪、坑在哪

SFM（Structure from Motion）是从一堆二维照片里同时恢复相机位姿和稀疏三维点云的经典算法，也是三维重建中最值得用 Python 手写一遍的入门项目。网上能下到打包好的源码，但真正跑通、并把「一张图怎么变成三维点」这件事彻底搞懂的人不多。项目路径很清晰：特征提取、特征匹配、位姿估计、三角化、光束法平差，全程用 Python 加 OpenCV 就能跑，不需要 GPU，也不用编译 C++ 库。适合正在学三维视觉的工程师、准备进入 SLAM 或三维重建方向的研究生，以及想验证自己多视图几何功底的人。用 COLMAP 出结果很容易，但自己实现一遍 SFM，你才会知道现成工具替你扛掉了哪些坑——本征矩阵分解有四个候选解、三角化出来的点可能落在相机背后、加帧后轨迹漂移，这些血泪经验是黑匣子工具永远不会主动告诉你的。

2. SFM 算法流程拆解：SIFT 特征、本征矩阵与三角化，为什么先后顺序不能乱

SFM 的完整链路看起来只有五步，但每两步之间都有强依赖关系：特征错了，匹配全是垃圾；匹配里有外点，位姿估计就偏；位姿偏了，三角化出来的三维点就是散的；三维点质量差，后面 PnP 和光束法平差全部跟着遭殃。这一章把每一步的原理和选型理由讲透，顺便解释为什么这个顺序是唯一解。

2.1 特征提取选 SIFT 而不是 ORB：尺度不变性怎么直接影响重建成败

三维重建的第一步是从图像里找出那些"换个角度看还能认出来"的点。SFM 的场景里，同一个物体会被不同距离、不同姿态的相机拍摄，尺度变化几乎是必然的。SIFT 之所以是默认选项，是因为它通过高斯差分金字塔在尺度空间里找极值点，每个特征点自带尺度信息和主方向，描述子也做了旋转归一化，所以对缩放和旋转都稳。这一套机制保证了「同一个墙角、近景拍和远景拍，描述子依然接近」。

对比一下就明白为什么 ORB 不适合当主力：ORB 用的是金字塔多尺度加 FAST 角点，尺度能力有限，遇到近大远小的场景，特征点对的描述子距离会明显拉大，ratio test 一过滤就剩不下几个好匹配了。ORB 的优势是快，适合实时 SLAM 前端，但离线三维重建里我们不缺那几毫秒，缺的是匹配质量。SURF 是 SIFT 的加速版，早年有专利问题，OpenCV 里调用也麻烦，现在基本被 SIFT 的免费实现取代了。

参数上有三个值得调的。nfeatures 控制最多保留多少特征点，室内小场景 3000 够用，室外大场景建议 8000 以上，特征太少会导致后面 RANSAC 没得挑。contrastThreshold 控制低对比度区域的过滤强度，默认 0.04，图像偏暗时升到 0.06 能明显减少噪点特征，但也会牺牲弱纹理区域的点。edgeThreshold 控制边缘响应的过滤，默认 10，把边缘上的长条特征去掉，因为这些点在沿边缘方向上定位漂移严重，三角化出来误差会很大。

提示：SIFT 在 OpenCV 4.4 之后直接进了主库，cv2.SIFT_create()就能用，不需要再装 opencv-contrib-python 老版本那一套。

学习型特征 SuperPoint 这两年很火，匹配质量确实比 SIFT 高一个档，尤其在弱纹理和重复纹理场景。但它的前提是有训练好的模型和 GPU 推理，新手阶段先用 SIFT 把流程跑通，后面再换不迟。特征提取这件事，稳定压倒一切，SIFT 是那个下限最高的选择。

2.2 匹配与几何验证组合拳：FLANN 匹配加 RANSAC 把外点按在地上

特征匹配这一步的任务是给两张图的特征点建立对应关系。暴力匹配器 BFMatcher 会计算每个描述子和对面所有描述子的距离，O(n²) 复杂度在小数据集上没问题，但 8000 个特征对 8000 个特征就是 6400 万次距离计算，Python 里直接卡到你怀疑人生。FLANN 是近似最近邻库，用 KDTree 索引把搜索复杂度压到接近 O(n log n)，速度能差一个数量级。SIFT 描述子是 128 维浮点向量，适合 KDTree 索引；如果换了 ORB 这种二进制描述子，FLANN 索引要换成 LSH，这个细节很多教程里没提，直接套 KDTree 会报类型错误。

匹配出来之后必须做过滤，因为描述子距离最近不代表几何上真的对应。第一个过滤器是 Lowe 的 ratio test：对第一个特征，在第二张图里找距离最近的两个候选，如果最近距离除以次近距离小于 0.75，说明匹配足够独特，才保留。这个 0.75 是作者论文里的经验值，我一般室外大基线场景放到 0.8，室内重复纹理多的场景收紧到 0.7。阈值放太松，外点率直接飙升，后面 RANSAC 压力很大；放太紧，匹配数量骤减，三角化点不够用。

第二个过滤器是几何验证，这一步是 SFM 和普通图像检索的本质区别。两幅图之间有一个本质的几何约束：匹配点对必须满足极线约束，即 x2 必须在 x1 对应的极线上。这个约束写成矩阵形式就是 x2ᵀ F x1 = 0，其中 F 是基础矩阵。用 8 点法可以从至少 8 对匹配点估计 F，然后用 RANSAC 反复采样、计算、统计内点，把不符合极线约束的外点剔除。

这两者的关系是 E = Kᵀ F K，所以只要知道相机内参 K，就能从 F 升到 E，进而分解出位姿。没有 K 的情况下也能用 F 做几何验证，但拿不到真正有尺度的位姿。SFM 项目里通常有两种做法：用棋盘格标定拿到 K，或者直接用假设（比如手机照片用主点居中、焦距近似）。后者精度有限，但作为入门项目完全够用。

顺序为什么不能乱，因为每一步都在给下一步消毒。ratio test 去掉了模糊匹配，RANSAC 去掉了几何矛盾匹配，剩下的匹配才能放心用来算位姿和三角化。跳过几何验证直接三角化，哪怕只有 5% 的外点，也会在点云里拉出一条条灾难性的长线。

2.3 位姿恢复与三角化：从一对图像到第一片三维点云

拿到内点匹配之后，第一步是从本征矩阵 E 恢复两帧之间的相对旋转 R 和平移 t。E 的奇异值结构是 (σ, σ, 0)，对 E 做奇异值分解 E = U diag(1,1,0) Vᵀ，就能构造出四组候选的 (R, t) 组合。这四组解里只有一组能让所有三维点落在两台相机前方，这个筛选动作叫 cheirality 检验，是 SIFT 之后最容易翻车的地方。

def decompose_essential(E): """手动分解本征矩阵，得到四组 (R, t) 候选解""" U, _, Vt = np.linalg.svd(E) W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) R1 = U @ W @ Vt R2 = U @ W.T @ Vt # 旋转矩阵的行列式必须是 +1，否则取反 if np.linalg.det(R1) < 0: R1 = -R1 if np.linalg.det(R2) < 0: R2 = -R2 t = U[:, 2] return [(R1, t), (R1, -t), (R2, t), (R2, -t)]

这段代码的逻辑是：W 和 Wᵀ 分别对应两种旋转构造，t 的正负号产生两种平移方向，所以是 2×2 共四组。行列式修正那两行很关键，SVD 分解不保证 U 和 V 的行列式符号，不修正的话 R 可能带反射分量，后面所有投影计算全部出错。实际项目里直接用cv2.recoverPose就能同时完成分解和 cheirality 筛选，但看懂这段手动分解，你才知道 recoverPose 背后在替你做什么。

位姿确定之后，三角化把两帧里匹配的 2D 点变成 3D 点。理想情况下，两条从相机光心出发的射线应该交于一点，但真实数据里噪声让射线永远不共面，所以要用 DLT 方法求一个最小化代数误差的近似交点。OpenCV 的cv2.triangulatePoints输入是两个 3×4 投影矩阵 P = K[R|t] 和两帧的 2D 点，输出是 4×N 的齐次坐标，必须除以第四个分量 w 才能得到真实的 3D 坐标。这一步很多人忘记归一化，点云直接飞到几千像素远的地方。

整个链条的误差是滚雪球的：位姿估计偏一度，三角化点就偏一截，下一帧的 PnP 又基于这批不靠谱的三维点和上一帧位姿，误差只会越来越大。这也是为什么最后一站必须放光束法平差——把所有相机位姿和三维点放在一起联合优化，让总重投影误差最小。理解了这个误差传导链，你就明白增量式 SFM 里「每几步做一次 BA」不是可选项，是刚需。

3. Python 实现 SFM 第一段代码：特征提取、FLANN 匹配与本征矩阵估计

这一章直接给能跑的代码。环境、特征提取、匹配、位姿估计四段，每一段都可以独立运行并打印中间结果，方便你对照检查。我自己写 SFM 的习惯是第一段代码就跑通最小闭环：两张图、一个 K、一组位姿，所有中间结果落盘，后面所有功能都在这个骨架上长出来。

3.1 环境搭建与版本锁定：OpenCV、NumPy 与 Python 安装的隐藏雷区

如果你还在照着 python 安装教程折腾裸解释器，建议直接装 Anaconda，建一个独立虚拟环境给三维重建用，别和爬虫、数据分析项目混在一起。依赖就三个：numpy、opencv-python、scipy。OpenCV 负责特征和几何计算，SciPy 只用在最后的优化，其余全靠标准库。

# 建虚拟环境并锁定版本，避免今天能跑明天不能跑 conda create -n sfm python=3.10 -y conda activate sfm pip install numpy==1.24.3 opencv-python==4.8.1.78 scipy==1.10.1

版本锁定不是强迫症，是血泪教训。opencv-python 4.8 和 numpy 2.x 之间存在 ABI 兼容问题，直接pip install numpy装到最新版之后，cv2.SIFT_create()可能报段错误或者内存访问异常，查半天发现是 numpy 版本太新。另一个常见坑是装成opencv-python-headless，这个包没有 GUI 模块，cv2.imshow会直接崩。服务器上无头运行可以用 headless，本机调试就老老实实用完整版。

注意：SIFT 在 OpenCV 4.4 之后从 contrib 挪进了主库，所有老教程里的cv2.xfeatures2d.SIFT_create()写法在 4.8 里已经删干净了，统一用cv2.SIFT_create(nfeatures=...)。

VS Code 调试这个项目非常顺手，launch.json 里加上"cwd": "${workspaceFolder}"就能保证相对路径读图不会迷路。Python 3.10 是我在这个项目上的推荐版本，3.8 太老有些新 API 不支持，3.12 又太快踩 numpy 和 scipy 的坑。

3.2 特征提取与匹配落地代码：SIFT 加 FLANN 加 ratio test 的最小实现

import cv2 import numpy as np def load_gray(path): """读图并转灰度，SIFT 只能处理单通道图像""" img = cv2.imread(path) if img is None: raise FileNotFoundError(f"图片读取失败: {path}") return cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) def extract_features(img, max_features=8000): """提取 SIFT 特征点与 128 维描述子""" sift = cv2.SIFT_create( nfeatures=max_features, contrastThreshold=0.04, edgeThreshold=10, sigma=1.6 ) keypoints, descriptors = sift.detectAndCompute(img, None) if descriptors is None: raise RuntimeError("这张图没提取到任何特征，检查图像是否过暗或过模糊") return keypoints, descriptors def match_features(des1, des2, ratio_thresh=0.75): """FLANN 匹配加 Lowe's ratio test，返回过滤后的匹配列表""" index_params = dict(algorithm=1, trees=5) # 1 = FLANN_INDEX_KDTREE search_params = dict(checks=50) # 遍历多少棵树，越大越准越慢 matcher = cv2.FlannBasedMatcher(index_params, search_params) matches = matcher.knnMatch(des1, des2, k=2) good = [] for m, n in matches: # 最近距离明显小于次近距离，匹配才可信 if m.distance < ratio_thresh * n.distance: good.append(m) return good

代码逻辑很简单，但参数值得逐一说。nfeatures=8000是特征上限，不是保底数，弱纹理图可能只提取出几百个，这正常。contrastThreshold和edgeThreshold的语义在 2.1 节讲过，这里给的是室内外通吃的默认值。FLANN 的index_params里algorithm=1是 KDTree 索引，trees=5表示建 5 棵树，树越多近邻搜索越准但内存越大；search_params里的checks=50控制搜索深度，50 是精度和速度的平衡点。

knnMatch(des1, des2, k=2)返回的是每个 query 特征在 train 集里的两个最近邻，ratio test 判断这两个距离的比值。这里一定要小心方向：des1 和 des2 谁在前谁在后，决定了后面取m.queryIdx还是m.trainIdx对应哪张图的特征点。我习惯让 queryIdx 永远指向第一张图，trainIdx 指向第二张图，全项目保持一致，否则 5.2 节那个深度为负的坑就在前面等你。

匹配做完先别急着往下算，落盘一张可视化图验证质量：

def visualize_matches(img1, kp1, img2, kp2, matches, out_path): """把匹配结果画出来，肉眼检查是否有明显错误匹配""" vis = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) cv2.imwrite(out_path, vis) print(f"匹配数量: {len(matches)}，可视化已保存: {out_path}")

这一步被我称为"后悔药检查"。匹配可视化会直接暴露两类问题：一类是重复纹理区域产生的横七竖八的连线，说明 ratio test 阈值太松；另一类是整块区域完全没匹配上，说明光照或视角变化太大，需要换图或调特征参数。肉眼确认这一步只要 30 秒，但能省掉后面排查位姿错误的好几个小时。

3.3 RANSAC 估计本征矩阵并恢复位姿：为什么必须传相机内参 K

def estimate_relative_pose(kp1, kp2, matches, K): """用 RANSAC 估计本征矩阵，并恢复两帧间相对位姿 R, t""" pts1 = np.float32([kp1[m.queryIdx].pt for m in matches]) pts2 = np.float32([kp2[m.trainIdx].pt for m in matches]) E, inlier_mask = cv2.findEssentialMat( pts1, pts2, K, method=cv2.RANSAC, prob=0.999, threshold=1.0 ) if E is None: return None, None, None, None _, R, t, pose_mask = cv2.recoverPose(E, pts1, pts2, K, mask=inlier_mask) inliers = pose_mask.ravel().astype(bool) return R, t, pts1[inliers], pts2[inliers]

cv2.findEssentialMat输入是两帧的像素坐标和一劳永逸的相机内参 K。K 是 3×3 矩阵，含焦距 fx、fy 和主点 cx、cy，手机照片如果没标定，可以先假设 fx=fy=焦距像素值，cx、cy 取图像中心。这个假设在入门阶段够用，但别指望精细重建，后面想做准确就老老实实拿棋盘格标定。

参数层面，threshold=1.0是 RANSAC 判定内点的极线距离阈值，单位是像素，1.0 是常用值。注意这个值跟图像分辨率相关，4000 像素宽的大图和 640 像素的缩略图不能共用同一个阈值，简单做法是先缩放到统一宽度再跑匹配。prob=0.999是 RANSAC 置信度，要求有 99.9% 的概率至少采样到一次全内点集合，0.999 意味着一万次以内的随机采样足够覆盖绝大多数情况。这两参数凑在一起的效果是：匹配外点率 30% 时也能稳定收敛。

recoverPose内部做了完整的 E 分解和 cheirality 筛选，返回的 mask 是在两帧中深度都为正的点。pose_mask的 shape 是 (N, 1)，索引前必须.ravel()成一维，否则布尔索引会报维度错误，这是 OpenCV 老接口的经典大便。返回的 pts1 和 pts2 已经被 mask 过滤过，后面直接拿去做三角化，不用再筛一遍。

4. 增量式 SFM 完整实现：初始化图像对、PnP 注册新帧与光束法平差

两帧能出位姿，但 SFM 的价值在多帧。这一章讲增量式重建的完整骨架：怎么挑初始帧、怎么把第三张以后的图注册进来、怎么生成新的三维点，以及最后怎么用光束法平差把整个模型的误差压下去。

4.1 初始化图像对怎么挑：基线大小、匹配数量与内点率的权衡

增量式 SFM 的第一步是选一对图像作为种子。这对图像的质量决定了整个重建的地基，选错了后面全白搭。选种子看两个指标：匹配数量和几何内点率。匹配太少，三角化出来的初始点云稀疏，新帧 PnP 找不到足够的 2D-3D 对应；内点率太低，说明这对图像间存在大量误匹配或视角变化太剧烈，初始位姿本身就不可靠。

def pick_initial_pair(match_scores, min_inliers=100, min_ratio=0.4): """从所有图像对的匹配结果里挑初始化帧""" candidates = [] for (i, j), score in match_scores.items(): inliers = score["inliers"] ratio = inliers / max(score["total"], 1) if inliers >= min_inliers and ratio >= min_ratio: # 分数 = 内点数 × 内点率，两者同时高的组合优先 candidates.append((inliers * ratio, i, j)) candidates.sort(reverse=True) return candidates[0][1], candidates[0][2]

min_inliers的下限建议 100，少于 100 个内点，后面三角化和 PnP 都会显得捉襟见肘。min_ratio是几何验证后内点占比，0.4 是底线，正常场景 0.5 到 0.7 都有。分数用「内点数 × 内点率」相乘，避免只选到内点多但占比低的对——那种情况说明两张图有大量重复纹理，匹配靠堆量，质量堪忧。

基线大小是个需要手感的地方。基线太小，比如两张图拍摄位置只差几厘米，三角化时两条射线夹角极小，深度估计对像素噪声极其敏感，点云会像拉丝一样往远处甩。基线太大，比如绕着一个物体转了 120 度，特征匹配率骤降，甚至直接匹配失败。我的经验是：序列采集的数据按时间相邻挑，环绕采集的数据选间隔 30 到 60 度的帧。判定标准很简单，看可视化匹配图，特征连线方向应该呈现出明显的视差偏移，而不是几乎平行的重叠。

4.2 新视角注册与增量三角化：solvePnPRansac 加 track 管理

初始对确定后，整个流程进入循环：找下一帧、和已有的三维点做匹配、用 PnP 估计新帧位姿、把新产生的 2D 匹配三角化成三维点。这里有个工程概念叫 track，它记录了每个三维点被哪些帧的哪些特征看到过。没有 track 管理，PnP 和三角化就是无源之水。

def register_new_frame(kp_prev, kp_cur, matches, point_tracks, points3d, K): """新视角注册：用已知三维点和当前帧二维点做 PnP 求位姿""" obj_pts, img_pts = [], [] for m in matches: track_id = point_tracks[m.queryIdx] # queryIdx 对应上一帧特征 if track_id >= 0: obj_pts.append(points3d[track_id]) img_pts.append(kp_cur[m.trainIdx].pt) if len(obj_pts) < 15: return None, None success, rvec, tvec, inliers = cv2.solvePnPRansac( np.float32(obj_pts), np.float32(img_pts), K, None, iterationsCount=200, reprojectionError=4.0, confidence=0.999, flags=cv2.SOLVEPNP_ITERATIVE ) if not success: return None, None R, _ = cv2.Rodrigues(rvec) P_cur = K @ np.hstack([R, tvec]) return P_cur, inliers.ravel()

point_tracks是个列表，长度等于上一帧特征点数，存的是每个特征对应的三维点索引，-1 表示还没三角化。PnP 需要的只是那些已有三维坐标的匹配，所以 loop 里先过滤出track_id >= 0的点。少于 15 个 2D-3D 对应就不值得做 PnP，这是经验阈值，对应太少时解出的位姿方差太大。

solvePnPRansac的参数值得盯两个。reprojectionError=4.0是判定内点的最大重投影误差，单位像素，4.0 比前面本征矩阵的 1.0 宽松不少，因为 PnP 的输入 2D-3D 对应本身经过了几轮过滤，残余误差应该小，但考虑到初始化时点云质量参差，放宽到 4 更稳。iterationsCount=200是 RANSAC 最大迭代次数，配合confidence=0.999，在外点率 30% 以下足够收敛。返回的 rvec 是旋转向量，必须用cv2.Rodrigues转成 3×3 旋转矩阵才能拼投影矩阵。

新帧位姿拿到后，把新帧和上一帧之间那些还没有三维坐标的匹配对三角化，这是点云增量的主要来源。三角化函数在前面 2.3 节出现过，但实际增量流程里必须加过滤，否则垃圾点会越攒越多，污染后续所有帧的 PnP。

def triangulate_new_points(P1, P2, pts1, pts2): """三角化新三维点，并用重投影误差过滤质量差的点""" pts4d = cv2.triangulatePoints(P1, P2, pts1.T, pts2.T) pts3d = (pts4d[:3] / pts4d[3]).T # 把三维点重新投影回两帧，误差大的直接扔掉 proj1 = project_points(P1, pts3d) proj2 = project_points(P2, pts3d) err1 = np.linalg.norm(proj1 - pts1, axis=1) err2 = np.linalg.norm(proj2 - pts2, axis=1) valid = (err1 < 2.0) & (err2 < 2.0) return pts3d[valid] def project_points(P, pts3d): """三维点经投影矩阵 P 得到像素坐标：先齐次变换，再除以深度""" homo = np.hstack([pts3d, np.ones((len(pts3d), 1))]) uv = (P @ homo.T).T return uv[:, :2] / uv[:, 2:3]

这个过滤逻辑分两层：第一层要求三角化点在两帧的重投影误差都小于 2 像素，把那些因为位姿误差或匹配错误产生的飘点全部挡在门外；第二层隐含在project_points里，如果投影后深度 w 是负数，uv会得到一个符号翻转的坐标，重投影误差自然巨大，自动被过滤。这一步能显著提升点云信噪比，我见过不少项目跳过这个过滤，结果点云里全是离群的长尾。

增量循环里还有一个 track 更新动作：新三角化的点要分配新的 track 索引，并把新帧里看到的特征点索引写进 track 表。这里的实现方式各家不同，核心原则是每个三维点必须记录所有可见帧的 (frame_id, feature_id)，这个信息是 4.3 节光束法平差观测数据的直接来源。偷懒不做 track 管理的话，BA 就只能退化成逐帧单独优化，失去全局一致性，等于白做。

4.3 光束法平差的最后一公里：用 SciPy 把重投影误差压下去

增量重建跑完几十帧之后，位姿和点云都有一定精度，但离可用还有距离。原因在于每一帧的位姿都是基于上一帧估计的，误差单向累积。光束法平差把所有相机位姿和三维点放进同一个代价函数，最小化所有观测的重投影误差平方和。这个优化问题里每个三维点只连接可见它的那些相机，整个残差向量呈稀疏结构，但对入门规模的两三百帧场景，SciPy 的least_squares完全扛得住。

from scipy.optimize import least_squares def run_bundle_adjustment(cam_params, points3d, observations, K, n_iter=50): """ cam_params: (N, 6) 每行是 rvec(3) + tvec(3) observations: (M, 4) 每行是 [cam_idx, pt_idx, x, y] 注意第一帧相机固定不优化，消除尺度漂移 """ n_cams = cam_params.shape[0] x0 = np.concatenate([cam_params.ravel(), points3d.ravel()]) def residuals(params): cams = params[:n_cams * 6].reshape(n_cams, 6) pts = params[n_cams * 6:].reshape(-1, 3) res = [] for cam_idx, pt_idx, u, v in observations: rvec = cams[cam_idx, :3] tvec = cams[cam_idx, 3:] proj, _ = cv2.projectPoints( pts[pt_idx].reshape(1, 1, 3), rvec, tvec, K, None ) res.append(proj[0, 0] - np.array([u, v])) return np.concatenate(res) result = least_squares(residuals, x0, method="lm", max_nfev=n_iter) n_cams_opt = cam_params.shape[0] opt_cams = result.x[:n_cams_opt * 6].reshape(n_cams_opt, 6) opt_pts = result.x[n_cams_opt * 6:].reshape(-1, 3) return opt_cams, opt_pts, result.cost

核心逻辑是残差函数：对每条观测，用当前相机位姿把三维点投影到像平面，和实际观测的像素坐标做差。优化的就是所有相机位姿和所有三维点，让这个差的平方和最小。least_squares的好处是不用手写雅可比矩阵，默认有限差分自动算，代价是求解慢一点，但几十帧几百点的规模在几秒到几十秒内都能收敛。

有三个参数必须盯住。第一，n_iter=50是最大迭代次数，不要给太大，50 次足够收敛，再多是浪费；如果 50 次没收敛，说明初始值烂到没法救，迭代 500 次也没用。第二，第一帧的位姿应该从优化变量里排除，或者固定不更新，否则整个模型可以在尺度、旋转、平移三个自由度上整体漂移，误差函数有无数个等价的全局最小值，这叫 gauge freedom，不固定一帧 BA 就是在打移动靶。第三，观测里绝对不能混入外点，一旦有错误的 2D-3D 对应进了残差，LM 算法会被单个巨大的残差拖着走，结果比优化前还差。这也是为什么 5.4 节那个坑那么常见。

BA 跑完之后，把优化前后的重投影误差打出来对比，是判断这次优化是否成功的硬指标。通常优化前单像素级误差在 3 到 10 像素之间很正常，优化后应该稳定压到 1 像素以内。如果压不下去，先怀疑观测里混了外点，再怀疑第一帧没固定。

5. SFM 避坑指南：五个会让你反复翻车的经典问题与排查路径

这一章是我自己反复踩过的坑汇总，按「现象 → 原因 → 解决」写清楚，每个问题都附带排错手段。SFM 这个方向最大的特点就是中间产物多，任何一环出错都只会让最终点云"看起来不太对"，而且很难一眼定位问题出在哪。养成把每一步输出落盘的习惯，能省下大量排查时间。

5.1 现象：匹配看着全对，重建出来却是一团乱麻

现象：特征匹配可视化图里连线整整齐齐，没有明显的交叉错配，但三角化出来的点云完全是一坨或者散成几条互相矛盾的线。原因：最常见的有三种。第一种是 K 矩阵传错了，比如把 fx 和 fy 的位置写反、主点给了 (0, 0) 但实际图像中心偏移几百像素，这会导致所有极线计算整体偏移，RANSAC 内点率看着还行，但几何关系全错。第二种是场景退化，比如所有特征点都落在同一个平面上（拍一面墙），这时基础矩阵的估计退化到单应关系，E 分解出来的位姿在纯旋转和纯平移之间摇摆不定。第三种是图像对之间只有小基线的连续帧，三角化角度太小，深度不确定性巨大。

解决：先打印 RANSAC 内点率和三角化后深度为正的点占比。内点率低于 40% 直接怀疑 K 或场景退化；正深度占比低于 90%，优先检查匹配顺序和 recoverPose 的 mask 使用。K 矩阵建议单独放在一个 JSON 配置文件里，从标定结果里读取，不要在代码里手写测试值。场景退化的唯一解是换初始化图像对，挑取景内容更立体的两帧。

5.2 现象：三角化出来的三维点深度为负

现象：点云里有相当比例的点在相机后方，渲染出来一团黑，重投影误差巨大。原因：90% 的情况是没有正确做 cheirality 检验，直接用了decompose_essential四组解里的第一组，或者recoverPose输入的两组点顺序不一致，导致它筛选解时基于的错误对应关系。剩下 10% 是三角化函数里忘了除以齐次坐标的 w 分量，点坐标直接放大无数倍。解决：写一个通用检查函数，对任何位姿和三维点组合都能摸清深度分布：

def cheirality_ratio(R, t, pts3d): """统计在 R,t 位姿下深度为正的点占比，选解和排错逻辑都靠它""" cam_pts = pts3d @ R.T + t.ravel() # 转到相机坐标系 return float(np.mean(cam_pts[:, 2] > 0))

使用方式很简单：decompose_essential得到四组解，分别算cheirality_ratio，取正深度占比最高且明显超过 50% 的那组。如果四组解都差不多、没有明显赢家，说明 E 的估计本身就是错的，回头查 K 和匹配质量。实际项目里直接用cv2.recoverPose就不会有这个烦恼，它内部已经做了同样的筛选，但前提是输入的 pts1、pts2 行一一对应。一个非常隐蔽的坑是用了m.queryIdx和m.trainIdx时搞反了两张图的对应关系，特征是从图 A 匹配到图 B，结果提取坐标时却从图 B 取了 queryIdx 的坐标，那所有几何关系全部镜像翻转。

5.3 现象：加入新帧后相机轨迹漂移得越来越快

现象：初始几十帧位姿还挺正常，越往后相机轨迹越偏，到后面直接飞出去或者原地打转，点云也跟着散架。原因：这就是增量式重建的误差累积问题。每一帧的位姿是拿上一帧的位姿和三维点估出来的，上一帧的误差原封不动传下来，再叠加这一帧的新估计误差，像滚雪球一样。中间一旦混进一帧匹配质量差的，后面全部被带偏。解决：三板斧。第一，每加入 2 到 3 帧就跑一次局部 BA，只优化最近窗口内的相机和点，把刚引入的误差及时消化掉。第二，PnP 之前只允许 track 被 ≥3 帧看见的三维点参与求解，这类点通常更可靠，能有效压低位姿误差方差。第三，每帧注册后记录内点数，如果某帧特征点数量或内点率突然断崖下降，直接跳过这帧，不要硬注册。这三板斧能解决 80% 的漂移问题。

5.4 现象：跑完 BA 重投影误差不降反升

现象：优化前重投影误差 2.5 像素，least_squares跑完变成 4 像素，或者误差没变但点云明显更乱了。原因：两个高频原因。第一个是观测数据里混了外点，错误匹配进入残差后，LM 算法会把大量迭代用在迁就这个离群残差上，把本来正确的位姿和点全部带偏。第二个是没固定第一帧，整个模型在 7 个自由度上整体漂移，优化器找到的"最小残差"和我们的语义目标完全不是一回事。解决：BA 前先彻底消毒观测数据：只用 RANSAC 内点、只用 track 观测数 ≥3 的点、只用在多帧里重投影误差一直稳定的点。BA 配置上固定第一帧位姿，并且用两步走：第一轮只优化三维点，固定所有相机，等点云收敛到合理位置后再放开相机一起优化。这样能避免优化器在点云还乱七八糟的时候就去调相机，把两者带进一个错误的局部最小。

5.5 现象：OpenCV 版本升级后同样代码结果对不上

现象：上个月还能跑出正常点云的同一套代码，升级了 opencv-python 或 numpy 后，匹配数量变了、内点率变了，重建结果完全不可复现。原因：OpenCV 从 4.4 到 4.8 之间调整过 SIFT 的默认参数和findEssentialMat的 RANSAC 随机性，甚至 numpy 版本都会影响随机数生成器的行为。SIFT 从 contrib 挪到主库时，nfeatures默认值和特征点排序方式有过变化。这些变化对黑盒用户来说完全透明，但 SFM 这种对中间结果敏感的流程就会被放大。解决：把环境和结果一起锁死。项目根目录放 requirements.txt，明确指出 opencv-python==4.8.1.78、numpy==1.24.3、scipy==1.10.1。每次跑实验前把匹配数量、内点率、初始点云数量三个指标打出来存成日志，版本一旦变化，diff 日志就知道是哪一步变了。最后，所有涉及随机采样的地方显式设置固定种子。RANSAC 的随机性无法通过 OpenCV 接口直接控制，但可以在调用前np.random.seed(...)，并在代码里记录本次运行用的种子值，出问题能复现。

6. 验证与进阶：重投影误差、点云可视化与下一步还能往哪走

SFM 跑完不是终点，验证结果质量、把点云和轨迹可视化出来，才算真正闭环。重投影误差是最核心的定量指标——每个三维点投影回它被观测到的每一帧，计算像素距离的平均值。健康范围是优化后均值小于 1.5 像素，超过 2 像素说明观测数据里还有外点，或者 K 矩阵本身就不准。辅助指标还有匹配内点率（几何验证后应大于 40%）和 cheirality 正深度占比（应大于 90%）。

可视化我用 Open3D，三行代码把稀疏点云和相机轨迹一起画出来：

import open3d as o3d pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(points3d) traj = o3d.geometry.PointCloud() traj.points = o3d.utility.Vector3dVector(camera_centers) o3d.visualization.draw_geometries([pcd, traj])

轨迹可视化是最直观的诊断工具：正常的相机轨迹应该平滑连续，出现跳跃或螺旋说明中间某帧位姿估计崩了。点云则应该能看出物体的基本轮廓和纹理结构，而不是一团均匀的噪声球。

验证通过之后，进阶方向有三条主流路线。第一条是特征升级，把 SIFT 换成 SuperPoint 加 SuperGlue 匹配，弱纹理和重复纹理场景的鲁棒性会明显提升，代价是需要 GPU。第二条是重建密度升级，稀疏点云可以喂给 MVS 做稠密重建，得到完整的表面模型。第三条是进入 NeRF 三维重建路线——NeRF 的前端位姿估计用的就是 SFM 思路，把这里实现的相机位姿能力迁移过去，正好踩在当下最热的点上。和结构光三维重建相比，SFM 的优势是不需要投影仪和标定设备，纯自然光下用照片就能工作，代价是精度受限于特征匹配，做不到结构光的亚毫米级。

我自己现在的习惯是：每次跑 SFM 都把中间产物全部落盘——匹配可视化、内点统计、初始点云、每帧位姿、BA 前后的误差日志，文件名带时间戳。这样任何一次翻车都能回放是哪一步出的问题，而不是对着最后一张烂点云瞎猜。这个习惯帮我省下的排查时间，比写算法本身还多。希望帮到你。

本文还有配套的精品资源，点击获取