使用仿射变换或齐次变换在两个三维坐标系之间找到一个转换关系（转换矩阵）

给定两个坐标系上对应的 4 个点，使用奇异值分解 (SVD) 是求解刚体变换最常见且稳定的方法。

#include <iostream>
#include <Eigen/Dense>
#include <vector>using namespace std;
using namespace Eigen;// 函数用于计算从点集 A 到点集 B 的刚体变换 (旋转 + 平移)
void findTransformation(const vector<Vector3d>& A, const vector<Vector3d>& B, Matrix3d& R, Vector3d& T) {// 1. 计算质心Vector3d centroid_A(0, 0, 0), centroid_B(0, 0, 0);for (int i = 0; i < A.size(); i++) {centroid_A += A[i];centroid_B += B[i];}centroid_A /= A.size();centroid_B /= B.size();// 2. 去质心vector<Vector3d> A_prime(A.size()), B_prime(B.size());for (int i = 0; i < A.size(); i++) {A_prime[i] = A[i] - centroid_A;B_prime[i] = B[i] - centroid_B;}// 3. 构造 H 矩阵Matrix3d H = Matrix3d::Zero();for (int i = 0; i < A.size(); i++) {H += A_prime[i] * B_prime[i].transpose();}// 4. SVD 分解JacobiSVD<MatrixXd> svd(H, ComputeFullU | ComputeFullV);Matrix3d U = svd.matrixU();Matrix3d V = svd.matrixV();// 5. 计算旋转矩阵 RR = V * U.transpose();// 6. 计算平移向量 TT = centroid_B - R * centroid_A;
}int main() {// 定义点集 A 和 Bvector<Vector3d> A = {{160.4, 432.8, -309.55},{204.32, 1006.91, -300.71},{-75.46, 443.4, -311.26}};vector<Vector3d> B = {{0.0, -575.82, 0.0},{0.0, 0.0, 0.0},{-236.12, -575.82, 0.0}};// 变量 R 用于存储旋转矩阵，T 用于存储平移向量Matrix3d R;Vector3d T;// 计算变换矩阵findTransformation(A, B, R, T);// 输出结果cout << "旋转矩阵 R: \n" << R << endl;cout << "\n平移向量 T: \n" << T.transpose() << endl;// 原始点 (0, 0, 0)Vector3d point(0, 0, 0);// 使用 R 和 T 计算旋转和平移后的点Vector3d transformed_point = R * point + T;// 输出变换后的点cout << "\n变换后的点坐标: " << transformed_point.transpose() << endl;return 0;
}

代码解释：

1. 1. 该函数接收两个点集 A 和 B，使用 SVD 分解来求解旋转矩阵 RRR 和平移向量 TTT。
   2. 首先计算每个点集的质心，然后将每个点去质心（减去质心）。
   3. 通过去质心后的点集构造矩阵 HHH，然后对 HHH 进行 SVD 分解来获取旋转矩阵 RRR。
   4. 平移向量 TTT 通过质心的差值和旋转后的质心计算得到。