最小二乘问题详解17：SFM仿真数据生成

1. 引言

在之前的系列文章中（《最小二乘问题详解：目录》），我们由浅入深地介绍了最小二乘法这一数学工具：从线性最小二乘的正规方程，到非线性优化的 Gauss-Newton 与 Levenberg-Marquardt 算法；从视觉几何中的 PnP、三角化、本质矩阵，到最终的束平差。接下来几篇文章，我们将聚焦于 运动恢复结构（Structure from Motion, SFM） 系统中的优化流程。

所谓 SFM ，是一个旨在仅从一组无序的二维图像序列中，同时恢复出相机三维运动轨迹与场景几何结构的宏大系统。在这个系统中，我们面对的是"鸡生蛋，蛋生鸡"的困境：为了计算三维点，我们需要知道相机位姿；而为了计算相机位姿，我们又需要知道三维点。总而言之，SFM 是一个包含成百上千张图像、数以万计特征点的巨型非线性优化过程。

SFM 系统主要分为增量式 和全局式两大流派。对于初学者来说，选择增量式 SFM 作为研究对象是一个比较好的选择。因为增量式逻辑直观，易于理解，并且正好可以应用本系列前置文章中提到一系列子问题：PnP、三角化、本质矩阵，以及束平差。通过实现一套完整的 SFM 优化流程，就可以构建一套完整的、可落地的视觉几何知识体系。

然而，SFM 是一个复杂的非线性迭代过程，任何一个环节的失败都可能导致最终重建的崩溃。在真实数据中，由于缺乏真值，我们很难判断是算法逻辑出了错，还是数据本身太恶劣。因此，在正式编写增量式SFM的流水线代码之前，我们需要先生成仿真数据，即生成虚拟的3D世界、虚拟的相机运动以及虚拟的成像，并实现数据的持久化。这样我们后续的SFM模块就可以像读取真实照片一样，读取这些"仿真照片"进行测试，让我们拥有了上帝视角：在运行算法前，我们就知道答案应该是什么。

2. 理论

SFM 的本质，是从观测数据（二维图像）中估计待定参数（三维结构与相机位姿）的逆向过程。然而，生成观测数据（仿真）往往比求解逆问题更具挑战性，因为它要求我们对正向物理过程有深刻的理解。如果不按照严格的物理和几何规则生成观测数据，优化过程极易陷入局部极值甚至完全失败。

一个典型的例子就是如果 基线（Baseline） 过短（即相机移动距离太小），三角化计算出的深度值将产生巨大的不确定性（深度退化）；反之，如果基线过长，特征描述子的外观变化过大，导致特征匹配失效。因此，仿真数据的生成必须基于一套严谨的"数字摄影测量"框架。以笔者从事的行业经验来说，最直观且符合工程实践的 SFM 仿真模型，便是数字摄影测量中的无人机航飞流程。我们从相机硬件参数出发，推导到成像模型，并最终确定航线规划策略。

2.1 相机内参的物理映射

我们首先模拟常见的消费级无人机相机（如 DJI Phantom / Mavic 系列）。在物理世界中，相机由传感器尺寸和焦距定义；而在计算机视觉中，我们使用针孔相机模型（Pinhole Model）将其映射为像素坐标系下的内参矩阵 \(K\)。

物理参数：

• 焦距 (\(f\))： \(8.8 \text{ mm}\)
• 传感器尺寸： \(13.2 \text{ mm} \times 8.8 \text{ mm}\) (对应 1英寸传感器)

像素参数：

• 图像分辨率： \(5472 \times 3648\) (2000万像素)

为了将物理焦距转换为像素焦距 (\(f_x, f_y\))，我们需要计算像素尺寸 (Pixel Size)，并利用相似三角形原理建立映射关系：

\[\text{Pixel Size} = \frac{\text{Sensor Size}}{\text{Image Resolution}} \]

\[f_{\text{pixels}} = \frac{f_{\text{mm}}}{\text{Pixel Size}} \]

代入数值计算：

\[\text{Pixel Size}_x = \frac{13.2 \text{ mm}}{5472 \text{ px}} \approx 0.002412 \text{ mm/px} \]

\[f_x = \frac{8.8 \text{ mm}}{0.002412 \text{ mm/px}} \approx 3648 \text{ px} \]

同理可得 \(f_y\)，通常假设像素为正方形且无倾斜，故 \(f_x \approx f_y\) 。因此，我们在代码中设定的内参具有明确的物理含义，而非随意的数值：

CameraIntrinsics cameraIntrinsics;
cameraIntrinsics.fx = 3650; // 由物理焦距和传感器规格推导而来
cameraIntrinsics.cx = 2736; // 主点位于图像中心 (width/2)

2.2 航高、比例尺与 GSD 的几何约束

在航空摄影测量中，航高（Flight Height, \(H\)） 是决定重建精度的核心变量。它直接决定了图像的地面采样距离（GSD, Ground Sample Distance），即图像中一个像素对应的实际地面距离。

GSD 的计算公式如下：

\[\text{GSD} = \frac{H \times \text{Pixel Size}}{f} \]

参数解释：

• \(H\)：相对航高（无人机距离地面的高度）。
• \(\text{Pixel Size}\)：传感器的物理像素大小。
• \(f\)：镜头物理焦距。

我们设定航高 \(H = 120 \text{ m}\)。代入上述参数：

\[\text{GSD} = \frac{120000 \text{ mm} \times 0.002412 \text{ mm}}{8.8 \text{ mm}} \approx 32.9 \text{ mm} \]

这意味着在 120 米的高度上，图像中的每一个像素代表地面上约 3.3 厘米的细节。这是一个典型的高精度测绘航高设置（通常 GSD 在 3-5 cm 时，能清晰分辨地物细节，适合生成高精度点云）。

2.3 重叠度与基线约束（航线规划）

为了保证 SFM 能够通过三角测量恢复深度，相邻图像之间必须有足够的重叠区域 。这直接决定了相机的拍摄间距（即基线长度 \(B\)）。我们设定两个关键的摄影测量参数：

• 航向重叠度 (\(P_l\))： \(80\%\)
• 旁向重叠度 (\(P_w\))： \(60\%\)

那么，如何推导基线与重叠度的关系 呢？假设单张影像覆盖的地面宽度为 \(L\)，则相邻两张影像的中心距（基线 \(B\)）必须满足：

\[P_l = \frac{L - B}{L} \times 100\% \]

\[\Rightarrow B = L \times (1 - P_l) \]

如果重叠度过低（例如低于 60%），特征匹配容易失败，且三角化角度过小导致深度计算误差极大；如果重叠度过高（例如高于 90%），虽然匹配容易，但会导致计算资源浪费且缺乏足够的视差来精确计算深度。从这里就可以知道，既然是仿真，就不能是真的"随机"生成数据，而是需要构建一个符合物理光学定律 和摄影测量规范的虚拟现实流程。

3. 实现

3.1 物方数据

在航空摄影测量的仿真流程中，物方数据（即真实世界的三维坐标）是整个重建过程的基准（Ground Truth）。为了模拟真实的地理环境，我们采用 DEM（Digital Elevation Model，数字高程模型） 作为场景的几何载体。DEM 是一种典型的栅格数据，它通过规则的网格矩阵来记录地表的高程信息。因此，需要实现一个读取 DEM 数据的接口：

struct Dem {
  Dem(const std::string& demPath) { ReadDem(demPath); }

  double GetZ(double x, double y) {
    double lx = (x - startX) / srcDx;
    double ly = (y - startY) / srcDy;

    if (lx < -0.5 || lx > srcDemWidth - 0.5 || ly < -0.5 ||
        ly > srcDemHeight - 0.5) {
      return 0;
    }

    return Bilinear(lx, ly);
  }

  //双线性插值
  double Bilinear(double lx, double ly) {
    int x0 = std::min(std::max((int)floor(lx), 0), srcDemWidth - 1);
    int y0 = std::min(std::max((int)floor(ly), 0), srcDemHeight - 1);
    int x1 = std::min(std::max(x0 + 1, 0), srcDemWidth - 1);
    int y1 = std::min(std::max(y0 + 1, 0), srcDemHeight - 1);

    double u = lx - x0;
    double v = ly - y0;

    size_t f00 = (size_t)srcDemWidth * y0 + x0;
    size_t f10 = (size_t)srcDemWidth * y0 + x1;
    size_t f01 = (size_t)srcDemWidth * y1 + x0;
    size_t f11 = (size_t)srcDemWidth * y1 + x1;

    double value = srcDemBuf[f00] * (1 - u) * (1 - v) +
                   srcDemBuf[f10] * u * (1 - v) + srcDemBuf[f01] * (1 - u) * v +
                   srcDemBuf[f11] * u * v;

    return value;
  }

  bool ReadDem(const std::string& demPath) {
    GDALDataset* dem = (GDALDataset*)GDALOpen(demPath.c_str(), GA_ReadOnly);
    if (!dem) {
      return false;
    }

    srcDemWidth = dem->GetRasterXSize();
    srcDemHeight = dem->GetRasterYSize();

    //坐标信息
    double geoTransform[6] = {0};
    dem->GetGeoTransform(geoTransform);
    srcDx = geoTransform[1];
    srcDy = geoTransform[5];
    startX = geoTransform[0] + 0.5 * srcDx;
    startY = geoTransform[3] + 0.5 * srcDy;
    endX = startX + (srcDemWidth - 1L) * srcDx;
    endY = startY + (srcDemHeight - 1L) * srcDy;

    double offsetX = (startX + endX) / 2;
    double offsetY = (startY + endY) / 2;
    startX -= offsetX;
    startY -= offsetY;
    endX -= offsetX;
    endY -= offsetY;

    size_t demBufNum = (size_t)srcDemWidth * srcDemHeight;
    srcDemBuf.resize(demBufNum, 0);

    int depth = sizeof(float);
    dem->GetRasterBand(1)->RasterIO(GF_Read, 0, 0, srcDemWidth, srcDemHeight,
                                    srcDemBuf.data(), srcDemWidth, srcDemHeight,
                                    GDT_Float32, depth, srcDemWidth * depth);

    GDALClose(dem);

    return true;
  }

  double startX;
  double startY;
  double endX;
  double endY;

  int srcDemWidth;
  int srcDemHeight;
  double srcDx;
  double srcDy;
  std::vector<float> srcDemBuf;
};

上述代码中的 Dem 类封装了两个核心功能：

1. 数据读取与解析： 利用开源地理空间数据抽象库 GDAL 读取栅格文件（如 GeoTIFF 格式），解析其投影信息、分辨率及像素值。
2. 连续坐标映射： 由于 DEM 是离散的像素矩阵，而相机在空间中可以位于任意连续坐标点。因此，我们需要通过 双线性插值（Bilinear Interpolation） 算法，在栅格矩阵中计算任意平面坐标 \((x, y)\) 对应的高程 \(z\)。

具体来说，ReadDem 函数负责加载文件并计算地理变换参数（GeoTransform），将图像像素坐标系转换为物理世界坐标系；而 GetZ 函数则通过 Bilinear 方法，根据周围四个像素点的高程值，计算出亚像素级别的精确高程。这为我们后续在地形上生成 3D 点云提供了精确的物理约束，确保了仿真的真实感与物理一致性。

3.2 观测模型

在计算机视觉中，我们将同一个三维空间点在多张不同图像上的二维投影集合，称为一条轨迹（Track） 。轨迹是连接二维图像与三维世界的桥梁，也是整个 SFM 优化问题的核心输入。SFM 的本质，就是寻找一组最优的相机位姿和三维点坐标，使得这些三维点投影到相机平面后，与观测到的二维轨迹点的 重投影误差（Reprojection Error） 最小。因此，轨迹数据的质量（如匹配的正确性、分布的均匀性）直接决定了优化的成败。

为了清晰地组织这些复杂的数据关系，我们设计了如下数据结构：

#pragma once
#include <Eigen/Dense>
#include <nlohmann/json.hpp>
#include <vector>

namespace sfm {

// 2D观测
struct Observation {
  int imgId;
  int kpId;

  // 序列化
  friend void to_json(nlohmann::json& j, const Observation& s);
  // 反序列化
  friend void from_json(const nlohmann::json& j, Observation& s);
};

// 轨迹
struct Track {
  std::vector<Observation> obs;
  int pointId = -1;

  // 序列化
  friend void to_json(nlohmann::json& j, const Track& s);
  // 反序列化
  friend void from_json(const nlohmann::json& j, Track& s);
};

// 捆绑数据
struct Bundle {
  std::vector<Track> tracks;
  std::vector<std::vector<Eigen::Vector2d>> views;
  std::vector<Eigen::Vector3d> points;

  // 序列化
  friend void to_json(nlohmann::json& j, const Bundle& s);

  // 反序列化
  friend void from_json(const nlohmann::json& j, Bundle& s);
};

}  // namespace sfm

#include "Bundle.h"

namespace sfm {

// 序列化
void to_json(nlohmann::json& j, const Observation& s) {
  j = nlohmann::json{{"imgId", s.imgId}, {"kpId", s.kpId}};
}

// 反序列化
void from_json(const nlohmann::json& j, Observation& s) {
  s.imgId = j.at("imgId").get<int>();
  s.kpId = j.at("kpId").get<int>();
}

// 序列化
void to_json(nlohmann::json& j, const Track& s) {
  j = nlohmann::json{{"obs", s.obs}, {"pointId", s.pointId}};
}

// 反序列化
void from_json(const nlohmann::json& j, Track& s) {
  s.obs = j.at("obs").get<std::vector<Observation>>();
  s.pointId = j.at("pointId").get<int>();
}

// 序列化
void to_json(nlohmann::json& j, const Bundle& s) {
  j["tracks"] = s.tracks;

  nlohmann::json viewsArray = nlohmann::json::array();
  for (const auto& pts : s.views) {
    nlohmann::json ptsArray = nlohmann::json::array();
    for (const auto& pt : pts) {
      ptsArray.push_back({pt.x(), pt.y()});
    }
    viewsArray.push_back(ptsArray);
  }
  j["views"] = viewsArray;

  nlohmann::json pointsArray = nlohmann::json::array();
  for (const auto& pt : s.points) {
    pointsArray.push_back({pt.x(), pt.y(), pt.z()});
  }
  j["points"] = pointsArray;
}

// 反序列化
void from_json(const nlohmann::json& j, Bundle& s) {
  s.tracks = j.at("tracks").get<std::vector<Track>>();

  const auto& viewsArray = j["views"];
  s.views.resize(viewsArray.size());
  for (size_t i = 0; i < viewsArray.size(); ++i) {
    const auto& ptsArray = viewsArray[i];
    s.views[i].resize(ptsArray.size());
    for (size_t j = 0; j < ptsArray.size(); ++j) {
      s.views[i][j].x() = ptsArray[j][0].get<double>();
      s.views[i][j].y() = ptsArray[j][1].get<double>();
    }
  }

  const auto& pointsArray = j["points"];
  s.points.resize(pointsArray.size());
  for (size_t i = 0; i < pointsArray.size(); ++i) {
    s.points[i].x() = pointsArray[i][0].get<double>();
    s.points[i].y() = pointsArray[i][1].get<double>();
    s.points[i].z() = pointsArray[i][2].get<double>();
  }
};

}  // namespace sfm

具体解析如下：

1. Observation（观测）：

   这是最基础的单元，代表一个三维点在某一张特定图像上的二维投影。它包含了两个关键索引：

   • imgId：图像的唯一标识符，指向某个相机的位姿。
   • kpId：该图像上特征点（Keypoint）的唯一标识符，指向一个具体的像素坐标。
     一个 Observation 对象就是一条"从相机到空间点"的观测射线。

2. Track（轨迹）：

   一条轨迹由一个或多个 Observation 组成，它代表了同一个三维点 在整个图像序列中的所有观测记录。例如，空间中一个角点在 5 张不同的照片上都被成功匹配，那么这 5 个 Observation 就共同构成了一条 Track。SFM 算法正是通过 triangulation（三角化）这些轨迹来恢复深度信息。

3. Bundle（捆绑数据）：

   这是整个仿真系统的最终输出，也是后续 SFM 流水线的输入。它将所有数据整合在一起：

   • tracks：所有轨迹的集合，构成了优化的观测数据。
   • points：三维点的真实坐标（Ground Truth），用于后续评估算法精度。
   • views：每张图像上所有二维特征点的集合，是 tracks 中观测的原始数据视图。

通过这种结构化的组织方式，我们将复杂的"图像-特征点-三维点"关系梳理得清晰明了。Bundle 结构体不仅方便进行序列化（如通过 nlohmann/json 库保存为文件），实现数据的持久化，更重要的是，它为后续的增量式 SFM 算法提供了一个标准、高效的"数据接口"，使得我们可以像搭积木一样，将之前实现的 PnP、三角化、BA 等模块无缝集成到这个框架中。

3.3 成像参数

接下来，我们需要定义成像的"眼睛"------即相机参数。在摄影测量和计算机视觉中，相机参数通常被严格划分为内参（Intrinsics） 和外参（Extrinsics）。

内参 描述了相机的光学特性，如焦距、主点位置等。正如我们在第2节理论部分所推导的，内参矩阵 \(K\) 由物理焦距和传感器尺寸决定。在实际的工程应用中，内参通常被视为已知量。我们一般会通过预先的 相机标定（Camera Calibration） 流程（如张正友标定法）获取，或者在仿真中直接根据相机型号（如 DJI Phantom 4 RTK）的出厂参数设定。因此，在 SFM 的优化过程中，内参通常保持不变，不是优化的重点。

外参 则是 SFM 求解的核心目标。它描述了相机在世界坐标系中的空间姿态，包含三个线元素 （位置 \(X, Y, Z\)）和三个角元素 （姿态 \(\omega, \phi, \kappa\) 或 Yaw, Pitch, Roll）。在数学上，这对应于投影方程中的旋转矩阵 \(R\) 和平移向量 \(t\)。旋转矩阵 \(R\) 的表达方式非常多，但是以三个欧拉角（Euler Angles）作为角元素来表达相机的姿态是最为直观和易于理解的。

代码结构如下：

#pragma once

#include <nlohmann/json.hpp>

struct CameraIntrinsics {
  int width;
  int height;

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

  friend void to_json(nlohmann::json& j, const CameraIntrinsics& s) {
    j = nlohmann::json{{"width", s.width}, {"height", s.height}, {"fx", s.fx},
                       {"fy", s.fy},       {"cx", s.cx},         {"cy", s.cy}};
  }

  friend void from_json(const nlohmann::json& j, CameraIntrinsics& s) {
    s.width = j.at("width").get<int>();
    s.height = j.at("height").get<int>();
    s.fx = j.at("fx").get<double>();
    s.fy = j.at("fy").get<double>();
    s.cx = j.at("cx").get<double>();
    s.cy = j.at("cy").get<double>();
  }
};

#pragma once

#include <nlohmann/json.hpp>

struct CameraPose {
  double px, py, pz;        // 位置
  double yaw, pitch, roll;  // 姿态

  friend void to_json(nlohmann::json& j, const CameraPose& s) {
    j = nlohmann::json{{"px", s.px},   {"py", s.py},       {"pz", s.pz},
                       {"yaw", s.yaw}, {"pitch", s.pitch}, {"roll", s.roll}};
  }

  friend void from_json(const nlohmann::json& j, CameraPose& s) {
    s.px = j.at("px").get<double>();
    s.py = j.at("py").get<double>();
    s.pz = j.at("pz").get<double>();
    s.yaw = j.at("yaw").get<double>();
    s.pitch = j.at("pitch").get<double>();
    s.roll = j.at("roll").get<double>();
  }
};

具体在上述代码实现中：

1. CameraIntrinsics（相机内参）：

   该结构体封装了我们在第2节中计算得出的物理参数。其中 fx, fy 是以像素为单位的焦距，cx, cy 是主点坐标。这些参数构成了内参矩阵 \(K\)，负责将相机坐标系下的三维点投影到二维图像平面上。

2. CameraPose（相机外参）：

   该结构体记录了相机在仿真环境中的位姿。

   • 线元素 (px, py, pz)： 对应平移向量 \(t\)，表示相机光心在世界坐标系中的位置。
   • 角元素 (yaw, pitch, roll)： 对应旋转矩阵 \(R\)。在仿真生成阶段，我们根据航线规划计算出这些角度；在 SFM 求解阶段，算法的目标就是尽可能准确地恢复出这些值。

通过分离内参与外参，我们不仅符合针孔相机模型的数学定义，也可以在后续引入"带噪声的初始值"。在真实的 SFM 流程中，我们往往拥有带有 GPS/IMU 误差的外参初始值，而 SFM 优化的过程，正是对这些外参进行精修的过程。

3.4 仿真流程

在完成了物方数据（DEM）、观测模型（Track）以及成像参数（Camera）的定义之后，我们进入仿真的核心执行阶段。整个仿真流程遵循 "由果溯因"的逆向逻辑 ：在真实世界中，我们通过相机拍摄获取图像，再通过 SFM 算法反推相机位姿和三维结构；而在仿真中，我们预先设定真实的三维结构和相机位姿，通过几何投影模型生成带有特定噪声的二维观测数据（即图像上的特征点坐标）。

这一过程不仅验证了我们数据结构的正确性，更为后续的 SFM 算法提供了一个已知"Ground Truth（基准真值）"的测试环境，使得我们能够精确量化算法的精度损失。具体代码实现如下所示：

#include <gdal_priv.h>

#include <Eigen/Dense>
#include <algorithm>
#include <fstream>
#include <iostream>
#include <map>
#include <random>
#include <set>

#include "Bundle.h"
#include "CameraIntrinsics.hpp"
#include "CameraPose.hpp"
#include "Dem.hpp"

using namespace std;

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

std::mt19937 rng(0);

double Rand(double a, double b) {
  std::uniform_real_distribution<double> dist(a, b);
  return dist(rng);
}

double Gauss(double sigma) {
  std::normal_distribution<double> dist(0.0, sigma);
  return dist(rng);
}

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

vector<CameraPose> GenerateCameras(Dem& dem, double flightHeight,
                                   double focalLength, double sensorWidth,
                                   double sensorHeight, double forwardOverlap,
                                   double sideOverlap) {
  // 单张图片覆盖范围
  double footprintWidth = flightHeight / focalLength * sensorWidth;
  double footprintHeight = flightHeight / focalLength * sensorHeight;

  // 宽度与航向垂直，尽量减少航线
  double spacingX = footprintHeight * (1 - forwardOverlap);  // 航向间距
  double spacingY = footprintWidth * (1 - sideOverlap);      // 航线间距

  // 航点
  int waypointCols = (int)round((dem.endX - dem.startX) / spacingX);
  int waypointRows = (int)round((dem.startY - dem.endY) / spacingY);

  vector<CameraPose> cameraPoseList;

  for (int i = 0; i < waypointRows; i++) {
    for (int j = 0; j < waypointCols; j++) {
      double x = dem.startX + j * spacingX;
      double y = dem.startY - i * spacingY;

      double zGround = dem.GetZ(x, y);
      double z = zGround + flightHeight;

      CameraPose cameraPose;

      // 相机位置
      Eigen::Vector3d C(x, y, z);

      // 姿态扰动
      cameraPose.pitch = Rand(-3.0, 3.0) * M_PI / 180.0;
      cameraPose.roll = Rand(-2.0, 2.0) * M_PI / 180.0;
      double yaw;
      if (i % 2 == 0)
        yaw = 0;  // 偶数行：正方向
      else
        yaw = M_PI;  // 奇数行：反方向(蛇形航线)
      yaw += Rand(-5.0, 5.0) * M_PI / 180.0;
      cameraPose.yaw = yaw;

      //
      Eigen::Matrix3d R = Rz(cameraPose.yaw) * Ry(cameraPose.pitch) *
                          Rx(cameraPose.roll) * R_down;

      Eigen::Vector3d t = -R * C;
      cameraPose.px = t.x();
      cameraPose.py = t.y();
      cameraPose.pz = t.z();

      cameraPoseList.push_back(cameraPose);
    }
  }

  cout << "Camera count: " << cameraPoseList.size() << endl;

  return cameraPoseList;
}

void GeneratePoints(Dem& dem, int N, sfm::Bundle& bundle) {
  bundle.tracks.resize(N);
  bundle.points.resize(N);

  for (int i = 0; i < N; i++) {
    double x = Rand(dem.startX, dem.endX);
    double y = Rand(dem.endY, dem.startY);
    double z = dem.GetZ(x, y);

    bundle.tracks[i].pointId = i;
    bundle.points[i].x() = x;
    bundle.points[i].y() = y;
    bundle.points[i].z() = z;
  }

  cout << "Points count: " << bundle.points.size() << endl;
}

bool Project(const CameraPose& cam, const Eigen::Vector3d& Pw,
             const Eigen::Matrix3d& K, Eigen::Vector2d& uv, int width,
             int height) {
  Eigen::Matrix3d R = Rz(cam.yaw) * Ry(cam.pitch) * Rx(cam.roll) * R_down;
  Eigen::Vector3d Pc = R * Pw + Eigen::Vector3d(cam.px, cam.py, cam.pz);

  if (Pc.z() <= 0) return false;

  double u = K(0, 0) * Pc.x() / Pc.z() + K(0, 2);
  double v = K(1, 1) * Pc.y() / Pc.z() + K(1, 2);

  if (u < 0 || u >= width || v < 0 || v >= height) return false;

  uv = Eigen::Vector2d(u, v);

  return true;
}

void GenerateObservations(const CameraIntrinsics& cameraIntrinsics,
                          const vector<CameraPose>& cameraPoses,
                          sfm::Bundle& bundle, double noise) {
  // 内参矩阵
  Eigen::Matrix3d K;
  K << cameraIntrinsics.fx, 0.0, cameraIntrinsics.cx, 0.0, cameraIntrinsics.fy,
      cameraIntrinsics.cy, 0.0, 0.0, 1.0;

  bundle.views.resize(cameraPoses.size());

  for (int pi = 0; pi < bundle.tracks.size(); pi++) {
    auto& track = bundle.tracks[pi];

    for (int ci = 0; ci < cameraPoses.size(); ci++) {
      Eigen::Vector2d uv;

      if (!Project(cameraPoses[ci], bundle.points[track.pointId], K, uv,
                   cameraIntrinsics.width, cameraIntrinsics.height))
        continue;

      uv.x() += Gauss(noise);
      uv.y() += Gauss(noise);

      track.obs.push_back({ci, (int)bundle.views[ci].size()});
      bundle.views[ci].push_back(uv);
    }
  }
}

void Output(const string& outputDir, CameraIntrinsics& cameraIntrinsics,
            vector<CameraPose>& cameraPoses,
            vector<CameraPose>& cameraPosesNoise, sfm::Bundle& bundle) {
  nlohmann::json cameraIntrinsicsJson = cameraIntrinsics;
  string cameraIntrinsicsFilePath = outputDir + "/CameraIntrinsics.json";
  {
    ofstream outfile(cameraIntrinsicsFilePath);
    outfile << cameraIntrinsicsJson.dump(2);
  }

  nlohmann::json cameraPosesJson = cameraPoses;
  string cameraPosesFilePath = outputDir + "/CameraPoses.json";
  {
    ofstream outfile(cameraPosesFilePath);
    outfile << cameraPosesJson.dump(2);
  }
  nlohmann::json cameraPosesNoiseJson = cameraPosesNoise;
  string cameraPosesNoiseFilePath = outputDir + "/CameraPosesNoise.json";
  {
    ofstream outfile(cameraPosesNoiseFilePath);
    outfile << cameraPosesNoiseJson.dump(2);
  }

  nlohmann::json bundleJson = bundle;
  string bundleFilePath = outputDir + "/Bundle.json";
  {
    ofstream outfile(bundleFilePath);
    outfile << bundleJson.dump(2);
  }
}

vector<CameraPose> AddNoise(const vector<CameraPose>& gtPoses) {
  vector<CameraPose> noisy = gtPoses;

  for (auto& cam : noisy) {
    // 位移噪声（RTK级）
    cam.px += Gauss(0.02);
    cam.py += Gauss(0.02);
    cam.pz += Gauss(0.05);

    // 姿态噪声（IMU级）
    cam.yaw += Gauss(0.05 * M_PI / 180.0);
    cam.pitch += Gauss(0.02 * M_PI / 180.0);
    cam.roll += Gauss(0.02 * M_PI / 180.0);
  }

  return noisy;
}

int main() {
  GDALAllRegister();
  CPLSetConfigOption("PROJ_LIB", "C:/Github/GISBasic3rdParty/share/proj");

  string demPath = "D:/Data/SFM/SurveyingArea.tif";
  string outputDir = "D:/Data/SFM";

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

  // 模拟常见无人机相机 DJI Phantom / Mavic
  // 传感器尺寸 13.2 mm × 8.8 mm，f ≈ 8.8 mm
  double focalLength = 0.0088;
  double sensorWidth = 0.0132;
  double sensorHeight = 0.0088;

  // 确定相机内参
  CameraIntrinsics cameraIntrinsics;
  cameraIntrinsics.width = 5472;
  cameraIntrinsics.height = 3648;
  cameraIntrinsics.fx = 3650;
  cameraIntrinsics.fy = 3650;
  cameraIntrinsics.cx = 2736;
  cameraIntrinsics.cy = 1824;

  // 定行高120米，GSD大约为 3--4 cm；GSD = H * pixel_size / f
  double flightHeight = 120.0;

  // 航测覆盖率
  double forwardOverlap = 0.8;  // 航向重叠
  double sideOverlap = 0.6;     // 航线重叠

  // 3D 点的个数
  int point3DNum = 20000;

  // 像点噪声
  double uvNoise = 1.0;

  // 加载DEM
  Dem dem(demPath);

  vector<CameraPose> cameraPoses =
      GenerateCameras(dem, flightHeight, focalLength, sensorWidth, sensorHeight,
                      forwardOverlap, sideOverlap);

  sfm::Bundle bundle;

  GeneratePoints(dem, point3DNum, bundle);

  GenerateObservations(cameraIntrinsics, cameraPoses, bundle, uvNoise);

  // 生成带噪声的位姿，模拟INS+RTK
  auto cameraPosesNoise = AddNoise(cameraPoses);

  Output(outputDir, cameraIntrinsics, cameraPoses, cameraPosesNoise, bundle);
}

整个仿真流程主要包含以下四个核心步骤：

1. 环境初始化与航线规划 (GenerateCameras)

仿真的第一步是构建场景的"观测者"。我们基于输入的 DEM 数据范围，模拟无人机航空摄影测量的蛇形航线（Lawnmower Pattern）。

• 覆盖计算： 根据设定的飞行高度（flightHeight）和相机内参（焦距、像幅），计算单张影像的地面覆盖范围（Footprint）。
• 基线确定： 根据用户设定的航向重叠度（forwardOverlap）和旁向重叠度（sideOverlap），计算相邻航线之间的间距（spacingX, spacingY）和航点间隔。
• 位姿生成： 遍历生成航点网格。在生成过程中，我们不仅赋予相机理想的位置（\(X, Y, Z\)），还引入了微小的姿态扰动 （Pitch, Roll, Yaw 的随机抖动），以模拟真实飞行中气流对无人机姿态的影响。同时，通过欧拉角构建旋转矩阵 \(R\)，并计算对应的平移向量 \(t\)，填充 CameraPose 结构。

2. 物方采样与场景构建 (GeneratePoints)

有了观测者，我们需要构建被观测的场景。不同于真实世界中复杂的物体，为了简化计算并保证点云分布的均匀性，我们在 DEM 定义的矩形区域内进行随机均匀采样。

• 点云生成： 在 DEM 的 \(X, Y\) 范围内随机生成 \(N\) 个点（代码中由 point3DNum 控制），并通过 Dem::GetZ 接口获取该平面坐标对应的高程 \(Z\)。
• 数据存储： 这些点构成了 Bundle 中的 points 集合。此时，我们已经拥有了一个已知坐标的"虚拟地球"和一组已知位姿的"虚拟相机"。

3. 几何投影与观测生成 (Project & GenerateObservations)

这是仿真的核心物理过程，即透视投影（Perspective Projection）。我们模拟光线从三维空间点投射到二维成像平面的过程。

• 可见性判断： 对于每一个三维点，遍历所有相机位姿。利用相机的内参矩阵 \(K\) 和外参矩阵 \([R|t]\)，计算该点在图像上的投影坐标 \((u, v)\)。
• 观测构建： 如果投影点位于图像边界内（0 < u < width, 0 < v < height），则认为该点在该视角下可见。我们将该二维坐标存入 views，并将对应的索引关系（图像 ID 和关键点 ID）记录为一条 Observation，并归入对应的 Track。
• 重投影误差的源头： 这一步生成的 \((u, v)\) 是理想无误差的观测值。

4. 噪声注入与数据持久化 (AddNoise & Output)

为了模拟真实的计算机视觉环境，我们必须打破"完美世界"。在输出前，我们对数据进行了双重污染：

• 像点噪声（Image Space Noise）： 在生成的二维坐标 \((u, v)\) 上叠加高斯白噪声（代码中的 uvNoise），模拟特征点提取算法（如 SIFT, ORB）的定位误差以及图像的像素量化误差。
• 位姿噪声（Pose Space Noise）： 对生成的相机外参（CameraPose）叠加高斯噪声，模拟 GNSS（定位误差）和 IMU（姿态误差）传感器的测量漂移。
• 数据输出： 最后，我们将纯净的 Ground Truth（用于算法评估）、带噪的初始值（用于算法输入）以及观测数据（Tracks）统一序列化为 JSON 格式输出。

通过上述流程，我们成功构建了一个 已知真值、可控变量、逼近真实 的 SFM 仿真数据集。这不仅为算法调试提供了基准，也让我们能够深入探究不同噪声水平对重建结果的具体影响。

3.5 外参解算

给定相机外参的具体实现代码如下所示：

vector<CameraPose> cameraPoseList;

for (int i = 0; i < waypointRows; i++) {
  for (int j = 0; j < waypointCols; j++) {
    double x = dem.startX + j * spacingX;
    double y = dem.startY - i * spacingY;

    double zGround = dem.GetZ(x, y);
    double z = zGround + flightHeight;

    CameraPose cameraPose;

    // 相机位置
    Eigen::Vector3d C(x, y, z);

    // 姿态扰动
    cameraPose.pitch = Rand(-3.0, 3.0) * M_PI / 180.0;
    cameraPose.roll = Rand(-2.0, 2.0) * M_PI / 180.0;
    double yaw;
    if (i % 2 == 0)
      yaw = 0;  // 偶数行：正方向
    else
      yaw = M_PI;  // 奇数行：反方向(蛇形航线)
    yaw += Rand(-5.0, 5.0) * M_PI / 180.0;
    cameraPose.yaw = yaw;

    //
    Eigen::Matrix3d R = Rz(cameraPose.yaw) * Ry(cameraPose.pitch) *
                        Rx(cameraPose.roll) * R_down;

    Eigen::Vector3d t = -R * C;
    cameraPose.px = t.x();
    cameraPose.py = t.y();
    cameraPose.pz = t.z();

    cameraPoseList.push_back(cameraPose);
  }
}

这段代码其实涉及到两个关键性问题：坐标系对齐和投影公式转换。

1. 坐标系对齐

在航空摄影测量中，我们通常假设相机是竖直朝下拍摄的（Nadir Looking）。然而，计算机视觉（OpenCV/SFM）与摄影测量的坐标系定义存在差异：

• 摄影测量坐标系（物方）： \(Z\) 轴通常指向上方（天顶）。
• 计算机视觉坐标系（像方）： 光轴为 \(Z\) 轴，且指向成像平面的前方 （即向下）；\(Y\) 轴通常指向下（屏幕坐标系惯例）。

为了消除这种定义差异，我们需要一个初始对齐矩阵 \(R_{down}\)。代码中定义的 R_down 实际上是一个坐标系翻转矩阵，其作用是将相机的默认姿态调整为"镜头垂直于地面"：

\[R_{down} = \begin{bmatrix} 1 & 0 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & -1 \end{bmatrix} \]

它将相机的 \(Y\) 轴和 \(Z\) 轴进行翻转，确保当欧拉角为 0 时，相机光轴与重力方向平行，从而实现标准的竖直摄影姿态。

2. 投影公式转换

在代码中，C 是我们在地图上规划的相机位置（物方坐标），而 t 是 SFM 投影公式 \(P = K[R|t]\) 中的平移向量。为什么 C 不能直接等于t呢？其实这两者在数学定义上是相反的。

根据坐标变换原理，世界坐标系到相机坐标系的转换公式为：

\[P_c = R_{w}^{c} \cdot (P_w - C) \]

其中， \(R_{w}^{c}\) 是从世界到相机的旋转矩阵。

将公式展开：

\[P_c = R_{w}^{c} \cdot P_w - R_{w}^{c} \cdot C \]

为了将其转换为标准的 \(P_c = R \cdot P_w + t\) 形式，于是我们就得到了：

\[t = -R_{w}^{c} \cdot C \]

我们可以总结成如下几点：

• \(C\) (Camera Center)： 是相机在世界中的位置（我们规划的点）。
• \(t\) (Translation)： 是世界原点在相机坐标系中的位置。
• 转换关系： 两者互为旋转后的反向向量。代码中的 R 即 \(R_{w}^{c}\)，因此必须通过 t = -R * C 计算出投影矩阵所需的平移分量。

4. 问题

尽管上述仿真系统已经能够生成符合物理规律和摄影测量规范的数据，并且足以支撑基础 SFM 流水线的验证，但我们必须清醒地认识到，当前的实现仍然处于理想化模型阶段。为了保持代码的简洁性和初期调试的稳定性，我们暂时屏蔽了真实场景中常见的极端干扰因素。

在正式进入鲁棒性（Robustness）测试之前，以下是目前仿真流程中刻意简化 或尚未覆盖的关键场景，这些将是后续进阶测试的重点方向：

4.1 缺乏外点（Outliers）模拟

在真实的特征匹配阶段，由于场景中存在重复纹理、动态物体或光照变化，匹配算法（如 SIFT, ORB）会产生大量错误的匹配点，即外点。

• 现状： 目前的仿真中，每一条轨迹（Track）都是由真实存在的三维点投影生成的，匹配正确率是 100%。
• 待优化： 后续需要引入随机的"虚假对应关系"，即在某些图像中随机生成二维点，强制它们与其他三维点形成错误的观测。这将用于测试 RANSAC 或其他鲁棒估计算法剔除外点的能力。

4.2 缺乏漏匹配（Missed Matches）机制

真实世界中的特征检测具有不稳定性。例如，当相机快速运动导致图像模糊，或者场景中存在大面积弱纹理区域（如白墙、天空）时，特征点检测会失效，导致本应存在的观测数据丢失。

• 现状： 只要三维点在视锥内（Frustum Culling），代码就会强制生成观测值。这是一种"完美检测"假设。
• 待优化： 需要引入 漏检率（Dropout Rate） 模型。根据点在图像中的位置（如边缘模糊）、深度角（视差过小）或随机概率，主动丢弃部分合法的观测数据，模拟特征提取失败的情况。

4.3 基线（Baseline）控制缺失

基线（即相邻两张照片拍摄位置之间的距离）是 SFM 中决定深度估计精度的核心几何约束。

• 现状： 目前的航线规划仅基于重叠度计算了固定的网格间距。虽然这保证了基本的连接性，但没有对极短基线 （Short Baseline）或长基线（Large Parallax）进行专门的控制。
• 风险： 如果基线过短（例如相机移动距离太小），三角化计算出的深度值将具有极高的不确定性（深度退化），导致重建失败。
• 待优化： 未来需要加入动态基线检查，确保参与初始化的图像对具有足够的视差，或者模拟由于基线过短导致的深度估计误差极大的病理情况。

5. 测试

在实际的工程应用与算法验证中，SFM 并不是一个孤立的算法，而是与外部传感器数据紧密耦合的系统。利用我们构建的仿真框架，可以模拟出多种不同的"先验信息"条件，从而验证算法在不同数据完备度下的表现。

局限于经典 SFM 场景，我们将测试情景按照约束条件的强弱 和数据来源的多少，划分为以下四个层级：

5.1 纯视觉（Pure 2D-2D）------ "鸡生蛋蛋生鸡"问题

这是最经典的 SFM 场景，也是所有算法的基准测试（Baseline）。

• 输入数据： 仅有图像序列及其提取的二维特征点坐标（Track）。
• 约束条件： 仅依靠特征点匹配（2D-2D Correspondences）。
• 求解难点： 尺度模糊（Scale Ambiguity）。纯视觉无法确定场景的真实物理尺度（米），重建结果是一个与真实场景相似的几何体（Metric Reconstruction up to a Scale）。
• 测试目的： 验证基础流水线（增量式/全局式）的几何一致性，以及在无任何先验信息下的鲁棒性。

5.2 摄影测量平差（2D-2D + 控制点）------ 引入绝对基准

在传统测绘中，我们通过实地测量少量关键点的坐标来约束整个模型。

• 输入数据： 二维特征点 + 控制点（GCPs, Ground Control Points）。
• 控制点定义： 少量的三维点，其在图像上的位置（2D）和真实地理坐标（3D）均为已知。
• 约束条件： 强制将重建的稀疏/密集点云"贴合"到真实的地理坐标系（WGS84 / UTM）中。
• 测试目的： 验证算法的 绝对定向（Absolute Orientation） 能力。测试 SFM 结果如何通过最小二乘平差，利用控制点消除累积误差，将模型从"局部坐标系"转换到"物方坐标系"。

5.3 视觉惯性融合（2D-2D + INS/GNSS）------ 引入相对先验

现代无人机和移动设备通常带有 IMU（惯性测量单元）和 GPS。

• 输入数据： 二维特征点 + 初始外参（Prior Poses）。
• 先验数据： GNSS 提供的粗略位置（经纬高）和 IMU 提供的姿态（yaw, pitch, roll）。
• 约束条件： 软约束（Soft Constraint） 。这些传感器数据通常带有漂移和噪声（如代码中的 AddNoise），不能直接作为真值，但可以作为优化的初始值或正则化项。
• 测试目的： 验证 Visual-Inertial SFM 的能力。测试算法如何在 Bundle Adjustment 中，将视觉重投影误差与 GNSS/IMU 的位置误差进行联合优化（Joint Optimization），从而解决纯视觉的尺度漂移问题，并加速稀疏非线性方程的收敛。

5.4 混合约束（2D-2D + 控制点 + INS/GNSS）

这是最接近真实高精度测绘工程的场景。

• 输入数据： 二维特征点 + 控制点 + 初始外参。
• 测试目的： 全网平差（Full Network Adjustment）。测试系统在拥有多种冗余信息时的处理能力。例如，利用 RTK-GNSS 提供的初始轨迹作为初值，利用控制点作为强约束，利用视觉特征作为主要连接点，进行全局的最小二乘平差。

6. 总结

本文详细构建了一套基于数字摄影测量原理的 SFM 仿真框架：从物理光学的角度出发，完成了从 DEM 地形建模、相机内参推导到航线外参规划的全流程。虽然比较麻烦，但是为了精准地验证逆向求解算法，构建一个逻辑严密的正向生成模型还是非常值得的。再后续几篇文章中，依托这一套仿真系统生成的数据，笔者将详细讲解前文提到的四大测试情景。

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