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

给定两个坐标系上对应的 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. 计算旋转矩阵 R
    R = V * U.transpose();

    // 6. 计算平移向量 T
    T = centroid_B - R * centroid_A;
}

int main() {
    // 定义点集 A 和 B
    vector<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 通过质心的差值和旋转后的质心计算得到。