简介基于Python实现的三维重建算法Structure from MotionSfM完整源码包面向需要进行期末大作业、课程设计或学习三维重建技术的计算机视觉方向学生与开发者。资源包含主程序脚本、配置文件与说明文档可帮助用户理解从多视角图像序列中恢复相机参数与稀疏三维点云的全流程。包体共3个文件含2个Python文件与1个Markdown说明文件整体约5KB结构精简便于直接阅读与二次修改。代码已经本地编译验证可运行据描述评审达到98分项目难度适中适合作为毕业设计或综合实践的参考。目前该资源已有173人浏览学习。下载后可获得可直接运行的SfM实现代码、关键参数配置示例以及配套的README讲解有助于快速搭建实验环境并掌握特征提取、匹配与三角化的核心步骤。1. 用 Python 实现 SfM 之前先搞清楚三维重建要解决什么拿到一包照片想得到相机拍摄位置和场景的稀疏点云这个坑我踩过不止一次。最直接的做法并不是马上训练 NeRF而是先把相机位姿解出来——这正是 Structure from MotionSfM要做的事从一组重叠图像的二维特征对应关系出发恢复每帧相机的旋转、平移和三维点坐标。很多人觉得三维重建等于深度学习但真正给 NeRF 生成训练位姿的往往还是传统 SfM 管线。用 Python 自己实现一遍 SfM能彻底看清特征匹配、对极几何、三角化和光束法平差之间怎么咬合。这篇记录会拆一套最小可运行的实现代码并列出最容易翻车的参数坑适合刚接触三维重建、想自己动手算相机位姿的开发者。2. SfM 的四块基石特征匹配、对极几何、三角化与 BASfM 整个管线从外看是“图片进、点云出”但内部是四段逻辑串起来的先找图像之间的对应点再用对应点估计相机位姿接着把对应点升级成三维点最后把所有位姿和三维点一起优化。每一段本身都是独立课题可它们之间靠数据格式一环扣一环特征是 2D 像素坐标位姿是 3x4 投影矩阵三维点是齐次坐标误差是像素重投影残差。2.1 特征提取与匹配从像素对应到几何约束图像间的对应关系是整个 SfM 最基本的原料。比较两帧图像可以直接比较像素块但光照变化、视点变化都会让像素颜色失真所以业界通常用局部特征。每个特征包含两部分关键点位置和描述子向量。SfM 只关心关键点的可重复性也就是同一个三维点在第二张图里能不能被再次找到。我常用的特征候选只有三个ORB、SIFT、AKAZE。ORB 免费、速度快适合实时粗配准SIFT 匹配率最稳尤其弱纹理和视角变化大的场景但速度慢而且 OpenCV 里的 SIFT 实现带专利授权商用前要确认边界AKAZE 是折中非线性尺度空间对模糊更友好但参数更多。选型上不用纠结先把 ORB 跑通匹配数不够再切 SIFT。代码里对应到cv2.ORB_create(nfeatures5000)这样的参数特征数量设 3000 到 8000 都可以太少匹配不稳太多让 RANSAC 变慢。匹配这一步有两个关键动作。第一是交叉校验A 中特征 f1 的最佳匹配是 B 中 f2同时 B 中 f2 的最佳匹配也必须是 f1 才保留否则误配概率很高。第二个动作是 ratio test取最近邻距离 d1 和次近邻距离 d2如果 d1/d2 大于 0.75说明这个最近邻不够“唯一”应当丢弃。这个 0.75 是 Lowe 当初留下的经验值弱纹理场景可以放到 0.8但再高会把误匹配放大。可执行要点在真实照片里ORB 匹配完往往会残留 5% 的误匹配后面的本质矩阵用 RANSAC 拟合能把这部分误匹配挤掉但前提是正确匹配数量足够。我一般要求匹配对在 100 对以上低于 50 对就放弃这一对相机的初始化。2.2 对极几何与本质矩阵两帧之间的平移旋转怎么解两帧之间不需要知道任何三维点只要有一组匹配像素坐标就能解出相机的相对运动。核心是对极约束p2^T * E * p1 0其中 p1、p2 是归一化平面上的点。E 是本质矩阵写成E [t]_x R。它的物理意义是第一相机坐标系下的点经过 R、t 变换到第二相机再由平移叉乘构成约束。这里的 t 是平移方向可以定但尺度是任意的——SfM 天然没法恢复绝对尺度这是接下来所有尺度问题的总根源。求解 E 一般用五点法OpenCV 的cv2.findEssentialMat就内置了 Nister 五点法并配合 RANSAC 自动剔除误匹配。传入前先把二维像素坐标用内参 K 转换到归一化平面不过实际操作中直接传像素坐标和内参 K 即可函数内部会处理。关键参数是threshold1.0单位是像素这个阈值决定 RANSAC 把多远的点判为外点太大容易放水太小把正确的匹配也砍掉。得到 E 之后用cv2.recoverPose分解成 R 和 t。E 的奇异值结构决定了分解有四组解recoverPose 会利用三角化出的点在两个相机前且深度为正自动选出一组。这里有个常见误区recoverPose 返回的 t 方向和真实平移只差一个尺度不能拿它做距离测量。另外如果场景近似平面或者相机做纯旋转E 会退化成单应约束这个问题放到第 5 章专门讲。可执行要点我在初始化时会把findEssentialMat的prob设为 0.999让 RANSAC 不容易因为采样坏点而早停recoverPose返回的内点如果少于 20 个我会直接判定这一对照片不适合作为重建起点。2.3 三角化把二维匹配升级成三维点有了两帧位姿就可以求对应特征点的三维坐标。两条视线在空间中应该交于一点由于噪声视线不会严格相交所以三角化本质上是在解一个最小二乘问题。OpenCV 的cv2.triangulatePoints接收两个 3x4 投影矩阵P1、P2和两个图像点集输出四维齐次坐标需要手动除以最后一维转成非齐次的 XYZ。实际操作中只做三角化是不够的还要筛选两条规则。第一是深度为正点在两个相机前面。triangulatePoints本身不保证这一点所以我通常把点分别投影到两个相机坐标系检查 z 分量是否大于 0。第二是重投影误差小把三维点重新投影回两张图和原匹配点的像素距离小于 2 像素大于 2 像素的三维点大多是靠后误差点。这个筛选第一次看会觉得苛刻但它能省去后续 BA 的很多麻烦。宁可初始点少一点也不要掺入一堆错误点否则位姿会被带崩。业界叫这种操作为“三角化外点过滤”等于是用几何先验把误匹配再杀一遍。2.4 光束法平差让所有误差一起变小如果只在初始化时三角化一次后续所有误差都会带着走每一帧的位姿估计都建立在上一帧基础上平移和旋转误差会逐渐累积最后轨迹漂移到完全不可用。光束法平差Bundle AdjustmentBA就是为了解决这个问题。BA 的优化变量是每帧相机的 R、t 和每个三维点的 XYZ目标是让所有可视点的重投影误差平方和最小。残差可以写成err project(K, R, t, X) - p其中project是针孔相机投影p 是该三维点在图像上的实际观测像素坐标。这个函数看起来简单但优化变量数量巨大几百张图就是几千个变量。好在误差只与“能看到该点的相机”有关所以 Hessian 矩阵有很强的稀疏结构工程上用 Ceres、g2o、COLMAP 里的稀疏求解器解决。对于纯 Python 实现我不建议自己写大规模 BA后面第 6 章会给一个两帧级的小 BA用来校正初始化帮助新手把“重投影误差”这个说法落到代码。理解 BA 的关键不是推导雅可比矩阵而是理解它的作用位姿和点都是变量谁都可以被修正修正之后误差会被重新分配到最合理的位置。可执行要点增量式 SfM 里我通常每加入 5 到 10 帧就做一次局部 BA只优化最近 20 帧位姿和它们共视的三维点。跑完整段序列后再做全局 BA。这个节奏是从 COLMAP 的增量策略里学来的能有效控制漂移。3. 用 Python 写一个最小 SfM 管线从两张图到增量重建这一章直接给代码。为了不淹没在细节里我把管线拆成三个可运行片段读取图片与内参、双视图初始化、增量注册新帧。完整代码合起来就是两百行左右的小系统拿到手里先用 OpenCV 4.x 跑通再逐步替换自己的图片序列。3.1 数据准备与相机内参假设SfM 的第一条输入是图片序列第二条是相机内参。内参矩阵 K 包含 fx、fy、主点 cx、cy。手机照片的 fx、fy 可以用标定板得到也可以从 EXIF 里的焦距毫米值和传感器尺寸换算新手阶段我一般先用一个近似 K因为初始化后 BA 会修正误差。近似时默认 fxfy图像宽乘以 1.2 左右cx、cy 取图像中心。这个近似只影响起始尺度不影响是否收敛。import cv2 import numpy as np from pathlib import Path def load_grayscale(path): img cv2.imread(str(path), cv2.IMREAD_GRAYSCALE) if img is None: raise FileNotFoundError(f读不到图片{path}) return img def make_default_k(width, height): # 近似内参没有标定数据时先用它开局 fx fy width * 1.2 cx, cy width / 2.0, height / 2.0 return np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtypenp.float64)make_default_k只是为了把流程跑通近似内参会直接让三维坐标的绝对尺度偏掉。load_grayscale强制灰度图特征提取在灰度上跑省去纹理分析的缓存开销。注意fx width * 1.2不是标定值而是根据水平视场角大约 60 度反推的经验公式工程上需要替换成真实标定结果。3.2 双视图初始化ORB 匹配、本质矩阵、recoverPose初始化是整个重建的“起步刹车”如果第一对相机的 R、t 错了后面所有帧都会跟着错。这里我给出一个完整的初始化函数它完成特征提取、匹配、本质矩阵估计和位姿恢复四件事。def extract_features(img, nfeatures5000): orb cv2.ORB_create(nfeaturesnfeatures) kps, descs orb.detectAndCompute(img, None) return kps, descs def match_features(desc1, desc2, ratio0.75): bf cv2.BFMatcher(cv2.NORM_HAMMING) matches bf.knnMatch(desc1, desc2, k2) good [] for m, n in matches: if m.distance ratio * n.distance: good.append(m) return good def init_pair(img1, img2, K): kps1, desc1 extract_features(img1) kps2, desc2 extract_features(img2) matches match_features(desc1, desc2) pts1 np.array([kps1[m.queryIdx].pt for m in matches], dtypenp.float32) pts2 np.array([kps2[m.trainIdx].pt for m in matches], dtypenp.float32) E, mask cv2.findEssentialMat(pts1, pts2, K, methodcv2.RANSAC, prob0.999, threshold1.0) retval, R, t, mask_new cv2.recoverPose(E, pts1, pts2, K) inliers mask.ravel().astype(bool) mask_new.ravel().astype(bool) return R, t, pts1[inliers], pts2[inliers], kps1, kps2, matchesfindEssentialMat的threshold1.0用像素距离评估内点recoverPose还有一步三角化筛选两者返回的 mask 要叠加使用。ratio test 的 0.75 是经验值弱纹理场景可以放到 0.8。ORB 的描述子是二进制所以BFMatcher用NORM_HAMMING如果切 SIFT 要改成NORM_L2并加crossCheckTrue。这段代码如果没有返回足够内点就要检查图片是否有重叠区域或相机是否几乎原地没移动。3.3 三角化第一对视图得到第一批三维点初始化得到的 R、t 只是相机运动场景里的三维点还没出来。把两张图匹配点投影到三维空间就是三角化。下面的代码会顺手做深度筛选和重投影误差过滤。def filter_triangulation(P1, P2, pts1, pts2, X): # X 是 Nx4 齐次坐标先转非齐次 X X / X[3] XYZ X[:3].T # Nx3 keep [] for i, (p1, p2) in enumerate(zip(pts1, pts2)): x1 P1 X[:, i] x2 P2 X[:, i] # 在各自相机系下深度为正 if x1[2] * x1[3] 0 or x2[2] * x2[3] 0: continue # 重投影误差约小于 2 像素 a1 p1[0] - x1[0] / x1[3] a2 p1[1] - x1[1] / x1[3] b1 p2[0] - x2[0] / x2[3] b2 p2[1] - x2[1] / x2[3] if (a1*a1 a2*a2 b1*b1 b2*b2) 2.0 * 2.0: keep.append(i) return XYZ[keep] def triangulate_pair(R, t, K, pts1, pts2): P1 K np.hstack([np.eye(3), np.zeros((3, 1))]) P2 K np.hstack([R, t]) X cv2.triangulatePoints(P1, P2, pts1.T, pts2.T) return filter_triangulation(P1, P2, pts1, pts2, X)P1 取第一相机为世界原点所以是K[I|0]P2 是K[R|t]。triangulatePoints返回的齐次坐标最后一维是 w必须先除 w 才是真实坐标。深度判断里x1[2] / x1[3]等价于相机坐标 z但要防止 x1[3] 为 0。重投影误差阈值 2 像素比较严格如果你的照片做过缩放要换算成原图尺寸再调。3.4 增量式注册新帧solvePnP 与新点三角化双视图初始化只是起点后续每加入一张图都要先估计出新相机位姿再为它三角化更多三维点。新帧与上一帧匹配后有一部分匹配点的上一帧特征已经在三维点库里用这些 2D-3D 对应关系调用solvePnPRansac求新帧的 R、t剩下没有三维点的匹配在 R、t 已知后双视图三角化补上。def register_frame(kps_prev, kps_cur, matches, points3d, point_id_of_prev_idx, K): pts3, pts2, matched [], [], [] for m in matches: pid point_id_of_prev_idx[m.queryIdx] if pid 0: pts3.append(points3d[pid]) pts2.append(kps_cur[m.trainIdx].pt) matched.append(m) pts3 np.array(pts3, dtypenp.float32) pts2 np.array(pts2, dtypenp.float32) ok, rvec, tvec, inliers cv2.solvePnPRansac( pts3, pts2, K, None, iterationsCount1000, reprojectionError8.0, flagscv2.SOLVEPNP_ITERATIVE) if not ok: return None R_cur, t_cur cv2.Rodrigues(rvec)[0], tvec.reshape(3, 1) inlier_set set(inliers.flatten()) matched [matched[i] for i in inlier_set] # 这一步必须保留 inlier 对应的原始匹配供后续三角化使用 return R_cur, t_cur, matchedpts3是世界坐标的三维点pts2是新帧图像上的观测。solvePnPRansac的reprojectionError8.0表示重投影误差在 8 个像素以内都算内点这个值比初始化时的 1.0 宽松因为 ORB 在长基线上噪声本来就更大。iterationsCount1000是 RANSAC 迭代次数别设置太小否则在窗口期最容易麻痹。返回的matched从 inlier 集合里筛一遍是为了后续三角化新点时不会混入外点。注册后如何三角化新点核心代码和triangulate_pair相同只是 P1、P2 都要换成世界坐标系到当前两个相机系的投影矩阵。这个简单但容易错的地方是第一帧初始化后R_prev、t_prev是世界系到上一帧相机的变换solvePnPRansac返回的同样是世界系到当前相机所以三角化时P K[R_cur|t_cur]直接能用不用额外变换。3.5 参数怎么调一张速查表下表是我在这个管线里用得比较稳的参数也是翻车重灾区阶段参数常用值调大的后果调小的后果特征提取nfeatures5000匹配多但计算慢匹配稀疏难初始化匹配ratio0.75误匹配变多位姿容易崩正确匹配被砍点云稀疏初始化RANSAC threshold1.0外点混入对极约束变软内点不足初始化失败solvePnPreprojectionError8.0外点污染位姿找不到足够内点跳过帧solvePnPiterationsCount1000慢随机采样不充分易翻车这些不是公式是经验区间。如果你的照片是长焦镜头拍的视场角小位姿对旋转非常敏感reprojectionError可能要降到 4.0。先用默认跑一遍再根据失败点调整。4. 重建结果落地PLY 导出、点云赋色与轨迹评估SfM 跑完内存里的三维点不是结果能投到三维软件里检查才是结果。这一章讲怎么把点云和相机轨迹写成通用格式以及用哪些指标判断重建质量。4.1 用 PLY 保存点云PLY 是点云常用格式MeshLab、CloudCompare、Open3D 都能直接读。写 PLY 最保险的是 ASCII 版本不需要额外库。每个顶点包含 x、y、z 和 RGB 颜色。def write_ply(path, points, colors): points np.asarray(points, dtypenp.float32) colors np.asarray(colors, dtypenp.uint8) with open(path, w) as f: f.write(ply\nformat ascii 1.0\n) f.write(felement vertex {len(points)}\n) f.write(property float x\nproperty float y\nproperty float z\n) f.write(property uchar red\nproperty uchar green\nproperty uchar blue\n) f.write(end_header\n) for p, c in zip(points, colors): f.write(f{p[0]:.6f} {p[1]:.6f} {p[2]:.6f} {c[0]} {c[1]} {c[2]}\n)注意 PLY 的 RGB 是 0-255 整数写成浮点会被很多软件当白点。如果你没有保存颜色可以给每个点赋一个固定的灰度值不要不写 RGB否则可视化时会一片黑。点在文件里的顺序就是数组顺序后面的指标统计也按这个索引对应。4.2 相机轨迹导出R、t 别直接当米制用相机位姿也是重建的一部分尤其做 AR、机器人定位时需要。相机平移在 SfM 里是任意尺度保存成文本格式时最好加一列注释。常见做法是保存每帧的旋转矩阵和平移向量再加一个 4x4 的变换矩阵方便和其他工具对接。def write_cameras(path, cam_poses): cam_poses: list of (R, t), R:3x3, t:3x1 with open(path, w) as f: f.write(# R[3x3] t[3x1] 单位为 SfM 尺度非米制\n) for i, (R, t) in enumerate(cam_poses): np.savetxt(f, np.hstack([R, t]), fmt%.8f) f.write(f# frame {i}\n)每个帧的 R、t 写在相同文件里#注释不干扰读取。注意 SfM 的尺度不是米所以不能用这份轨迹做尺寸测量至少得有一个已知真实长度做一次相似变换。4.3 点云赋色从图像像素拿 RGB很多入门教程把颜色列直接置零结果点云在软件里黑成一团。给三维点赋色的做法很直接三角化时记录每个三维点第一次被观测到的那一帧编号和像素坐标事后从彩色图里取 BGR 再转 RGB。def give_colors(points3d, tracks, image_list): tracks: list of (point_id, image_index, x, y) colors np.zeros((len(points3d), 3), dtypenp.uint8) for pid, img_idx, x, y in tracks: img cv2.imread(image_list[img_idx]) yy, xx int(round(y)), int(round(x)) if 0 yy img.shape[0] and 0 xx img.shape[1]: colors[pid] img[yy, xx][::-1] # BGR - RGB return colors这里的前提是tracks里的坐标确实来自某个成功三角化的观测不能随便从匹配结果里取。如果一个三维点被多个相机看到取第一次观测的像素颜色即可避免不同帧曝光差异造成颜色边缘斑驳。4.4 三个指标判断重建是否正常重建结果不能只看点云好不好看还要量化。我每次跑完都会打印三个指标指标计算方法正常范围异常表现平均重投影误差所有可视点重投影到观测图的像素距离均值 1.5 像素位姿或内参有系统性误差内点率solvePnP 返回的 inliers / 参与匹配的点数 60%特征质量差或初始位姿不准三角化点保留率三角化后通过深度/重投影过滤的比例 50%匹配对应错误或基线太短平均重投影误差小于 0.5 像素说明重建质量很好但也存在过拟合风险大于 2 像素基本不可用。内点率低时可以先看特征匹配图确认是不是有重复纹理。保留率低通常不是三角化的问题而是本质矩阵把错误匹配放过了。4.5 可视化检查的姿势把 PLY 拖进 MeshLab 后先看相机轨迹是否平滑。一个合格的 SfM 轨迹应该是一条连续、没有跳变的曲线如果轨迹在某帧突然折回去或者点云像炸开的毛球绝大多数原因是该帧没有足够内点被 solvePnP 硬算出来。这时不要急着调参数先把reprojectionError放宽到 12 像素看看 inliers 的位置是否集中在图像某一侧。还可以在 CloudCompare 里叠加真实参考点做距离对比。没有真值时我会看点云的当地法线方向如果同一平面上的点法线杂乱说明三角化噪声大BA 没有把误差压下去。这些都是经验判断几分钟能出结果。坐标系方向也要知道第一帧的相机被固定在世界原点后续轨迹起点自然落在原点附近这不是错误是 SfM 坐标系的定义方式。5. SfM 常见问题避坑特征稀疏、位姿退化、漂移与内参不准写 SfM 最大的玄学是同样的代码换一组照片就翻车。下面这五类问题我几乎每次都会遇到按现象、原因、解决三段式写。5.1 匹配数太少初始化直接失败现象findEssentialMat返回的内点不到 20 个recoverPose 后两个相机相对位姿明显不对点云乱飞。原因图片重叠区域太小、重复纹理太强、ORB 特征本身尺度不变性有限或者两帧之间的基线太短导致视差太小。解决先检查匹配分布把匹配线画出来如果匹配集中在少数区域说明图片没有充分重叠。把 ORB 的nfeatures提到 8000并打开cv2.ORB_create(scaleFactor1.2, nlevels8)。如果还是不行换成 SIFT 特征。基线太短时跳过当前帧隔几帧再初始化。另一个经验是不要选择两张视图角度差小于 5 度的图片作为初始化对否则三角化深度误差会呈几何级数放大。5.2 纯旋转场景本质矩阵退化初始化全乱现象图片确实是同一场景但三角化点全部在无穷远或深度为负点云呈“壳状”包住相机。原因相机没有平移或接近纯旋转本质矩阵自由度退化成单应约束三角化没有有效视差。人眼在转动头部时还能感觉深度是因为实际有微小的平移和立体视差而相机固定在云台上转圈时纯旋转场景无法用对极几何恢复结构。解决拍摄时让相机有前进或横向位移不要站在一个点转圈。代码层面可以在findEssentialMat之前检查单应矩阵的拟合内点数若单应内点占比超过 80%放弃这一帧对。也可以用cv2.findHomography配合判断当单应模型和本质矩阵模型给出一致内点时优先怀疑退化。这个坑在室内拍摄墙面纹理时特别容易出现。5.3 增量过程中位姿爆掉点云像烟花现象前几十帧好好的某一帧之后点云随机乱飞相机轨迹突然折返。原因solvePnP 的reprojectionError设置太大把外点当内点或者 ORB 匹配在重复纹理区域产生了大量误匹配。增量式 SfM 本身也会累积误差位姿一旦被一帧汚染后面的帧都会跟着错。解决打印该帧的 inliers 数量低于 30 就不注册这一帧。把reprojectionError从 8.0 降到 4.0同时只保留与关键帧的匹配。关键帧不是上一帧而是“与当前帧位姿距离适中的已注册帧”工程上叫 view selection。最简单的做法是每隔 5 帧设一个关键帧当前帧与最近关键帧匹配这样漂移比始终和上一帧匹配小很多。如果 inliers 数量不稳定判断是不是图片序错了或者图像被裁切过。5.4 点云尺度不对距离没法用现象重建出来的房子楼梯高度和真实世界完全对不上人走后点云只到膝盖高。原因SfM 本来就没有绝对尺度t 的模长被归一化成 1整个场景的单位是“任意单位”。本质矩阵分解出的 t 只能确定方向不能确定真实距离。解决放一个已知尺寸的参照物比如 A4 纸或标定板重建后用相似变换把点云缩放到真实尺寸。Python 里可以用 OpenCV 的estimateAffine3D配合已知点对做尺度对齐。注意这只能在稀疏点云上做稠密重建前就要修正尺度否则后面生成的面片全都白做。我在实际项目里会提前量取场景中一段真实长度例如门框高度重建后在 CloudCompare 里用两点测量工具点选门框两端把比例因子算出来。5.5 内参不准点云凸起或凹陷轨迹越走越歪现象平均重投影误差很小但点云平面全是弯曲的墙面像被一拳打凹。原因fx、fy、cx、cy 误差会转变成像素射线的角度误差误差朝一个方向累积最后体现在三维形状弯曲上。频繁的相机内参变化、手机数字变焦、防抖裁切都会让标定失效。解决先用标定板做标定不要偷懒用width*1.2的近似值。至少也要从 EXIF 读焦距再换算成像素单位fx 焦距毫米 / 传感器宽度毫米 * 图像宽度。BA 可以修正一部分内参误差但不稳定的内参会让它很难收敛。如果镜头带自动变焦务必锁死焦距竖拍横拍混用也会让 EXIF 里的焦距方向混淆尽量统一拍摄朝向。6. 把玩具代码推向实用在管线里加一次光束法平差你以为代码跑到点云出来就算完真正让位姿稳定的是光束法平差。两视图的 BA 规模小但足够让你理解残差、迭代收敛这两个词。6.1 一个只优化两帧的 BA 残差函数这里用scipy.optimize.least_squares做优化。变量是第二帧的旋转向量和平移以及三维点坐标。第一帧固定为I|0。from scipy.optimize import least_squares def reproject(params, K, pts1, pts2): rvec params[:3] tvec params[3:6] xyz params[6:].reshape(-1, 3) R, _ cv2.Rodrigues(rvec) P1 K np.hstack([np.eye(3), np.zeros((3, 1))]) P2 K np.hstack([R, tvec.reshape(3, 1)]) err [] for i, (X, p1, p2) in enumerate(zip(xyz, pts1, pts2)): x1 P1 np.append(X, 1) x2 P2 np.append(X, 1) err.append(p1 - x1[:2] / x1[2]) err.append(p2 - x2[:2] / x2[2]) return np.array(err)初始值直接喂三角化输出的三维点和初始化得到的 R、t用least_squares跑几十轮。优化完后重投影误差通常会从 1.5 像素降到 0.5 像素说明位姿和点云的耦合关系被重新捋顺了。6.2 怎么判断优化有效这个两帧优化只能修正局部问题不能替代全局 BA。我会在增量重建完成后用它做一次全局的“复查”把所有位姿和三维点拼成一个长向量用同样的残差函数跑上几十次迭代。几十张图规模还承受得住几百张图就必须换 Ceres 或 COLMAP backend。我现在的习惯是每次重建完先把平均重投影误差打印出来超过 1.5 像素就不急着出点云先回查初始化那两张图和内参。这套流程是从“图像进、点云出”到“图像进、可信点云出”的分界线。希望帮到你。本文还有配套的精品资源点击获取