当前位置：首页 > news >正文

通过ICP算法实现三维点云配准

news 2025/11/9 2:42:58

ICP（Iterative Closest Point，迭代最近点）算法是三维点云配准中最常用的方法之一。

ICP算法原理

ICP算法的基本思想是通过迭代的方式，逐步优化两个点云之间的变换关系（旋转矩阵R和平移向量t），使得两个点云之间的距离最小化。主要步骤包括：

寻找最近点对应关系
计算最优变换（R和t）
应用变换到源点云
重复上述步骤直到收敛

MATLAB

%% 三维点云ICP配准算法实现
clear; close all; clc;%% 1. 生成示例点云数据
fprintf('生成示例点云数据...\n');% 创建原始点云（一个立方体）
[x, y, z] = meshgrid(0:0.1:1, 0:0.1:1, 0:0.1:1);
original_cloud = [x(:), y(:), z(:)];% 添加一些噪声
noise_level = 0.01;
noisy_cloud = original_cloud + noise_level * randn(size(original_cloud));% 定义变换参数（旋转和平移）
theta_x = 15; % 绕X轴旋转角度（度）
theta_y = 25; % 绕Y轴旋转角度（度）
theta_z = 10; % 绕Z轴旋转角度（度）
translation = [0.5, 0.3, 0.2]; % 平移向量% 创建变换矩阵
Rx = [1, 0, 0; 0, cosd(theta_x), -sind(theta_x); 0, sind(theta_x), cosd(theta_x)];
Ry = [cosd(theta_y), 0, sind(theta_y); 0, 1, 0; -sind(theta_y), 0, cosd(theta_y)];
Rz = [cosd(theta_z), -sind(theta_z), 0; sind(theta_z), cosd(theta_z), 0; 0, 0, 1];
R = Rx * Ry * Rz; % 组合旋转矩阵
t = translation'; % 平移向量% 应用变换到点云
transformed_cloud = (R * noisy_cloud' + t)';% 添加一些离群点（模拟实际数据中的噪声）
outlier_ratio = 0.05;
num_outliers = round(outlier_ratio * size(transformed_cloud, 1));
outlier_indices = randperm(size(transformed_cloud, 1), num_outliers);
transformed_cloud(outlier_indices, :) = transformed_cloud(outlier_indices, :) + 0.5 * randn(num_outliers, 3);fprintf('点云数据生成完成:\n');
fprintf('  原始点云点数: %d\n', size(original_cloud, 1));
fprintf('  变换后点云点数: %d\n', size(transformed_cloud, 1));
fprintf('  旋转角度: (%.1f, %.1f, %.1f)度\n', theta_x, theta_y, theta_z);
fprintf('  平移向量: (%.2f, %.2f, %.2f)\n', translation);%% 2. 可视化初始点云
figure('Name', 'ICP点云配准', 'Position', [100, 100, 1200, 500]);subplot(1, 3, 1);
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);
axis equal;
grid on;
xlabel('X');
ylabel('Y');
zlabel('Z');
title('初始点云');
legend('目标点云', '源点云');
view(45, 30);%% 3. ICP算法实现
fprintf('\n开始ICP配准...\n');% 初始化变换参数
R_current = eye(3);
t_current = zeros(3, 1);
source_cloud = transformed_cloud; % 源点云（需要配准的点云）
target_cloud = original_cloud;    % 目标点云（参考点云）% ICP参数设置
max_iterations = 50;
tolerance = 1e-6;
mean_errors = zeros(max_iterations, 1);
stop_criterion = false;
iteration = 1;% 创建kd树加速最近邻搜索
kd_tree = KDTreeSearcher(target_cloud);% 记录变换历史
transformation_history = cell(max_iterations, 1);% ICP主循环
while iteration <= max_iterations && ~stop_criterion% 步骤1: 寻找最近点对应关系[indices, distances] = knnsearch(kd_tree, source_cloud);correspondences = target_cloud(indices, :);% 步骤2: 去除离群点（基于距离阈值）distance_threshold = 3 * median(distances);valid_indices = distances < distance_threshold;valid_source = source_cloud(valid_indices, :);valid_target = correspondences(valid_indices, :);% 步骤3: 计算最优变换（SVD方法）mean_source = mean(valid_source, 1);mean_target = mean(valid_target, 1);centered_source = valid_source - mean_source;centered_target = valid_target - mean_target;% 计算协方差矩阵H = centered_source' * centered_target;% SVD分解[U, ~, V] = svd(H);% 计算旋转矩阵R_iter = V * U';% 确保是纯旋转（行列式为1）if det(R_iter) < 0V(:,3) = -V(:,3);R_iter = V * U';end% 计算平移向量t_iter = mean_target' - R_iter * mean_source';% 步骤4: 应用变换到源点云source_cloud = (R_iter * source_cloud' + t_iter)';% 更新累积变换R_current = R_iter * R_current;t_current = R_iter * t_current + t_iter;% 记录当前变换transformation_history{iteration} = struct('R', R_iter, 't', t_iter);% 计算并记录误差mean_errors(iteration) = mean(distances(valid_indices));% 检查收敛条件if iteration > 1error_change = abs(mean_errors(iteration) - mean_errors(iteration-1));if error_change < tolerancestop_criterion = true;fprintf('  迭代 %d: 误差变化小于容差，收敛!\n', iteration);endend% 显示迭代信息if mod(iteration, 10) == 0 || iteration == 1 || stop_criterionfprintf('  迭代 %d: 平均误差 = %.6f\n', iteration, mean_errors(iteration));enditeration = iteration + 1;
end% 截断未使用的迭代
mean_errors = mean_errors(1:iteration-1);fprintf('ICP配准完成! 共进行了 %d 次迭代\n', iteration-1);%% 4. 计算配准精度
% 计算最终变换与真实变换之间的差异
R_error = norm(R - R_current, 'fro');
t_error = norm(t - t_current);fprintf('\n配准精度评估:\n');
fprintf('  旋转矩阵误差: %.6f\n', R_error);
fprintf('  平移向量误差: %.6f\n', t_error);
fprintf('  最终平均对应点距离: %.6f\n', mean_errors(end));%% 5. 可视化配准结果
subplot(1, 3, 2);
plot3(original_cloud(:,1), original_cloud(:,2), original_cloud(:,3), 'b.', 'MarkerSize', 8);
hold on;
plot3(source_cloud(:,1), source_cloud(:,2), source_cloud(:,3), 'g.', 'MarkerSize', 8);
axis equal;
grid on;
xlabel('X');
ylabel('Y');
zlabel('Z');
title('配准后点云');
legend('目标点云', '配准后的源点云');
view(45, 30);% 可视化误差收敛过程
subplot(1, 3, 3);
plot(1:length(mean_errors), mean_errors, 'LineWidth', 2);
xlabel('迭代次数');
ylabel('平均对应点距离');
title('ICP收敛过程');
grid on;%% 6. 高级可视化 - 配准过程动画
fprintf('\n生成配准过程动画...\n');% 创建动画图形窗口
animation_fig = figure('Name', 'ICP配准过程动画', 'Position', [200, 200, 800, 600]);
axis equal;
grid on;
hold on;% 绘制目标点云
h_target = plot3(target_cloud(:,1), target_cloud(:,2), target_cloud(:,3), 'b.', 'MarkerSize', 8);
% 重新初始化源点云
source_for_animation = transformed_cloud;% 绘制初始源点云
h_source = plot3(source_for_animation(:,1), source_for_animation(:,2), source_for_animation(:,3), 'r.', 'MarkerSize', 8);xlabel('X');
ylabel('Y');
zlabel('Z');
title('ICP配准过程');
legend('目标点云', '源点云');
view(45, 30);% 设置动画参数
pause_time = 0.5; % 每帧暂停时间% 记录动画帧
frames(iteration) = getframe(animation_fig);% 逐帧应用变换
for i = 1:iteration-1% 应用当前迭代的变换R_iter = transformation_history{i}.R;t_iter = transformation_history{i}.t;source_for_animation = (R_iter * source_for_animation' + t_iter)';% 更新源点云显示set(h_source, 'XData', source_for_animation(:,1), ...'YData', source_for_animation(:,2), ...'ZData', source_for_animation(:,3));% 更新标题显示当前迭代title(sprintf('ICP配准过程 (迭代 %d/%d)', i, iteration-1));% 暂停并捕获帧pause(pause_time);frames(i) = getframe(animation_fig);
end% 保存动画为GIF文件
fprintf('保存配准过程动画...\n');
saveAnimation(frames, 'icp_registration_process.gif');fprintf('动画已保存为 icp_registration_process.gif\n');%% 7. 辅助函数：保存动画
function saveAnimation(frames, filename)% 创建GIF文件for idx = 1:length(frames)% 将帧转换为图像im = frame2im(frames(idx));[imind, cm] = rgb2ind(im, 256);% 写入GIF文件if idx == 1imwrite(imind, cm, filename, 'gif', 'Loopcount', inf, 'DelayTime', 0.5);elseimwrite(imind, cm, filename, 'gif', 'WriteMode', 'append', 'DelayTime', 0.5);endend
end%% 8. 使用MATLAB内置ICP函数进行比较（可选）
fprintf('\n使用MATLAB内置pcregistericp函数进行比较...\n');% 将点云转换为pointCloud对象（需要Computer Vision Toolbox）
if exist('pointCloud', 'class') == 8target_pc = pointCloud(target_cloud);source_pc = pointCloud(transformed_cloud);% 使用内置ICP函数[tform, moved_pc] = pcregistericp(source_pc, target_pc);% 提取变换矩阵R_builtin = tform.R';t_builtin = tform.Translation';% 计算误差R_error_builtin = norm(R - R_builtin, 'fro');t_error_builtin = norm(t - t_builtin);fprintf('内置ICP函数结果:\n');fprintf('  旋转矩阵误差: %.6f\n', R_error_builtin);fprintf('  平移向量误差: %.6f\n', t_error_builtin);% 可视化比较figure('Name', '内置ICP与自定义ICP比较', 'Position', [100, 100, 1000, 400]);subplot(1, 2, 1);plot3(target_cloud(:,1), target_cloud(:,2), target_cloud(:,3), 'b.', 'MarkerSize', 8);hold on;plot3(source_cloud(:,1), source_cloud(:,2), source_cloud(:,3), 'g.', 'MarkerSize', 8);axis equal;grid on;title('自定义ICP结果');legend('目标点云', '配准后的源点云');view(45, 30);subplot(1, 2, 2);plot3(target_cloud(:,1), target_cloud(:,2), target_cloud(:,3), 'b.', 'MarkerSize', 8);hold on;plot3(moved_pc.Location(:,1), moved_pc.Location(:,2), moved_pc.Location(:,3), 'm.', 'MarkerSize', 8);axis equal;grid on;title('内置ICP结果');legend('目标点云', '配准后的源点云');view(45, 30);
elsefprintf('未找到Computer Vision Toolbox，跳过内置ICP比较\n');
endfprintf('\nICP点云配准演示完成!\n');