坐标变换矩阵

在刚性变换中，点云的转换是通过旋转和平移两个操作完成的。“转换矩阵”一般指的是将一个点云从一个坐标系转换到另一个坐标系的变换矩阵，包含了旋转和平移信息。旋转矩阵 R 和平移向量 t 就是构成 刚性变换矩阵 的核心部分。我们可以通过它们将一个点云从原始坐标系变换到目标坐标系。

matlab代码

format long;
Q = [%3895.17	12019.19	3352.5
     5694.31	7867.43	    3352.5
     5694.31	9867.43	    3352.5
     5694.31	11867.43	3352.5
     5694.31	13867.43	3352.5
     5694.31	16867.43	3352.5
     4694.31	16867.43	3352.5
     3694.31	16867.43	3352.5
     2194.31	16867.43	3352.5
     2194.31	13867.43	3352.5
     2194.31	11867.43	3352.5
     2194.31	9867.43	    3352.5
     2194.31	7867.43	    3352.5
     3694.31	7867.43	    3352.5
     4694.31	7867.43	    3352.5
     4121.5	    9721.82	    3352.5
     5066.72	14642.49	3352.5
     4121.5	    15867.43	3352.5
     5066.72	17937.77	3352.5
     4066.72	17937.77	3352.5];
 P = [%57.59	    -7.52	    7221.36
     1816.63	-4156.66	7160.64
      1861.89	-2161.92	7180.98
      1878.65	-156.41	    7196.6
      1921.05	1850.84	    7176.54
      2000.18	4849.46	    7207.3
      989.33	4846.08	    7238.17
      -14.4	    4860.34	    7286.39
      -1540	    4879.37	    7278.36
      -1593	    1862.2	    7259.72
      -1618.32	-145.97	    7244.29
      -1658.36	-2137.07	7260.32
      -1657.02	-4124.85	7200.25
      -214.97	-4123.58	7183.49
      821.66	-4143.99	7210.08
      257.85	-2290.43	7186.9
      1325	    2616.39	    7213.17
      383.02	3847.19	    7183.32
      1371.96	5913.64	    7176.1
      382.6	    5923.52	    7213.88];
if size(P, 1) ~= size(Q, 1)
        error('点云数量不一致');
    end

    % 计算质心
    centroid_P = mean(P);
    centroid_Q = mean(Q);

    % 去质心化
    P_centered = P - centroid_P;
    Q_centered = Q - centroid_Q;
    % 绘制点云图

    % 计算协方差矩阵
    H = P_centered' * Q_centered;

    % 使用 SVD 分解
    [U, ~, V] = svd(H);

    % 计算旋转矩阵 R
    R = V * U';

    % 确保 R 是正交矩阵
    if det(R) < 0
        V(:, 3) = -V(:, 3);
        R = V * U';
    end

    % 计算平移向量 t
    t = centroid_Q' - R * centroid_P';

disp('旋转矩阵 R:');
disp(R);
disp('平移向量 t:');
disp(t);


X = R*transpose(P);

for i=1:19
    X(:,i) = X(:,i)+t;
end

python代码

 import numpy as np

def align_point_clouds(P, Q):
    # 确保点云数量一致
    if P.shape[0] != Q.shape[0]:
        raise ValueError('点云数量不一致')

    # 计算质心
    centroid_P = np.mean(P, axis=0)
    centroid_Q = np.mean(Q, axis=0)

    # 去质心化
    P_centered = P - centroid_P
    Q_centered = Q - centroid_Q

    # 计算协方差矩阵
    H = np.dot(P_centered.T, Q_centered)

    # 使用 SVD 分解
    U, _, Vt = np.linalg.svd(H)
    V = Vt.T

    # 计算旋转矩阵 R
    R = np.dot(V, U.T)

    # 确保 R 是正交矩阵
    if np.linalg.det(R) < 0:
        V[:, 2] = -V[:, 2]
        R = np.dot(V, U.T)

    # 计算平移向量 t
    t = centroid_Q - np.dot(R, centroid_P)

    return R, t

R,t = align_point_clouds(P, Q)
print(R)
print(t)