是计算机视觉 / 机器人领域中用于求解两组 3D 点集间最优刚体变换（旋转 + 平移） 的经典算法。
简单来说：给定两组对应（或近似对应）的 3D 点集（比如 RGB-D 相机不同帧的 3D 点、激光扫描的点云），ICP 通过迭代方式找到一个旋转矩阵R和平移向量t，使得其中一组点集经过R和t变换后，与另一组点集的距离误差最小。
ICP 的核心目标找到最优的 R(旋转）和 t（平移），满足：
ICP 的两种核心实现方式
你代码中用到的是 SVD-ICP（解析解 ICP）（无迭代），而传统 ICP 是迭代版本，两者的核心流程对比：
传统迭代 ICP
- 点对匹配（找最近点）
- 求解 R,t（如最小二乘）→
- 变换点集
- 重复至收敛
特点：通用，但需迭代，速度较慢
SVD-ICP（解析解） - 去中心化
- 计算协方差矩阵 W
- SVD 分解求 R
- 计算 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的Vector3dEigen::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^TEigen::Matrix3d R_ = U * (V.transpose());// 处理旋转矩阵行列式为-1的情况（保证旋转矩阵为正交矩阵）if (R_.determinant() < 0) {R_ = -R_;}// 计算平移向量 t = p1 - R*p2Eigen::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));
}