思路在确立了“稀疏 GCP 约束”这一核心目标后我们需要解决的关键问题是如何将少量的 GCP 观测值有效地融合进增量式 SfM 的优化流程中通常来说处理这类多源信息融合问题主要有两种常规思路。一种是在优化过程中实时引入约束另一种则是先构建相对模型再进行整体对齐。1. 增量式联合优化第一种思路是边重建边约束。即在增量式 SfM 的每一步迭代中无论是 PnP 位姿估计还是局部/全局 BA都将 GCP 作为一个特殊的 3D 点加入优化目标函数。实现逻辑在构建 BA 问题时为 GCP 对应的 3D 点添加一个额外的残差项GCP 坐标先验误差。随着新图像的注册和新点的三角化GCP 的约束力会实时地“拉扯”整个模型试图将其固定在绝对坐标系中。面临的挑战这种方案在理论上很完美但在稀疏 GCP场景下实现难度极大。权重难以平衡GCP 数量极少可能只有几个而视觉重投影残差数量巨大成千上万。在优化初期模型漂移严重GCP 的微弱约束力很容易被海量的视觉残差“淹没”导致无法生效若强行增加 GCP 权重又可能导致模型为了迎合 GCP 而发生局部扭曲。冷启动困难在重建的初始阶段自由网尚未形成稳定的几何结构此时引入强约束容易导致优化器陷入局部极小值甚至直接导致初始化失败。2. 两步法绝对定向第二种思路是先重建后对齐。即先完全忽略 GCP按照第 20 篇的方法完成纯视觉的自由网平差待模型内部结构稳定后再利用 GCP 计算一个全局变换矩阵将整个模型“搬”到绝对坐标系中。实现逻辑利用第 20 篇生成的自由网模型提取 GCP 对应的 3D 点坐标与 GCP 的真实坐标进行匹配求解一个Sim3相似变换矩阵。面临的挑战这种方案实现简单且能保证内部结构的完整性。但它存在一个理论上的缺陷——刚性假设。Sim3 变换假设自由网与真实世界之间仅存在线性的相似变换关系旋转、平移、统一尺度。然而实际的重建误差往往是非线性的例如镜头畸变校正残差、累积漂移导致的弯曲等。简单的 Sim3 变换无法消除这些非线性变形只能做到“整体对齐”而无法做到“局部贴合”。3. Sim3引导的联合优化为了兼顾实现的鲁棒性与结果的绝对精度本文最终采用了一种结合上述两者之长的三步走策略。该方案利用方案二的 Sim3 结果为方案一提供完美的初值从而规避了方案一“冷启动”和“权重平衡”的难题。具体流程如下第一步纯视觉自由网平差完全沿用第 20 篇的流程不引入任何 GCP 信息独立完成增量式 SfM。此时我们获得了一个内部几何结构高度一致重投影误差极低但坐标系任意的 3D 模型。第二步Sim3 绝对定向在自由网平差完成后利用已知的 GCP 坐标与模型中对应的 3D 点计算 Sim3 变换矩阵将所有相机位姿和 3D 点一次性变换到绝对坐标系下。这一步解决了尺度、旋转和平移的 7 自由度不确定性问题。第三步带 GCP 约束的全局 BA将 Sim3 对齐后的结果作为初值再次运行全局光束法平差。此时我们在优化函数中加入 GCP 的坐标先验残差项。关键点由于初值已经非常接近真实值得益于 Sim3 对齐优化器不再需要处理巨大的漂移GCP 的约束项可以平滑地修正模型中的非线性误差如微小的弯曲实现视觉结构与地理坐标的完美统一。这种策略既保留了纯视觉 SfM 的灵活性又利用 Sim3 解决了绝对定向的初值问题最后通过联合 BA 榨干了 GCP 的精度潜力是目前工程实践中处理稀疏 GCP 最稳健的方案。三、Sim3问题在完成了纯视觉自由网平差后我们获得了一个内部几何结构完美但处于任意坐标系下的 3D 模型。为了将其“锚定”到真实世界坐标系中我们需要求解一个三维空间的相似变换Similarity Transformation, Sim3。在摄影测量学中这一过程被称为绝对定向Absolute Orientation。其本质是寻找一个 7 自由度的变换3 个旋转、3 个平移、1 个统一尺度使得自由网中的点集 Xsfm 与真实世界中的控制点集 Xgcp 之间的误差最小化。1. 数学模型Sim3 变换的数学模型可以表示为Xworlds⋅R⋅Xsfmt其中Xsfm自由网中的 3D 点坐标观测值。XworldGCP 的真实世界坐标真值。s尺度因子Scale用于纠正单目视觉的尺度不确定性。R3×3 的旋转矩阵Rotation属于 SO(3) 群。t3×1 的平移向量Translation。我们的目标是求解最优的 s,R,t使得所有对应点对的重投影误差欧氏距离平方和最小mins,R,tN∑i1∥Xiworld−(sRXisfmt)∥2这是一个典型的非线性最小二乘问题。在工程实践中通常采用线性初值 非线性优化的两步策略来求解以保证精度和鲁棒性。2. Umeyama线性解为了获得非线性优化的良好初值我们首先使用Umeyama 算法基于 SVD 分解来求解 Sim3 的闭式解Closed-form Solution。这是一种代数解法计算速度快且稳定。该算法的核心思想是通过去中心化操作将平移参数 t 与旋转 R、尺度 s 解耦从而将复杂的非线性问题转化为线性的 SVD 问题。推导过程如下1. 计算质心与去中心化首先我们需要计算源点集自由网点 Xsfm和目标点集GCP点 Xworld的几何中心质心。μsfm1NN∑i1Xisfm,μworld1NN∑i1Xiworld接着计算每个点相对于其质心的偏移向量去中心化坐标记为 xi 和 yixiXisfm−μsfm,yiXiworld−μworld此时原始的最小二乘目标函数 ∑∥yiμworld−(sR(xiμsfm)t)∥2 可以简化。由于质心是最小二乘问题的最优平移估计点我们可以直接得出平移量 t 与 R,s 的关系tμworld−sRμsfm这意味着只要我们要解出了 R 和 st 就迎刃而解。因此问题转化为最小化去中心化后的残差mins,RN∑i1∥yi−sRxi∥22. 构建协方差矩阵并进行 SVD 分解展开上述目标函数并忽略与变量无关的项最大化 s∑yTiRxi 等价于最小化原目标函数。利用迹Trace的性质 tr(AB)tr(BA)我们可以构建源点集与目标点集的协方差矩阵 ΣΣ1NN∑i1xiyTi对协方差矩阵 Σ 进行奇异值分解SVDΣUDVT其中 U,V 是正交矩阵D 是包含奇异值的对角矩阵。3. 求解旋转矩阵 R根据正交普鲁克问题Orthogonal Procrustes Problem的解法最优旋转矩阵 R 可以通过 U 和 V 计算得出。为了保证解出的是纯旋转矩阵行列式为 1而不是反射矩阵行列式为 -1我们需要引入一个修正矩阵 SRVSUT其中 Sdiag(1,1,det(VUT))。如果 det(VUT)−1则 S 的最后一个元素为 -1用于修正反射分量。4. 求解尺度因子 s在求得最优旋转 R 后我们可以通过对目标函数关于 s 求导并令其为 0得到最优尺度因子的闭式解。它等于变换后的协方差迹与源点集方差的比值strace(SD)σ2sfm∑3k1sk∑Ni1∥xi∥2其中 sk 是 Σ 的奇异值分母是源点集去中心化后的方差和即X.squaredNorm()。5. 求解平移向量 t最后将求得的 s 和 R 代入第一步推导的公式中即可恢复平移向量tμworld−sRμsfmUmeyama 算法为我们提供了一个非常接近全局最优的线性解但这仅仅是代数意义上的最优。为了获得统计意义上的最优解考虑噪声分布我们需要在此基础上进行非线性优化。3. 非线性优化虽然 Umeyama 算法给出了闭式解但它假设噪声是高斯分布的且无法方便地引入鲁棒核函数Robust Kernel来剔除 GCP 中的粗差。因此我们以 Umeyama 的结果为初值构建非线性优化问题。我们定义残差函数如下ri(s,q,t)Xiworld−(elogs⋅R(q)⋅Xisfmt)在代码实现中有几个关键的工程技巧参数化旋转使用四元数 q[w,x,y,z] 表示旋转避免欧拉角的万向节死锁并利用 Ceres 的QuaternionManifold保持流形约束。参数化尺度优化变量不是直接优化 s而是优化 logs。在计算时通过 sexp(logs) 还原。这样做的好处是强制尺度 s 始终为正值防止优化过程中尺度变为负数导致模型翻转。鲁棒核函数在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::vectorEigen::Vector3d src_pts, const std::vectorEigen::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::AutoDiffCostFunctionSim3Residual, 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::vectorEigen::Vector3d pts_sfm, const std::vectorEigen::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::JacobiSVDEigen::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::AutoDiffCostFunctionReprojectionError, 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::AutoDiffCostFunctionGCPPriorError, 3, 3( new GCPPriorError(xyz_prior, sigma)); } Eigen::Vector3d xyz_prior_; Eigen::Vector3d weight_; // 1 / sigma }; } // namespace BAProblem::BAProblem(const Eigen::Matrix3d K, const vectorObservation obs, vectorCameraExtrinsics est_cams, vectorObjectPoint 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::mapint, 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 约束的估计GlobalBundleAdjustmentWithGcpint 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(vectorcl::CameraExtrinsics cameraExtrinsics, const sfm::Bundle bundle, vectorcl::ObjectPoint objectPoints) { std::vectorEigen::Vector3d src_pts; std::vectorEigen::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, vectorcl::CameraExtrinsics cameras, vectorcl::ObjectPoint objectPoints, const std::mapint, Eigen::Vector3d golbal_gcp_3d_priors) { mapint, int cameraIdMap; vectorBAProblem::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)); } } vectorBAProblem::ObjectPoint est_pts; vectorBAProblem::Observation obs; mapint, int pointIdMap; std::mapint, 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 度。这一结果验证了从线性初值计算到非线性全局优化这一整套数学推导与代码实现的正确性。至此我们不仅解决了单目视觉重建的尺度不确定性问题更在稀疏控制点的约束下实现了从“相对结构”到“绝对位姿”的完整闭环。上一篇 | 目录 | 下一篇代码存档分类: 计算机视觉 / 三维重建与SLAM标签: Ceres优化, 光束法平差, 增量式SFM, GCP约束, Sim3绝对定向免责声明本内容来自平台创作者博客园系信息发布平台仅提供信息存储空间服务。好文要顶 关注我 收藏该文 微信分享​编辑charlee44 ​编辑粉丝 - 212 关注 - 10会员号2280加关注00升级成为会员« 上一篇 最小二乘问题详解20无先验约束下的增量式SFM自由网平差» 下一篇 最小二乘问题详解22抗差估计与增量式SFM的工程稳健实现posted 2026-04-22 09:29 charlee44 阅读(155) 评论(0) 收藏 举报