最小二乘问题详解20：无先验约束下的增量式SFM自由网平差

📰 来源: 博客园

在上两篇系列文章中，我们分别探讨了增量式 SfM 的核心骨架实现与多源信息融合策略。在第 18 篇《最小二乘问题详解18：增量式SFM核心流程实现》中，我们构建了算法的逻辑基座；而在第 19 篇《带先验约束的增量式 SFM 优化与实现》中，我们引入了 INS/GNSS 等外部位姿先验，构建了工业级稳健的重建系统。

回顾第 19 篇的带先验约束流程，我们不难发现，虽然该方案对数据采集设备提出了更高要求，但它实际上简化了 SfM 的核心难题。借助高精度的位姿先验，我们绕过了繁琐且脆弱的初始化步骤，能够直接基于已知的相机位置进行特征匹配与三角化，从而迅速展开全局光束法平差（BA）。在这一过程中，先验信息充当了“上帝之手”，直接为优化问题提供了高质量的初值，规避了纯视觉系统中常见的尺度漂移与拓扑错误。

然而，当我们剥离掉这些外部传感器提供的“辅助信息”，回归到计算机视觉最本源的2D-2D 特征对应关系时，增量式 SfM 才真正展现出其“从无到有”的魅力与挑战。这就是本文将要深入探讨的主题——无先验约束下的增量式 SfM 自由网平差。

在这种纯视觉模式下，我们失去了绝对的坐标参考系，面对的是一片数据的“黑暗森林”。我们不仅不知道场景的绝对位置，甚至连相机之间的绝对距离（尺度）都无从得知。因此，我们无法像带先验流程那样直接从三角化开始，而是必须从 对极几何（Epipolar Geometry） 出发，通过计算本质矩阵（Essential Matrix, \(E\)）来推导两视图之间的相对运动。

这一 初始化（Initialization） 过程至关重要，它是整个增量式 SfM 的关键点。本质矩阵 \(E\) 的求解质量，直接决定了初始两张像片的相对定向精度，进而作为后续所有相机位姿与 3D 结构生长的基准。一旦初始化失败或误差过大，这种误差将像病毒一样在增量扩展的过程中不断累积和放大，最终导致重建崩溃。因此，理解并掌握无先验约束下的自由网平差，不仅是对 SfM 算法本质的深刻洞察，也是解决最通用、最广泛视觉重建问题的核心能力。

首先还是先关注增量式 SfM 的几个核心子问题。

第一点当然就是前文中提到了通过对极几何求本质矩阵 \(E\)。这个问题我们在《最小二乘问题详解13：对极几何中本质矩阵求解》和《最小二乘问题详解14：鲁棒估计与5点算法求解本质矩阵》这两篇文章中详细讨论过，将其具体实现封装一下：

#pragma once

#include <Eigen/Core>

class EssentialProblem {
 public:
  EssentialProblem();

  Eigen::Matrix3d Solve(const Eigen::Matrix3d& K,
                        const std::vector<Eigen::Vector3d>& x1s_noisy,
                        const std::vector<Eigen::Vector3d>& x2s_noisy,
                        double pixelThreshold, int ransacIterations);

 private:
  std::pair<Eigen::Matrix3d, std::vector<bool>> RansacEstimate(
      const std::vector<Eigen::Vector3d>& x1s_noisy,
      const std::vector<Eigen::Vector3d>& x2s_noisy, double pixel_threshold,
      double focal_length, int max_iterations);

  Eigen::Matrix3d EightPointAlgorithm(const std::vector<Eigen::Vector3d>& x1s,
                                      const std::vector<Eigen::Vector3d>& x2s);

  Eigen::Matrix3d OptimizeSampsonError(Eigen::Matrix3d E_init,
                                       const std::vector<Eigen::Vector3d>& x1s,
                                       const std::vector<Eigen::Vector3d>& x2s);

  Eigen::Matrix3d ComputeNormalization(
      const std::vector<Eigen::Vector3d>& points);

  double SampsonDistance(const Eigen::Matrix3d& E, const Eigen::Vector3d& x1,
                         const Eigen::Vector3d& x2);

  double EpipolarResidual(const Eigen::Matrix3d& E, const Eigen::Vector3d& x1,
                          const Eigen::Vector3d& x2);

  Eigen::Matrix3d NormalizeEssentialMatrix(const Eigen::Matrix3d& E);
};

#include "SolveEssential.h"

#include <ceres/ceres.h>
#include <Eigen/Geometry>

#include <iomanip>

#include <iostream>

#include <opengv/relative_pose/CentralRelativeAdapter.hpp>

#include <opengv/sac/Ransac.hpp>

#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>

#include <random>

#include <vector>
#include "Math.h"
using namespace std;

using namespace Eigen;
namespace {

struct SampsonError {

  SampsonError(const Eigen::Vector3d& x1, const Eigen::Vector3d& x2)

      : x1_(x1), x2_(x2) {}
  template <typename T>

  bool operator

🔗 原文链接: 点击阅读原文