最小二乘问题详解19：带先验约束的增量式SFM优化与实现

1 引言

在上一篇《最小二乘问题详解18：增量式SFM核心流程实现》中，我们成功构建了一个完整的增量式SFM（Structure from Motion）系统骨架。该系统从仿真数据出发，通过"初始化 -> PnP注册 -> 三角化扩展 -> 全局BA优化"的闭环迭代，最终实现了对相机位姿和场景结构的联合重建，并获得了亚像素级的优秀重投影精度。

然而，在上一篇的实现中，我们忽略了一个关键性细节：我们使用了仿真数据中的 INS/GNSS 的先验位姿信息，但是仅被用作优化求解的初始值。这种做法虽然能有效引导优化器快速收敛，避免陷入局部最优，但它存在一个潜在的风险：一旦视觉观测数据质量不佳（例如，在纹理缺失区域、或存在少量未被剔除的错误匹配时），优化结果可能会严重偏离可靠的先验值，导致位姿发生不可控的"漂移"。

一个更为合理的方法是，不仅仅将先验信息视为一个起点，而是将其作为一种软约束（Soft Constraint） ，直接融入到整个非线性最小二乘优化的目标函数中。这意味着，我们的优化目标不再仅仅是追求最小的重投影误差，而是在重投影误差 与先验偏离代价之间寻求一个全局最优的平衡。

2 思路

在一般的SFM框架中，为了克服纯视觉方法固有的尺度不确定性、初始化困难以及累积漂移等问题，引入外部先验信息是一种行之有效的策略。其中，最常见的两类先验是位姿先验（Pose Prior） 和控制点先验（Ground Control Point, GCP Prior）。

1. 位姿先验 (INS/GNSS)
   现代无人机或移动测绘平台通常集成了高精度的组合导航系统，即惯性导航系统（INS）与全球导航卫星系统（GNSS）的融合。这类系统能够以较高的频率（如10Hz、50Hz甚至更高）输出载体（相机）在世界坐标系下的六自由度位姿信息，包括位置 (X, Y, Z) 和姿态（通常以欧拉角或四元数表示）。其核心原理是利用GNSS提供绝对位置基准，并通过INS在GNSS信号失锁时进行航位推算（Dead Reckoning），两者通过卡尔曼滤波等算法深度融合，以获得平滑、连续且可靠的位姿估计。目前，采用RTK（实时动态定位）或PPK（后处理动态定位）技术的消费级或工业级无人机，其定位精度通常可以达到厘米级（例如，水平1-3cm + 1ppm，高程2-5cm + 1ppm） ，姿态精度也能达到0.1°至0.5° 的量级。这些高精度的位姿数据为 SFM 提供了极其宝贵的先验约束。
2. 控制点先验 (GCP)
   在传统的摄影测量流程中，地面控制点（GCP）是实现高精度绝对定向的关键。其标准做法是：外业人员使用高精度GNSS接收机（如RTK/PPK设备）在测区布设并精确测量若干个具有已知三维坐标的控制点；内业人员则在影像上人工或半自动地刺点，获取这些控制点对应的二维像素坐标。通过这些"已知3D-2D对应关系"，可以在空三加密（即SFM的BA阶段）中对整个模型进行绝对尺度、位置和姿态的约束与校正。随着自动化程度的提高，如今并不需要像过去那样密集布设GCP，稀疏的、均匀分布的少量高质量GCP（例如，每平方公里几个点）就足以将区域网的整体精度提升到厘米级，这极大地降低了外业成本。

结合以上两种先验，"2D-2D匹配 + INS/GNSS位姿先验 + 稀疏GCP控制点" 构成了当前近景与低空摄影测量中最接近工业级应用的优化流程。它充分利用了多源传感器的优势，实现了高效率与高精度的统一。

基于此，本文的实现思路是在完全保留上一篇文章所确立的增量式SFM主体框架 （即三角化、PnP、BA三大核心问题构成的迭代闭环）的前提下，对其中的PnP问题 和BA问题进行针对性的增强。具体而言，我们需要在这两个优化问题的残差模型中，分别加入对应的先验约束项：

• BA问题：在联合优化所有相机位姿和3D点坐标时，为每个已注册相机的位姿参数添加位姿先验残差项，并为所有已知坐标的GCP点添加3D坐标先验残差项。
• PnP问题：在最小化重投影误差的同时，增加一个关于待求解相机位姿与INS/GNSS先验位姿之间差异的残差项。另外，由于使用的GCP点一般较少，并且最终还是要全局BA优化，PnP问题的GCP先验可以省略。

值得注意的是，三角化（Triangulation）问题本身无需修改。因为三角化的输入是已经确定的相机位姿和2D观测，其目标是求解单个3D点。先验信息（无论是位姿还是GCP）的作用主要体现在对这些输入（位姿）和输出（3D点）的全局优化上，而非单次三角化计算内部。

3 子问题

重投影误差是SFM中最基础的残差项，其目标是最小化观测到的2D像素坐标与由当前估计的相机位姿和3D点重新投影得到的像素坐标之间的差异。对于第 i 个3D-2D对应点对，其残差向量为：

\[\mathbf{r}_i^{\text{vis}} = \begin{bmatrix} u_i - u_i^{\text{proj}} \\ v_i - v_i^{\text{proj}} \end{bmatrix} \]

其中：

• \((u_i, v_i)\) 是图像中观测到的2D像素坐标。
• \((u_i^{\text{proj}}, v_i^{\text{proj}})\) 是通过针孔相机模型从当前估计的相机位姿 \((\mathbf{R}, \mathbf{t})\) 和3D点 \(\mathbf{X}_i\) 投影得到的坐标：

\[\begin{aligned} u_i^{\text{proj}} &= \frac{f_x (\mathbf{R} \mathbf{X}_i + \mathbf{t})_x}{(\mathbf{R} \mathbf{X}_i + \mathbf{t})_z} + c_x \\ v_i^{\text{proj}} &= \frac{f_y (\mathbf{R} \mathbf{X}_i + \mathbf{t})_y}{(\mathbf{R} \mathbf{X}_i + \mathbf{t})_z} + c_y \end{aligned} \]

重投影误差是PnP和BA问题的基础残差项。接下来我们在PnP问题中加入位姿先验。

3.1 PnP问题

位姿先验将外部传感器（如INS/GNSS）提供的位姿信息作为软约束融入优化目标。其核心思想是惩罚优化后的位姿与先验位姿之间的偏差，并根据先验的不确定性（标准差）对不同自由度进行加权。

对于一个待优化的相机位姿（由四元数 \(\mathbf{q}\) 和平移向量 \(\mathbf{t}\) 表示）及其对应的先验（\(\mathbf{q}{\text{prior}}, \mathbf{t}{\text{prior}}\)），其6维残差向量为：

\[\mathbf{r}^{\text{pose}} = \begin{bmatrix} \mathbf{W}{\text{trans}} (\mathbf{t} - \mathbf{t}{\text{prior}}) \\ \mathbf{W}_{\text{rot}} \cdot (2 \cdot \boldsymbol{\epsilon}) \end{bmatrix} \]

其中：

• 平移部分 ：\(\mathbf{W}{\text{trans}} = \text{diag}(1/\sigma{t_x}, 1/\sigma_{t_y}, 1/\sigma_{t_z})\) 是一个对角权重矩阵，其元素是平移先验标准差的倒数。

• 旋转部分：计算相对更复杂。首先计算两个四元数之间的相对旋转：

  \[mathbf{q}{\text{diff}} = \mathbf{q}{\text{prior}}^{-1} \otimes \mathbf{q} \]

  在小角度假设下（即优化结果应接近先验值），相对旋转四元数的虚部 \(\boldsymbol{\epsilon} = [\epsilon_x, \epsilon_y, \epsilon_z]^\top\) 近似等于旋转向量的一半。因此，旋转残差为 \(2 \cdot \boldsymbol{\epsilon}\)。

• 旋转权重 ：\(\mathbf{W}{\text{rot}} = \text{diag}(1/\sigma{r_x}, 1/\sigma_{r_y}, 1/\sigma_{r_z})\)，其元素是旋转先验标准差（以弧度为单位）的倒数。

给定权重使用的平移先验标准差和旋转先验标准差来源于仿真数据生成的时候，按照这个标准差来生成正态分布的噪声。在实际的应用中，INS/GNSS组合导航系统（如基于卡尔曼滤波的方案）通常会输出平移先验标准差和旋转先验标准差。

具体的代码实现如下所示：

#pragma once

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

class PnPProblem {
 public:
  PnPProblem(const Eigen::Matrix3d& K, const Eigen::Matrix3d& ins_R_prior,
             const Eigen::Vector3d& ins_t_prior,
             const std::vector<Eigen::Vector3d>& points_world,
             const std::vector<Eigen::Vector2d>& observations);

  bool Solve(Eigen::Matrix3d& R_opt, Eigen::Vector3d& t_opt);

 private:
  Eigen::Quaterniond CeresQuatToEigen(const double* q_ceres);
  void EigenQuatToCeres(const Eigen::Quaterniond& q_eigen, double* q_ceres);

  double fx;
  double fy;
  double cx;
  double cy;

  Eigen::Matrix3d ins_R_prior_;  //  旋转先验
  Eigen::Vector3d ins_t_prior_;  // 平移先验

  const std::vector<Eigen::Vector3d>& points_world;
  const std::vector<Eigen::Vector2d>& observations;
};

#include "SolvePnP.h"

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

#include <iomanip>
#include <iostream>
#include <vector>

#include "Math.h"

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

struct PosePriorError {
  PosePriorError(const Eigen::Vector3d& t_prior,
                 const Eigen::Quaterniond& q_prior,
                 const Eigen::Vector3d& trans_sigma,
                 const Eigen::Vector3d& rot_sigma)  // 旋转标准差（弧度）
      : t_prior_(t_prior),
        q_prior_(q_prior),
        w_trans_(1.0 / trans_sigma.x(), 1.0 / trans_sigma.y(),
                 1.0 / trans_sigma.z()),
        w_rot_(1.0 / rot_sigma.x(), 1.0 / rot_sigma.y(), 1.0 / rot_sigma.z()) {}

  template <typename T>
  bool operator()(const T* const q, const T* const t, T* residuals) const {
    // 平移残差（加权）
    residuals[0] = T(w_trans_.x()) * (t[0] - T(t_prior_.x()));
    residuals[1] = T(w_trans_.y()) * (t[1] - T(t_prior_.y()));
    residuals[2] = T(w_trans_.z()) * (t[2] - T(t_prior_.z()));

    // 旋转残差（加权）
    // 旋转：小角度近似下，四元数虚部 ≈ 0.5 * 旋转向量
    Eigen::Quaternion<T> q_est(q[0], q[1], q[2], q[3]);
    Eigen::Quaternion<T> q_ref = q_prior_.cast<T>();
    Eigen::Quaternion<T> q_diff = q_ref.conjugate() * q_est;
    q_diff.normalize();

    // 注意：这里假设旋转误差小，使用局部参数化
    residuals[3] = T(w_rot_.x()) * T(2.0) * q_diff.x();
    residuals[4] = T(w_rot_.y()) * T(2.0) * q_diff.y();
    residuals[5] = T(w_rot_.z()) * T(2.0) * q_diff.z();

    return true;
  }

  static ceres::CostFunction* Create(const Eigen::Vector3d& t_prior,
                                     const Eigen::Quaterniond& q_prior,
                                     const Eigen::Vector3d& trans_sigma,
                                     const Eigen::Vector3d& rot_sigma) {
    return new ceres::AutoDiffCostFunction<PosePriorError, 6, 4, 3>(
        new PosePriorError(t_prior, q_prior, trans_sigma, rot_sigma));
  }

  Eigen::Vector3d t_prior_;
  Eigen::Quaterniond q_prior_;
  Eigen::Vector3d w_trans_;  // 权重 = 1 / sigma
  Eigen::Vector3d w_rot_;
};

}  // namespace

PnPProblem::PnPProblem(const Eigen::Matrix3d& K,
                       const Eigen::Matrix3d& ins_R_prior,
                       const Eigen::Vector3d& ins_t_prior,
                       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)),
      ins_R_prior_(ins_R_prior),
      ins_t_prior_(ins_t_prior),
      points_world(points_world),
      observations(observations) {}

bool PnPProblem::Solve(Eigen::Matrix3d& R_opt, Eigen::Vector3d& t_opt) {
  // 使用 INS+RTK 作为初始值
  Eigen::Quaterniond q_init(ins_R_prior_);
  double q_est[4] = {q_init.w(), q_init.x(), q_init.y(), q_init.z()};
  double t_est[3] = {ins_t_prior_.x(), ins_t_prior_.y(), ins_t_prior_.z()};

  // --- 构建 Ceres 问题 ---
  ceres::Problem problem;
  ceres::Manifold* quaternion_manifold = new ceres::QuaternionManifold();

  problem.AddParameterBlock(q_est, 4, quaternion_manifold);
  problem.AddParameterBlock(t_est, 3);

  // 1. 添加普通 3D-2D 重投影
  for (size_t i = 0; i < points_world.size(); ++i) {
    problem.AddResidualBlock(
        ReprojectionError::Create(points_world[i], observations[i], fx, fy, cx,
                                  cy),
        nullptr, q_est, t_est);
  }

  // 2. 添加 INS+RTK 先验
  Eigen::Quaterniond q_prior(ins_R_prior_);

  // 平移 sigma: [0.02, 0.02, 0.05] 米
  Eigen::Vector3d trans_sigma(0.02, 0.02, 0.05);

  // 旋转 sigma: 注意单位是弧度！
  // yaw=0.05°, pitch=0.02°, roll=0.02°
  Eigen::Vector3d rot_sigma_deg(0.02, 0.02, 0.05);  // 注意顺序！
  Eigen::Vector3d rot_sigma = rot_sigma_deg * PI / 180.0;

  problem.AddResidualBlock(
      PosePriorError::Create(ins_t_prior_, q_prior, trans_sigma, rot_sigma),
      nullptr, 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();
  R_opt = q_opt.toRotationMatrix();
  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();
    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 > 6.0) {  // 阈值根据情况调整，合成数据可以给小一点
    std::cout << "PnP Failed: Converged but RMSE too high." << std::endl;
    return false;
  }

  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();
}

在上述代码实现中，PosePriorError 实现了姿态先验的残差计算，并且使用了 q_ref.conjugate() * q_est 来计算相对旋转，并提取其虚部作为残差。

姿态先验残差项可以同时加入到PnP和BA问题中。另外，可以在BA问题中额外加入控制点先验。

3.2 BA问题

控制点先验利用已知精确3D坐标的地面控制点（GCP）来约束场景结构。其残差直接衡量优化得到的3D点坐标与已知真值之间的加权差异。

对于一个待优化的3D点 \(\mathbf{X} = [X, Y, Z]^\top\) 及其已知的先验坐标 \(\mathbf{X}_{\text{prior}}\)，其3维残差向量为：

\[\mathbf{r}^{\text{gcp}} = \mathbf{W}{\text{gcp}} (\mathbf{X} - \mathbf{X}{\text{prior}}) \]

其中：

• 权重矩阵 ：\(\mathbf{W}{\text{gcp}} = \text{diag}(1/\sigma{X}, 1/\sigma_{Y}, 1/\sigma_{Z})\)，其元素是GCP坐标先验标准差的倒数。

给定权重使用的GCP坐标先验标准差来源于仿真数据生成的时候，按照这个标准差来生成正态分布的噪声。在实际的应用中，GCP的先验标准差通常来源于外业测量时所使用的高精度设备（如RTK/PPK GNSS接收机或全站仪）的标称精度或实测精度报告。另外，GCP在影像上的2D坐标通常由内业人员在立体像对上进行高精度人工刺点获得，精度可达 0.1--0.3 像素，通常优于或至少与通用自动特征匹配算法在自然场景下的定位精度相当。

具体的代码实现如下所示：

#pragma once

#include <Eigen/Core>
#include <map>
#include <vector>

class BAProblem {
 public:
  struct CameraExtrinsics {
    double q[4];  // quaternion
    double t[3];  // translation
  };

  struct ObjectPoint {
    double xyz[3];
  };

  struct Observation {
    int cam_id;
    int pt_id;
    double x;
    double y;
    bool is_gcp;
  };

  BAProblem(const Eigen::Matrix3d& K, const std::vector<Observation>& obs,
            std::vector<CameraExtrinsics>& est_cams,
            std::vector<ObjectPoint>& est_pts,
            const std::vector<Eigen::Matrix3d>& ins_R_priors,
            const std::vector<Eigen::Vector3d>& ins_t_priors,
            const std::map<int, Eigen::Vector3d>& gcp_3d_priors);

  bool Solve();

 private:
  double ComputeRMSE();

  double fx;
  double fy;
  double cx;
  double cy;
  const std::vector<Observation>& obs;
  std::vector<CameraExtrinsics>& est_cams;
  std::vector<ObjectPoint>& est_pts;
  std::vector<Eigen::Matrix3d> ins_R_priors_;
  std::vector<Eigen::Vector3d> ins_t_priors_;
  std::map<int, Eigen::Vector3d> gcp_3d_priors_;  // pt_id -> GCP 真实坐标
};

#include "SolveBA.h"

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

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

#include "Math.h"

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 PosePriorError {
  PosePriorError(const Eigen::Vector3d& t_prior,
                 const Eigen::Quaterniond& q_prior,
                 const Eigen::Vector3d& trans_sigma,
                 const Eigen::Vector3d& rot_sigma)  // 旋转标准差（弧度）
      : t_prior_(t_prior),
        q_prior_(q_prior),
        w_trans_(1.0 / trans_sigma.x(), 1.0 / trans_sigma.y(),
                 1.0 / trans_sigma.z()),
        w_rot_(1.0 / rot_sigma.x(), 1.0 / rot_sigma.y(), 1.0 / rot_sigma.z()) {}

  template <typename T>
  bool operator()(const T* const q, const T* const t, T* residuals) const {
    // 平移残差（加权）
    residuals[0] = T(w_trans_.x()) * (t[0] - T(t_prior_.x()));
    residuals[1] = T(w_trans_.y()) * (t[1] - T(t_prior_.y()));
    residuals[2] = T(w_trans_.z()) * (t[2] - T(t_prior_.z()));

    // 旋转残差（加权）
    // 旋转：小角度近似下，四元数虚部 ≈ 0.5 * 旋转向量
    Eigen::Quaternion<T> q_est(q[0], q[1], q[2], q[3]);
    Eigen::Quaternion<T> q_ref = q_prior_.cast<T>();
    Eigen::Quaternion<T> q_diff = q_ref.conjugate() * q_est;
    q_diff.normalize();

    // 注意：这里假设旋转误差小，使用局部参数化
    residuals[3] = T(w_rot_.x()) * T(2.0) * q_diff.x();
    residuals[4] = T(w_rot_.y()) * T(2.0) * q_diff.y();
    residuals[5] = T(w_rot_.z()) * T(2.0) * q_diff.z();

    return true;
  }

  static ceres::CostFunction* Create(const Eigen::Vector3d& t_prior,
                                     const Eigen::Quaterniond& q_prior,
                                     const Eigen::Vector3d& trans_sigma,
                                     const Eigen::Vector3d& rot_sigma) {
    return new ceres::AutoDiffCostFunction<PosePriorError, 6, 4, 3>(
        new PosePriorError(t_prior, q_prior, trans_sigma, rot_sigma));
  }

  Eigen::Vector3d t_prior_;
  Eigen::Quaterniond q_prior_;
  Eigen::Vector3d w_trans_;  // 权重 = 1 / sigma
  Eigen::Vector3d w_rot_;
};

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,
                     const vector<Eigen::Matrix3d>& ins_R_priors,
                     const vector<Eigen::Vector3d>& ins_t_priors,
                     const std::map<int, Eigen::Vector3d>& gcp_3d_priors)
    : 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),
      ins_R_priors_(ins_R_priors),
      ins_t_priors_(ins_t_priors),
      gcp_3d_priors_(gcp_3d_priors) {}

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

  // 1. 视觉重投影残差
  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);
    }
  }

  // 2. INS+RTK 先验残差（对每个相机）
  const double DEG_TO_RAD = PI / 180.0;
  Eigen::Vector3d trans_sigma(0.02, 0.02, 0.05);    // [x, y, z]
  Eigen::Vector3d rot_sigma_deg(0.02, 0.02, 0.05);  // [roll, pitch, yaw]
  Eigen::Vector3d rot_sigma = rot_sigma_deg * DEG_TO_RAD;

  for (int i = 0; i < est_cams.size(); i++) {
    Eigen::Quaterniond q_prior(ins_R_priors_[i]);
    problem.AddResidualBlock(PosePriorError::Create(ins_t_priors_[i], q_prior,
                                                    trans_sigma, rot_sigma),
                             nullptr, est_cams[i].q, est_cams[i].t);
  }

  // 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) {
    if (o.is_gcp) continue;  // 跳过 GCP

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

在上述代码实现中，不仅通过PosePriorError实现了姿态先验的残差计算，还加入了GCPPriorError作为GCP先验误差约束。

另一方面，在重投影误差的残差项部分，根据观测点是否为（GCP）动态调整其对应的图像观测权重：对于标记为 GCP 的观测（o.is_gcp == true），由于其 2D 坐标定位精度显著高于普通特征匹配点（代码设定 gcp_pixel_sigma = 0.3），通过 Ceres 的 ScaledLoss 将残差缩放为 \((1/\sigma) \cdot r\)，从而在整体代价函数中赋予 GCP 观测更高的置信度；而对于普通特征点（非 GCP），则采用默认的单位权重（隐含 \(\sigma = 1.0\) 像素），反映其相对较低的观测可靠性。这种加权策略有效平衡了不同类型观测对优化结果的影响，提升了重建的绝对精度。

3.3 残差项联合

在上文的实现中，我们构建了一个包含多种信息源的联合优化问题。其总体目标函数可以形式化地表示为（以 BA 问题为例）：

\[\min_{\mathbf{X}, \{\mathbf{q}i, \mathbf{t}i\}} \quad \underbrace{\sum{j} \left\| \mathbf{r}j^{\text{vis}} \right\|^2}{\text{重投影误差}} + \underbrace{\sum{i} \left\| \mathbf{r}i^{\text{pose}} \right\|^2}{\text{位姿先验}} + \underbrace{\sum_{k} \left\| \mathbf{r}k^{\text{gcp}} \right\|^2}{\text{GCP 先验}} \]

乍看之下，这三种残差项似乎"格格不入"：它们作用于不同的参数子集，且输出的残差向量维度也各不相同。

• 重投影误差 (ReprojectionError) ：作用于一个相机（7个参数：4维四元数 q + 3维平移 t）和一个3D点（3个参数），输出一个 2维 的像素坐标残差。
• 位姿先验误差 (PosePriorError) ：仅作用于一个相机（7个参数），输出一个 6维 的残差（3维平移 + 3维旋转）。
• GCP 先验误差 (GCPPriorError) ：仅作用于一个3D点（3个参数），输出一个 3维 的空间坐标残差。

那么，Ceres 是如何将这些维度各异、参数依赖不同的残差项"直接加到一起"的呢？

关键在于，非线性最小二乘优化器（如 Ceres）并不直接操作完整的雅可比矩阵，而是通过增量式的局部线性化来构建并求解法方程（Normal Equation） 。优化器维护一个全局的参数向量 \(\mathbf{p}\)（例如，所有相机的 q, t 和所有3D点的 xyz 拼接而成）。对于每一个残差块（Residual Block），Ceres 只需要知道：

1. 该残差块依赖于哪些参数块（Parameter Blocks）。
2. 如何计算该残差块关于其依赖参数的局部雅可比矩阵。

优化器会自动将所有局部雅可比矩阵"拼装"成一个巨大的、稀疏的全局雅可比矩阵 \(\mathbf{J}\)，然后求解 \(\mathbf{J}^\top \mathbf{J} \Delta \mathbf{p} = -\mathbf{J}^\top \mathbf{r}\) 来更新参数。

为了更直观地感受这个过程，我们可以对比 PnP 和 BA 问题中雅可比矩阵的结构：

1. PnP问题中的矩阵维度

在 PnP 问题中，待优化的参数是单个相机的位姿（7维），假设有 \(N\) 个3D-2D观测点。

• 残差向量 \(\mathbf{r}\) 的维度 ：\((2N + 6) \times 1\)。其中 \(2N\) 维来自重投影误差，6维来自位姿先验。
• 雅可比矩阵 \(\mathbf{J}\) 的维度 ：\((2N + 6) \times 7\)。
  • 前 \(2N\) 行：每行对应一个像素坐标的残差，对7个位姿参数求导。
  • 后 6 行：对应位姿先验的6个残差，对7个位姿参数求导。

这是一个高而窄 的矩阵，通常使用 DENSE_QR 等稠密分解方法求解。

2. BA问题中的矩阵维度

在 BA 问题中，待优化的参数包括 \(C\) 个相机（共 \(7C\) 维）和 \(P\) 个3D点（共 \(3P\) 维），总参数维度为 \((7C + 3P)\)。假设共有 \(M\) 个2D观测。

• 残差向量 \(\mathbf{r}\) 的维度 ：\((2M + 6C + 3G) \times 1\)。其中 \(2M\) 维来自重投影误差，\(6C\) 维来自所有相机的位姿先验，\(3G\) 维来自 \(G\) 个 GCP 的先验。
• 雅可比矩阵 \(\mathbf{J}\) 的维度 ：\((2M + 6C + 3G) \times (7C + 3P)\)。

这个矩阵具有非常典型的稀疏块结构：

• 重投影部分 （\(2M\) 行）：每一行只与一个相机（7列）和一个3D点（3列）相关，其余元素为0。
• 位姿先验部分 （\(6C\) 行）：每一组6行只与对应的一个相机（7列）相关。
• GCP 先验部分 （\(3G\) 行）：每一组3行只与对应的一个3D点（3列）相关。

正是这种稀疏性，使得即使面对成千上万个参数和观测，BA 问题依然可以通过 SPARSE_SCHUR 或 SPARSE_NORMAL_CHOLESKY 等稀疏矩阵算法高效求解。

总结来说，Ceres 通过将复杂的联合优化问题分解为一系列独立的、小规模的残差块，巧妙地绕开了直接处理巨大、异构残差向量的难题。每个残差块只需关注自己的局部世界，而 Ceres 负责将它们无缝集成到一个统一的全局优化框架中。这正是现代非线性优化库强大和灵活的核心所在。

4 主流程

增量式 SFM 优化的主体流程框架并没有太大的变化，实现代码如下所示：

#include <Eigen/Dense>
#include <algorithm>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <map>
#include <optional>

#include "Bundle.h"
#include "CameraIntrinsics.hpp"
#include "CameraPose.hpp"
#include "SolveBA.h"
#include "SolvePnP.h"
#include "SolveTriangulate.h"

using namespace std;

namespace cl {

// ===========================
// 相机外参
// ===========================
struct CameraExtrinsics {
  Eigen::Matrix3d R;
  Eigen::Vector3d t;
  bool registered = false;
};

// ===========================
// 3D点
// ===========================
struct ObjectPoint {
  Eigen::Vector3d pos;
  bool triangulated = false;
};

};  // namespace cl

bool GlobalBundleAdjustment(
    const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
    vector<cl::CameraExtrinsics>& cameras,
    vector<cl::ObjectPoint>& objectPoints,
    const vector<cl::CameraExtrinsics>& priorExtrinsics,
    const std::map<int, Eigen::Vector3d>& golbal_gcp_3d_priors) {
  map<int, int> cameraIdMap;
  vector<BAProblem::CameraExtrinsics> est_cams;
  std::vector<Eigen::Matrix3d> ins_R_priors;
  std::vector<Eigen::Vector3d> ins_t_priors;
  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));
      ins_R_priors.push_back(priorExtrinsics[ci].R);
      ins_t_priors.push_back(priorExtrinsics[ci].t);
    }
  }

  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 = false;
          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, ins_R_priors, ins_t_priors,
                    local_gcp_3d_priors);
  if (!problem.Solve()) {
    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;
}

Eigen::Matrix3d R_down;  // 相机朝下

Eigen::Matrix3d Rz(double yaw) {
  return Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()).toRotationMatrix();
}

Eigen::Matrix3d Ry(double pitch) {
  return Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()).toRotationMatrix();
}

Eigen::Matrix3d Rx(double roll) {
  return Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()).toRotationMatrix();
}

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;
  for (const auto& [id3d, id2d] : idsMap) {
    points.emplace_back(objectPoints[id3d].pos);
    observations.emplace_back(bundle.views[bestImgId][id2d]);
  }

  PnPProblem problem(K, cameraExtrinsics[bestImgId].R,
                     cameraExtrinsics[bestImgId].t, points, observations);
  Eigen::Matrix3d R_est;
  Eigen::Vector3d t_est;
  if (problem.Solve(R_est, t_est)) {
    cameraExtrinsics[bestImgId].R = R_est;
    cameraExtrinsics[bestImgId].t = t_est;
    cameraExtrinsics[bestImgId].registered = true;
    return true;
  }

  return false;
}

void TriangulateNewPoints(const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
                          vector<cl::CameraExtrinsics>& cameraExtrinsics,
                          vector<cl::ObjectPoint>& objectPoints) {
  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) {
        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;
    }

    objectPoints[track.pointId].pos = X;
    objectPoints[track.pointId].triangulated = true;
  }
}

void Init(const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
          vector<cl::CameraExtrinsics>& cameraExtrinsics,
          vector<cl::ObjectPoint>& objectPoints,
          const vector<cl::CameraExtrinsics>& priorExtrinsics,
          const std::map<int, Eigen::Vector3d>& gcp_3d_priors) {
  // 从匹配图中找到重叠度最高、基线最合适的两张图

  const auto& tracks = bundle.tracks;

  map<pair<int, int>, int> imagePairCounter;
  for (const auto& track : 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) {
        pair<int, int> imagePair{track.obs[i].imgId, track.obs[j].imgId};
        auto iter = imagePairCounter.find(imagePair);
        if (imagePairCounter.end() == iter) {
          imagePairCounter.emplace(imagePair, 1);
        } else {
          iter->second += 1;
        }
      }
    }
  }

  // 暂时只选取匹配点最多的
  int counter = 0;
  int image1 = -1;
  int image2 = -1;
  for (const auto& [imagePair, count] : imagePairCounter) {
    if (count > counter) {
      counter = count;
      image1 = imagePair.first;
      image2 = imagePair.second;
    }
  }
  if (image1 == -1 || image2 == -1) {
    return;
  }

  cameraExtrinsics[image1].registered = true;
  cameraExtrinsics[image2].registered = true;

  TriangulateNewPoints(K, bundle, cameraExtrinsics, objectPoints);

  GlobalBundleAdjustment(K, bundle, cameraExtrinsics, objectPoints,
                         priorExtrinsics, gcp_3d_priors);
}

void Iterate(const Eigen::Matrix3d& K, const sfm::Bundle& bundle,
             vector<cl::CameraExtrinsics>& cameraExtrinsics,
             vector<cl::ObjectPoint>& objectPoints,
             const vector<cl::CameraExtrinsics>& priorExtrinsics,
             const std::map<int, Eigen::Vector3d>& gcp_3d_priors) {
  while (true) {
    cout << "--- 新一轮迭代 ---" << endl;

    // 1. 尝试注册一张新图片
    // 返回 true 表示成功注册了一张，false 表示没有更多图片可注册
    bool success = RegisterNextImage(K, bundle, cameraExtrinsics, objectPoints);

    if (!success) {
      cout << "没有更多图片可以注册，结束。" << endl;
      break;
    }

    // 2. 三角化新点
    // 利用新注册的相机，与已注册相机之间的匹配，生成新的 3D 点
    TriangulateNewPoints(K, bundle, cameraExtrinsics, objectPoints);

    // 4. 全局光束法平差 (Global BA)
    // 优化所有已注册相机的位姿和所有 3D 点坐标
    cout << "----GlobalBundleAdjustment----" << endl;
    if (!GlobalBundleAdjustment(K, bundle, cameraExtrinsics, objectPoints,
                                priorExtrinsics, gcp_3d_priors)) {
      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;
  }
}

void IncrementalSFM(const Eigen::Matrix3d& K,
                    vector<cl::CameraExtrinsics>& cameraExtrinsics,
                    const sfm::Bundle& bundle,
                    vector<cl::ObjectPoint>& objectPoints,
                    const vector<cl::CameraExtrinsics>& priorExtrinsics,
                    const std::map<int, Eigen::Vector3d>& gcp_3d_priors) {
  // 初始化启动
  Init(K, bundle, cameraExtrinsics, objectPoints, priorExtrinsics,
       gcp_3d_priors);
  // 迭代优化
  Iterate(K, bundle, cameraExtrinsics, objectPoints, priorExtrinsics,
          gcp_3d_priors);
}

std::tuple<Eigen::Matrix3d, vector<cl::CameraExtrinsics>, sfm::Bundle,
           vector<cl::ObjectPoint>, std::map<int, Eigen::Vector3d>>
ReadData(const std::string& cameraIntrinsicsPath,
         const std::string& cameraExtrinsicsPath,
         const std::string& bundlePath) {
  std::cout << std::fixed << std::setprecision(6);

  R_down << 1, 0, 0, 0, -1, 0, 0, 0, -1;

  CameraIntrinsics cameraIntrinsics;
  Eigen::Matrix3d K;
  {
    ifstream infile(cameraIntrinsicsPath);
    nlohmann::json cameraIntrinsicsJson;
    infile >> cameraIntrinsicsJson;
    cameraIntrinsics = cameraIntrinsicsJson;
    K << cameraIntrinsics.fx, 0.0, cameraIntrinsics.cx, 0.0,
        cameraIntrinsics.fy, cameraIntrinsics.cy, 0.0, 0.0, 1.0;
  }

  vector<cl::CameraExtrinsics> cameraExtrinsics;
  {
    ifstream infile(cameraExtrinsicsPath);
    nlohmann::json cameraPosesJson;
    infile >> cameraPosesJson;
    vector<CameraPose> cameraPoses = cameraPosesJson;

    cameraExtrinsics.resize(cameraPoses.size());
    for (size_t i = 0; i < cameraPoses.size(); ++i) {
      cameraExtrinsics[i].t.x() = cameraPoses[i].px;
      cameraExtrinsics[i].t.y() = cameraPoses[i].py;
      cameraExtrinsics[i].t.z() = cameraPoses[i].pz;
      cameraExtrinsics[i].R = Rz(cameraPoses[i].yaw) *
                              Ry(cameraPoses[i].pitch) *
                              Rx(cameraPoses[i].roll) * R_down;
    }
  }

  sfm::Bundle bundle;
  {
    ifstream infile(bundlePath);
    nlohmann::json bundleJson;
    infile >> bundleJson;
    bundle = bundleJson;
  }

  vector<cl::ObjectPoint> objectPoints(bundle.tracks.size());

  std::map<int, Eigen::Vector3d> gcp_3d_priors;
  for (const auto& track : bundle.tracks) {
    if (track.isPrior) {
      gcp_3d_priors.emplace(track.pointId, bundle.points[track.pointId]);
    }
  }

  return {K, cameraExtrinsics, bundle, objectPoints, gcp_3d_priors};
}

void EvaluateReprojectionError(const Eigen::Matrix3d& K,
                               const sfm::Bundle& bundle,
                               const std::vector<cl::CameraExtrinsics>& cameras,
                               const std::vector<cl::ObjectPoint>& objectPoints,
                               const std::string& reportFilePath) {
  double sum_squared_error = 0.0;  // Σ(du² + dv²)
  double max_point_error = 0.0;    // 最大单点重投影距离
  int point_count = 0;             // 有效观测点数量（不是残差数）

  // 统计每个相机的误差（存储的是每个点的重投影距离）
  std::map<int, std::vector<double>> camera_errors;
  std::map<int, int> camera_obs_counts;

  std::cout << "\n=== 开始评估重投影误差 ===" << std::endl;

  for (const auto& track : bundle.tracks) {
    if (!objectPoints[track.pointId].triangulated) {
      continue;
    }

    Eigen::Vector3d P_world = objectPoints[track.pointId].pos;

    for (const auto& ob : track.obs) {
      int imgId = ob.imgId;
      int kpId = ob.kpId;

      if (!cameras[imgId].registered) {
        continue;
      }

      Eigen::Vector2d observed_point = bundle.views[imgId][kpId];
      Eigen::Matrix3d R = cameras[imgId].R;
      Eigen::Vector3d t = cameras[imgId].t;
      Eigen::Vector3d P_cam = R * P_world + t;

      if (P_cam.z() <= 0) {
        continue;
      }

      Eigen::Vector2d p_normalized(P_cam.x() / P_cam.z(),
                                   P_cam.y() / P_cam.z());
      Eigen::Vector2d projected_point =
          K.topLeftCorner<2, 2>() * p_normalized + K.topRightCorner<2, 1>();

      // 计算重投影距离（欧氏距离）
      double point_error = (observed_point - projected_point).norm();

      // 累加平方误差（用于 RMSE）
      sum_squared_error += point_error * point_error;  // = du² + dv²
      max_point_error = std::max(max_point_error, point_error);
      point_count++;

      // 收集每个相机的点误差（用于平均/最大 per-camera）
      camera_errors[imgId].push_back(point_error);
      camera_obs_counts[imgId]++;
    }
  }

  // 准备写入报告
  std::ofstream reportFile(reportFilePath);
  if (!reportFile.is_open()) {
    std::cerr << "错误：无法打开报告文件 " << reportFilePath << " 进行写入！"
              << std::endl;
    return;
  }

  std::string report_content = "\n=== SfM 重建精度报告 ===\n\n";
  report_content += "=== 总体重投影误差评估 ===\n";

  if (point_count > 0) {
    // ✅ 标准 RMSE：按残差计算（分母 = 2 * 点数）
    double rmse = std::sqrt(sum_squared_error / (2.0 * point_count));

    // 平均重投影距离（按点）------用于评估"典型点偏了多少"
    double mean_reproj_distance = std::sqrt(sum_squared_error / point_count);

    report_content += "总有效观测点数量: " + std::to_string(point_count) + "\n";
    report_content +=
        "均方根重投影误差 (RMSE): " + std::to_string(rmse) + " 像素\n";
    report_content +=
        "平均重投影距离: " + std::to_string(mean_reproj_distance) + " 像素\n";
    report_content +=
        "最大重投影误差: " + std::to_string(max_point_error) + " 像素\n\n";

    // 评估基于 RMSE（行业标准）
    if (rmse < 1.0) {
      report_content += "评估结果: 优秀 (RMSE < 1.0 px)\n";
    } else if (rmse < 2.0) {
      report_content += "评估结果: 良好 (RMSE < 2.0 px)\n";
    } else if (rmse < 5.0) {
      report_content += "评估结果: 一般 (RMSE < 5.0 px)\n";
    } else {
      report_content +=
          "评估结果: 较差 (RMSE >= 5.0 px)，可能需要检查数据或算法参数。\n";
    }
  } else {
    report_content += "错误：没有找到有效的观测点来计算重投影误差！\n";
  }

  // 写入各相机统计信息
  report_content += "\n=== 各相机重投影误差统计 ===\n";
  report_content += "相机ID\t观测点数\t平均误差(px)\t最大误差(px)\n";
  report_content += "------\t--------\t----------\t----------\n";

  for (const auto& [cam_id, errors] : camera_errors) {
    if (errors.empty()) continue;

    double sum_err = 0.0;
    double max_err = 0.0;
    for (double err : errors) {
      sum_err += err;
      if (err > max_err) max_err = err;
    }
    double mean_err = sum_err / static_cast<double>(errors.size());

    report_content += std::to_string(cam_id) + "\t" +
                      std::to_string(camera_obs_counts.at(cam_id)) + "\t\t" +
                      std::to_string(mean_err) + "\t\t" +
                      std::to_string(max_err) + "\n";
  }

  // 写入和打印
  reportFile << report_content;
  std::cout << report_content;
  reportFile.close();

  std::cout << "精度报告已保存至: " << reportFilePath << std::endl;
  std::cout << "=== 评估完成 ===\n" << std::endl;
}

std::vector<CameraPose> ExtrinsicsToPoses(
    const std::vector<cl::CameraExtrinsics>& cameraExtrinsics) {
  std::vector<CameraPose> cameraPoses;
  cameraPoses.resize(cameraExtrinsics.size());

  for (size_t i = 0; i < cameraExtrinsics.size(); ++i) {
    // 1. 提取位置
    cameraPoses[i].px = cameraExtrinsics[i].t.x();
    cameraPoses[i].py = cameraExtrinsics[i].t.y();
    cameraPoses[i].pz = cameraExtrinsics[i].t.z();

    // 2. 提取欧拉角
    // 首先，需要撤销 R_down 的影响
    // R_original = R_final * R_down.transpose()
    Eigen::Matrix3d R_original = cameraExtrinsics[i].R * R_down.transpose();

    // 使用 eulerAngles(2, 1, 0) 来提取 Z-Y-X 顺序的欧拉角
    // 返回的 Vector3d 分别是 (yaw, pitch, roll)
    Eigen::Vector3d euler_angles = R_original.eulerAngles(2, 1, 0);

    cameraPoses[i].yaw = euler_angles[0];
    cameraPoses[i].pitch = euler_angles[1];
    cameraPoses[i].roll = euler_angles[2];
  }
  return cameraPoses;
}

// 输出物点为 PLY 文件
void OutputPointCloud(const vector<cl::ObjectPoint>& objectPoints,
                      const std::string& filePath) {
  int point_count = 0;
  for (const auto& pt : objectPoints) {
    if (pt.triangulated) point_count++;
  }

  ofstream plyfile(filePath);
  if (!plyfile.is_open()) {
    cerr << "错误：无法打开PLY文件 " << filePath << " 进行写入！" << endl;
    return;
  }

  // PLY Header
  plyfile << "ply\n";
  plyfile << "format ascii 1.0\n";
  plyfile << "element vertex " << point_count << "\n";
  plyfile << "property float x\n";
  plyfile << "property float y\n";
  plyfile << "property float z\n";
  plyfile << "end_header\n";

  // Write vertices
  for (const auto& pt : objectPoints) {
    if (pt.triangulated) {
      plyfile << pt.pos.x() << " " << pt.pos.y() << " " << pt.pos.z() << "\n";
    }
  }

  plyfile.close();
  cout << "物点云已保存至: " << filePath << endl;
}

void Output(const Eigen::Matrix3d& K,
            vector<cl::CameraExtrinsics>& cameraExtrinsics,
            const sfm::Bundle& bundle, vector<cl::ObjectPoint>& objectPoints,
            const std::string& outputDir) {
  // 生成误差报告
  EvaluateReprojectionError(K, bundle, cameraExtrinsics, objectPoints,
                            outputDir + "/AccuracyReport.txt");

  ofstream outfile(outputDir + "/CameraPosesAdjustment.json");
  nlohmann::json cameraPosesJson = ExtrinsicsToPoses(cameraExtrinsics);
  outfile << cameraPosesJson;

  // 输出物点
  OutputPointCloud(objectPoints, outputDir + "/PointCloud.ply");
}

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

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

  auto priorExtrinsics = cameraExtrinsics;  // 保存一份初始的相机外参作为先验

  IncrementalSFM(K, cameraExtrinsics, bundle, objectPoints, priorExtrinsics,
                 gcp_3d_priors);

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

关键的改动在于，需要将先验数据额外传递一份给 PnP 子问题和 BA 子问题。虽然已经传递了一份先验数据用于初值，但是在持续的迭代过程中，初值会迭代变化成估计值。所谓的先验约束就是要最小化估计值与先验值的残差组成的目标函数。

最终得到的精度报告为：

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

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

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

=== 各相机重投影误差统计 ===
相机ID	观测点数	平均误差(px)	最大误差(px)
------	--------	----------	----------
0	669		1.103996		3.215678
1	800		1.188519		3.787785
2	963		1.159849		4.606919
3	1236		1.174092		4.618646
4	1384		1.152313		4.285081
5	1273		1.165583		3.653735
6	1419		1.206832		3.813891
7	1307		1.153816		3.726210
8	1283		1.194639		3.536850
9	1125		1.150845		3.437385
10	1300		1.169682		3.954889
11	1171		1.173622		4.464807
12	1203		1.171590		3.661129
13	1249		1.186361		3.766469
14	1225		1.196754		3.776826
15	982		1.207069		3.901001
16	862		1.177507		3.678526
17	814		1.165936		3.344151
18	1163		1.132381		3.662889
19	1553		1.169632		3.966303
20	1930		1.184805		3.874274
21	2123		1.199673		3.984016
22	2463		1.169834		3.959614
23	2486		1.162714		3.801258
24	2481		1.181428		3.570980
25	2536		1.168067		4.079269
26	2507		1.177750		3.864967
27	2472		1.140818		3.580117
28	2441		1.164259		3.690751
29	2483		1.160213		4.482690
30	2484		1.163722		3.973253
31	2492		1.153696		3.652235
32	2397		1.153585		3.779775
33	2044		1.179351		3.420708
34	1734		1.143586		3.468612
35	1407		1.090445		3.222293
36	1043		1.106007		3.476004
37	1350		1.137453		4.235689
38	1859		1.166758		4.163045
39	2214		1.163615		3.670133
40	2403		1.166541		4.347165
41	2410		1.187281		4.200410
42	2455		1.185204		3.898258
43	2530		1.171803		4.109872
44	2513		1.189704		3.550072
45	2486		1.165070		3.529235
46	2473		1.188758		3.515314
47	2486		1.170048		4.463537
48	2418		1.185271		3.951601
49	2485		1.191950		3.699804
50	2481		1.158200		4.024577
51	2049		1.151223		3.677067
52	1771		1.134630		3.551807
53	1659		1.122223		3.426358
54	1354		1.141831		3.517256
55	1600		1.159034		3.661072
56	1752		1.167906		3.682431
57	2157		1.169140		3.712916
58	2391		1.162357		3.350571
59	2429		1.191694		3.807318
60	2378		1.162508		3.675286
61	2437		1.159101		3.694250
62	2497		1.168688		3.885175
63	2457		1.165065		4.148172
64	2586		1.174597		4.080618
65	2575		1.174258		4.300667
66	2599		1.160629		3.857797
67	2607		1.168975		3.567506
68	2451		1.185940		3.917070
69	2056		1.136336		3.500279
70	1671		1.165143		3.229073
71	1263		1.086242		3.411957
72	1122		1.121016		3.785196
73	1567		1.160132		4.387328
74	1955		1.171555		3.938410
75	2223		1.165945		3.942396
76	2526		1.153451		3.847215
77	2529		1.174420		3.573378
78	2509		1.189135		3.637788
79	2513		1.175341		3.682958
80	2552		1.180607		3.625518
81	2525		1.148565		3.699084
82	2513		1.154687		3.717525
83	2503		1.155892		3.675713
84	2502		1.163323		4.247894
85	2492		1.159056		3.680684
86	2346		1.154871		3.686850
87	1984		1.161799		4.278044
88	1868		1.151346		4.003326
89	1289		1.128527		3.420835
90	1075		1.042449		3.115364
91	1441		1.136836		3.438693
92	1756		1.150026		3.914118
93	2055		1.151813		3.656219
94	2242		1.121412		3.438542
95	2224		1.142633		3.558038
96	2380		1.143972		3.525655
97	2379		1.140161		3.535617
98	2219		1.137640		3.272629
99	2356		1.146977		3.896702
100	2341		1.147111		3.655515
101	2263		1.128515		3.745004
102	2241		1.126339		3.894322
103	2374		1.138700		3.597217
104	2180		1.129509		3.596745
105	1911		1.123461		3.622450
106	1699		1.109850		3.402561
107	1365		1.050805		3.232449

可以看到最终的均方根重投影误差 (RMSE)与上一篇《最小二乘问题详解18：增量式SFM核心流程实现》中的结果差距并不大。这主要是因为本文所使用的仿真数据本身过于理想化：只有正态分布的噪声而没有误匹配的外点。在这种条件下，先验信息所能带来的提升空间确实非常有限。

其实，先验约束的真正价值在于稳健性而不是单纯的精度提升。在真实的场景中，视觉观测不可避免地会受到各种干扰：弱纹理区域会导致特征匹配失败或精度下降；动态物体会引入错误的2D-2D对应关系；长距离飞行或复杂轨迹会累积不可忽视的漂移。在这些情况下，仅依赖视觉信息的优化很容易"跑偏"，导致整个重建结果失真。

而位姿先验（如INS/GNSS）和控制点先验（GCP）则扮演了"锚点"的角色。它们将优化过程牢牢地约束在物理世界的真实坐标系附近，有效防止了估值因局部视觉信息的短暂失效而发生灾难性的漂移。即使视觉残差项暂时将解推向错误的方向，先验残差项也会产生一个强大的恢复力，将其拉回合理的范围内。因此，在实际工程应用中，尤其是在挑战性环境下，这种多源信息融合的策略对于保障系统的可靠性与最终成果的绝对精度至关重要。

总而言之，本篇通过在PnP和BA两个核心环节融入先验约束，构建了一个更为健壮和贴近工业实践的增量式SFM框架。虽然在理想的仿真环境中其优势未能完全彰显，但这一设计为处理真实世界的复杂数据奠定了坚实的基础。

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