一、引言
在本系列的前四篇文章中，我们完成了一次从理论到实践的增量式SFM系统构建，并且这个过程由简入繁、层层递进：
- 最小二乘问题详解18：增量式SFM核心流程实现：核心骨架的搭建
我们首先在一个"理想世界"中起步，使用不含外点的完美仿真数据，实现了增量式SFM最核心的骨架流程。通过"初始化 -> PnP注册 -> 三角化扩展 -> 全局BA优化"的闭环，我们验证了算法逻辑的正确性，成功恢复了场景的几何结构。 - 最小二乘问题详解19：带先验约束的增量式SFM优化与实现：多源信息的融合
接着，我们引入了INS/GNSS位姿先验和稀疏GCP（地面控制点）作为强约束。这模拟了配备了高精度组合导航设备的工业场景，极大地简化了求解难度，使系统能够直接获得具有绝对尺度和地理坐标的重建结果，展现了多源信息融合的强大威力。 - 最小二乘问题详解20：无先验约束下的增量式SFM自由网平差：纯视觉的自由探索
然后，我们剥离了所有外部先验，回归计算机视觉的本源。在仅有2D-2D特征对应的情况下，我们通过对极几何和本质矩阵求解，实现了"从无到有"的纯视觉自由网平差。虽然重建结果存在7自由度的不确定性（尺度、旋转、平移），但它证明了仅凭图像序列恢复相对结构的可能性。 - 最小二乘问题详解21：稀疏GCP约束下的自由网平差与弱约束融合：弱约束的巧妙融合
最后，我们探讨了一个更具普遍性的场景：在没有密集位姿先验，但拥有少量GCP的情况下，如何将自由网模型精确对齐到真实世界坐标系。我们提出的"Sim3引导的联合优化"策略，巧妙地解决了这一难题，以最低的成本实现了高精度的绝对定向。
纵观这四篇文章，我们使用的数据虽然加入了高斯噪声，但本质上是"干净"的------所有的特征匹配都是正确的内点（Inliers）。这让我们能够专注于SFM的优化理论本身，而不被数据质量问题所干扰。
然而，真实的工程世界远非如此理想。在实际的图像采集与特征匹配过程中，由于重复纹理、光照变化、动态物体、遮挡等因素，会产生大量的错误匹配 ，即外点（Outliers）。这些外点就像混入精密仪器中的沙砾，如果直接将它们纳入我们之前构建的最小二乘优化框架，将会导致灾难性的后果：
- 初始化失败 ：用于初始化的图像对如果包含大量外点，本质矩阵 \(E\) 的求解将完全错误，导致整个重建流程在第一步就宣告失败。
- 位姿估计错误：在PnP阶段，错误匹配会误导相机位姿的求解，将相机"放置"在错误的位置和姿态上。
- 模型扭曲甚至崩溃：在BA优化中，最小二乘法对误差的平方敏感，外点产生的巨大残差会严重"拉扯"整个模型，导致重建出的3D点云和相机轨迹发生扭曲，甚至使优化器无法收敛。
因此，如何处理这些不可避免的外点，是SFM算法从理论走向工程必须跨越的一道鸿沟。这便引出了本文的核心主题------抗差估计（Robust Estimation）。
二、数据生成
在《最小二乘问题详解17：SFM仿真数据生成》的实现中，我们已经构建了一个相对"完美"的SFM仿真数据。要评估算法在真实世界中的鲁棒性，必须主动为这个"完美"系统引入外点，以模拟真实特征匹配中不可避免的错误。
在真实的SFM流程中，外点通常源于特征描述子相似但空间位置无关的误匹配。为了在仿真环境中复现这一现象，我们无需重新进行复杂的特征提取与匹配，而是可以直接对已有的、正确的2D观测点坐标进行"污染"。具体实现逻辑封装在如下 CorruptData 函数中，其核心思想是：随机选择一部分观测，并将其2D像素坐标替换为图像平面内的随机值。具体实现代码如下：
cpp
void CorruptData(sfm::Bundle& bundle, const string& outputDir,
double outlierRatio, double missingRatio,
double outlierMaxOffset = 200.0) {
std::cout << "=== Injecting Robustness Noise ===" << std::endl;
std::cout << " - Outlier Ratio: " << outlierRatio * 100 << "%" << std::endl;
std::cout << " - Missing Ratio: " << missingRatio * 100 << "%" << std::endl;
std::cout << " - Max Offset: " << outlierMaxOffset << " pixels" << std::endl;
int totalObs = 0;
for (const auto& track : bundle.tracks) {
totalObs += track.obs.size();
}
int outlierCount = static_cast<int>(totalObs * outlierRatio);
std::uniform_int_distribution<int> trackDist(0, bundle.tracks.size() - 1);
int actualOutliers = 0;
std::uniform_int_distribution<int> distU(0, 5472);
std::uniform_int_distribution<int> distV(0, 3648);
for (int i = 0; i < outlierCount; ++i) {
// 随机找一个 Track
int tIdx = trackDist(rng);
if (bundle.tracks[tIdx].isPrior) continue; // 不修改先验点
if (bundle.tracks[tIdx].obs.empty()) continue;
// 随机找该 Track 中的一个观测
std::uniform_int_distribution<int> obsDist(
0, bundle.tracks[tIdx].obs.size() - 1);
int oIdx = obsDist(rng);
auto& obs = bundle.tracks[tIdx].obs[oIdx];
int imgId = obs.imgId;
int kpId = obs.kpId;
// 确保索引合法
if (kpId >= 0 && kpId < bundle.views[imgId].size()) {
// 修改 2D 坐标
auto& point2d = bundle.views[imgId][kpId];
point2d.x() = distU(rng);
point2d.y() = distV(rng);
actualOutliers++;
}
}
std::cout << " - Injected " << actualOutliers
<< " outliers (coordinate offsets)." << std::endl;
}经过这一步处理，我们便得到了一份"不完美"的仿真数据。这份数据在保留了原始几何结构的同时，混入了指定比例（10%）的随机噪声匹配，为接下来测试和实现SFM的抗差估计能力提供了理想的试验场。
三、初始化
在生成了带外点的仿真数据之后，就可以改进和构建新的SFM系统了。这里基于上一篇文章《最小二乘问题详解21：稀疏GCP约束下的自由网平差与弱约束融合》中的实现来进行改造，因为这种情况最为复杂和典型，可以较好地说明抗差估计在真实工程场景中的关键作用。
增量式SFM的第一步是初始化。该过程需要从图像序列中选取一个最佳的图像对，通过求解它们之间的对极几何关系来恢复初始的相机位姿和3D点云。这个过程非常关键，因为初始化是整个SFM重建流程的"种子"。如果这一步失败，例如本质矩阵 \(E\) 被错误估计，那么由此恢复出的初始相机位姿和3D点云将完全是错误的。这个错误的"种子"会在后续的增量式扩展中被不断放大，最终导致整个重建模型扭曲、漂移，甚至直接崩溃。因此，一个能够抵抗外点干扰的稳健初始化策略，是构建工程级SFM系统的基石。
在这里，先给出改进后的初始化的实现：
cpp
bool Init(const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
vector<cl::CameraExtrinsics>& cameraExtrinsics,
vector<cl::ObjectPoint>& objectPoints) {
auto pairs = BuildCandidatePairs(bundle, K);
for (int pi = 0; pi < std::min(kTopKPairs, (int)pairs.size()); pi++) {
int img1 = pairs[pi].img1;
int img2 = pairs[pi].img2;
// 提取匹配
std::vector<Eigen::Vector2d> pts1, pts2;
std::vector<Eigen::Vector3d> x1s, x2s;
std::vector<int> kpId1s, kpId2s;
ExtractMatches(bundle, img1, img2, K, pts1, pts2, x1s, x2s, kpId1s, kpId2s);
EssentialProblem solver;
auto [E, inlierMask] = solver.Solve(K, x1s, x2s, 1, 100);
// inlier筛选
std::vector<Eigen::Vector2d> in1, in2;
for (int mi = 0; mi < inlierMask.size(); mi++) {
if (inlierMask[mi]) {
in1.push_back(pts1[mi]);
in2.push_back(pts2[mi]);
} else {
outliers.emplace(sfm::Observation{img1, kpId1s[mi]});
outliers.emplace(sfm::Observation{img2, kpId2s[mi]});
}
}
if (in1.size() < 50) continue;
std::vector<Eigen::Matrix3d> Rs;
std::vector<Eigen::Vector3d> ts;
DecomposeEssentialMatrix(E, Rs, ts);
double bestScore = -1;
int bestIdx = -1;
for (int ki = 0; ki < Rs.size(); ki++) {
double score = EvaluatePose(K, {Eigen::Matrix3d::Identity(), Rs[ki]},
{Eigen::Vector3d::Zero(), ts[ki]}, in1, in2);
if (score > bestScore) {
bestScore = score;
bestIdx = ki;
}
}
if (bestScore < 0.3) continue;
// 初始化成功
cameraExtrinsics[img1].registered = true;
cameraExtrinsics[img1].R = Eigen::Matrix3d::Identity();
cameraExtrinsics[img1].t = Eigen::Vector3d::Zero();
cameraExtrinsics[img2].registered = true;
cameraExtrinsics[img2].R = Rs[bestIdx];
cameraExtrinsics[img2].t = ts[bestIdx];
//
auto [new3dPoint, id3ds, max_errors] =
TriangulateNewPoints(K, bundle, cameraExtrinsics, objectPoints);
//
RefineObjectPointsAdaptive(new3dPoint, id3ds, max_errors, objectPoints);
std::vector<int> localOutliers;
do {
localOutliers.clear();
GlobalBundleAdjustment(K, bundle, cameraExtrinsics, objectPoints,
localOutliers);
} while (localOutliers.size() > 0);
return true;
}
return false;
}(一) 初始像对选取
在之前的系列文章中，为了简化SFM的设计与实现，我们采用了最朴素的初始像对选取策略：选取同名点对数量最多的图像对。在理想无外点的数据中，这个策略是有效的，因为匹配点越多，通常意味着两幅图像的重叠区域越大，几何关系越可靠。然而，在引入了大量外点的场景下，单纯追求匹配数量存在明显的弊端：匹配点数量多，并不代表几何关系可靠。
这一现象在笔者的航飞蛇形航线仿真试验中体现得尤为典型。在蛇形航线（→ → → → / ← ← ← ←）中，相邻帧之间虽然拥有极高的重叠度（Overlap），但也因此产生了极大的视差角（Parallax）问题。如果仅按匹配数量选取，算法极易选中同一航线内相邻的图像对。
根据笔者的试验经验，在这种 弱基线（小视差） 条件下，本质矩阵估计问题接近退化，导致不同模型之间的区分度降低，从而使RANSAC难以有效区分内点与外点。这不仅容易造成初始化失败，更危险的是容易筛选出大量伪内点 。一旦这些伪内点被纳入初始模型，解算出的本质矩阵 \(E\) 会略有偏差，平移向量 \(t\) 的方向也会发生偏移。这种初始的微小误差在后续流程中会被急剧放大，导致三角化误差和BA优化误差偏大，最终使整个重建模型崩溃。
反之，如果通过引入基线约束，同时考虑匹配数量 和几何强度两个维度，综合评估每一对候选图像，选取跨航线的图像对作为初始像对，几何条件将得到显著改善，从而大幅提升系统的稳健性：
cpp
struct PairInfo {
int img1;
int img2;
int matchCount;
double avgParallax;
double score;
};
constexpr double kMinParallaxDeg = 1.0;
constexpr double kMinTriAngleDeg = 1.0;
constexpr double kMaxReprojError = 3.0;
constexpr int kTopKPairs = 5;
std::vector<PairInfo> BuildCandidatePairs(const sfm::Bundle& bundle,
const Eigen::Matrix3d& K) {
std::map<std::pair<int, int>, std::vector<std::pair<int, int>>> pairMatches;
// 收集 matches
for (const auto& track : bundle.tracks) {
if (track.obs.size() < 2) continue;
for (size_t i = 0; i < track.obs.size(); i++) {
for (size_t j = i + 1; j < track.obs.size(); j++) {
auto key = std::make_pair(track.obs[i].imgId, track.obs[j].imgId);
pairMatches[key].push_back({track.obs[i].kpId, track.obs[j].kpId});
}
}
}
std::vector<PairInfo> pairs;
for (auto& [key, matches] : pairMatches) {
int img1 = key.first;
int img2 = key.second;
double sumAngle = 0;
for (auto& m : matches) {
const auto& p1 = bundle.views[img1][m.first];
const auto& p2 = bundle.views[img2][m.second];
Eigen::Vector3d x1 =
(K.inverse() * Eigen::Vector3d(p1.x(), p1.y(), 1)).normalized();
Eigen::Vector3d x2 =
(K.inverse() * Eigen::Vector3d(p2.x(), p2.y(), 1)).normalized();
double cosv = std::clamp(x1.dot(x2), -1.0, 1.0);
sumAngle += acos(cosv);
}
double avgAngle = sumAngle / matches.size();
// 筛掉低视差
if (avgAngle * 180.0 / PI < kMinParallaxDeg) continue;
PairInfo info;
info.img1 = img1;
info.img2 = img2;
info.matchCount = matches.size();
info.avgParallax = avgAngle;
// score = match × parallax
info.score = matches.size() * avgAngle;
pairs.push_back(info);
}
std::sort(
pairs.begin(), pairs.end(),
[](const PairInfo& a, const PairInfo& b) { return a.score > b.score; });
return pairs;
}在这段代码实现中，核心思想是为每一对候选图像计算一个综合得分（Score），并依此进行排序。
-
遍历与统计
首先，我们遍历所有的3D点（Track），统计每两幅图像之间的共视匹配点。
pairMatches这个数据结构就用于存储图像对(img1, img2)与其所有匹配的关键点对(kpId1, kpId2)之间的映射关系。cppstd::map<std::pair<int, int>, std::vector<std::pair<int, int>>> pairMatches; // ... 遍历 tracks 填充 pairMatches ... -
计算平均视差角
对于每一个候选图像对，我们不再仅仅统计匹配点数量，而是进一步计算它们之间的平均视差角（Average Parallax Angle）。视差角是指同一个3D点在两个相机光心处形成的视线夹角。
- 物理意义：视差角越大，意味着基线越长，三角化时的交会角越好，计算出的3D点深度就越精确。反之，如果视差角趋近于0，说明两幅图像几乎在同一位置拍摄，三角化结果将极不稳定。
- 代码实现 ：我们通过将2D像素点反投影到归一化相机平面上，得到其方向向量
x1和x2，然后计算这两个向量的夹角。
cpp// 将像素坐标反投影到归一化平面 Eigen::Vector3d x1 = (K.inverse() * Eigen::Vector3d(p1.x(), p1.y(), 1)).normalized(); Eigen::Vector3d x2 = (K.inverse() * Eigen::Vector3d(p2.x(), p2.y(), 1)).normalized(); // 计算夹角 double cosv = std::clamp(x1.dot(x2), -1.0, 1.0); double angle = acos(cosv); sumAngle += angle;在累加了所有匹配点的视差角后，我们求其平均值
avgAngle。 -
设定阈值与打分
有了平均视差角，我们就可以进行初步筛选和最终打分。
- 阈值筛选 ：首先，我们会剔除掉那些平均视差角过小的图像对。代码中设定了
kMinParallaxDeg（例如1.0度）作为阈值。如果avgAngle小于该阈值，说明这对图像的基线太短，直接放弃，不予考虑。 - 综合打分 ：对于通过筛选的图像对，我们计算其最终得分。这里的策略非常简单而有效：得分 = 匹配点数量 × 平均视差角。
cpp// score = match × parallax info.score = matches.size() * avgAngle;这个公式的含义是，我们既希望匹配点足够多以保证RANSAC的成功率，又希望视差角足够大以保证三角化的精度。两者相乘，可以很好地平衡这两个需求。
- 阈值筛选 ：首先，我们会剔除掉那些平均视差角过小的图像对。代码中设定了
-
排序与选取
最后，我们将所有候选图像对按照得分
score从高到低进行排序。在初始化时，我们只需从排好序的列表中依次尝试得分最高的前kTopKPairs（例如5对）即可。
通过这种"数量与质量并重"的选取策略，我们能够极大地提高初始化的成功率，为后续的稳健重建打下坚实的基础。
(二) 三角化误差筛选
在成功选取最佳像对并求解出本质矩阵 \(E\) 之后，下一步便是对共视的内点进行三角化，以生成初始的3D点云。
然而，由于 \(E\) 矩阵是在存在外点的条件下通过RANSAC估算得到的，它本身并非完美无缺。RANSAC虽然能剔除大部分外点，但仍可能残留一些"伪内点"（即几何上不完全满足约束，但侥幸落入误差阈值内的错误匹配）。此外，RANSAC求解出的 \(E\) 矩阵本身也存在一定误差。这些因素叠加，导致三角化后计算出的重投影误差呈现出一种非常不理想的分布：
- 大部分点的误差：集中在二三十个像素的范围内。
- 少量点的误差：却异常巨大，可能达到数百甚至上千个像素。
面对这种误差分布，传统的固定阈值筛选法（例如，简单地剔除所有重投影误差大于5个像素的点）就显得不太合理。如果阈值设得太小，会误伤大量误差在二三十像素的"准内点"，导致初始化点云过于稀疏；如果阈值设得太大，又无法有效剔除那些误差巨大的"坏点"，污染初始模型。
为了解决这个问题，我们采用了一种自适应的误差筛选策略。其核心思想是：不预设一个绝对的误差阈值，而是根据当前所有三角化点误差的统计分布，动态地确定一个合理的筛选门限。具体代码实现如下所示：
cpp
std::tuple<std::vector<Eigen::Vector3d>, std::vector<int>, std::vector<double>>
TriangulateNewPoints(const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
vector<cl::CameraExtrinsics>& cameraExtrinsics,
vector<cl::ObjectPoint>& objectPoints) {
std::vector<Eigen::Vector3d> new3dPoint;
std::vector<int> id3ds;
std::vector<double> max_errors;
for (auto track : bundle.tracks) {
if (objectPoints[track.pointId].triangulated) {
continue;
}
vector<sfm::Observation> registerObservation;
for (const auto& ob : track.obs) {
if (cameraExtrinsics[ob.imgId].registered &&
outliers.find(ob) == outliers.end()) {
registerObservation.push_back(ob);
}
}
if (registerObservation.size() < 2) {
continue;
}
std::vector<Eigen::Matrix3d> Rs;
std::vector<Eigen::Vector3d> ts;
std::vector<Eigen::Vector2d> observations;
for (const auto& ob : registerObservation) {
Rs.push_back(cameraExtrinsics[ob.imgId].R);
ts.push_back(cameraExtrinsics[ob.imgId].t);
observations.push_back(bundle.views[ob.imgId][ob.kpId]);
}
// 尝试三角化
Eigen::Vector3d X = Eigen::Vector3d::Zero();
TriangulateProblem triangulateProblem(K, Rs, ts, observations);
if (!triangulateProblem.Solve(X)) {
continue;
}
// Cheirality check: 确保三角化点在所有相机前方
bool valid = true;
for (size_t i = 0; i < Rs.size(); i++) {
Eigen::Vector3d Zc = Rs[i] * X + ts[i];
if (Zc.z() <= 0) {
valid = false;
break;
}
}
if (!valid) continue;
// cout << X.transpose() << '\n';
double max_error = 0;
for (size_t i = 0; i < observations.size(); i++) {
Eigen::Vector3d Zc = Rs[i] * X + ts[i];
Eigen::Vector3d pc = K * Zc;
Eigen::Vector2d proj(pc.x() / pc.z(), pc.y() / pc.z());
double err = (proj - observations[i]).norm();
max_error = std::max(max_error, err);
}
// std::cout << max_error << '\t';
new3dPoint.push_back(X);
id3ds.push_back(track.pointId);
max_errors.push_back(max_error);
// objectPoints[track.pointId].pos = X;
// objectPoints[track.pointId].triangulated = true;
}
return {new3dPoint, id3ds, max_errors};
}
void RefineObjectPointsAdaptive(std::vector<Eigen::Vector3d>& new3dPoint,
std::vector<int>& id3ds,
std::vector<double>& max_errors,
std::vector<cl::ObjectPoint>& objectPoints) {
vector<int> idx(max_errors.size());
iota(idx.begin(), idx.end(), 0);
sort(idx.begin(), idx.end(),
[&](int a, int b) { return max_errors[a] < max_errors[b]; });
double median = max_errors[idx[idx.size() / 2]];
double th = 5 * median;
for (size_t ii = 0; ii < idx.size(); ++ii) {
int mi = idx[ii];
if (max_errors[mi] > th) {
// cout << max_errors[mi] << '\t';
} else {
objectPoints[id3ds[mi]].pos = new3dPoint[mi];
objectPoints[id3ds[mi]].triangulated = true;
}
}
}在这段代码实现中：
-
三角化与误差计算
在
TriangulateNewPoints函数中，我们对所有满足条件的Track进行三角化。对于每一个成功三角化出的3D点 \(X\)，我们会将其重投影回所有观测到它的相机中，并计算其最大重投影误差（Max Reprojection Error）。cpp// 计算重投影误差 double max_error = 0; for (size_t i = 0; i < observations.size(); i++) { Eigen::Vector3d Zc = Rs[i] * X + ts[i]; Eigen::Vector3d pc = K * Zc; Eigen::Vector2d proj(pc.x() / pc.z(), pc.y() / pc.z()); double err = (proj - observations[i]).norm(); max_error = std::max(max_error, err); // 记录最大误差 } // 存储3D点、ID和对应的最大误差 new3dPoint.push_back(X); id3ds.push_back(track.pointId); max_errors.push_back(max_error);这一步完成后，我们得到了一组初始的3D点，以及它们各自对应的最大重投影误差列表
max_errors。 -
自适应阈值筛选
接下来，
RefineObjectPointsAdaptive函数接管这些误差数据，执行自适应筛选。-
计算中位数 ：首先，将所有误差值从小到大排序，并找出它们的中位数（Median）。中位数相比平均值，对极端大值（Outliers）不敏感，能更好地反映这批数据的"典型"误差水平。
-
动态设定阈值 ：然后，我们将筛选阈值
th设定为中位数的若干倍（代码中为5倍）。cppsort(idx.begin(), idx.end(),[&](int a, int b) { return max_errors[a] < max_errors[b]; }); double median = max_errors[idx[idx.size() / 2]]; // 取中位数 double th = 5 * median; // 阈值为中位数的5倍这个策略非常巧妙：
- 如果三角化质量普遍较好，中位数很小（例如2像素），那么阈值也会很严格（10像素），能有效剔除坏点。
- 如果由于初始 \(E\) 矩阵不准，导致整体误差偏大，中位数也会随之变大（例如20像素），阈值会自动放宽（100像素），从而保留了大部分"准内点"，避免了因阈值过严而丢失过多数据。
-
执行筛选 ：最后，遍历所有三角化点，只保留那些最大重投影误差小于动态阈值
th的点，将其加入到初始模型中。cppfor (size_t ii = 0; ii < idx.size(); ++ii) { int mi = idx[ii]; if (max_errors[mi] > th) { // 误差过大，舍弃该点 } else { // 误差可接受，采纳该点 objectPoints[id3ds[mi]].pos = new3dPoint[mi]; objectPoints[id3ds[mi]].triangulated = true; } }
-
通过这种自适应的筛选机制，我们能够在不依赖人工经验设定绝对阈值的情况下，智能地剔除三角化产生的"坏点"，为后续的Bundle Adjustment提供一个既干净又足够丰富的初始点云，极大地提升了系统在弱约束条件下的鲁棒性。
(三) BA重投影误差筛选
初始化的过程较为复杂，是一个从无到有的过程。即使经过了RANSAC求解本质矩阵 \(E\) 和三角化后的自适应误差筛选，模型中仍可能残留少量外点。面对这种情况，只能在BA的过程中进行迭代筛选。
具体的实现原理我们在《最小二乘问题详解16：束平差工程实践总结》中已有介绍，简单来说就是在进行一次BA优化之后，根据重投影误差去除误差比较大的观测作为外点，然后再次进行BA。这个过程可能会迭代多次，直到没有新的外点被剔除为止。代码实现如下所示：
cpp
bool Init(...) {
//...
std::vector<int> localOutliers;
do {
localOutliers.clear();
GlobalBundleAdjustment(K, bundle, cameraExtrinsics, objectPoints,
localOutliers);
} while (localOutliers.size() > 0);
//...
}
bool GlobalBundleAdjustment(const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
vector<cl::CameraExtrinsics>& cameras,
vector<cl::ObjectPoint>& objectPoints,
std::vector<int>& localOutliers) {
map<int, int> cameraIdMap;
vector<BAProblem::CameraExtrinsics> est_cams;
for (size_t ci = 0; ci < cameras.size(); ++ci) {
if (cameras[ci].registered) {
BAProblem::CameraExtrinsics cameraExtrinsics;
Eigen::Quaterniond quaternion(cameras[ci].R);
cameraExtrinsics.q[0] = quaternion.w();
cameraExtrinsics.q[1] = quaternion.x();
cameraExtrinsics.q[2] = quaternion.y();
cameraExtrinsics.q[3] = quaternion.z();
cameraExtrinsics.t[0] = cameras[ci].t.x();
cameraExtrinsics.t[1] = cameras[ci].t.y();
cameraExtrinsics.t[2] = cameras[ci].t.z();
cameraIdMap[ci] = est_cams.size();
est_cams.push_back(std::move(cameraExtrinsics));
}
}
vector<BAProblem::ObjectPoint> est_pts;
vector<BAProblem::Observation> obs;
map<int, int> pointIdMap;
map<int, sfm::Observation> newObIdAndOldOb;
for (const auto& track : bundle.tracks) {
if (objectPoints[track.pointId].triangulated) {
BAProblem::ObjectPoint objectPoint;
objectPoint.xyz[0] = objectPoints[track.pointId].pos.x();
objectPoint.xyz[1] = objectPoints[track.pointId].pos.y();
objectPoint.xyz[2] = objectPoints[track.pointId].pos.z();
for (auto ob : track.obs) {
auto iter = cameraIdMap.find(ob.imgId);
if (iter != cameraIdMap.end() && outliers.find(ob) == outliers.end()) {
BAProblem::Observation observation;
observation.cam_id = iter->second;
observation.pt_id = est_pts.size();
observation.x = bundle.views[ob.imgId][ob.kpId].x();
observation.y = bundle.views[ob.imgId][ob.kpId].y();
newObIdAndOldOb.emplace(obs.size(), ob);
obs.push_back(std::move(observation));
}
}
pointIdMap.emplace(track.pointId, est_pts.size());
est_pts.push_back(objectPoint);
}
}
BAProblem problem(K, obs, est_cams, est_pts);
if (!problem.Solve(localOutliers)) {
return false;
}
for (auto newObId : localOutliers) {
auto iter = newObIdAndOldOb.find(newObId);
if (iter != newObIdAndOldOb.end()) {
// cout << "Observation in image " << iter->second.imgId << " with
// keypoint "
// << iter->second.kpId << " is an outlier." << endl;
outliers.insert(iter->second);
}
}
for (const auto& [oldId, id] : cameraIdMap) {
Eigen::Quaternion q(est_cams[id].q[0], est_cams[id].q[1], est_cams[id].q[2],
est_cams[id].q[3]);
cameras[oldId].R = q.toRotationMatrix();
cameras[oldId].t.x() = est_cams[id].t[0];
cameras[oldId].t.y() = est_cams[id].t[1];
cameras[oldId].t.z() = est_cams[id].t[2];
}
for (auto track : bundle.tracks) {
if (objectPoints[track.pointId].triangulated) {
auto iter = pointIdMap.find(track.pointId);
if (iter != pointIdMap.end()) {
objectPoints[track.pointId].pos.x() = est_pts[iter->second].xyz[0];
objectPoints[track.pointId].pos.y() = est_pts[iter->second].xyz[1];
objectPoints[track.pointId].pos.z() = est_pts[iter->second].xyz[2];
}
}
}
for (auto track : bundle.tracks) {
if (objectPoints[track.pointId].triangulated) {
auto iter = pointIdMap.find(track.pointId);
if (iter != pointIdMap.end()) {
objectPoints[track.pointId].pos.x() = est_pts[iter->second].xyz[0];
objectPoints[track.pointId].pos.y() = est_pts[iter->second].xyz[1];
objectPoints[track.pointId].pos.z() = est_pts[iter->second].xyz[2];
}
}
}
return true;
}这个算法有一个缺点，就是同样可能会误剔除一部分正确的内点。但对于初始化阶段来说，这是值得的。因为初始化是整个重建流程的基石，其质量至关重要，不能容忍任何外点的存在。一个纯净但相对稀疏的初始模型，远比一个包含大量外点的稠密模型要稳健得多。
另外，许多资料建议可以引入鲁棒核函数（如Huber Loss）来强化BA的结果：
cpp
ceres::Problem problem;
ceres::Manifold* q_manifold = new ceres::QuaternionManifold();
for (int i = 0; i < est_cams.size(); i++) {
problem.AddParameterBlock(est_cams[i].q, 4, q_manifold);
problem.AddParameterBlock(est_cams[i].t, 3);
}
for (auto& o : obs) {
ceres::CostFunction* cost = ReprojectionError::Create(o.x, o.y, fx, fy, cx, cy);
// new ceres::HuberLoss(3.0)
problem.AddResidualBlock(cost, nullptr, est_cams[o.cam_id].q, est_cams[o.cam_id].t, est_pts[o.pt_id].xyz);
}但在笔者的实践中，引入Huber核函数的效果并不理想。原因可能在于，Huber核函数的阈值（如代码中的3.0）是一个固定值，它假设大部分观测的误差都集中在这个阈值之内。然而，在初始化阶段，由于本质矩阵 \(E\) 本身存在偏差，导致初始三角化的3D点不够精确，其重投影误差的分布范围可能本身就比较大。在这种情况下，一个固定的、较小的阈值会将大量"准内点"的权重降低，从而无法有效纠正初始模型的整体偏差；而如果将阈值设得过大，又失去了抗差的意义。相比之下，直接进行迭代剔除策略虽然简单粗暴，但它能根据每次优化后的残差分布动态地确定剔除标准，在初始化这个特殊阶段反而更加直接有效。这也体现了工程实践与理论之间的差异，没有一种方法是万能的，需要根据具体场景选择最合适的策略。
(四) 两种本质矩阵求解
在《最小二乘问题详解14：鲁棒估计与5点算法求解本质矩阵》中，我们采用了一种较为直接的代数方法求解本质矩阵 \(E\)：将 \(E\) 视为一个9维向量进行优化，并在每次迭代后将结果投影回本质矩阵流形上。
这种方法虽然可行，但从几何角度看并非最优。本质矩阵 \(E\) 是由旋转 \(R\) 和平移 \(t\) 构成的，其自由度为5，且必须满足内在的代数约束（即其奇异值为 \((\sigma, \sigma, 0)\)）。直接优化9个参数不仅效率不高，还可能引入不必要的数值误差。
一个更严谨的做法是直接在 \(R\) 和 \(t\) 的流形上进行优化。我们可以将旋转表示为四元数（Quaternion），并使用 ceres::QuaternionManifold 约束其为单位四元数；将平移向量 \(t\) 约束在单位球面上（ceres::SphereManifold<3>），从而确保其模长为1。这样，优化过程始终在正确的几何空间中进行，保证了结果的物理意义。
cpp
// SolveEssential2.cpp 中的关键代码片段
Eigen::Quaterniond q(R_est);
double q_param[4] = {q.w(), q.x(), q.y(), q.z()};
double t_param[3] = {t_est.x(), t_est.y(), t_est.z()};
ceres::Problem problem;
// 在流形上优化
problem.AddParameterBlock(q_param, 4, new ceres::QuaternionManifold());
problem.AddParameterBlock(t_param, 3, new ceres::SphereManifold<3>());
for (int i = 0; i < x1s.size(); ++i) {
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<SampsonErrorRt, 1, 4, 3>(
new SampsonErrorRt(x1s[i], x2s[i]));
problem.AddResidualBlock(cost, nullptr, q_param, t_param);
}尽管流形优化在理论上更为严谨，但在笔者的实践中，两种方法对最终SFM重建结果的影响差异并不显著。为了保持与前序文章的连贯性，并简化代码逻辑，本文的初始化流程仍沿用了基于9维向量的求解方法。读者在实际工程中可根据需求选择更优的流形优化实现。
四、迭代循环
在完成初始化之后，增量式SFM的重建主循环反而还简单一点。其核心流程代码如下所示：
cpp
void Iterate(const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
vector<cl::CameraExtrinsics>& cameraExtrinsics,
vector<cl::ObjectPoint>& objectPoints) {
while (true) {
cout << "--- 新一轮迭代 ---" << endl;
// 1. 尝试注册一张新图片
bool success = RegisterNextImage(K, bundle, cameraExtrinsics, objectPoints);
if (!success) {
cout << "没有更多图片可以注册，结束。" << endl;
break;
}
// 2. 三角化新点
auto [new3dPoint, id3ds, max_errors] =
TriangulateNewPoints(K, bundle, cameraExtrinsics, objectPoints);
// 3. 过滤三角化点
RefineObjectPointsStatic(new3dPoint, id3ds, max_errors, objectPoints);
// 4. 全局光束法平差 (Global BA)
cout << "----GlobalBundleAdjustment----" << endl;
std::vector<int> localOutliers;
if (!GlobalBundleAdjustment(K, bundle, cameraExtrinsics, objectPoints,
localOutliers)) {
break;
}
// break;
}
cout << "=== SfM 完成 ===" << endl;
{
int counter = 0;
for (const auto& cam : cameraExtrinsics) {
if (cam.registered) {
counter++;
}
}
cout << "最终相机数: " << counter << endl;
}
{
int counter = 0;
for (const auto& point : objectPoints) {
if (point.triangulated) {
counter++;
}
}
cout << "最终 3D 点数: " << counter << endl;
}
}(一) 注册新图片
在初始化完成并拥有了初始的相机位姿和3D点云后，增量式SFM的核心任务就是不断地将新的图像"注册"到当前模型中，以扩展重建的规模。对于每一张尚未注册的图像，我们首先会查找它与当前已三角化的3D点之间的2D-3D对应关系。如果找到的对应点数量足够，就可以通过求解PnP问题来得到该图像的相机位姿（旋转 \(R\) 和平移 \(t\)）：
cpp
bool RegisterNextImage(const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
vector<cl::CameraExtrinsics>& cameraExtrinsics,
vector<cl::ObjectPoint>& objectPoints) {
map<int, vector<pair<int, int>>> mapImgTo3dAnd2d;
for (const auto& track : bundle.tracks) {
for (const auto& ob : track.obs) {
if (cameraExtrinsics[ob.imgId].registered) {
continue;
}
if (objectPoints[track.pointId].triangulated) {
mapImgTo3dAnd2d[ob.imgId].push_back({track.pointId, ob.kpId});
}
}
}
// 暂时选择点数最多的影像
int bestImgId = -1;
int counter = 0;
for (const auto& [imgId, ids] : mapImgTo3dAnd2d) {
if (ids.size() > counter) {
bestImgId = imgId;
counter = ids.size();
}
}
if (bestImgId == -1) {
return false;
}
const auto& idsMap = mapImgTo3dAnd2d[bestImgId];
vector<Eigen::Vector3d> points;
vector<Eigen::Vector2d> observations;
vector<int> oldIds;
for (const auto& [id3d, id2d] : idsMap) {
points.emplace_back(objectPoints[id3d].pos);
observations.emplace_back(bundle.views[bestImgId][id2d]);
oldIds.emplace_back(id2d);
}
Eigen::Matrix3d R_est = cameraExtrinsics[bestImgId].R;
Eigen::Vector3d t_est = cameraExtrinsics[bestImgId].t;
std::vector<bool> mask_out(points.size(), false);
PnPProblem problem(K, points, observations);
if (!problem.Solve(R_est, t_est, mask_out)) {
return false;
}
cameraExtrinsics[bestImgId].R = R_est;
cameraExtrinsics[bestImgId].t = t_est;
cameraExtrinsics[bestImgId].registered = true;
for (size_t i = 0; i < mask_out.size(); ++i) {
if (!mask_out[i]) {
outliers.insert(sfm::Observation{bestImgId, oldIds[i]});
}
}
return true;
}在上述代码实现中可以看到：在求解成功后，所有被判定为外点的观测都会被记录下来，并加入到全局的外点集合中，防止它们在后续流程中污染模型。
另外，在决定注册哪一张新图像时，本文采用了一个简化的策略：贪心地选择能够与当前模型产生最多2D-3D匹配点的图像。这个策略的逻辑是，匹配点越多，PnP求解的稳定性通常越高。当然，这是一个相对朴素的实现。更完善的SFM系统可能会综合考虑匹配点数量、匹配点在图像中的分布均匀性、以及与已注册相机的视差角大小等多个因素，来选择最优的下一张图像，以保证重建的稳健性和精度。
(二) 稳健PnP求解
用于PnP求解的2D-3D匹配中仍可能混杂着外点。如果直接使用这些包含外点的数据进行求解，相机的位姿就会被严重误导，导致注册失败，甚至污染整个重建模型。关于PnP问题的求解，在之前的系列文章中也已经详述过，不过这里同样做出了一些稳健性改进：
cpp
#include "SolvePnP.h"
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <iomanip>
#include <iostream>
#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
#include <vector>
#include "Math.h"
using namespace std;
// ============================================================================
// RANSAC + P3P + Cheirality + 可选Refine
// ============================================================================
bool SolveInitialPoseWithOpenGV(
const std::vector<Eigen::Vector3d>& points_world,
const std::vector<Eigen::Vector2d>& observations, double fx, double fy,
double cx, double cy,
double* q_out, // [w, x, y, z]
double* t_out, // [x, y, z]
std::vector<bool>& mask_out) {
if (points_world.size() < 4) {
std::cerr << "[OpenGV] Not enough points (need >= 4)" << std::endl;
return false;
}
// =========================
// 1. 构建 bearing vectors
// =========================
opengv::points_t pts_world;
opengv::bearingVectors_t bears;
pts_world.reserve(points_world.size());
bears.reserve(observations.size());
for (size_t i = 0; i < points_world.size(); ++i) {
pts_world.push_back(points_world[i]);
double u = observations[i].x();
double v = observations[i].y();
double nx = (u - cx) / fx;
double ny = (v - cy) / fy;
Eigen::Vector3d bearing(nx, ny, 1.0);
bearing.normalize();
bears.push_back(bearing);
}
// =========================
// 2. Adapter
// =========================
opengv::absolute_pose::CentralAbsoluteAdapter adapter(bears, pts_world);
// =========================
// 3. RANSAC + P3P
// =========================
typedef opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem Problem;
std::shared_ptr<Problem> problem =
std::make_shared<Problem>(adapter, Problem::KNEIP); // 👈 P3P
opengv::sac::Ransac<Problem> ransac;
ransac.sac_model_ = problem;
ransac.threshold_ = 0.001; // bearing error（弧度）
ransac.max_iterations_ = 100;
ransac.computeModel();
if (!ransac.model_coefficients_.allFinite()) {
std::cerr << "[OpenGV] RANSAC failed." << std::endl;
return false;
}
opengv::transformation_t T_cw = ransac.model_coefficients_;
// =========================
// 4. 提取 R t
// =========================
Eigen::Matrix3d R_cw = T_cw.block<3, 3>(0, 0);
Eigen::Vector3d t_cw = T_cw.block<3, 1>(0, 3);
// =========================
// 5. Cheirality check（保险）
// =========================
int front_count = 0;
for (size_t i = 0; i < pts_world.size(); ++i) {
Eigen::Vector3d pc = R_cw * pts_world[i] + t_cw;
if (pc.z() > 0) front_count++;
}
if (front_count < pts_world.size() / 2) {
R_cw = -R_cw;
t_cw = -t_cw;
}
// =========================
// 6. 可选：用 inliers refine（强烈推荐）
// =========================
if (!ransac.inliers_.empty()) {
std::vector<int> inliers = ransac.inliers_;
opengv::transformation_t T_refined =
opengv::absolute_pose::optimize_nonlinear(adapter, inliers);
Eigen::Matrix3d R_ref = T_refined.block<3, 3>(0, 0);
Eigen::Vector3d t_ref = T_refined.block<3, 1>(0, 3);
// 再做一次 cheirality（保险）
int front_count_ref = 0;
for (size_t i = 0; i < pts_world.size(); ++i) {
Eigen::Vector3d pc = R_ref * pts_world[i] + t_ref;
if (pc.z() > 0) front_count_ref++;
}
if (front_count_ref >= pts_world.size() / 2) {
R_cw = R_ref;
t_cw = t_ref;
}
}
// =========================
// 7. 填充 Mask (标识内点/外点)
// =========================
for (int idx : ransac.inliers_) {
if (idx >= 0 && idx < static_cast<int>(mask_out.size())) {
mask_out[idx] = true;
}
}
// =========================
// 8. 输出
// =========================
Eigen::Quaterniond q_cw(R_cw);
q_cw.normalize();
q_out[0] = q_cw.w();
q_out[1] = q_cw.x();
q_out[2] = q_cw.y();
q_out[3] = q_cw.z();
t_out[0] = t_cw.x();
t_out[1] = t_cw.y();
t_out[2] = t_cw.z();
// =========================
// Debug
// =========================
std::cout << "[OpenGV RANSAC+P3P] Initial pose computed." << std::endl;
std::cout << " Inliers: " << ransac.inliers_.size() << " / "
<< points_world.size() << std::endl;
std::cout << " Init Rot (w,x,y,z): [" << q_out[0] << ", " << q_out[1] << ", "
<< q_out[2] << ", " << q_out[3] << "]" << std::endl;
std::cout << " Init Trans (x,y,z): [" << t_out[0] << ", " << t_out[1] << ", "
<< t_out[2] << "]" << std::endl;
return true;
}
namespace {
struct ReprojectionError {
ReprojectionError(const Eigen::Vector3d& X_, const Eigen::Vector2d& obs_,
double fx_, double fy_, double cx_, double cy_)
: X(X_), obs(obs_), fx(fx_), fy(fy_), cx(cx_), cy(cy_) {}
template <typename T>
bool operator()(const T* const q, const T* const t, T* residuals) const {
T p_camera[3];
T X_world[3] = {T(X.x()), T(X.y()), T(X.z())};
// Ceres 期望 q 顺序为 [w, x, y, z]
ceres::QuaternionRotatePoint(q, X_world, p_camera);
p_camera[0] += t[0];
p_camera[1] += t[1];
p_camera[2] += t[2];
T xp = p_camera[0] / p_camera[2];
T yp = p_camera[1] / p_camera[2];
T u = T(fx) * xp + T(cx);
T v = T(fy) * yp + T(cy);
residuals[0] = u - T(obs.x());
residuals[1] = v - T(obs.y());
return true;
}
static ceres::CostFunction* Create(const Eigen::Vector3d& X,
const Eigen::Vector2d& obs, double fx,
double fy, double cx, double cy) {
return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3>(
new ReprojectionError(X, obs, fx, fy, cx, cy));
}
Eigen::Vector3d X;
Eigen::Vector2d obs;
double fx, fy, cx, cy;
};
} // namespace
PnPProblem::PnPProblem(const Eigen::Matrix3d& K,
const std::vector<Eigen::Vector3d>& points_world,
const std::vector<Eigen::Vector2d>& observations)
: fx(K(0, 0)),
fy(K(1, 1)),
cx(K(0, 2)),
cy(K(1, 2)),
points_world(points_world),
observations(observations) {}
bool PnPProblem::Solve(Eigen::Matrix3d& R_opt, Eigen::Vector3d& t_opt,
std::vector<bool>& mask_out) {
// --- 使用 OpenGV EPnP 计算初值 ---
double q_est[4] = {1, 0, 0, 0};
double t_est[3] = {0, 0, 0};
SolveInitialPoseWithOpenGV(points_world, observations, fx, fy, cx, cy, q_est,
t_est, mask_out);
std::vector<Eigen::Vector3d> inliers3ds;
std::vector<Eigen::Vector2d> inliers2ds;
std::vector<int> newIdAndOldId; // 新索引 -> 旧索引
for (size_t mi = 0; mi < mask_out.size(); ++mi) {
if (mask_out[mi]) {
newIdAndOldId.push_back(mi);
inliers3ds.push_back(points_world[mi]);
inliers2ds.push_back(observations[mi]);
}
}
std::cout << "\n=== Computing Initial Value with OpenGV EPnP ==="
<< std::endl;
if (!SolveInitialPose(inliers3ds, inliers2ds, fx, fy, cx, cy, q_est, t_est)) {
std::cerr << "Failed to compute initial pose. Falling back to identity."
<< std::endl;
// Fallback
Eigen::Quaterniond q_init = Eigen::Quaterniond::Identity();
EigenQuatToCeres(q_init, q_est);
t_est[0] = t_est[1] = t_est[2] = 0.0;
}
std::set<int> outliersIndices;
if (!NonlinearSolve(inliers3ds, inliers2ds, q_est, t_est, outliersIndices)) {
return false;
}
// cout << "\n=== Outliers Detected After Initial Optimization ===" << endl;
std::vector<Eigen::Vector3d> newInliers3ds;
std::vector<Eigen::Vector2d> newInliers2ds;
for (size_t ii = 0; ii < inliers3ds.size(); ++ii) {
if (outliersIndices.find(ii) == outliersIndices.end()) {
newInliers3ds.push_back(inliers3ds[ii]);
newInliers2ds.push_back(inliers2ds[ii]);
} else {
mask_out[newIdAndOldId[ii]] = false;
// cout << ii << '\t';
}
}
// cout << endl;
outliersIndices.clear();
NonlinearSolve(newInliers3ds, newInliers2ds, q_est, t_est, outliersIndices);
Eigen::Quaterniond q_opt = CeresQuatToEigen(q_est);
q_opt.normalize();
R_opt = q_opt.toRotationMatrix();
t_opt << t_est[0], t_est[1], t_est[2];
return true;
}
bool PnPProblem::NonlinearSolve(
const std::vector<Eigen::Vector3d>& points_world,
const std::vector<Eigen::Vector2d>& observations, double* q_est,
double* t_est, std::set<int>& outliersIndices) {
// --- 构建 Ceres 问题 ---
ceres::Problem problem;
ceres::Manifold* quaternion_manifold = new ceres::QuaternionManifold();
problem.AddParameterBlock(q_est, 4, quaternion_manifold);
problem.AddParameterBlock(t_est, 3);
for (size_t i = 0; i < points_world.size(); ++i) {
problem.AddResidualBlock(
ReprojectionError::Create(points_world[i], observations[i], fx, fy, cx,
cy),
new ceres::HuberLoss(3.0), q_est, t_est);
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
options.max_num_iterations = 50;
ceres::Solver::Summary summary;
std::cout << "\n=== Starting Non-linear Optimization (Ceres) ==="
<< std::endl;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
// --- 结果评估 ---
Eigen::Quaterniond q_opt = CeresQuatToEigen(q_est);
q_opt.normalize();
Eigen::Matrix3d R_opt = q_opt.toRotationMatrix();
Eigen::Vector3d t_opt(t_est[0], t_est[1], t_est[2]);
// 计算误差
double total_error = 0.0;
for (size_t i = 0; i < points_world.size(); ++i) {
Eigen::Vector3d pc = q_opt * points_world[i] + t_opt;
double u = fx * pc.x() / pc.z() + cx;
double v = fy * pc.y() / pc.z() + cy;
double err_x = u - observations[i].x();
double err_y = v - observations[i].y();
double pixel_error = sqrt(err_x * err_x + err_y * err_y);
if (pixel_error > 5) {
outliersIndices.insert(i);
}
total_error += err_x * err_x + err_y * err_y;
}
double rmse = std::sqrt(total_error / points_world.size());
std::cout << "\n=== Accuracy Check ===" << std::endl;
std::cout << "Final RMSE: " << rmse << " pixels" << std::endl;
// === 3. 核心判断：仅根据终止类型决定成败 ===
if (summary.termination_type != ceres::CONVERGENCE) {
return false;
}
// if (rmse > 5.0) { // 阈值根据情况调整，合成数据可以给小一点
// std::cout << "PnP Failed: Converged but RMSE too high." << std::endl;
// return false;
// }
return true;
}
// 使用 OpenGV EPnP 计算初值
bool PnPProblem::SolveInitialPose(
const std::vector<Eigen::Vector3d>& points_world,
const std::vector<Eigen::Vector2d>& observations, double fx, double fy,
double cx, double cy, double* q_out, double* t_out) {
if (points_world.size() < 4) {
std::cerr << "[OpenGV EPnP] Not enough points (need >= 4)" << std::endl;
return false;
}
// 1. 准备 OpenGV 的数据结构
// OpenGV 通常使用 bearing vectors (归一化射线) 或者直接在 adapter 中处理内参
// 这里我们使用 CentralAbsoluteAdapter，它需要归一化坐标 (x, y, 1)
opengv::points_t pts_world; // 3D 点 (世界系)
opengv::bearingVectors_t bears; // 归一化观测射线 (相机系)
pts_world.reserve(points_world.size());
bears.reserve(observations.size());
for (size_t i = 0; i < points_world.size(); ++i) {
// 填入世界点
pts_world.push_back(points_world[i]);
// 将像素坐标转为归一化平面坐标 (x, y, 1)
double u = observations[i].x();
double v = observations[i].y();
double nx = (u - cx) / fx;
double ny = (v - cy) / fy;
Eigen::Vector3d bearing_vector(nx, ny, 1.0);
bearing_vector.normalize(); // 确保是单位向量
bears.push_back(bearing_vector);
}
// 2. 创建 Adapter
// CentralAbsoluteAdapter 适用于单相机绝对位姿估计
opengv::absolute_pose::CentralAbsoluteAdapter adapter(bears, pts_world);
// 3. 调用 EPnP
// 返回值是 4x4 的变换矩阵 T_cam_world (即 [R|t])
// 注意：OpenGV 的坐标系定义通常是 X 右，Y 下，Z 前 (符合计算机视觉习惯)
opengv::transformation_t T_cw = opengv::absolute_pose::epnp(adapter);
// 4. 提取 R 和 t
Eigen::Matrix3d R_cw = T_cw.block<3, 3>(0, 0);
Eigen::Vector3d t_cw = T_cw.block<3, 1>(0, 3);
// Cheirality check：初值的 cheirality 比误差更重要
int front_count = 0;
for (size_t i = 0; i < pts_world.size(); ++i) {
Eigen::Vector3d pc = R_cw * pts_world[i] + t_cw;
if (pc.z() > 0) front_count++;
}
if (front_count < pts_world.size() / 2) {
R_cw = -R_cw;
t_cw = -t_cw;
}
// 5. 转换为四元数
Eigen::Quaterniond q_cw(R_cw);
q_cw.normalize();
// 6. 输出到 Ceres 格式数组
// Ceres 期望四元数顺序: [w, x, y, z]
q_out[0] = q_cw.w();
q_out[1] = q_cw.x();
q_out[2] = q_cw.y();
q_out[3] = q_cw.z();
t_out[0] = t_cw.x();
t_out[1] = t_cw.y();
t_out[2] = t_cw.z();
std::cout << "[OpenGV EPnP] Initial pose computed successfully." << std::endl;
std::cout << " Init Rot (w,x,y,z): [" << q_out[0] << ", " << q_out[1] << ", "
<< q_out[2] << ", " << q_out[3] << "]" << std::endl;
std::cout << " Init Trans (x,y,z): [" << t_out[0] << ", " << t_out[1] << ", "
<< t_out[2] << "]" << std::endl;
return true;
}
Eigen::Quaterniond PnPProblem::CeresQuatToEigen(const double* q_ceres) {
return Eigen::Quaterniond(q_ceres[0], q_ceres[1], q_ceres[2], q_ceres[3]);
}
void PnPProblem::EigenQuatToCeres(const Eigen::Quaterniond& q_eigen,
double* q_ceres) {
q_ceres[0] = q_eigen.w();
q_ceres[1] = q_eigen.x();
q_ceres[2] = q_eigen.y();
q_ceres[3] = q_eigen.z();
}以上代码实现中主要进行了两阶段的改进，使其具备强大的抗差能力：
1.RANSAC + P3P 粗筛外点
PnP求解的第一步是获得一个足够好的初始位姿。我们采用"RANSAC + P3P"的组合策略来完成这一任务。
- P3P (Perspective-3-Point)：P3P算法只需要3对2D-3D匹配点即可求解出相机的位姿。它的计算速度极快，非常适合作为RANSAC的内部假设生成器。
- RANSAC (RANdom Sample Consensus)：RANSAC通过迭代的方式，每次随机抽取3对匹配点，用P3P计算一个位姿假设，然后用这个假设去检验所有其他匹配点。如果某个匹配点根据该位姿投影后的误差小于设定阈值，则被认为是该假设的"内点"（Inlier）。经过多次迭代后，拥有最多内点数量的位姿假设即被选为最优解。
这个过程能够有效地将大部分明显错误的外点剔除掉，为我们提供一个由"内点"构成的、相对干净的匹配集合，以及一个可靠的相机位姿初值。
cpp
// SolvePnP.h 中的关键代码片段
// 1. 构建 Adapter，将3D点和2D观测（转为归一化射线）传入OpenGV库
opengv::absolute_pose::CentralAbsoluteAdapter adapter(bears, pts_world);
// 2. 使用RANSAC框架，内部求解器指定为P3P (KNEIP)
typedef opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem Problem;
std::shared_ptr<Problem> problem = std::make_shared<Problem>(adapter, Problem::KNEIP);
opengv::sac::Ransac<Problem> ransac;
ransac.sac_model_ = problem;
ransac.threshold_ = 0.001; // 设定内点判断的误差阈值（弧度）
ransac.max_iterations_ = 100;
ransac.computeModel(); // 执行RANSAC
// 3. 获取RANSAC筛选后的内点索引
std::vector<int> inliers = ransac.inliers_;2.非线性优化与迭代精筛
RANSAC筛选后，我们得到了一组质量较高的内点和一个不错的位姿初值。但这还不够，为了获得亚像素级的精度并进一步剔除那些"伪装"得比较好的外点（例如，重投影误差略大于阈值但未被RANSAC剔除的点），我们引入了第二阶段的非线性优化。
这个阶段的核心与之前的BA优化类似，同样是一个"优化 -> 评估 -> 剔除 -> 再优化"的迭代过程：
-
非线性优化 (Nonlinear Optimization) ：以RANSAC得到的位姿为初值，使用Ceres Solver对所有内点进行非线性最小二乘优化，最小化所有匹配点的重投影误差。为了增加鲁棒性，这里引入了Huber核函数（Huber Loss）。Huber Loss的作用是降低大误差项（可能是残留的外点）在优化中的权重，防止它们过度影响优化结果。
cpp// PnPProblem::NonlinearSolve 函数中的代码片段 ceres::Problem problem; // ... 添加参数块 ... for (size_t i = 0; i < points_world.size(); ++i) { problem.AddResidualBlock( ReprojectionError::Create(...), new ceres::HuberLoss(3.0), // 使用Huber核函数，阈值设为3.0像素 q_est, t_est); } ceres::Solve(options, &problem, &summary); // 执行优化 -
误差评估与外点剔除 (Error Evaluation & Outlier Rejection)：优化完成后，我们计算每个匹配点在当前最优位姿下的重投影误差。
cpp// PnPProblem::NonlinearSolve 函数中的代码片段 std::set<int> outliersIndices; for (size_t i = 0; i < points_world.size(); ++i) { // ... 计算重投影误差 pixel_error ... if (pixel_error > 5) { // 设定一个更严格的像素误差阈值 outliersIndices.insert(i); // 标记为外点 } }任何重投影误差超过设定阈值（例如5个像素）的匹配点，都会被判定为外点并记录下来。
-
迭代执行 (Iterative Refinement) ：将上一步识别出的外点从匹配集合中移除，然后使用剩下的、更纯净的内点集合，再次执行非线性优化。这个过程可以迭代数次，直到没有新的外点被剔除为止。
通过这种两阶段的策略，我们首先用RANSAC快速排除大量明显的外点，获得一个稳健的初值；然后用"非线性优化+迭代精筛"的方式，在更精细的层面上打磨结果，剔除顽固的伪内点。这确保了注册进来的每一张新图像，其位姿都是高度精确和可靠的，为后续的三角化和全局BA奠定了坚实的基础。
(三) 三角化误差筛选
在增量式重建的迭代循环中，我们同样会调用 TriangulateNewPoints 函数来生成新的3D点。然而，与初始化阶段不同，这里的筛选策略从自适应阈值切换为了一个固定的阈值，即 RefineObjectPointsStatic 函数。
这种看似"风格不统一"的设计，并非随意为之，而是由SFM系统在不同阶段所处的几何状态和误差分布的根本差异所决定的。简单来说，初始化阶段和收敛阶段的误差模型完全不同，因此不能采用同一套筛选标准。
1.初始化自适应阈值
在SFM的初始化阶段，系统刚刚起步，几何结构非常脆弱。
- 位姿未收敛 ：此时的相机位姿仅由两幅图像通过本质矩阵 \(E\) 求解而来。这个解只是在两视图下最优，并非全局最优，本身存在较大误差。
- 尺度不稳定 ：从 \(E\) 分解出的平移向量 \(t\) 是一个单位向量，其尺度是未知的。这导致三角化出的3D点深度（scale）非常不稳定。
- 误差分布呈"长尾"：上述因素叠加，使得三角化过程对噪声极其敏感。即使是很小的角度误差，也可能导致深度计算出现巨大偏差。因此，初始化后产生的重投影误差分布是"重尾分布"（heavy-tail），即大部分点误差尚可，但存在大量误差极大的"坏点"。
面对这种非高斯、不均匀的误差分布，任何固定的阈值都难以胜任。阈值设小了会误伤大量"准内点"，设大了又无法剔除"坏点"。因此采用了基于中位数的自适应阈值策略（median * 5），它能根据当前误差的整体水平动态调整，是一种在几何质量较差时的稳健选择。
2.迭代阶段固定阈值
当SFM进入增量式迭代循环后，情况发生了根本性的改变。
- 全局BA的校正作用：在上一轮迭代结束时，系统已经进行过一次全局光束法平差。BA同时优化了所有已注册相机的位姿和所有已三角化的3D点，极大地校正了相机的基线（baseline）和旋转（R），使得整个模型的几何结构趋于一致和精确。
- 误差分布趋近高斯：经过BA的"打磨"，模型的几何质量显著提升。此时再进行三角化，其误差来源主要是特征点提取的像素级噪声，误差分布会非常集中，趋近于高斯分布。
- 固定阈值的有效性：在这种"健康"的几何状态下，重投影误差通常会稳定在一个很小的范围内（例如1-2个像素）。此时，一个经验性的固定阈值（如代码中的10像素）就足以有效地区分出真正的内点和外点。
cpp
void RefineObjectPointsStatic(std::vector<Eigen::Vector3d>& new3dPoint,
std::vector<int>& id3ds,
std::vector<double>& max_errors,
std::vector<cl::ObjectPoint>& objectPoints) {
for (size_t mi = 0; mi < max_errors.size(); ++mi) {
if (max_errors[mi] > 10) {
// cout << max_errors[mi] << '\t';
} else {
objectPoints[id3ds[mi]].pos = new3dPoint[mi];
objectPoints[id3ds[mi]].triangulated = true;
}
}
}(四) 全局光束法平差
经过前述的稳健PnP求解与固定阈值三角化筛选，进入全局BA时，模型中的外点已基本被清除，几何结构也趋于一致和精确。因此，在迭代循环的这一步，我们的目标不再是"抗差"，而是"求精"。
此时，我们只需对所有已注册的相机位姿和已三角化的3D点执行一次标准的全局BA优化即可。与初始化阶段不同，这里不再需要引入Huber核函数等鲁棒损失函数，也不再需要进行"优化-剔除"的迭代循环。因为当前的重投影误差分布已非常接近高斯分布，直接使用标准的最小二乘进行优化，就能让优化器高效、稳定地收敛到全局最优解，对所有参数进行最后的精细化调整。
当然，工程实践中总会遇到一些边界情况。如果在全局BA优化后发现模型仍未能良好收敛（例如，优化器报告失败或最终的重投影误差依然很大），我们可以退回到初始化阶段的策略，作为一种"急救"措施：根据本次BA优化后的重投影误差，再次剔除一批误差最大的观测点，然后重新执行BA。但这通常意味着之前的步骤（如PnP或三角化）可能出现了问题，在正常的流程中并不常见。
五、运行结果
在引入了完整的抗差估计流程后，增量式SFM系统展现出了强大的鲁棒性。尽管输入数据中包含了10%的随机外点，系统依然成功完成了重建。最终的优化结果可以从重投影误差和位姿精度两个维度进行评估。
(一) 重投影误差分析
重投影误差是衡量重建模型几何一致性的核心指标。它反映了3D点投影到图像平面上的位置与实际观测到的2D特征点位置之间的偏差。
text
=== SfM 重建精度报告 ===
=== 总体重投影误差评估 ===
总有效观测点数量: 158346
均方根重投影误差 (RMSE): 0.910320 像素
平均重投影距离: 1.287387 像素
最大重投影误差: 4.811146 像素
评估结果: 优秀 (RMSE < 1.0 px)
=== 各相机重投影误差统计 ===
相机ID 观测点数 平均误差(px) 最大误差(px)
------ -------- ---------- ----------
0 456 1.160726 3.654246
1 552 1.140559 3.412655
2 703 1.165773 3.210440
3 897 1.160505 3.703844
4 1033 1.139018 3.379397
5 939 1.131949 3.643942
6 1092 1.164063 3.645862
7 1002 1.165015 3.574125
8 983 1.162389 3.955606
9 854 1.140990 3.489339
10 1007 1.168084 3.225381
11 873 1.138831 3.005494
12 893 1.131672 3.292365
13 926 1.132133 3.557290
14 878 1.167067 3.580271
15 698 1.125364 3.263339
16 593 1.110921 3.466642
17 556 1.149139 3.347862
18 788 1.098220 3.438222
19 1108 1.160068 3.153773
20 1421 1.141354 3.108471
21 1567 1.139729 3.856223
22 1869 1.169882 3.868188
23 1909 1.138128 4.096676
24 1919 1.143421 3.636680
25 2034 1.151842 4.062898
26 2002 1.149146 4.067647
27 1892 1.159754 4.071960
28 1868 1.140757 4.052728
29 1931 1.142952 3.407792
30 2036 1.146749 4.039635
31 1962 1.163722 3.527574
32 1831 1.116492 3.487301
33 1562 1.160196 3.252001
34 1266 1.148594 4.145857
35 1015 1.101520 3.626629
36 701 1.082469 3.232087
37 906 1.127365 3.420659
38 1285 1.157636 3.778903
39 1609 1.151669 3.566195
40 1766 1.127421 3.613223
41 1811 1.139478 3.594838
42 1862 1.138992 4.056387
43 1931 1.138315 3.839115
44 1906 1.150553 3.630259
45 1919 1.142064 3.700338
46 1917 1.170532 3.766990
47 1989 1.126286 3.415634
48 1946 1.151600 3.757533
49 2027 1.123049 3.990468
50 2106 1.107097 3.583416
51 1682 1.125304 3.489665
52 1428 1.128973 3.563444
53 1303 1.109352 4.126809
54 893 1.078893 3.107746
55 1067 1.127902 3.431932
56 1203 1.123785 3.590684
57 1520 1.147100 3.891271
58 1726 1.140571 4.065321
59 1773 1.140122 3.428772
60 1741 1.156348 3.745118
61 1817 1.144360 3.331729
62 1865 1.139872 3.342476
63 1822 1.158691 3.909029
64 1982 1.169392 3.875252
65 1983 1.163993 3.835914
66 2126 1.129457 3.782194
67 2102 1.145731 3.707583
68 1846 1.138519 3.690163
69 1522 1.158848 3.429666
70 1232 1.157067 3.413116
71 912 1.105816 3.616195
72 732 1.120840 3.132312
73 1074 1.138998 3.331055
74 1347 1.145878 3.841319
75 1567 1.147768 4.263388
76 1841 1.141852 3.688816
77 1906 1.151572 3.624481
78 1862 1.142722 3.158114
79 1901 1.164034 3.894184
80 1959 1.116659 3.422346
81 1967 1.141048 3.705319
82 2025 1.127521 4.758624
83 1961 1.167351 4.048944
84 1990 1.159640 4.811146
85 1901 1.137160 3.731344
86 1709 1.160965 3.797808
87 1413 1.128536 3.806028
88 1289 1.154902 3.746034
89 854 1.102819 3.264790
90 655 1.066679 3.092039
91 887 1.098106 3.263669
92 1130 1.132391 3.780804
93 1343 1.100847 3.599244
94 1547 1.115899 3.577101
95 1563 1.120640 3.412822
96 1778 1.095427 3.339170
97 1790 1.109207 4.071044
98 1574 1.116763 3.286307
99 1621 1.116036 3.392304
100 1630 1.138634 4.134481
101 1595 1.105375 3.510544
102 1572 1.104229 3.791930
103 1621 1.127844 3.158207
104 1474 1.107871 4.650959
105 1221 1.078606 3.415592
106 1067 1.097506 3.363769
107 839 1.053307 3.099617从总体报告可以看出，系统最终的重投影误差均方根（RMSE）仅为 0.91像素，这是一个非常优秀的结果。它表明，经过全局BA的优化，整个3D模型与所有2D观测之间达到了高度的一致。平均误差为1.29像素，而最大误差被控制在4.81像素以内，说明误差分布健康，残留的外点已被有效剔除，剩下的观测点误差分布非常集中，接近理想的高斯分布。
(二) 相机位姿精度分析
相机位姿（旋转R和平移t）的精度直接决定了重建模型的绝对准确性。通过与仿真数据的真值（Ground Truth）进行比较，我们可以量化评估位姿估计的误差。
text
=== 逐相机误差报告 ===
ID Trans Error (m) Rot Error (deg)
--------------------------------------
0 0.113096 0.028642
1 0.047527 0.009968
2 0.053958 0.017165
3 0.028833 0.006480
4 0.009563 0.008755
5 0.083875 0.016288
6 0.021312 0.005929
7 0.017102 0.005720
8 0.012537 0.005235
9 0.032831 0.012978
10 0.038793 0.015837
11 0.036596 0.015399
12 0.027471 0.012273
13 0.046647 0.006843
14 0.023964 0.003797
15 0.045656 0.009545
16 0.077971 0.015626
17 0.027125 0.011246
18 0.012264 0.010137
19 0.023517 0.008315
20 0.018575 0.007552
21 0.037260 0.008345
22 0.010103 0.008186
23 0.008450 0.008293
24 0.007912 0.006416
25 0.015132 0.004589
26 0.002223 0.006630
27 0.015569 0.004288
28 0.012938 0.003748
29 0.006980 0.004227
30 0.009977 0.005529
31 0.011077 0.006936
32 0.019678 0.008642
33 0.011941 0.007916
34 0.017347 0.002346
35 0.011838 0.005531
36 0.028446 0.010696
37 0.021068 0.007882
38 0.012489 0.010055
39 0.010629 0.006970
40 0.016870 0.009321
41 0.012515 0.008200
42 0.006806 0.003972
43 0.005864 0.006506
44 0.013528 0.009935
45 0.002954 0.005250
46 0.012028 0.009508
47 0.003015 0.005866
48 0.007456 0.007740
49 0.013123 0.004643
50 0.013343 0.006048
51 0.012169 0.008263
52 0.018632 0.010681
53 0.013981 0.007349
54 0.047611 0.014123
55 0.011677 0.004538
56 0.031091 0.010823
57 0.010804 0.004256
58 0.007291 0.004215
59 0.001493 0.002441
60 0.006253 0.004320
61 0.006964 0.006312
62 0.012176 0.006289
63 0.005332 0.005658
64 0.004507 0.005806
65 0.005614 0.005894
66 0.003895 0.003825
67 0.003828 0.003589
68 0.006320 0.003432
69 0.007275 0.002971
70 0.022441 0.002132
71 0.030730 0.013567
72 0.040508 0.014070
73 0.037827 0.011724
74 0.007475 0.003622
75 0.014048 0.006488
76 0.014655 0.007138
77 0.008017 0.004574
78 0.002972 0.002846
79 0.002614 0.002726
80 0.009053 0.006811
81 0.006829 0.005982
82 0.011581 0.007763
83 0.007591 0.004373
84 0.007236 0.003722
85 0.008783 0.006518
86 0.012607 0.006565
87 0.004779 0.006396
88 0.016686 0.007729
89 0.033694 0.012555
90 0.077535 0.022365
91 0.035309 0.014353
92 0.010885 0.003987
93 0.019396 0.006719
94 0.031863 0.010126
95 0.015705 0.008216
96 0.003775 0.005245
97 0.004949 0.006324
98 0.003070 0.003770
99 0.019434 0.003959
100 0.008854 0.007247
101 0.073426 0.020668
102 0.061576 0.020749
103 0.009096 0.007740
104 0.018462 0.009121
105 0.011555 0.005673
106 0.011006 0.008430
107 0.039243 0.009116
=== 整体精度统计 ===
-------------------
平移误差 (Translation Error):
平均 (Mean): 0.019907 m
均方根 (RMSE): 0.027757 m
最大 (Max): 0.113096 m (Camera 0)
旋转误差 (Rotation Error):
平均 (Mean): 0.007999 deg
均方根 (RMSE): 0.009178 deg
最大 (Max): 0.028642 deg (Camera 0)
===================位姿精度报告的结果同样令人满意：
- 旋转精度极高：所有相机的旋转误差均方根（RMSE）仅为 0.009度。如此微小的角度偏差意味着相机的朝向被估计得极其精确，这是保证三角化和BA优化能够正确收敛的基础。
- 平移精度可靠：平移误差的均方根（RMSE）为 2.7厘米。考虑到这是一个稀少GCP约束的自由网平差结果，这个精度水平是相当高的。它表明，通过稳健的相对位姿估计和全局优化，系统成功地恢复出了场景的正确几何结构。
综上所述，无论是从衡量内部一致性的重投影误差，还是从衡量绝对精度的位姿误差来看，这套集成了抗差估计的增量式SFM系统都交出了一份优异的答卷，证明了其在处理含外点的真实数据时的有效性和可靠性。
六、工程实现
至此，我们已经完成了一个具备强大抗差能力的增量式SFM系统，并在包含10%外点的仿真数据上验证了其有效性。然而，仿真环境终究是对真实世界的理想化抽象。当我们将算法部署到复杂的实际工程中时，会发现挑战远不止于"外点"这一项。
(一) 漏匹配与数据稀疏性
在仿真中，我们通常假设只要两个相机能看到同一个3D点，就一定能生成匹配。但在现实中，由于光照变化、视角差异、物体遮挡或重复纹理等原因，特征匹配算法常常会失败，导致本该存在的匹配点丢失，即"漏匹配"。
- 问题：漏匹配会导致共视关系（Co-visibility）稀疏，使得PnP注册时找不到足够的2D-3D对应点，或三角化时缺少必要的观测，从而导致重建中断或点云不完整。
- 应对策略 ：
- 图像检索（Image Retrieval）：在特征匹配前，先通过全局图像描述子（如NetVLAD）快速筛选出与当前图像最可能相关的候选图像集，避免进行全量匹配，提高在大数据集下的效率和召回率。
- 引导匹配（Guided Matching）：利用已有的稀疏重建模型，将3D点投影到未注册的图像上，预测特征点可能出现的位置，然后在该位置附近的小范围内进行特征匹配。这能极大提高在弱纹理或重复纹理区域的匹配成功率。
- 更鲁棒的特征：使用对光照、视角变化更鲁棒的特征描述子（如SuperPoint等学习型特征），从源头上提升匹配的成功率。
(二) 不良几何条件
仿真数据通常会保证良好的基线和视差。但在实际采集中，我们常会遇到不利的几何条件。
- 问题 ：
- 弱基线（Small Baseline）：如无人机在高空进行"刷墙式"航拍时，相邻帧之间的视差角极小。这会导致三角化深度不确定性极大，重建的点云会非常"飘"。
- 纯旋转（Pure Rotation）：相机在原地旋转拍摄全景图时，没有平移运动。此时无法通过三角化恢复3D结构，SFM会完全失败。
- 近平面场景（Near-Planar Scene）：拍摄墙面、地面等近似平面的场景时，对极几何约束会退化，导致本质矩阵求解不稳定。
- 应对策略 ：
- 更严格的初始化筛选：正如我们在初始化阶段所做的那样，不仅要看匹配点数量，更要严格检查平均视差角，主动避开弱基线的图像对。
- 轨迹规划：在数据采集阶段就进行规划，例如采用"蛇形"或"环绕式"航线，确保从不同角度、不同位置对场景进行拍摄，以获得良好的几何交会条件。
- 退化检测：在算法内部增加对纯旋转或近平面场景的检测。一旦检测到退化情况，可以暂停增量式重建，或切换到专门的运动恢复（如纯旋转下的图像拼接）流程。
(三) 动态物体与运动模糊
真实世界是动态的，而SFM的基本假设是"静态场景"。
- 问题：场景中的行人、车辆等动态物体会在不同图像中出现在不同位置。SFM算法会误认为它们是静态场景的一部分，从而三角化出一些"鬼影"点，或者更糟的是，这些动态点会作为外点干扰相机位姿的求解，导致轨迹漂移。此外，快速运动或曝光时间过长会导致图像模糊，使得特征点定位不准，引入系统性误差。
- 应对策略 ：
- 运动分割（Motion Segmentation）：利用光流或多视图几何一致性检查，识别并剔除那些不符合刚性场景运动模式的特征点（即动态点）。
- 语义SFM（Semantic SFM）：结合深度学习模型，在特征提取阶段就识别并忽略图像中的动态物体（如人、车），只在静态背景（如建筑、道路）上进行重建。这是目前非常主流且有效的方法。
- 图像去模糊：在SFM前端增加图像预处理步骤，对模糊图像进行去模糊处理，以提升特征点提取和定位的精度。
(四) 大规模场景与计算效率
仿真数据规模通常较小。但在城市级重建等大规模场景下，计算效率成为瓶颈。
- 问题：随着图像和3D点数量的增加，全局BA的计算复杂度会急剧上升（通常是超线性的），导致优化过程变得极其缓慢，甚至无法在有限内存中完成。
- 应对策略 ：
- 分层式SFM（Hierarchical SFM）：将大规模图像集聚类成多个小团体，先对每个团体内部进行独立的SFM重建，然后再将这些小模型合并成一个全局模型。
- 局部BA与全局BA结合：在每次注册新图像后，只对该图像及其邻近的相机和点进行局部BA优化，以快速修正局部误差。全局BA则以较低的频率执行，用于消除累积误差。
- 视图选择（View Selection）：在进行三角化或BA时，并非使用所有观测，而是智能地选择一部分最具代表性的视图（例如，视差角大、图像清晰度高的视图），在不损失精度的前提下大幅减少计算量。
七、结语
从理想数据到抗差仿真，再到应对真实世界的种种挑战，SFM的工程实现是一个不断"打补丁"和"做权衡"的过程。本文所实现的抗差估计框架，是构建一个稳健SFM系统最核心、最基础的一步。它确保了算法在面对数据污染时不会轻易崩溃。
然而，要打造一个真正能在复杂环境中稳定运行的工业级产品，还需要在上述的漏匹配、不良几何、动态物体和计算效率等问题上投入大量的工程智慧。希望本系列文章能为你提供一个坚实的理论与实践起点，让你在面对这些更高级的挑战时，能够知其然，更知其所以然。