MATLAB实现两组点云ICP配准

一、算法原理与流程

1. ICP算法核心步骤

1. 初始化变换矩阵T0
2. 循环迭代:a. 寻找最近点对b. 计算最优变换Tic. 更新源点云:Pi+1=Ti⋅Pid. 判断收敛条件
3. 输出最终变换Tn

2. 参数说明

参数	典型值	作用说明
MaxIterations	100	最大迭代次数
Tolerance	1e-6	变换矩阵收敛阈值
DistanceThreshold	0.01	最近点搜索距离阈值
InlierRatio	0.9	有效点对比例

---

二、代码

1. 基础配准代码

% 读取点云数据
source = pcread('source.ply');  % 源点云
target = pcread('target.ply');  % 目标点云

% 数据预处理
source_denoised = pcdenoise(source, 'NumNeighbors', 10, 'Threshold', 1.5);
target_denoised = pcdenoise(target, 'NumNeighbors', 10, 'Threshold', 1.5);

ptCloudA = pcdownsample(source_denoised, 'gridAverage', 0.01);
ptCloudB = pcdownsample(target_denoised, 'gridAverage', 0.01);

% 初始变换估计（可选）
initialTform = pcregistericp(ptCloudA, ptCloudB, 'Extrapolate', true);

% ICP配准参数设置
opt = pcregistericp('MaxIterations', 200, ...
                    'Tolerance', 1e-8, ...
                    'DistanceThreshold', 0.005, ...
                    'InlierRatio', 0.9, ...
                    'InitialTransform', initialTform);

% 执行配准
[icpTransform, inlierIndices] = pcregistericp(ptCloudA, ptCloudB, opt);

% 应用变换
registeredPtCloud = pctransform(ptCloudA, icpTransform);

% 可视化结果
figure;
pcshowpair(registeredPtCloud, target);
title('ICP配准结果');
xlabel('X'); ylabel('Y'); zlabel('Z');

2. 增强版代码（含鲁棒性优化）

function [tform, rmse] = robust_icp(source, target, maxIter)
    % 参数设置
    options = statset('MaxIter', maxIter);
    
    % 初始变换
    tform = affine3d(eye(4));
    
    % 预处理
    source_normals = pcnormals(source, 30);
    target_normals = pcnormals(target, 30);
    
    % 迭代优化
    for iter = 1:maxIter
        % 寻找最近点
        kdtree = KDTreeSearcher(target.Location);
        [~, idx] = knnsearch(kdtree, source.Location);
        
        % 计算误差
        errors = sqrt(sum((source.Location - target.Location(idx,:)).^2, 2));
        inliers = errors < 3*mean(errors);
        
        % 鲁棒损失函数（Huber核）
        weights = huber_loss(errors(inliers), 1.0);
        
        % 构建线性方程组
        A = source(inliers).Location * target(inliers).Location' * diag(weights);
        B = source(inliers).Location * ones(size(target(inliers).Location,1),1) * diag(weights);
        
        % 求解变换矩阵
        tform = estimateGeometricTransform3D(A, B);
        
        % 更新点云
        source = pctransform(source, tform);
        
        % 收敛判断
        if iter > 10 && norm(tform.T(1:3,4)) < 1e-6
            break;
        end
    end
    
    % 计算RMSE
    [~, dist] = knnsearch(target.Location, source.Location);
    rmse = sqrt(mean(dist.^2));
end

function w = huber_loss(r, c)
    w = 1.0 ./ max(1.0, abs(r)/c);
end

---

三、改进

1. 多尺度配准

% 多尺度金字塔配准
pyramidLevels = 3;
scaleFactors = [0.5, 0.25, 0.125];

currentSource = source;
currentTarget = target;

for level = 1:pyramidLevels
    % 下采样
    currentSource = pcdownsample(currentSource, 'gridAverage', scaleFactors(level));
    currentTarget = pcdownsample(currentTarget, 'gridAverage', scaleFactors(level));
    
    % 计算初始变换
    [tform, ~] = pcregistericp(currentSource, currentTarget);
    
    % 更新源点云
    currentSource = pctransform(currentSource, tform);
end

2. 特征辅助配准

% 提取FPFH特征
sourceFeatures = pcfeatures(source, 'Method', 'fpfh', 'Radius', 0.05);
targetFeatures = pcfeatures(target, 'Method', 'fpfh', 'Radius', 0.05);

% 特征匹配
indexPairs = matchFeatures(sourceFeatures, targetFeatures, ...
    'Method', 'Approximate', 'Unique', true);

% 获取初始变换
[tform, ~] = estimateGeometricTransform3D(source(indexPairs(:,1)), ...
    target(indexPairs(:,2)));

---

四、性能评估

1. 定量评估代码

% 计算配准误差
transformedPoints = applyTransform(source, tform);
errors = sqrt(sum((transformedPoints - target.Location).^2, 2));

% 输出结果
fprintf('配准精度评估:');
fprintf('  最大误差: %.4f mm\n', max(errors)*1000);
fprintf('  平均误差: %.4f mm\n', mean(errors)*1000);
fprintf('  RMSE: %.4f mm\n', sqrt(mean(errors.^2))*1000);

2. 可视化验证

% 配准效果可视化
figure;
subplot(1,2,1);
pcshow(source);
title('原始源点云');

subplot(1,2,2);
pcshow(target);
title('目标点云');

figure;
pcshowpair(registeredPtCloud, target);
title('配准结果对比');

---

五、典型应用案例

1. 三维重建配准

% 多视角点云配准
ptClouds = load('multi_view_data.mat');  % 包含多个视角点云

% 逐次配准
refCloud = ptClouds{1};
for i = 2:length(ptClouds)
    [tform, ~] = pcregistericp(ptClouds{i}, refCloud);
    ptClouds{i} = pctransform(ptClouds{i}, tform);
end

% 合并点云
mergedCloud = pcmerge(ptClouds{1}, ptClouds{2}, 0.001);
for i = 3:length(ptClouds)
    mergedCloud = pcmerge(mergedCloud, ptClouds{i}, 0.001);
end

2. 机器人SLAM配准

% 激光雷达点云配准
lidarData = load('lidar_data.mat');  % 包含时间序列点云

% 初始化参考帧
refFrame = lidarData{1};

% 实时配准循环
for i = 2:length(lidarData)
    [tform, ~] = pcregistericp(lidarData{i}, refFrame, ...
        'MaxIterations', 50, 'Tolerance', 1e-5);
    
    % 更新地图
    map = pcmerge(map, pctransform(lidarData{i}, tform), 0.01);
end

---

六、常见问题解决方案

问题现象	解决方法	理论依据
配准不收敛	增加最大迭代次数 + 调整距离阈值	收敛条件设置不当
局部最优解	添加随机采样 + 多尺度配准	初始位置敏感
计算速度慢	启用GPU加速 + 降采样	计算复杂度高
噪声干扰大	使用鲁棒核函数(Huber/Cauchy)	异常值影响
初始偏移过大	特征匹配预配准	全局搜索缺失

---

七、参考

1. MATLAB官方文档 ：Point Cloud Registration ww2.mathworks.cn/help/vision/ref/pcregistericp.html
2. 参考代码 ： ICP点云数据配准 www.youwenfan.com/contentcsm/78279.html
3. 学术论文： "Fast Point Cloud Registration using Gaussian Mixture Models" (IEEE 2020) "Robust ICP with Geometric Feature Constraints" (ICRA 2021)