最小二乘问题详解12：三角化中的非线性优化

#include <ceres/ceres.h>

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

constexpr double PI = 3.14159265358979323846;

// 投影函数：将世界坐标 X 投影到图像平面（对应式 (1)）
Eigen::Vector2d Project(const Eigen::Matrix3d& K, const Eigen::Matrix3d& R_i,
                        const Eigen::Vector3d& t_i,
                        const Eigen::Vector3d& X_world) {
  // 相机坐标系下的点: X_c = R_i * X + t_i （式 (1) 中间步骤）
  Eigen::Vector3d X_cam = R_i * X_world + t_i;
  // 像素齐次坐标: s * [u, v, 1]^T = K * X_cam
  Eigen::Vector3d px_hom = K * X_cam;
  // 去齐次化（式 (2)）
  return Eigen::Vector2d(px_hom.x() / px_hom.z(), px_hom.y() / px_hom.z());
}

// DLT 三角化（基于式 (5)-(8)）
Eigen::Vector3d TriangulateDLT(const std::vector<Eigen::Matrix3d>& Rs,
                               const std::vector<Eigen::Vector3d>& ts,
                               const std::vector<Eigen::Vector2d>& observations,
                               const Eigen::Matrix3d& K) {
  size_t N = Rs.size();
  Eigen::MatrixXd A(2 * N, 4);  // 式 (8): A ∈ ℝ^{2N×4}

  for (size_t i = 0; i < N; ++i) {
    // 构造完整的投影矩阵 P_i = K [R_i | t_i] ∈ ℝ^{3×4} （式 (5)）
    Eigen::Matrix<double, 3, 4> P_i;
    P_i.block<3, 3>(0, 0) = K * Rs[i];  // K * R_i
    P_i.col(3) = K * ts[i];             // K * t_i

    double u_i = observations[i].x();  // 观测像素 u_i
    double v_i = observations[i].y();  // 观测像素 v_i

    // 构造 DLT 约束（式 (7)）:
    // (u_i * p_{i3}^T - p_{i1}^T) * X_tilde = 0
    // (v_i * p_{i3}^T - p_{i2}^T) * X_tilde = 0
    A.row(2 * i) = u_i * P_i.row(2) - P_i.row(0);
    A.row(2 * i + 1) = v_i * P_i.row(2) - P_i.row(1);
  }

  // SVD 求解 min ||A X_tilde|| s.t. ||X_tilde||=1 （式 (9) 前）
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullV);
  Eigen::Vector4d X_tilde = svd.matrixV().col(3);  // 对应 v_4

  // 齐次坐标 X_tilde = [X, Y, Z, W]^T → 欧氏坐标 = [X/W, Y/W, Z/W]^T
  if (std::abs(X_tilde.w()) < 1e-8) {
    // 点在无穷远处，无法有效三角化；返回原点或可抛异常
    return Eigen::Vector3d::Zero();
  }
  Eigen::Vector3d X_euclid(X_tilde.x() / X_tilde.w(), X_tilde.y() / X_tilde.w(),
                           X_tilde.z() / X_tilde.w());

  // 符号校正：由于 X_tilde 和 -X_tilde 都是齐次解，
  // 去齐次化后对应 X_euclid 和 -X_euclid，需选择使更多相机看到正深度的解
  Eigen::Vector3d X1 = X_euclid;
  Eigen::Vector3d X2 = -X_euclid;

  auto count_positive_depth = [&](const Eigen::Vector3d& X) -> int {
    int cnt = 0;
    for (size_t i = 0; i < N; ++i) {
      double z_cam = (Rs[i] * X + ts[i]).z();  // (R_i X + t_i)_z
      if (z_cam > 0) cnt++;
    }
    return cnt;
  };

  Eigen::Vector3d X_dlt =
      (count_positive_depth(X1) >= count_positive_depth(X2)) ? X1 : X2;

  // 最终保障：至少第一个相机深度为正
  if ((Rs[0] * X_dlt + ts[0]).z() <= 0) {
    X_dlt = -X_dlt;
  }

  return X_dlt;
}

// Ceres 残差块：重投影误差（对应式 (3)）
struct ReprojectionError {
  ReprojectionError(const Eigen::Vector2d& u_obs, const Eigen::Matrix3d& K,
                    const Eigen::Matrix3d& R_i, const Eigen::Vector3d& t_i)
      : u_obs_(u_obs), K_(K), R_i_(R_i), t_i_(t_i) {}

  template <typename T>
  bool operator()(const T* const X_world, T* residuals) const {
    // X_world: 优化变量，对应式 (4) 中的 X ∈ ℝ^3
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> X(X_world);

    // 转换到相机坐标系: X_cam = R_i * X + t_i
    Eigen::Matrix<T, 3, 3> R_i_T = R_i_.template cast<T>();
    Eigen::Matrix<T, 3, 1> t_i_T = t_i_.template cast<T>();
    Eigen::Matrix<T, 3, 1> X_cam = R_i_T * X + t_i_T;

    // 投影到像素平面（式 (2)）
    Eigen::Matrix<T, 3, 3> K_T = K_.template cast<T>();
    Eigen::Matrix<T, 3, 1> px_hom = K_T * X_cam;
    T u_proj = px_hom[0] / px_hom[2];  // f_x * x_c / z_c + c_x
    T v_proj = px_hom[1] / px_hom[2];  // f_y * y_c / z_c + c_y

    // 残差 = 投影值 - 观测值（式 (3)）
    residuals[0] = u_proj - T(u_obs_.x());
    residuals[1] = v_proj - T(u_obs_.y());

    return true;
  }

  static ceres::CostFunction* Create(const Eigen::Vector2d& u_obs,
                                     const Eigen::Matrix3d& K,
                                     const Eigen::Matrix3d& R_i,
                                     const Eigen::Vector3d& t_i) {
    return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3>(
        new ReprojectionError(u_obs, K, R_i, t_i));
  }

 private:
  Eigen::Vector2d u_obs_;    // 观测像素坐标 [u_i, v_i]^T
  Eigen::Matrix3d K_, R_i_;  // 内参、旋转
  Eigen::Vector3d t_i_;      // 平移
};

int main() {
  // === 相机内参 K（式 (1)）===
  double f_x = 800.0, f_y = 800.0, c_x = 320.0, c_y = 240.0;
  Eigen::Matrix3d K;
  K << f_x, 0, c_x, 0, f_y, c_y, 0, 0, 1;

  // === 真实3D点 X_gt（世界坐标）===
  Eigen::Vector3d X_gt(1.2, -0.5, 3.0);

  // === 相机位姿 {R_i, t_i} ===
  std::vector<Eigen::Matrix3d> Rs;
  std::vector<Eigen::Vector3d> ts;

  // 相机1: 单位位姿
  Rs.push_back(Eigen::Matrix3d::Identity());
  ts.push_back(Eigen::Vector3d::Zero());

  // 相机2: 绕Y轴旋转30度，平移(0.5, 0, 0)
  double angle = PI / 6.0;
  Eigen::AngleAxisd rot(angle, Eigen::Vector3d::UnitY());
  Rs.push_back(rot.toRotationMatrix());
  ts.push_back(Eigen::Vector3d(0.5, 0.0, 0.0));

  // 相机3: 绕Y轴旋转 -20 度，平移 (-0.3, 0.1, 0.2)
  Rs.push_back(Eigen::AngleAxisd(-PI / 9.0, Eigen::Vector3d::UnitY())
                   .toRotationMatrix());
  ts.push_back(Eigen::Vector3d(-0.3, 0.1, 0.2));

  // === 生成带噪声观测 {u_i} ===
  std::vector<Eigen::Vector2d> observations_clean;
  for (size_t i = 0; i < Rs.size(); ++i) {
    Eigen::Vector2d proj = Project(K, Rs[i], ts[i], X_gt);
    observations_clean.push_back(proj);
  }

  std::mt19937 gen(42);
  std::normal_distribution<double> noise(0.0, 2);  // 2像素高斯噪声
  std::vector<Eigen::Vector2d> observations;
  for (const auto& obs : observations_clean) {
    double u_noisy = obs.x() + noise(gen);
    double v_noisy = obs.y() + noise(gen);
    observations.emplace_back(u_noisy, v_noisy);
  }

  std::cout << std::fixed << std::setprecision(6);
  std::cout << "=== Ground Truth 3D Point X_gt ===" << std::endl;
  std::cout << "[" << X_gt.x() << ", " << X_gt.y() << ", " << X_gt.z() << "]"
            << std::endl;

  // === 1. DLT 初值 ===
  Eigen::Vector3d X_dlt = TriangulateDLT(Rs, ts, observations, K);
  std::cout << "\n=== DLT Estimate X_dlt ===" << std::endl;
  std::cout << "[" << X_dlt.x() << ", " << X_dlt.y() << ", " << X_dlt.z() << "]"
            << std::endl;

  // === 2. 非线性优化（最小化式 (4)）===
  double X_opt[3] = {X_dlt.x(), X_dlt.y(), X_dlt.z()};  // 初值

  ceres::Problem problem;
  for (size_t i = 0; i < Rs.size(); ++i) {
    problem.AddResidualBlock(
        ReprojectionError::Create(observations[i], K, Rs[i], ts[i]), nullptr,
        X_opt);
  }

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 20;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << "\n" << summary.BriefReport() << "\n";

  Eigen::Vector3d X_est(X_opt[0], X_opt[1], X_opt[2]);
  std::cout << "=== Nonlinear Optimization Estimate X_est ===" << std::endl;
  std::cout << "[" << X_est.x() << ", " << X_est.y() << ", " << X_est.z() << "]"
            << std::endl;

  // === 3. 评估 ===
  double dlt_error = (X_gt - X_dlt).norm();
  double opt_error = (X_gt - X_est).norm();

  double total_reproj_err_sq = 0.0;
  for (size_t i = 0; i < Rs.size(); ++i) {
    Eigen::Vector2d proj = Project(K, Rs[i], ts[i], X_est);
    double err = (proj - observations[i]).norm();
    total_reproj_err_sq += err * err;
  }
  double reproj_rmse = std::sqrt(total_reproj_err_sq / Rs.size());

  std::cout << "\n=== Evaluation ===" << std::endl;
  std::cout << "DLT 3D error:      " << dlt_error << " meters" << std::endl;
  std::cout << "Optimized 3D error: " << opt_error << " meters" << std::endl;
  std::cout << "Final reprojection RMSE: " << reproj_rmse << " pixels"
            << std::endl;

  return 0;
}

=== Ground Truth 3D Point X_gt ===
[1.200000, -0.500000, 3.000000]

=== DLT Estimate X_dlt ===
[1.195321, -0.500514, 2.982901]
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  3.862688e+00    0.00e+00    6.30e+02   0.00e+00   0.00e+00  1.00e+04        0    9.30e-05    2.91e-04
   1  3.171789e+00    6.91e-01    4.62e-01   0.00e+00   1.00e+00  3.00e+04        1    1.12e-04    5.51e-04

Ceres Solver Report: Iterations: 2, Initial cost: 3.862688e+00, Final cost: 3.171789e+00, Termination: CONVERGENCE
=== Nonlinear Optimization Estimate X_est ===
[1.195772, -0.498440, 2.983621]

=== Evaluation ===
DLT 3D error:      0.017735 meters
Optimized 3D error: 0.016988 meters
Final reprojection RMSE: 1.454141 pixels