登录/ 注册
 找回密码
 立即注册
关闭

CSDN热搜

程序园 精品问答 技术交流 资源下载
返回列表 发新帖
首页 › 业界区 › 业界 ›最小二乘问题详解17：SFM仿真数据生成

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

轩辕娅童 昨天 20:56
程序园永久vip申请，无限下载程序园所有程序/软件/数据/等
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\) 。因此，我们在代码中设定的内参具有明确的物理含义，而非随意的数值：
  1. CameraIntrinsics cameraIntrinsics;
  2. cameraIntrinsics.fx = 3650; // 由物理焦距和传感器规格推导而来
  3. 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 数据的接口：
  1. struct Dem {
  2.   Dem(const std::string& demPath) { ReadDem(demPath); }
  4.   double GetZ(double x, double y) {
  5.     double lx = (x - startX) / srcDx;
  6.     double ly = (y - startY) / srcDy;
  8.     if (lx < -0.5 || lx > srcDemWidth - 0.5 || ly < -0.5 ||
  9.         ly > srcDemHeight - 0.5) {
  10.       return 0;
  11.     }
  13.     return Bilinear(lx, ly);
  14.   }
  16.   //双线性插值
  17.   double Bilinear(double lx, double ly) {
  18.     int x0 = std::min(std::max((int)floor(lx), 0), srcDemWidth - 1);
  19.     int y0 = std::min(std::max((int)floor(ly), 0), srcDemHeight - 1);
  20.     int x1 = std::min(std::max(x0 + 1, 0), srcDemWidth - 1);
  21.     int y1 = std::min(std::max(y0 + 1, 0), srcDemHeight - 1);
  23.     double u = lx - x0;
  24.     double v = ly - y0;
  26.     size_t f00 = (size_t)srcDemWidth * y0 + x0;
  27.     size_t f10 = (size_t)srcDemWidth * y0 + x1;
  28.     size_t f01 = (size_t)srcDemWidth * y1 + x0;
  29.     size_t f11 = (size_t)srcDemWidth * y1 + x1;
  31.     double value = srcDemBuf[f00] * (1 - u) * (1 - v) +
  32.                    srcDemBuf[f10] * u * (1 - v) + srcDemBuf[f01] * (1 - u) * v +
  33.                    srcDemBuf[f11] * u * v;
  35.     return value;
  36.   }
  38.   bool ReadDem(const std::string& demPath) {
  39.     GDALDataset* dem = (GDALDataset*)GDALOpen(demPath.c_str(), GA_ReadOnly);
  40.     if (!dem) {
  41.       return false;
  42.     }
  44.     srcDemWidth = dem->GetRasterXSize();
  45.     srcDemHeight = dem->GetRasterYSize();
  47.     //坐标信息
  48.     double geoTransform[6] = {0};
  49.     dem->GetGeoTransform(geoTransform);
  50.     srcDx = geoTransform[1];
  51.     srcDy = geoTransform[5];
  52.     startX = geoTransform[0] + 0.5 * srcDx;
  53.     startY = geoTransform[3] + 0.5 * srcDy;
  54.     endX = startX + (srcDemWidth - 1L) * srcDx;
  55.     endY = startY + (srcDemHeight - 1L) * srcDy;
  57.     double offsetX = (startX + endX) / 2;
  58.     double offsetY = (startY + endY) / 2;
  59.     startX -= offsetX;
  60.     startY -= offsetY;
  61.     endX -= offsetX;
  62.     endY -= offsetY;
  64.     size_t demBufNum = (size_t)srcDemWidth * srcDemHeight;
  65.     srcDemBuf.resize(demBufNum, 0);
  67.     int depth = sizeof(float);
  68.     dem->GetRasterBand(1)->RasterIO(GF_Read, 0, 0, srcDemWidth, srcDemHeight,
  69.                                     srcDemBuf.data(), srcDemWidth, srcDemHeight,
  70.                                     GDT_Float32, depth, srcDemWidth * depth);
  72.     GDALClose(dem);
  74.     return true;
  75.   }
  77.   double startX;
  78.   double startY;
  79.   double endX;
  80.   double endY;
  82.   int srcDemWidth;
  83.   int srcDemHeight;
  84.   double srcDx;
  85.   double srcDy;
  86.   std::vector<float> srcDemBuf;
  87. };
复制代码
上述代码中的 Dem 类封装了两个核心功能：

  • 数据读取与解析： 利用开源地理空间数据抽象库 GDAL 读取栅格文件（如 GeoTIFF 格式），解析其投影信息、分辨率及像素值。
  • 连续坐标映射： 由于 DEM 是离散的像素矩阵，而相机在空间中可以位于任意连续坐标点。因此，我们需要通过 双线性插值（Bilinear Interpolation） 算法，在栅格矩阵中计算任意平面坐标 \((x, y)\) 对应的高程 \(z\)。
具体来说，ReadDem 函数负责加载文件并计算地理变换参数（GeoTransform），将图像像素坐标系转换为物理世界坐标系；而 GetZ 函数则通过 Bilinear 方法，根据周围四个像素点的高程值，计算出亚像素级别的精确高程。这为我们后续在地形上生成 3D 点云提供了精确的物理约束，确保了仿真的真实感与物理一致性。
3.2 观测模型

在计算机视觉中，我们将同一个三维空间点在多张不同图像上的二维投影集合，称为一条轨迹（Track）。轨迹是连接二维图像与三维世界的桥梁，也是整个 SFM 优化问题的核心输入。SFM 的本质，就是寻找一组最优的相机位姿和三维点坐标，使得这些三维点投影到相机平面后，与观测到的二维轨迹点的 重投影误差（Reprojection Error） 最小。因此，轨迹数据的质量（如匹配的正确性、分布的均匀性）直接决定了优化的成败。
为了清晰地组织这些复杂的数据关系，我们设计了如下数据结构：
  1. #pragma once
  2. #include <Eigen/Dense>
  3. #include <nlohmann/json.hpp>
  4. #include <vector>
  6. namespace sfm {
  8. // 2D观测
  9. struct Observation {
  10.   int imgId;
  11.   int kpId;
  13.   // 序列化
  14.   friend void to_json(nlohmann::json& j, const Observation& s);
  15.   // 反序列化
  16.   friend void from_json(const nlohmann::json& j, Observation& s);
  17. };
  19. // 轨迹
  20. struct Track {
  21.   std::vector<Observation> obs;
  22.   int pointId = -1;
  24.   // 序列化
  25.   friend void to_json(nlohmann::json& j, const Track& s);
  26.   // 反序列化
  27.   friend void from_json(const nlohmann::json& j, Track& s);
  28. };
  30. // 捆绑数据
  31. struct Bundle {
  32.   std::vector<Track> tracks;
  33.   std::vector<std::vector<Eigen::Vector2d>> views;
  34.   std::vector<Eigen::Vector3d> points;
  36.   // 序列化
  37.   friend void to_json(nlohmann::json& j, const Bundle& s);
  39.   // 反序列化
  40.   friend void from_json(const nlohmann::json& j, Bundle& s);
  41. };
  43. }  // namespace sfm
复制代码
  1. #include "Bundle.h"
  3. namespace sfm {
  5. // 序列化
  6. void to_json(nlohmann::json& j, const Observation& s) {
  7.   j = nlohmann::json{{"imgId", s.imgId}, {"kpId", s.kpId}};
  8. }
  10. // 反序列化
  11. void from_json(const nlohmann::json& j, Observation& s) {
  12.   s.imgId = j.at("imgId").get<int>();
  13.   s.kpId = j.at("kpId").get<int>();
  14. }
  16. // 序列化
  17. void to_json(nlohmann::json& j, const Track& s) {
  18.   j = nlohmann::json{{"obs", s.obs}, {"pointId", s.pointId}};
  19. }
  21. // 反序列化
  22. void from_json(const nlohmann::json& j, Track& s) {
  23.   s.obs = j.at("obs").get<std::vector<Observation>>();
  24.   s.pointId = j.at("pointId").get<int>();
  25. }
  27. // 序列化
  28. void to_json(nlohmann::json& j, const Bundle& s) {
  29.   j["tracks"] = s.tracks;
  31.   nlohmann::json viewsArray = nlohmann::json::array();
  32.   for (const auto& pts : s.views) {
  33.     nlohmann::json ptsArray = nlohmann::json::array();
  34.     for (const auto& pt : pts) {
  35.       ptsArray.push_back({pt.x(), pt.y()});
  36.     }
  37.     viewsArray.push_back(ptsArray);
  38.   }
  39.   j["views"] = viewsArray;
  41.   nlohmann::json pointsArray = nlohmann::json::array();
  42.   for (const auto& pt : s.points) {
  43.     pointsArray.push_back({pt.x(), pt.y(), pt.z()});
  44.   }
  45.   j["points"] = pointsArray;
  46. }
  48. // 反序列化
  49. void from_json(const nlohmann::json& j, Bundle& s) {
  50.   s.tracks = j.at("tracks").get<std::vector<Track>>();
  52.   const auto& viewsArray = j["views"];
  53.   s.views.resize(viewsArray.size());
  54.   for (size_t i = 0; i < viewsArray.size(); ++i) {
  55.     const auto& ptsArray = viewsArray[i];
  56.     s.views[i].resize(ptsArray.size());
  57.     for (size_t j = 0; j < ptsArray.size(); ++j) {
  58.       s.views[i][j].x() = ptsArray[j][0].get<double>();
  59.       s.views[i][j].y() = ptsArray[j][1].get<double>();
  60.     }
  61.   }
  63.   const auto& pointsArray = j["points"];
  64.   s.points.resize(pointsArray.size());
  65.   for (size_t i = 0; i < pointsArray.size(); ++i) {
  66.     s.points[i].x() = pointsArray[i][0].get<double>();
  67.     s.points[i].y() = pointsArray[i][1].get<double>();
  68.     s.points[i].z() = pointsArray[i][2].get<double>();
  69.   }
  70. };
  72. }  // namespace sfm
复制代码
具体解析如下：

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

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

  • Track（轨迹）：
    一条轨迹由一个或多个 Observation 组成，它代表了同一个三维点在整个图像序列中的所有观测记录。例如，空间中一个角点在 5 张不同的照片上都被成功匹配，那么这 5 个 Observation 就共同构成了一条 Track。SFM 算法正是通过 triangulation（三角化）这些轨迹来恢复深度信息。
  • 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）作为角元素来表达相机的姿态是最为直观和易于理解的。
代码结构如下：
  1. #pragma once
  3. #include <nlohmann/json.hpp>
  5. struct CameraIntrinsics {
  6.   int width;
  7.   int height;
  9.   double fx;
  10.   double fy;
  11.   double cx;
  12.   double cy;
  14.   friend void to_json(nlohmann::json& j, const CameraIntrinsics& s) {
  15.     j = nlohmann::json{{"width", s.width}, {"height", s.height}, {"fx", s.fx},
  16.                        {"fy", s.fy},       {"cx", s.cx},         {"cy", s.cy}};
  17.   }
  19.   friend void from_json(const nlohmann::json& j, CameraIntrinsics& s) {
  20.     s.width = j.at("width").get<int>();
  21.     s.height = j.at("height").get<int>();
  22.     s.fx = j.at("fx").get<double>();
  23.     s.fy = j.at("fy").get<double>();
  24.     s.cx = j.at("cx").get<double>();
  25.     s.cy = j.at("cy").get<double>();
  26.   }
  27. };
复制代码
  1. #pragma once
  3. #include <nlohmann/json.hpp>
  5. struct CameraPose {
  6.   double px, py, pz;        // 位置
  7.   double yaw, pitch, roll;  // 姿态
  9.   friend void to_json(nlohmann::json& j, const CameraPose& s) {
  10.     j = nlohmann::json{{"px", s.px},   {"py", s.py},       {"pz", s.pz},
  11.                        {"yaw", s.yaw}, {"pitch", s.pitch}, {"roll", s.roll}};
  12.   }
  14.   friend void from_json(const nlohmann::json& j, CameraPose& s) {
  15.     s.px = j.at("px").get<double>();
  16.     s.py = j.at("py").get<double>();
  17.     s.pz = j.at("pz").get<double>();
  18.     s.yaw = j.at("yaw").get<double>();
  19.     s.pitch = j.at("pitch").get<double>();
  20.     s.roll = j.at("roll").get<double>();
  21.   }
  22. };
复制代码
具体在上述代码实现中：

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

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

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

在完成了物方数据（DEM）、观测模型（Track）以及成像参数（Camera）的定义之后，我们进入仿真的核心执行阶段。整个仿真流程遵循 “由果溯因”的逆向逻辑 ：在真实世界中，我们通过相机拍摄获取图像，再通过 SFM 算法反推相机位姿和三维结构；而在仿真中，我们预先设定真实的三维结构和相机位姿，通过几何投影模型生成带有特定噪声的二维观测数据（即图像上的特征点坐标）。
这一过程不仅验证了我们数据结构的正确性，更为后续的 SFM 算法提供了一个已知“Ground Truth（基准真值）”的测试环境，使得我们能够精确量化算法的精度损失。具体代码实现如下所示：
[code]#include #include #include #include #include #include #include #include #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 dist(a, b);  return dist(rng);}double Gauss(double sigma) {  std::normal_distribution 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 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 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
最小二乘问题详解SFM

使用道具 举报

相关推荐
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册
发帖
轩辕娅童
昨天 20:56

0

粉丝关注

24

主题发布

板块介绍填写区域，请于后台编辑
微信扫一扫