极客资料网推荐最小二乘问题详解21：稀疏GCP约束下的自由网平差与弱约束融合这篇文章给大家，欢迎收藏极客资料网享受知识的乐趣

一、引言

在本系列的前两篇文章中，我们分别探讨了增量式 SfM 的两个场景：带完全位姿先验的稳健重建（《最小二乘问题详解19：带先验约束的增量式SFM优化与实现》）与无先验约束的纯视觉自由网平差（《最小二乘问题详解20：无先验约束下的增量式SFM自由网平差》）。这两者代表了数据驱动与模型驱动的两种截然不同的技术路线，各自有着鲜明的优势与局限。

在《最小二乘问题详解19：带先验约束的增量式SFM优化与实现》中，我们利用 INS/GNSS 提供的高精度位姿先验，极大地简化了 SfM 的求解难度。由于拥有可靠的绝对位置初值，系统可以直接跳过脆弱的初始化环节，基于已知位姿进行特征匹配与三角化。这种方案虽然实现了极高的稳健性，但其前提是必须配备昂贵的组合导航设备，数据采集成本极高。而在《最小二乘问题详解20：无先验约束下的增量式SFM自由网平差》中，我们回归视觉本源，仅利用 2D-2D 的特征对应关系，通过复杂的对极几何（本质矩阵 \(E\)）求解来完成初始化。虽然这种方法极大地降低了硬件门槛，实现了“从无到有”的重建，但其代价是重建结果仅为相对度量（Similarity Transformation），缺乏绝对的地理尺度与位置信息。

然而，在实际的工业应用与科研场景中，我们往往面临一个尴尬的中间地带：我们既无法承担每张影像都配有高精度 POS 数据的高昂成本，又希望能视觉重建出基于真实世界的绝对坐标系的结果。这就是本文将要深入探讨的核心主题——稀疏 GCP 约束下的自由网平差与弱约束融合。

稀疏 GCP（地面控制点）代表了一种典型的弱先验约束（Weak Prior Constraint）。与第19篇中“强约束”的密集位姿先验不同，稀疏 GCP 仅在极少数的特征点上提供了 3D 物理坐标信息。这种约束无法像 INS/GNSS 那样直接引导每一步的 PnP 求解，也不能简化初始化流程；相反，它要求我们在保持纯视觉自由网平差（第20篇流程）复杂性的基础上，额外引入全局优化策略，将这些零星的绝对坐标信息“渗透”到整个重建网络中。

因此，本文的实现流程将是目前最麻烦但也最实用的：它保留了第20篇中复杂的本质矩阵初始化与增量扩展逻辑，同时引入了第19篇中的多源信息融合思想，将稀疏的 GCP 作为软约束嵌入到光束法平差（BA）中。通过这种融合，我们旨在以最低的数据采集成本（仅需少量人工测量的 GCP），获得接近第19篇的绝对地理定位精度。

二、思路

在确立了“稀疏 GCP 约束”这一核心目标后，我们需要解决的关键问题是：如何将少量的 GCP 观测值有效地融合进增量式 SfM 的优化流程中？

通常来说，处理这类多源信息融合问题主要有两种常规思路。一种是在优化过程中实时引入约束，另一种则是先构建相对模型再进行整体对齐。

1. 增量式联合优化

第一种思路是边重建，边约束。即在增量式 SfM 的每一步迭代中（无论是 PnP 位姿估计还是局部/全局 BA），都将 GCP 作为一个特殊的 3D 点加入优化目标函数。

• 实现逻辑：在构建 BA 问题时，为 GCP 对应的 3D 点添加一个额外的残差项（GCP 坐标先验误差）。随着新图像的注册和新点的三角化，GCP 的约束力会实时地“拉扯”整个模型，试图将其固定在绝对坐标系中。
• 面临的挑战：这种方案在理论上很完美，但在稀疏 GCP 场景下实现难度极大。
  1. 权重难以平衡：GCP 数量极少（可能只有几个），而视觉重投影残差数量巨大（成千上万）。在优化初期，模型漂移严重，GCP 的微弱约束力很容易被海量的视觉残差“淹没”，导致无法生效；若强行增加 GCP 权重，又可能导致模型为了迎合 GCP 而发生局部扭曲。
  2. 冷启动困难：在重建的初始阶段，自由网尚未形成稳定的几何结构，此时引入强约束容易导致优化器陷入局部极小值，甚至直接导致初始化失败。

2. 两步法绝对定向

第二种思路是先重建，后对齐。即先完全忽略 GCP，按照第 20 篇的方法完成纯视觉的自由网平差，待模型内部结构稳定后，再利用 GCP 计算一个全局变换矩阵，将整个模型“搬”到绝对坐标系中。

• 实现逻辑：利用第 20 篇生成的自由网模型，提取 GCP 对应的 3D 点坐标，与 GCP 的真实坐标进行匹配，求解一个 Sim3（相似变换） 矩阵。
• 面临的挑战：这种方案实现简单，且能保证内部结构的完整性。但它存在一个理论上的缺陷——刚性假设。Sim3 变换假设自由网与真实世界之间仅存在线性的相似变换关系（旋转、平移、统一尺度）。然而，实际的重建误差往往是非线性的（例如镜头畸变校正残差、累积漂移导致的弯曲等）。简单的 Sim3 变换无法消除这些非线性变形，只能做到“整体对齐”，而无法做到“局部贴合”。

3. Sim3引导的联合优化

为了兼顾实现的鲁棒性与结果的绝对精度，本文最终采用了一种结合上述两者之长的三步走策略。

该方案利用方案二的 Sim3 结果为方案一提供完美的初值，从而规避了方案一“冷启动”和“权重平衡”的难题。具体流程如下：

1. 第一步：纯视觉自由网平差
   完全沿用第 20 篇的流程，不引入任何 GCP 信息，独立完成增量式 SfM。此时我们获得了一个内部几何结构高度一致（重投影误差极低），但坐标系任意的 3D 模型。
2. 第二步：Sim3 绝对定向
   在自由网平差完成后，利用已知的 GCP 坐标与模型中对应的 3D 点，计算 Sim3 变换矩阵，将所有相机位姿和 3D 点一次性变换到绝对坐标系下。这一步解决了尺度、旋转和平移的 7 自由度不确定性问题。
3. 第三步：带 GCP 约束的全局 BA
   将 Sim3 对齐后的结果作为初值，再次运行全局光束法平差。此时，我们在优化函数中加入 GCP 的坐标先验残差项。
   • 关键点：由于初值已经非常接近真实值（得益于 Sim3 对齐），优化器不再需要处理巨大的漂移，GCP 的约束项可以平滑地修正模型中的非线性误差（如微小的弯曲），实现视觉结构与地理坐标的完美统一。

这种策略既保留了纯视觉 SfM 的灵活性，又利用 Sim3 解决了绝对定向的初值问题，最后通过联合 BA 榨干了 GCP 的精度潜力，是目前工程实践中处理稀疏 GCP 最稳健的方案。

三、Sim3问题

在完成了纯视觉自由网平差后，我们获得了一个内部几何结构完美但处于任意坐标系下的 3D 模型。为了将其“锚定”到真实世界坐标系中，我们需要求解一个三维空间的相似变换（Similarity Transformation, Sim3）。

在摄影测量学中，这一过程被称为绝对定向（Absolute Orientation）。其本质是寻找一个 7 自由度的变换（3 个旋转、3 个平移、1 个统一尺度），使得自由网中的点集 \(\mathbf{X}_{sfm}\) 与真实世界中的控制点集 \(\mathbf{X}_{gcp}\) 之间的误差最小化。

1. 数学模型

Sim3 变换的数学模型可以表示为：

其中：

• \(\mathbf{X}_{sfm}\)：自由网中的 3D 点坐标（观测值）。
• \(\mathbf{X}_{world}\)：GCP 的真实世界坐标（真值）。
• \(s\)：尺度因子（Scale），用于纠正单目视觉的尺度不确定性。
• \(\mathbf{R}\)：\(3 \times 3\) 的旋转矩阵（Rotation），属于 \(SO(3)\) 群。
• \(\mathbf{t}\)：\(3 \times 1\) 的平移向量（Translation）。

我们的目标是求解最优的 \(s, \mathbf{R}, \mathbf{t}\)，使得所有对应点对的重投影误差（欧氏距离）平方和最小：

这是一个典型的非线性最小二乘问题。在工程实践中，通常采用线性初值 + 非线性优化的两步策略来求解，以保证精度和鲁棒性。

2. Umeyama线性解

为了获得非线性优化的良好初值，我们首先使用 Umeyama 算法（基于 SVD 分解）来求解 Sim3 的闭式解（Closed-form Solution）。这是一种代数解法，计算速度快且稳定。

该算法的核心思想是通过去中心化操作，将平移参数 \(\mathbf{t}\) 与旋转 \(\mathbf{R}\)、尺度 \(s\) 解耦，从而将复杂的非线性问题转化为线性的 SVD 问题。推导过程如下：

1. 计算质心与去中心化
首先，我们需要计算源点集（自由网点 \(\mathbf{X}_{sfm}\)）和目标点集（GCP点 \(\mathbf{X}_{world}\)）的几何中心（质心）。

接着，计算每个点相对于其质心的偏移向量（去中心化坐标），记为 \(\mathbf{x}_i\) 和 \(\mathbf{y}_i\)：

此时，原始的最小二乘目标函数 \(\sum \| \mathbf{y}_i + \mu_{world} - (s \mathbf{R} (\mathbf{x}_i + \mu_{sfm}) + \mathbf{t}) \|^2\) 可以简化。由于质心是最小二乘问题的最优平移估计点，我们可以直接得出平移量 \(\mathbf{t}\) 与 \(\mathbf{R}, s\) 的关系：

这意味着，只要我们要解出了 \(\mathbf{R}\) 和 \(s\)，\(\mathbf{t}\) 就迎刃而解。因此，问题转化为最小化去中心化后的残差：

2. 构建协方差矩阵并进行 SVD 分解
展开上述目标函数并忽略与变量无关的项，最大化 \(s \sum \mathbf{y}_i^T \mathbf{R} \mathbf{x}_i\) 等价于最小化原目标函数。利用迹（Trace）的性质 \(\text{tr}(\mathbf{A}\mathbf{B}) = \text{tr}(\mathbf{B}\mathbf{A})\)，我们可以构建源点集与目标点集的协方差矩阵 \(\mathbf{\Sigma}\)：

对协方差矩阵 \(\mathbf{\Sigma}\) 进行奇异值分解（SVD）：

其中 \(\mathbf{U}, \mathbf{V}\) 是正交矩阵，\(\mathbf{D}\) 是包含奇异值的对角矩阵。

3. 求解旋转矩阵 \(\mathbf{R}\)
根据正交普鲁克问题（Orthogonal Procrustes Problem）的解法，最优旋转矩阵 \(\mathbf{R}\) 可以通过 \(\mathbf{U}\) 和 \(\mathbf{V}\) 计算得出。为了保证解出的是纯旋转矩阵（行列式为 1）而不是反射矩阵（行列式为 -1），我们需要引入一个修正矩阵 \(\mathbf{S}\)：

其中 \(\mathbf{S} = \text{diag}(1, 1, \det(\mathbf{V}\mathbf{U}^T))\)。如果 \(\det(\mathbf{V}\mathbf{U}^T) = -1\)，则 \(\mathbf{S}\) 的最后一个元素为 -1，用于修正反射分量。

4. 求解尺度因子 \(s\)
在求得最优旋转 \(\mathbf{R}\) 后，我们可以通过对目标函数关于 \(s\) 求导并令其为 0，得到最优尺度因子的闭式解。它等于变换后的协方差迹与源点集方差的比值：

其中 \(s_k\) 是 \(\mathbf{\Sigma}\) 的奇异值，分母是源点集去中心化后的方差和（即 X.squaredNorm()）。

5. 求解平移向量 \(\mathbf{t}\)
最后，将求得的 \(s\) 和 \(\mathbf{R}\) 代入第一步推导的公式中，即可恢复平移向量：

Umeyama 算法为我们提供了一个非常接近全局最优的线性解，但这仅仅是代数意义上的最优。为了获得统计意义上的最优解（考虑噪声分布），我们需要在此基础上进行非线性优化。

3. 非线性优化

虽然 Umeyama 算法给出了闭式解，但它假设噪声是高斯分布的，且无法方便地引入鲁棒核函数（Robust Kernel）来剔除 GCP 中的粗差。因此，我们以 Umeyama 的结果为初值，构建非线性优化问题。我们定义残差函数如下：

在代码实现中，有几个关键的工程技巧：

1. 参数化旋转：使用四元数 \(\mathbf{q} = [w, x, y, z]\) 表示旋转，避免欧拉角的万向节死锁，并利用 Ceres 的 QuaternionManifold 保持流形约束。

2. 参数化尺度：优化变量不是直接优化 \(s\)，而是优化 \(\log s\)。在计算时通过 \(s = \exp(\log s)\) 还原。这样做的好处是强制尺度 \(s\) 始终为正值，防止优化过程中尺度变为负数导致模型翻转。

3. 鲁棒核函数：在 AddResidualBlock 时加入了 HuberLoss。

   problem.AddResidualBlock(cost, new ceres::HuberLoss(0.5), q, t, log_s);
   

   这使得优化器对 GCP 坐标中的粗差（Outliers）不敏感，提高了系统的鲁棒性。

4. 实现代码

通过这种“线性初值 + 非线性优化”的组合操作，我们既能保证求解的稳定性，又能获得亚像素级的绝对定位精度，完美解决了自由网到真实世界的映射问题。具体实现代码如下：

#include "Sim3Problem.h"

#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <iostream>

namespace {

// 残差结构体
struct Sim3Residual {
  Sim3Residual(const Eigen::Vector3d& sfm, const Eigen::Vector3d& world)
      : sfm_(sfm), world_(world) {}

  template <typename T>
  bool operator()(const T* const q,      // Quaternion (w, x, y, z)
                  const T* const t,      // Translation
                  const T* const log_s,  // Log-Scale (为了强制 s > 0)
                  T* residuals) const {
    // 1. 旋转
    T p[3];
    T sfm[3] = {T(sfm_(0)), T(sfm_(1)), T(sfm_(2))};
    ceres::QuaternionRotatePoint(q, sfm, p);

    // 2. 尺度 (通过 exp 保证正数)
    T s = ceres::exp(log_s[0]);

    // 3. 变换: p = s * R * x + t
    p[0] = s * p[0] + t[0];
    p[1] = s * p[1] + t[1];
    p[2] = s * p[2] + t[2];

    // 4. 残差: 预测值 - 观测值
    residuals[0] = p[0] - T(world_(0));
    residuals[1] = p[1] - T(world_(1));
    residuals[2] = p[2] - T(world_(2));

    return true;
  }

  Eigen::Vector3d sfm_, world_;
};

}  // namespace

Sim3Problem::Sim3Problem(const std::vector<Eigen::Vector3d>& src_pts,
                         const std::vector<Eigen::Vector3d>& dst_pts)
    : src_pts_(src_pts), dst_pts_(dst_pts) {}

bool Sim3Problem::Solve(Sim3& sim3) {
  // 1. 线性初值 (Umeyama)
  sim3 = Umeyama(src_pts_, dst_pts_);

  // 2. 设置优化变量初值
  Eigen::Quaterniond qTemp(sim3.R);
  qTemp.normalize();  // 防止初值有微小误差
  double q[4] = {qTemp.w(), qTemp.x(), qTemp.y(), qTemp.z()};
  double t[3] = {sim3.t.x(), sim3.t.y(), sim3.t.z()};
  double log_s[1] = {std::log(sim3.scale)};  // 关键：使用 log(s)

  ceres::Problem problem;

  // 3. 构建残差
  for (int i = 0; i < src_pts_.size(); ++i) {
    auto* cost = new ceres::AutoDiffCostFunction<Sim3Residual, 3, 4, 3, 1>(
        new Sim3Residual(src_pts_[i], dst_pts_[i]));

    // 关键：加上 Huber Loss 增加鲁棒性，阈值设为 0.5 (根据单位调整)
    // 如果你的坐标单位是米，0.5 意味着允许 0.5米的误差而不被惩罚
    problem.AddResidualBlock(cost, new ceres::HuberLoss(0.5), q, t, log_s);
  }

  //
  problem.AddParameterBlock(q, 4, new ceres::QuaternionManifold());
  problem.AddParameterBlock(t, 3);
  problem.AddParameterBlock(log_s, 1);

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.max_num_iterations = 100;  // 稍微增加迭代次数
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  std::cout << summary.BriefReport() << std::endl;

  ComputeRMSE(log_s, q, t);

  // 5. 提取结果
  if (summary.IsSolutionUsable()) {  // 比 != CONVERGENCE 更宽松一点的判断
    sim3.R = Eigen::Quaterniond(q[0], q[1], q[2], q[3]).toRotationMatrix();
    sim3.t.x() = t[0];
    sim3.t.y() = t[1];
    sim3.t.z() = t[2];
    sim3.scale = std::exp(log_s[0]);  // 关键：还原尺度
    return true;
  }

  return false;
}

void Sim3Problem::ComputeRMSE(double* log_s, double* q, double* t) {
  // 1. 获取优化后的参数
  double final_scale = std::exp(log_s[0]);
  Eigen::Quaterniond final_q(q[0], q[1], q[2], q[3]);
  Eigen::Vector3d final_t(t[0], t[1], t[2]);

  // 2. 计算 RMSE
  double sum_squared_error = 0.0;
  for (const auto& src_pt : src_pts_) {
    // 变换源点: p' = s * R * p + t
    Eigen::Vector3d p_transformed = final_scale * (final_q * src_pt) + final_t;

    // 这里我们需要找到 src_pt 对应的 dst_pt
    // 注意：因为 src_pts_ 和 dst_pts_ 是按顺序对应的，我们可以用索引，
    // 但为了代码安全，建议在这里重新建立映射或者假设顺序一致。
    // 假设顺序一致（因为你的循环是按索引来的）：
    int idx = &src_pt - &src_pts_[0];
    const auto& dst_pt = dst_pts_[idx];

    double error = (p_transformed - dst_pt).norm();
    sum_squared_error += error * error;
  }

  double rmse = std::sqrt(sum_squared_error / src_pts_.size());

  std::cout << "----------------------------------------" << std::endl;
  std::cout << "Sim3 Optimization Result:" << std::endl;
  std::cout << "  Scale (s): " << final_scale << std::endl;
  std::cout << "  Final RMSE: " << rmse << " meters" << std::endl;
  std::cout << "  Translation: " << final_t.transpose() << std::endl;
  std::cout << "----------------------------------------" << std::endl;
}

Sim3Problem::Sim3 Sim3Problem::Umeyama(
    const std::vector<Eigen::Vector3d>& pts_sfm,
    const std::vector<Eigen::Vector3d>& pts_world) {
  int N = pts_sfm.size();

  // 1. 计算均值
  Eigen::Vector3d mu_sfm = Eigen::Vector3d::Zero();
  Eigen::Vector3d mu_world = Eigen::Vector3d::Zero();
  for (int i = 0; i < N; ++i) {
    mu_sfm += pts_sfm[i];
    mu_world += pts_world[i];
  }
  mu_sfm /= N;
  mu_world /= N;

  // 2. 去中心
  Eigen::MatrixXd X(3, N), Y(3, N);
  for (int i = 0; i < N; ++i) {
    X.col(i) = pts_sfm[i] - mu_sfm;
    Y.col(i) = pts_world[i] - mu_world;
  }

  // 3. 协方差
  Eigen::Matrix3d S = X * Y.transpose();  // 不需要除以 N，不影响 SVD

  // 4. SVD
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(
      S, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();

  // 5. 旋转 (处理反射)
  Eigen::Matrix3d D = Eigen::Matrix3d::Identity();
  if ((V * U.transpose()).determinant() < 0) {
    D(2, 2) = -1;
  }
  Eigen::Matrix3d R = V * D * U.transpose();

  // 6. 尺度 (更稳健的公式)
  // scale = trace(D * S) / ||X||^2
  double scale = (svd.singularValues().dot(D.diagonal())) / X.squaredNorm();

  // 7. 平移
  Eigen::Vector3d t = mu_world - scale * R * mu_sfm;

  return {scale, R, t};
}

四、流程改进

在第 20 篇的实现基础上， 仿照第 19 篇的实现给 BA 问题增加一个带 GCP 约束的求解实现 BAProblem::SolveWithGcp：

#include "SolveBA.h"

#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
#include <random>
#include <vector>

using namespace std;

namespace {

struct ReprojectionError {
  ReprojectionError(double x, double y, double fx, double fy, double cx,
                    double cy)
      : x_(x), y_(y), fx_(fx), fy_(fy), cx_(cx), cy_(cy) {}

  template <typename T>
  bool operator()(const T* const q, const T* const t, const T* const point,
                  T* residuals) const {
    T p[3];

    ceres::QuaternionRotatePoint(q, point, p);

    p[0] += t[0];
    p[1] += t[1];
    p[2] += t[2];

    T xp = p[0] / p[2];
    T yp = p[1] / p[2];

    T u = T(fx_) * xp + T(cx_);
    T v = T(fy_) * yp + T(cy_);

    residuals[0] = u - T(x_);
    residuals[1] = v - T(y_);

    return true;
  }

  static ceres::CostFunction* Create(double x, double y, double fx, double fy,
                                     double cx, double cy) {
    return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3, 3>(
        new ReprojectionError(x, y, fx, fy, cx, cy));
  }

  double x_, y_;
  double fx_, fy_, cx_, cy_;
};

struct GCPPriorError {
  GCPPriorError(const Eigen::Vector3d& xyz_prior, const Eigen::Vector3d& sigma)
      : xyz_prior_(xyz_prior),
        weight_(1.0 / sigma.x(), 1.0 / sigma.y(), 1.0 / sigma.z()) {}

  template <typename T>
  bool operator()(const T* const point, T* residuals) const {
    residuals[0] = T(weight_.x()) * (point[0] - T(xyz_prior_.x()));
    residuals[1] = T(weight_.y()) * (point[1] - T(xyz_prior_.y()));
    residuals[2] = T(weight_.z()) * (point[2] - T(xyz_prior_.z()));
    return true;
  }

  static ceres::CostFunction* Create(const Eigen::Vector3d& xyz_prior,
                                     const Eigen::Vector3d& sigma) {
    return new ceres::AutoDiffCostFunction<GCPPriorError, 3, 3>(
        new GCPPriorError(xyz_prior, sigma));
  }

  Eigen::Vector3d xyz_prior_;
  Eigen::Vector3d weight_;  // 1 / sigma
};

}  // namespace

BAProblem::BAProblem(const Eigen::Matrix3d& K, const vector<Observation>& obs,
                     vector<CameraExtrinsics>& est_cams,
                     vector<ObjectPoint>& est_pts)
    : fx(K(0, 0)),
      fy(K(1, 1)),
      cx(K(0, 2)),
      cy(K(1, 2)),
      obs(obs),
      est_cams(est_cams),
      est_pts(est_pts) {}

bool BAProblem::Solve() {
  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);

    problem.AddResidualBlock(cost, nullptr, est_cams[o.cam_id].q,
                             est_cams[o.cam_id].t, est_pts[o.pt_id].xyz);
  }

  // Fix first camera
  problem.SetParameterBlockConstant(est_cams[0].q);
  problem.SetParameterBlockConstant(est_cams[0].t);

  ceres::Solver::Options options;

  options.linear_solver_type = ceres::SPARSE_SCHUR;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 50;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  cout << summary.BriefReport() << endl;

  cout << "Final RMSE: " << ComputeRMSE() << endl;

  // === 3. 核心判断：仅根据终止类型决定成败 ===
  if (summary.termination_type != ceres::CONVERGENCE) {
    return false;
  }
  return true;
}

bool BAProblem::SolveWithGcp(
    const std::map<int, Eigen::Vector3d>& gcp_3d_priors) {
  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);

    if (o.is_gcp) {
      // GCP 人工刺点精度高，典型值: 0.2 ~ 0.5 像素
      double gcp_pixel_sigma = 0.3;
      // 在最小二乘中，应最小化 (residual / sigma)^2
      // ScaledLoss(nullptr, s) 产生 cost = (s * residual)^2
      // 因此 s = 1.0 / sigma
      const double scale = 1.0 / gcp_pixel_sigma;
      ceres::LossFunction* loss =
          new ceres::ScaledLoss(nullptr, scale, ceres::TAKE_OWNERSHIP);
      problem.AddResidualBlock(cost, loss, est_cams[o.cam_id].q,
                               est_cams[o.cam_id].t, est_pts[o.pt_id].xyz);
    } else {
      // 普通特征点，σ ≈ 1.0 px
      problem.AddResidualBlock(cost, nullptr, est_cams[o.cam_id].q,
                               est_cams[o.cam_id].t, est_pts[o.pt_id].xyz);
    }
  }

  // 3. GCP 先验残差
  Eigen::Vector3d gcp_sigma(0.01, 0.01, 0.01);  // 假设 GCP 精度为 1cm
  for (const auto& kv : gcp_3d_priors) {
    int pt_id = kv.first;
    const Eigen::Vector3d& xyz_true = kv.second;

    // 确保 pt_id 有效
    if (pt_id < 0 || pt_id >= est_pts.size()) {
      cerr << "Warning: Invalid GCP pt_id=" << pt_id << endl;
      continue;
    }

    ceres::CostFunction* gcp_cost = GCPPriorError::Create(xyz_true, gcp_sigma);
    problem.AddResidualBlock(gcp_cost, nullptr, est_pts[pt_id].xyz);
  }

  ceres::Solver::Options options;

  options.linear_solver_type = ceres::SPARSE_SCHUR;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 50;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  cout << summary.BriefReport() << endl;

  cout << "Final RMSE: " << ComputeRMSE() << endl;

  // === 3. 核心判断：仅根据终止类型决定成败 ===
  if (summary.termination_type != ceres::CONVERGENCE) {
    return false;
  }
  return true;
}

double BAProblem::ComputeRMSE() {
  double err = 0;
  int n = 0;

  for (auto& o : obs) {
    const CameraExtrinsics& c = est_cams[o.cam_id];
    const ObjectPoint& p = est_pts[o.pt_id];

    Eigen::Quaterniond q(c.q[0], c.q[1], c.q[2], c.q[3]);

    Eigen::Vector3d t(c.t[0], c.t[1], c.t[2]);

    Eigen::Vector3d P(p.xyz[0], p.xyz[1], p.xyz[2]);

    Eigen::Vector3d Pc = q * P + t;

    double u = fx * Pc.x() / Pc.z() + cx;
    double v = fy * Pc.y() / Pc.z() + cy;

    double du = u - o.x;
    double dv = v - o.y;

    err += du * du + dv * dv;
    n += 2;
  }

  return sqrt(err / n);
}

调整 SFM 优化的流程，在最后加上绝对定向 AbsoluteOrientation 和联合稀少 GCP 约束的估计 GlobalBundleAdjustmentWithGcp ：

int main() {
  std::string cameraIntrinsicsPath =
      "D:/Work/SFMWork/SFM/CameraIntrinsics.json";
  std::string bundlePath = "D:/Work/SFMWork/SFM/Bundle.json";
  std::string outputDir = "D:/Work/SFMWork/SFM";

  auto [K, cameraExtrinsics, bundle, objectPoints, gcp_3d_priors] =
      ReadData(cameraIntrinsicsPath, bundlePath);

  IncrementalSFM(K, cameraExtrinsics, bundle, objectPoints);

  AbsoluteOrientation(cameraExtrinsics, bundle, objectPoints);

  // GlobalBundleAdjustment(K, bundle, cameraExtrinsics, objectPoints);
  GlobalBundleAdjustmentWithGcp(K, bundle, cameraExtrinsics, objectPoints,
                                gcp_3d_priors);

  Output(K, cameraExtrinsics, bundle, objectPoints, outputDir);
}

在绝对定向 AbsoluteOrientation 中，会获取自由网物方点和 GCP 点，然后调用 Sim3Problem 进行优化求解，最后更新位姿参数：

void AbsoluteOrientation(vector<cl::CameraExtrinsics>& cameraExtrinsics,
                         const sfm::Bundle& bundle,
                         vector<cl::ObjectPoint>& objectPoints) {
  std::vector<Eigen::Vector3d> src_pts;
  std::vector<Eigen::Vector3d> dst_pts;

  // 1. 收集匹配点对
  for (const auto& track : bundle.tracks) {
    // 确保点是三角化过的，且是 GCP (isPrior)
    if (track.isPrior && objectPoints[track.pointId].triangulated) {
      src_pts.push_back(objectPoints[track.pointId].pos);
      dst_pts.push_back(bundle.points[track.pointId]);
      cout << objectPoints[track.pointId].pos.transpose() << " <-> "
           << bundle.points[track.pointId].transpose() << endl;
    }
  }

  if (src_pts.size() < 3) {
    cout << "警告: GCP 匹配点少于 3 个，无法进行绝对定向" << endl;
    return;
  }

  // 2. 求解 Sim3
  Sim3Problem sim3Problem(src_pts, dst_pts);
  Sim3Problem::Sim3 sim3;
  if (!sim3Problem.Solve(sim3)) {
    cout << "Sim3 求解失败" << endl;
    return;
  }

  cout << "=== 应用 Sim3 变换 ===" << endl;
  cout << "Scale: " << sim3.scale << endl;

  // 3. 变换 3D 点
  for (auto& point : objectPoints) {
    // 公式: X_world = s * R * X_sfm + t
    point.pos = sim3.scale * sim3.R * point.pos + sim3.t;
  }

  // 4. 变换相机
  for (auto& cam : cameraExtrinsics) {
    if (!cam.registered) continue;  // 跳过未注册的相机

    // --- 修正点：旋转矩阵的更新 ---
    // 解释: 如果世界坐标系旋转了 R_sim3，相机姿态需要反向旋转来补偿
    // 公式: R_new = R_old * R_sim3.transpose()
    Eigen::Matrix3d R_world = cam.R * sim3.R.transpose();

    // --- 平移向量的更新  ---
    // 计算相机中心 C_sfm (在世界坐标系下)
    Eigen::Vector3d C_sfm = -cam.R.transpose() * cam.t;
    // 变换相机中心: C_world = s * R_sim3 * C_sfm + t_sim3
    Eigen::Vector3d C_world = sim3.scale * sim3.R * C_sfm + sim3.t;

    // 重新计算 t_world: t = -R * C
    Eigen::Vector3d t_world = -R_world * C_world;

    // 更新相机参数
    cam.R = R_world;
    cam.t = t_world;
  }
}

GlobalBundleAdjustmentWithGcp 则进行最后的带约束的 BA 优化估计：

bool GlobalBundleAdjustmentWithGcp(
    const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
    vector<cl::CameraExtrinsics>& cameras,
    vector<cl::ObjectPoint>& objectPoints,
    const std::map<int, Eigen::Vector3d>& golbal_gcp_3d_priors) {
  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;
  std::map<int, Eigen::Vector3d> local_gcp_3d_priors;

  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()) {
          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();
          observation.is_gcp = track.isPrior;
          obs.push_back(std::move(observation));
        }
      }

      int current3DIdx = est_pts.size();
      pointIdMap.emplace(track.pointId, current3DIdx);

      auto iter = golbal_gcp_3d_priors.find(track.pointId);
      if (golbal_gcp_3d_priors.end() != iter) {
        local_gcp_3d_priors.emplace(current3DIdx, iter->second);
      }

      est_pts.push_back(objectPoint);
    }
  }

  BAProblem problem(K, obs, est_cams, est_pts);
  if (!problem.SolveWithGcp(local_gcp_3d_priors)) {
    return false;
  }

  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];
      }
    }
  }

  return true;
}

最终得到精度报告与之前的精度报告差不多：

=== SfM 重建精度报告 ===

=== 总体重投影误差评估 ===
总有效观测点数量: 213825
均方根重投影误差 (RMSE): 0.925549 像素
平均重投影距离: 1.308924 像素
最大重投影误差: 4.737442 像素

评估结果: 优秀 (RMSE < 1.0 px)

=== 各相机重投影误差统计 ===
相机ID	观测点数	平均误差(px)	最大误差(px)
------	--------	----------	----------
0	669		1.138498		3.506387
1	800		1.143440		3.333284
2	963		1.120040		3.558708
3	1236		1.134163		3.825562
4	1384		1.160838		4.003462
5	1273		1.186830		3.400867
6	1419		1.183719		3.495600
7	1307		1.175630		3.276562
8	1283		1.154504		4.027606
9	1125		1.173727		3.303618
10	1300		1.176120		3.562757
# ...
107	1365		1.084929		3.216329

不过这次我们还比较了真实误差，即真实位姿和估计位姿的差异：

=== 逐相机误差报告 ===
ID      Trans Error (m) Rot Error (deg)
--------------------------------------
0       0.091888                0.020811
1       0.020144                0.002362
2       0.035423                0.007638
3       0.017054                0.003444
4       0.017345                0.004937
5       0.025189                0.007842
6       0.021996                0.005809
7       0.006136                0.001724
8       0.037700                0.010153
9       0.016937                0.004562
10      0.011840                0.003652
# ...
107     0.039919                0.008512

=== 整体精度统计 ===
-------------------
平移误差 (Translation Error):
  平均 (Mean): 0.020618 m
  均方根 (RMSE): 0.023966 m
  最大 (Max): 0.091888 m (Camera 0)
旋转误差 (Rotation Error):
  平均 (Mean): 0.006283 deg
  均方根 (RMSE): 0.007199 deg
  最大 (Max): 0.020811 deg (Camera 0)
===================

这充分说明，通过本文提出的“Sim3 引导的联合优化”策略，我们已经成功将自由网模型从任意坐标系精确对齐到了真实世界坐标系中。从误差统计来看，平移误差的均方根（RMSE）仅为 2.4 厘米，旋转误差低至 0.007 度。这一结果验证了从线性初值计算到非线性全局优化这一整套数学推导与代码实现的正确性。至此，我们不仅解决了单目视觉重建的尺度不确定性问题，更在稀疏控制点的约束下，实现了从“相对结构”到“绝对位姿”的完整闭环。

上一篇 | 目录 | 下一篇
代码存档

文章来源:https://www.cnblogs.com/charlee44/p/19905614
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若内容造成侵权/违法违规/事实不符，请联系邮箱：jacktools123@163.com进行投诉反馈，一经查实，立即删除！