迭代最近点 ICP(Iterative Closest Point)算法

是计算机视觉 / 机器人领域中用于求解两组 3D 点集间最优刚体变换（旋转 + 平移） 的经典算法。
简单来说：给定两组对应（或近似对应）的 3D 点集（比如 RGB-D 相机不同帧的 3D 点、激光扫描的点云），ICP 通过迭代方式找到一个旋转矩阵R和平移向量t，使得其中一组点集经过R和t变换后，与另一组点集的距离误差最小。
ICP 的核心目标找到最优的 R(旋转）和 t（平移），满足：

ICP 的两种核心实现方式
你代码中用到的是 SVD-ICP（解析解 ICP）（无迭代），而传统 ICP 是迭代版本，两者的核心流程对比：
传统迭代 ICP

1. 点对匹配（找最近点）
2. 求解 R,t（如最小二乘）→
3. 变换点集
4. 重复至收敛
   特点：通用，但需迭代，速度较慢
   SVD-ICP（解析解）
5. 去中心化
6. 计算协方差矩阵 W
7. SVD 分解求 R
8. 计算 t
   SVD-ICP示例代码

点击查看代码

/**
 * @brief 基于SVD的ICP位姿估计具体实现
 * 算法步骤：
 * 1. 计算两组点的质心
 * 2. 对点集进行去中心化
 * 3. 计算协方差矩阵 W = sum(q1*q2^T)
 * 4. SVD分解W，得到旋转矩阵R=U*V^T
 * 5. 计算平移向量t = 质心1 - R*质心2
 */
void pose_estimation_3d3d(const vector<Point3f> &pts1,
                          const vector<Point3f> &pts2,
                          Mat &R, Mat &t) {
  // 计算两组点的质心
  Point3f p1, p2;
  int N = pts1.size();
  for (int i = 0; i < N; i++) {
    p1 += pts1[i];
    p2 += pts2[i];
  }
  // 质心坐标 = 所有点的平均值
  p1 = Point3f(Vec3f(p1) / N);
  p2 = Point3f(Vec3f(p2) / N);

  // 去中心化：每个点减去质心
  vector<Point3f> q1(N), q2(N);
  for (int i = 0; i < N; i++) {
    q1[i] = pts1[i] - p1;
    q2[i] = pts2[i] - p2;
  }

  // 计算协方差矩阵 W = sum(q1_i * q2_i^T)
  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  for (int i = 0; i < N; i++) {
    // 将OpenCV的Point3f转换为Eigen的Vector3d
    Eigen::Vector3d v1(q1[i].x, q1[i].y, q1[i].z);
    Eigen::Vector3d v2(q2[i].x, q2[i].y, q2[i].z);
    W += v1 * v2.transpose();
  }
  cout << "W=" << W << endl;

  // 对协方差矩阵W进行SVD分解
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();   // 左奇异矩阵
  Eigen::Matrix3d V = svd.matrixV();   // 右奇异矩阵

  cout << "U=" << U << endl;
  cout << "V=" << V << endl;

  // 计算旋转矩阵 R = U * V^T
  Eigen::Matrix3d R_ = U * (V.transpose());
  // 处理旋转矩阵行列式为-1的情况（保证旋转矩阵为正交矩阵）
  if (R_.determinant() < 0) {
    R_ = -R_;
  }

  // 计算平移向量 t = p1 - R*p2
  Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

  // 将Eigen矩阵转换为OpenCV的Mat类型
  R = (Mat_<double>(3, 3) <<
    R_(0, 0), R_(0, 1), R_(0, 2),
    R_(1, 0), R_(1, 1), R_(1, 2),
    R_(2, 0), R_(2, 1), R_(2, 2)
  );
  t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

这段代码成功估计出了相机的相对位姿，但需明确其表示的是「相机 2 → 相机 1」的位姿关系。 R 和 t 表示的是「相机 2 到相机 1 的刚体变换」