基于 MATLAB 的坐标变换程序

一、坐标变换体系架构

┌─────────────────────────────────────────────────────────────┐
│                    坐标变换系统架构                       │
├─────────────────────────────────────────────────────────────┤
│  基础变换层   │  复合变换层   │  应用接口层   │  可视化层   │
│               │               │               │             │
│  • 平移变换   │  • 齐次坐标   │  • 点云变换   │  • 3D绘图   │
│  • 旋转变换   │  • 变换组合   │  • 坐标系对齐 │  • 轨迹显示 │
│  • 缩放变换   │  • 逆变换    │  • 坐标转换   │  • 对比分析 │
│  • 反射变换   │  • 插值变换  │  • 数据配准   │  • 误差评估 │
└─────────────────────────────────────────────────────────────┘

二、核心坐标变换函数

2.1 基础变换函数 (coordinate_transformations.m)

%% 坐标变换函数库
% 功能：提供各种坐标变换矩阵和变换函数

function [transform_matrix] = translation(tx, ty, tz)
    % 平移变换矩阵
    % 输入：tx, ty, tz - 沿x,y,z轴的平移量
    % 输出：4x4齐次变换矩阵
    
    transform_matrix = eye(4);
    transform_matrix(1,4) = tx;
    transform_matrix(2,4) = ty;
    transform_matrix(3,4) = tz;
end

function [transform_matrix] = rotation(axis, angle_deg)
    % 旋转变换矩阵
    % 输入：axis - 旋转轴 ('x', 'y', 'z')
    %       angle_deg - 旋转角度（度）
    % 输出：4x4齐次变换矩阵
    
    angle_rad = deg2rad(angle_deg);
    c = cos(angle_rad);
    s = sin(angle_rad);
    
    switch lower(axis)
        case 'x'
            transform_matrix = [1, 0, 0, 0;
                               0, c, -s, 0;
                               0, s, c, 0;
                               0, 0, 0, 1];
        case 'y'
            transform_matrix = [c, 0, s, 0;
                               0, 1, 0, 0;
                               -s, 0, c, 0;
                               0, 0, 0, 1];
        case 'z'
            transform_matrix = [c, -s, 0, 0;
                               s, c, 0, 0;
                               0, 0, 1, 0;
                               0, 0, 0, 1];
        otherwise
            error('未知的旋转轴: %s', axis);
    end
end

function [transform_matrix] = scaling(sx, sy, sz)
    % 缩放变换矩阵
    % 输入：sx, sy, sz - 沿x,y,z轴的缩放因子
    % 输出：4x4齐次变换矩阵
    
    transform_matrix = diag([sx, sy, sz, 1]);
end

function [transform_matrix] = reflection(axis)
    % 反射变换矩阵
    % 输入：axis - 反射平面 ('xy', 'xz', 'yz')
    % 输出：4x4齐次变换矩阵
    
    switch lower(axis)
        case 'xy'
            transform_matrix = [1, 0, 0, 0;
                               0, 1, 0, 0;
                               0, 0, -1, 0;
                               0, 0, 0, 1];
        case 'xz'
            transform_matrix = [1, 0, 0, 0;
                               0, -1, 0, 0;
                               0, 0, 1, 0;
                               0, 0, 0, 1];
        case 'yz'
            transform_matrix = [-1, 0, 0, 0;
                               0, 1, 0, 0;
                               0, 0, 1, 0;
                               0, 0, 0, 1];
        otherwise
            error('未知的反射平面: %s', axis);
    end
end

function [transform_matrix] = shear(xy, xz, yx, yz, zx, zy)
    % 剪切变换矩阵
    % 输入：剪切系数
    % 输出：4x4齐次变换矩阵
    
    transform_matrix = [1, xy, xz, 0;
                       yx, 1, yz, 0;
                       zx, zy, 1, 0;
                       0, 0, 0, 1];
end

function [point_transformed] = apply_transformation(point, transform_matrix)
    % 应用变换到点
    % 输入：point - 3D点或Nx3点集
    %       transform_matrix - 4x4变换矩阵
    % 输出：变换后的点
    
    % 转换为齐次坐标
    if size(point, 2) == 3
        point_homogeneous = [point, ones(size(point, 1), 1)]';
    else
        point_homogeneous = point;
    end
    
    % 应用变换
    point_transformed_homogeneous = transform_matrix * point_homogeneous;
    
    % 转换回3D坐标
    point_transformed = point_transformed_homogeneous(1:3, :)';
end

function [composite_matrix] = compose_transforms(varargin)
    % 组合多个变换矩阵
    % 输入：多个4x4变换矩阵
    % 输出：组合的变换矩阵
    
    composite_matrix = eye(4);
    for i = 1:nargin
        composite_matrix = varargin{i} * composite_matrix;
    end
end

function [inverse_matrix] = inverse_transform(transform_matrix)
    % 计算变换矩阵的逆
    % 输入：4x4变换矩阵
    % 输出：逆变换矩阵
    
    inverse_matrix = inv(transform_matrix);
end

2.2 坐标系转换函数 (coordinate_systems.m)

%% 坐标系转换函数
% 功能：在不同坐标系之间进行转换

function [cartesian_coords] = spherical_to_cartesian(spherical_coords)
    % 球坐标系转直角坐标系
    % 输入：spherical_coords - Nx3矩阵 [r, θ, φ]
    %       r: 径向距离, θ: 极角(0到π), φ: 方位角(0到2π)
    % 输出：cartesian_coords - Nx3矩阵 [x, y, z]
    
    r = spherical_coords(:, 1);
    theta = spherical_coords(:, 2);  % 极角
    phi = spherical_coords(:, 3);    % 方位角
    
    x = r .* sin(theta) .* cos(phi);
    y = r .* sin(theta) .* sin(phi);
    z = r .* cos(theta);
    
    cartesian_coords = [x, y, z];
end

function [spherical_coords] = cartesian_to_spherical(cartesian_coords)
    % 直角坐标系转球坐标系
    % 输入：cartesian_coords - Nx3矩阵 [x, y, z]
    % 输出：spherical_coords - Nx3矩阵 [r, θ, φ]
    
    x = cartesian_coords(:, 1);
    y = cartesian_coords(:, 2);
    z = cartesian_coords(:, 3);
    
    r = sqrt(x.^2 + y.^2 + z.^2);
    theta = acos(z ./ r);  % 极角
    phi = atan2(y, x);     % 方位角
    
    spherical_coords = [r, theta, phi];
end

function [cylindrical_coords] = cartesian_to_cylindrical(cartesian_coords)
    % 直角坐标系转柱坐标系
    % 输入：cartesian_coords - Nx3矩阵 [x, y, z]
    % 输出：cylindrical_coords - Nx3矩阵 [ρ, φ, z]
    
    x = cartesian_coords(:, 1);
    y = cartesian_coords(:, 2);
    z = cartesian_coords(:, 3);
    
    rho = sqrt(x.^2 + y.^2);
    phi = atan2(y, x);
    
    cylindrical_coords = [rho, phi, z];
end

function [cartesian_coords] = cylindrical_to_cartesian(cylindrical_coords)
    % 柱坐标系转直角坐标系
    % 输入：cylindrical_coords - Nx3矩阵 [ρ, φ, z]
    % 输出：cartesian_coords - Nx3矩阵 [x, y, z]
    
    rho = cylindrical_coords(:, 1);
    phi = cylindrical_coords(:, 2);
    z = cylindrical_coords(:, 3);
    
    x = rho .* cos(phi);
    y = rho .* sin(phi);
    
    cartesian_coords = [x, y, z];
end

function [polar_coords] = cartesian_to_polar(cartesian_coords_2d)
    % 二维直角坐标系转极坐标系
    % 输入：cartesian_coords_2d - Nx2矩阵 [x, y]
    % 输出：polar_coords - Nx2矩阵 [r, θ]
    
    x = cartesian_coords_2d(:, 1);
    y = cartesian_coords_2d(:, 2);
    
    r = sqrt(x.^2 + y.^2);
    theta = atan2(y, x);
    
    polar_coords = [r, theta];
end

function [cartesian_coords_2d] = polar_to_cartesian(polar_coords)
    % 极坐标系转二维直角坐标系
    % 输入：polar_coords - Nx2矩阵 [r, θ]
    % 输出：cartesian_coords_2d - Nx2矩阵 [x, y]
    
    r = polar_coords(:, 1);
    theta = polar_coords(:, 2);
    
    x = r .* cos(theta);
    y = r .* sin(theta);
    
    cartesian_coords_2d = [x, y];
end

2.3 点云配准函数 (point_cloud_registration.m)

%% 点云配准函数
% 功能：使用ICP算法进行点云配准

function [R, t, transformed_source] = icp_registration(source_points, target_points, max_iterations, tolerance)
    % ICP (Iterative Closest Point) 点云配准
    % 输入：source_points - 源点云 Nx3
    %       target_points - 目标点云 Mx3
    %       max_iterations - 最大迭代次数
    %       tolerance - 收敛容差
    % 输出：R - 3x3旋转矩阵
    %       t - 3x1平移向量
    %       transformed_source - 变换后的源点云
    
    if nargin < 3
        max_iterations = 50;
    end
    if nargin < 4
        tolerance = 1e-6;
    end
    
    % 初始化变换
    R = eye(3);
    t = zeros(3, 1);
    prev_error = inf;
    
    for iter = 1:max_iterations
        % 1. 寻找最近邻点
        correspondences = zeros(size(source_points, 1), 1);
        distances = zeros(size(source_points, 1), 1);
        
        for i = 1:size(source_points, 1)
            % 计算源点到所有目标点的距离
            diff = target_points - (source_points(i, :) * R' + t')';
            dist_sq = sum(diff.^2, 2);
            [min_dist, idx] = min(dist_sq);
            
            correspondences(i) = idx;
            distances(i) = sqrt(min_dist);
        end
        
        % 2. 计算当前误差
        current_error = mean(distances);
        if abs(prev_error - current_error) < tolerance
            fprintf('ICP收敛于第 %d 次迭代\n', iter);
            break;
        end
        prev_error = current_error;
        
        % 3. 计算最优变换
        source_centroid = mean(source_points, 1);
        target_centroid = mean(target_points(correspondences, :), 1);
        
        % 中心化点云
        source_centered = source_points - source_centroid;
        target_centered = target_points(correspondences, :) - target_centroid;
        
        % 计算协方差矩阵
        H = source_centered' * target_centered;
        
        % SVD分解求旋转矩阵
        [U, ~, V] = svd(H);
        R_new = V * U';
        
        % 确保右手坐标系
        if det(R_new) < 0
            V(:, 3) = -V(:, 3);
            R_new = V * U';
        end
        
        % 计算平移向量
        t_new = target_centroid' - R_new * source_centroid';
        
        % 更新变换
        R = R_new * R;
        t = R_new * t + t_new;
        
        % 4. 应用变换
        transformed_source = (source_points * R' + t')';
        transformed_source = transformed_source';
        
        if iter == max_iterations
            fprintf('ICP达到最大迭代次数 %d\n', max_iterations);
        end
    end
    
    transformed_source = (source_points * R' + t')';
    transformed_source = transformed_source';
end

function [transform_matrix] = estimate_rigid_transform(source_points, target_points)
    % 估计刚性变换矩阵
    % 使用最小二乘法估计最优旋转和平移
    
    % 计算质心
    source_centroid = mean(source_points, 1);
    target_centroid = mean(target_points, 1);
    
    % 中心化点云
    source_centered = source_points - source_centroid;
    target_centered = target_points - target_centroid;
    
    % 计算协方差矩阵
    H = source_centered' * target_centered;
    
    % SVD分解
    [U, ~, V] = svd(H);
    R = V * U';
    
    % 确保右手坐标系
    if det(R) < 0
        V(:, 3) = -V(:, 3);
        R = V * U';
    end
    
    % 计算平移
    t = target_centroid' - R * source_centroid';
    
    % 构建变换矩阵
    transform_matrix = eye(4);
    transform_matrix(1:3, 1:3) = R;
    transform_matrix(1:3, 4) = t;
end

2.4 主程序 (coordinate_transformation_system.m)

%% 坐标变换系统主程序
clear all; close all; clc;

fprintf('=== 坐标变换系统 ===\n\n');

%% 1. 基础变换演示
fprintf('1. 基础变换演示\n');

% 创建原始点
original_point = [1, 2, 3];
fprintf('原始点: [%.1f, %.1f, %.1f]\n', original_point(1), original_point(2), original_point(3));

% 平移变换
T = translation(2, -1, 3);
translated_point = apply_transformation(original_point, T);
fprintf('平移后: [%.1f, %.1f, %.1f]\n', translated_point(1), translated_point(2), translated_point(3));

% 旋转变换（绕z轴旋转45度）
R = rotation('z', 45);
rotated_point = apply_transformation(original_point, R);
fprintf('旋转后: [%.1f, %.1f, %.1f]\n', rotated_point(1), rotated_point(2), rotated_point(3));

% 缩放变换
S = scaling(2, 1.5, 0.5);
scaled_point = apply_transformation(original_point, S);
fprintf('缩放后: [%.1f, %.1f, %.1f]\n\n', scaled_point(1), scaled_point(2), scaled_point(3));

%% 2. 复合变换演示
fprintf('2. 复合变换演示\n');

% 组合变换：先旋转再平移
composite_matrix = compose_transforms(T, R);
transformed_point = apply_transformation(original_point, composite_matrix);
fprintf('先旋转45°再平移[2,-1,3]: [%.1f, %.1f, %.1f]\n', ...
        transformed_point(1), transformed_point(2), transformed_point(3));

% 逆变换
inv_matrix = inverse_transform(composite_matrix);
restored_point = apply_transformation(transformed_point, inv_matrix);
fprintf('逆变换恢复: [%.1f, %.1f, %.1f]\n\n', restored_point(1), restored_point(2), restored_point(3));

%% 3. 坐标系转换演示
fprintf('3. 坐标系转换演示\n');

% 球坐标系
spherical_point = [5, pi/3, pi/4];  % r=5, θ=60°, φ=45°
fprintf('球坐标: [r=%.1f, θ=%.1f°, φ=%.1f°]\n', ...
        spherical_point(1), rad2deg(spherical_point(2)), rad2deg(spherical_point(3)));

% 转换为直角坐标
cartesian_from_spherical = spherical_to_cartesian(spherical_point);
fprintf('直角坐标: [%.1f, %.1f, %.1f]\n', ...
        cartesian_from_spherical(1), cartesian_from_spherical(2), cartesian_from_spherical(3));

% 柱坐标系
cylindrical_point = [3, pi/3, 2];  % ρ=3, φ=60°, z=2
fprintf('柱坐标: [ρ=%.1f, φ=%.1f°, z=%.1f]\n', ...
        cylindrical_point(1), rad2deg(cylindrical_point(2)), cylindrical_point(3));

% 转换为直角坐标
cartesian_from_cylindrical = cylindrical_to_cartesian(cylindrical_point);
fprintf('直角坐标: [%.1f, %.1f, %.1f]\n\n', ...
        cartesian_from_cylindrical(1), cartesian_from_cylindrical(2), cartesian_from_cylindrical(3));

%% 4. 点云变换演示
fprintf('4. 点云变换演示\n');

% 创建原始点云（立方体）
cube_points = [0, 0, 0; 1, 0, 0; 1, 1, 0; 0, 1, 0; 0, 0, 1; 1, 0, 1; 1, 1, 1; 0, 1, 1];
fprintf('原始立方体点云: %d 个点\n', size(cube_points, 1));

% 应用变换：缩放+旋转+平移
scale_matrix = scaling(2, 1.5, 1);
rotate_matrix = rotation('y', 30);
translate_matrix = translation(5, 0, 0);
transform_sequence = compose_transforms(translate_matrix, rotate_matrix, scale_matrix);

transformed_cube = apply_transformation(cube_points, transform_sequence);
fprintf('变换后点云: %d 个点\n', size(transformed_cube, 1));

%% 5. 点云配准演示
fprintf('5. 点云配准演示\n');

% 创建源点云和目标点云
source_cloud = cube_points + 0.1*randn(size(cube_points));  % 添加噪声
target_cloud = apply_transformation(cube_points, transform_sequence);

% 使用ICP进行配准
[R_icp, t_icp, aligned_cloud] = icp_registration(source_cloud, target_cloud, 30, 1e-6);
fprintf('ICP配准完成\n');
fprintf('旋转矩阵:\n');
disp(R_icp);
fprintf('平移向量: [%.3f, %.3f, %.3f]\n', t_icp(1), t_icp(2), t_icp(3));

%% 6. 可视化结果
fprintf('6. 生成可视化结果...\n');
visualize_coordinate_transformations(original_point, transformed_point, cube_points, transformed_cube, source_cloud, aligned_cloud);

fprintf('\n=== 坐标变换系统演示完成 ===\n');

2.5 可视化模块 (visualize_coordinate_transformations.m)

function visualize_coordinate_transformations(original_point, transformed_point, original_cloud, transformed_cloud, source_cloud, aligned_cloud)
    % 可视化坐标变换结果
    
    figure('Position', [100, 100, 1400, 900]);
    
    % 1. 单点变换对比
    subplot(3, 3, 1);
    plot3(original_point(1), original_point(2), original_point(3), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
    hold on;
    plot3(transformed_point(1), transformed_point(2), transformed_point(3), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
    plot3([original_point(1), transformed_point(1)], [original_point(2), transformed_point(2)], ...
          [original_point(3), transformed_point(3)], 'k--', 'LineWidth', 1);
    xlabel('X'); ylabel('Y'); zlabel('Z');
    title('单点变换对比');
    legend('原始点', '变换后点');
    grid on; axis equal;
    
    % 2. 点云变换对比
    subplot(3, 3, 2);
    plot3(original_cloud(:,1), original_cloud(:,2), original_cloud(:,3), 'b.', 'MarkerSize', 8);
    hold on;
    plot3(transformed_cloud(:,1), transformed_cloud(:,2), transformed_cloud(:,3), 'r.', 'MarkerSize', 8);
    xlabel('X'); ylabel('Y'); zlabel('Z');
    title('点云变换对比');
    legend('原始点云', '变换后点云');
    grid on; axis equal;
    
    % 3. 坐标系转换
    subplot(3, 3, 3);
    % 绘制坐标轴
    axis_length = 3;
    plot3([0, axis_length], [0, 0], [0, 0], 'r-', 'LineWidth', 2);  % X轴
    plot3([0, 0], [0, axis_length], [0, 0], 'g-', 'LineWidth', 2);  % Y轴
    plot3([0, 0], [0, 0], [0, axis_length], 'b-', 'LineWidth', 2);  % Z轴
    xlabel('X'); ylabel('Y'); zlabel('Z');
    title('坐标系');
    grid on; axis equal;
    
    % 4. 球坐标系可视化
    subplot(3, 3, 4);
    [theta, phi] = meshgrid(linspace(0, pi, 20), linspace(0, 2*pi, 40));
    r = 3;
    x = r * sin(theta) .* cos(phi);
    y = r * sin(theta) .* sin(phi);
    z = r * cos(theta);
    surf(x, y, z, 'FaceAlpha', 0.3, 'EdgeColor', 'none');
    hold on;
    plot3(0, 0, 0, 'ko', 'MarkerSize', 10, 'LineWidth', 2);
    xlabel('X'); ylabel('Y'); zlabel('Z');
    title('球坐标系 (r=3)');
    grid on; axis equal;
    
    % 5. 柱坐标系可视化
    subplot(3, 3, 5);
    [rho, phi] = meshgrid(linspace(0, 3, 10), linspace(0, 2*pi, 40));
    z = linspace(0, 4, 10)';
    x = rho .* cos(phi);
    y = rho .* sin(phi);
    surf(x, y, repmat(z, 1, size(x, 2)), 'FaceAlpha', 0.3, 'EdgeColor', 'none');
    xlabel('X'); ylabel('Y'); zlabel('Z');
    title('柱坐标系 (ρ=3, z∈[0,4])');
    grid on; axis equal;
    
    % 6. 点云配准结果
    subplot(3, 3, 6);
    plot3(source_cloud(:,1), source_cloud(:,2), source_cloud(:,3), 'b.', 'MarkerSize', 6);
    hold on;
    plot3(target_cloud(:,1), target_cloud(:,2), target_cloud(:,3), 'r.', 'MarkerSize', 6);
    plot3(aligned_cloud(:,1), aligned_cloud(:,2), aligned_cloud(:,3), 'g.', 'MarkerSize', 6);
    xlabel('X'); ylabel('Y'); zlabel('Z');
    title('点云配准结果');
    legend('源点云', '目标点云', '配准后点云');
    grid on; axis equal;
    
    % 7. 变换矩阵热力图
    subplot(3, 3, 7);
    T_example = compose_transforms(translation(2, -1, 3), rotation('z', 45), scaling(1.5, 1.5, 1.5));
    imagesc(T_example);
    colorbar;
    xlabel('列索引'); ylabel('行索引');
    title('变换矩阵热力图');
    
    % 8. 变换轨迹
    subplot(3, 3, 8);
    trajectory_points = zeros(10, 3);
    current_point = [1, 1, 1];
    for i = 1:10
        trajectory_points(i, :) = current_point;
        % 每次旋转30度并平移
        T_step = compose_transforms(translation(0.5, 0, 0), rotation('z', 30));
        current_point = apply_transformation(current_point, T_step);
    end
    plot3(trajectory_points(:,1), trajectory_points(:,2), trajectory_points(:,3), 'bo-', 'LineWidth', 2, 'MarkerSize', 8);
    xlabel('X'); ylabel('Y'); zlabel('Z');
    title('变换轨迹');
    grid on; axis equal;
    
    % 9. 误差分析
    subplot(3, 3, 9);
    % 计算配准误差
    registration_errors = sqrt(sum((aligned_cloud - target_cloud).^2, 2));
    histogram(registration_errors, 20, 'FaceColor', 'g', 'EdgeColor', 'none');
    xlabel('配准误差 (距离)');
    ylabel('频次');
    title('点云配准误差分布');
    grid on;
    
    sgtitle('坐标变换系统可视化结果');
end

2.6 测试脚本 (test_coordinate_transformations.m)

%% 坐标变换系统测试脚本
clear all; close all; clc;

fprintf('=== 坐标变换系统测试 ===\n\n');

%% 测试1: 基础变换正确性验证
fprintf('测试1: 基础变换正确性验证\n');

% 测试点
test_point = [1, 0, 0];

% 绕z轴旋转90度
R_z90 = rotation('z', 90);
rotated_point = apply_transformation(test_point, R_z90);
expected_point = [0, 1, 0];

error = norm(rotated_point - expected_point);
fprintf('  绕z轴旋转90度: 误差 = %.6f\n', error);
assert(error < 1e-10, '旋转变换测试失败！');

% 平移测试
T = translation(2, 3, 4);
translated_point = apply_transformation(test_point, T);
expected_point = [3, 3, 4];

error = norm(translated_point - expected_point);
fprintf('  平移[2,3,4]: 误差 = %.6f\n', error);
assert(error < 1e-10, '平移变换测试失败！');

% 缩放测试
S = scaling(2, 2, 2);
scaled_point = apply_transformation(test_point, S);
expected_point = [2, 0, 0];

error = norm(scaled_point - expected_point);
fprintf('  缩放因子2: 误差 = %.6f\n', error);
assert(error < 1e-10, '缩放变换测试失败！');

fprintf('  基础变换测试通过！\n\n');

%% 测试2: 变换组合结合律验证
fprintf('测试2: 变换组合结合律验证\n');

% 创建两个不同的变换序列
T1 = translation(1, 2, 3);
R1 = rotation('x', 30);
S1 = scaling(2, 1, 1.5);

% 序列1: T * R * S
seq1 = compose_transforms(S1, R1, T1);

% 序列2: (T * R) * S
temp = compose_transforms(R1, T1);
seq2 = compose_transforms(S1, temp);

% 应用两个序列到同一点
test_point = [1, 1, 1];
result1 = apply_transformation(test_point, seq1);
result2 = apply_transformation(test_point, seq2);

error = norm(result1 - result2);
fprintf('  变换组合结合律误差: %.6f\n', error);
assert(error < 1e-10, '变换组合结合律测试失败！');

fprintf('  变换组合测试通过！\n\n');

%% 测试3: 逆变换验证
fprintf('测试3: 逆变换验证\n');

% 创建复合变换
composite_transform = compose_transforms(translation(5, -2, 3), rotation('y', 45), scaling(1.5, 1, 2));
inverse_transform = inverse_transform(composite_transform);

% 应用变换和其逆变换
test_point = [2, 3, 4];
transformed_point = apply_transformation(test_point, composite_transform);
restored_point = apply_transformation(transformed_point, inverse_transform);

error = norm(test_point - restored_point);
fprintf('  逆变换误差: %.6f\n', error);
assert(error < 1e-10, '逆变换测试失败！');

fprintf('  逆变换测试通过！\n\n');

%% 测试4: 坐标系转换一致性验证
fprintf('测试4: 坐标系转换一致性验证\n');

% 创建测试点
test_point_cartesian = [3, 4, 5];

% 直角坐标 -> 球坐标 -> 直角坐标
spherical = cartesian_to_spherical(test_point_cartesian);
restored_cartesian = spherical_to_cartesian(spherical);

error = norm(test_point_cartesian - restored_cartesian);
fprintf('  球坐标系转换误差: %.6f\n', error);
assert(error < 1e-10, '球坐标系转换测试失败！');

% 直角坐标 -> 柱坐标 -> 直角坐标
cylindrical = cartesian_to_cylindrical(test_point_cartesian);
restored_cartesian = cylindrical_to_cartesian(cylindrical);

error = norm(test_point_cartesian - restored_cartesian);
fprintf('  柱坐标系转换误差: %.6f\n', error);
assert(error < 1e-10, '柱坐标系转换测试失败！');

fprintf('  坐标系转换测试通过！\n\n');

%% 测试5: 点云配准精度验证
fprintf('测试5: 点云配准精度验证\n');

% 创建已知变换的点云
source_cloud = randn(100, 3);
true_R = rotation('z', 30);
true_t = [2, -1, 3]';
target_cloud = (source_cloud * true_R' + true_t')';

% 添加少量噪声
target_cloud_noisy = target_cloud + 0.01*randn(size(target_cloud));

% 执行ICP配准
[R_est, t_est, aligned_cloud] = icp_registration(source_cloud, target_cloud_noisy, 50, 1e-6);

% 计算估计误差
R_error = norm(true_R - R_est, 'fro');
t_error = norm(true_t - t_est);

fprintf('  旋转矩阵误差: %.6f\n', R_error);
fprintf('  平移向量误差: %.6f\n', t_error);

if R_error < 0.1 && t_error < 0.1
    fprintf('  点云配准测试通过！\n\n');
else
    warning('点云配准误差较大，可能需要调整参数');
end

%% 测试6: 性能测试
fprintf('测试6: 性能测试\n');

% 测试大规模点云变换
large_point_cloud = randn(10000, 3);
T_large = compose_transforms(translation(10, 20, 30), rotation('x', 45), scaling(2, 2, 2));

tic;
transformed_large_cloud = apply_transformation(large_point_cloud, T_large);
time_large = toc;

fprintf('  10000个点变换耗时: %.4f 秒\n', time_large);
fprintf('  每个点平均耗时: %.6f 秒\n', time_large/10000);

% 测试多次变换组合
tic;
for i = 1:1000
    T_combined = compose_transforms(T_large, T_large, T_large);
end
time_composition = toc;

fprintf('  1000次变换组合耗时: %.4f 秒\n', time_composition);
fprintf('  每次组合平均耗时: %.6f 秒\n\n', time_composition/1000);

fprintf('所有测试完成！\n');

三、实际应用示例

3.1 机器人运动学应用

%% 机器人运动学坐标变换示例
clear all; clc;

% 机器人DH参数
links = [0, 0.5, 0, pi/2;      % 关节1
         0, 0, 0.4, 0;         % 关节2
         0, 0, 0.3, 0];        % 关节3

% 计算各关节变换矩阵
T_base_to_joint1 = dh_transform(links(1, :));
T_joint1_to_joint2 = dh_transform(links(2, :));
T_joint2_to_joint3 = dh_transform(links(3, :));

% 末端执行器相对于基座的变换
T_base_to_end = compose_transforms(T_base_to_joint1, T_joint1_to_joint2, T_joint2_to_joint3);

% 计算末端位置
end_effector_position = apply_transformation([0, 0, 0], T_base_to_end);

fprintf('机器人末端位置: [%.3f, %.3f, %.3f]\n', end_effector_position(1), end_effector_position(2), end_effector_position(3));

function T = dh_transform(dh_params)
    % DH参数变换矩阵
    % dh_params = [a, d, alpha, theta]
    a = dh_params(1);
    d = dh_params(2);
    alpha = dh_params(3);
    theta = dh_params(4);
    
    T = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
         sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
         0, sin(alpha), cos(alpha), d;
         0, 0, 0, 1];
end

3.2 三维重建应用

%% 三维重建坐标变换示例
clear all; clc;

% 模拟从不同视角拍摄的物体
object_points = randn(100, 3) * 10;  % 物体表面点

% 相机位置和姿态
camera_positions = [0, 0, 50; 50, 0, 0; 0, 50, 0; -50, 0, 0];
camera_orientations = [0, 0, 0; 0, pi/2, 0; pi/2, 0, 0; pi, 0, 0];

% 将物体点转换到各个相机坐标系
for i = 1:size(camera_positions, 1)
    % 相机外参矩阵
    R_cam = rotation('z', rad2deg(camera_orientations(i, 3))) * ...
            rotation('y', rad2deg(camera_orientations(i, 2))) * ...
            rotation('x', rad2deg(camera_orientations(i, 1)));
    
    t_cam = camera_positions(i, :)';
    
    % 变换矩阵
    T_camera = eye(4);
    T_camera(1:3, 1:3) = R_cam;
    T_camera(1:3, 4) = t_cam;
    
    % 将物体点转换到相机坐标系
    points_in_camera = apply_transformation(object_points, T_camera);
    
    fprintf('相机%d: %d个点转换到相机坐标系\n', i, size(points_in_camera, 1));
end

参考代码 基于matlab的坐标变换程序 www.youwenfan.com/contentcsu/63365.html

四、使用建议

4.1 最佳实践

1. 使用齐次坐标：始终使用4x4齐次变换矩阵，便于组合变换
2. 注意变换顺序 ：变换顺序很重要，T1 * T2 表示先应用T2再应用T1
3. 验证变换正确性：使用已知点验证变换矩阵的正确性
4. 考虑数值稳定性：对于大角度旋转，使用四元数可以避免万向节锁

4.2 常见错误避免

错误	原因	解决方法
变换结果不对	变换顺序错误	检查变换组合顺序
坐标系混乱	未明确坐标系定义	明确每个变换的坐标系
数值不稳定	大角度旋转	使用四元数或小角度近似
配准失败	初始值不好	提供更好的初始估计

4.3 扩展功能建议

1. 四元数支持：添加四元数旋转表示
2. 李群李代数：实现SE(3)群上的变换
3. 非线性优化：使用Ceres或g2o进行优化
4. GPU加速：使用GPU进行大规模点云变换