Matlab通过GUI实现点云的Loss配准
本次分享点云的Loss配准，即鲁棒核函数修改配准。迭代最近点（ICP）算法是点云配准的核心方法，通过 “寻找对应点→估计变换矩阵→最小化点对距离误差” 的迭代过程，实现两组点云（源点云 / 目标点云）的坐标系统一。但其传统形式依赖最小二乘误差准则，对噪声、离群点（如遮挡、扫描误差导致的异常点）敏感，易出现配准偏移或不收敛。
一、鲁棒核函数可变的意义
鲁棒核函数：通过引入非二次核函数（如 Cauchy 核、Huber 核、Tukey 核）替代传统最小二乘的平方核，降低离群点对误差计算的权重（例如 Cauchy 核对大残差点赋予低权重），提升配准抗干扰能力。
“可变” 特性：区别于固定核函数的 ICP，该方法可根据点云数据特征（如噪声强度、离群点比例、场景复杂度）动态选择或调整核函数类型 / 参数（如高斯核的 σ 值、Tukey 核的阈值），适配不同数据场景。
Matlab 的PointCloud Processing Toolbox（R2019b 及以后版本）提供点云预处理、配准、可视化的完整工具链，支持：
点云数据结构（pointCloud类）与基础操作（去噪、降采样）；
原生 ICP 函数（pcregistericp）的参数自定义（可嵌入核函数逻辑）；
自定义目标函数（通过fmincon等优化函数实现核函数可变逻辑）；
配准结果可视化（pcshowpair、pcshow）与误差评估（均方根误差 RMSE）。
二、主要流程（Matlab 实现）
步骤 1：点云数据预处理
目的：降低噪声与冗余数据对配准的干扰，为 ICP 提供高质量输入。
去噪：使用pcdenoise函数去除孤立噪声点，基于局部点密度或统计距离（如标准差阈值）筛选有效点；
% 示例：对源点云去噪sourceCloud = pcread('source.pcd');[denoisedSource, ~] = pcdenoise(sourceCloud, 'NumNeighbors', 10, 'Threshold', 1.5);
降采样：通过pcdownsample（体素网格 / 随机采样）减少点云数量，提升迭代效率；
初始配准：若源点云与目标点云初始偏差大，先用pcalign（基于特征 / 手动对齐）获取粗略变换矩阵，避免 ICP 陷入局部最优。
步骤 2：鲁棒核函数的选择与构建
根据点云特征动态确定核函数类型，核心是定义核加权误差函数：
核函数选择逻辑：
- 离群点比例 < 10%：Huber 核（小残差用平方核，大残差用线性核，平衡精度与鲁棒性）；
- 离群点比例 10%-30%：Cauchy 核（对大残差权重衰减更显著，抗差性更强）；
- 噪声呈高斯分布：高斯核（通过 σ 值控制权重衰减速度，适配噪声强度）；
Matlab 核函数实现：定义自定义函数计算核权重，例如 Cauchy 核：
function w = cauchyKernel(residual, c)% residual：点对残差（对应点距离），c：核参数（根据数据自适应调整）w = 1 ./ (1 + (residual / c).^2);end步骤 3：ICP 迭代优化（核函数可变逻辑嵌入）
基于核加权误差函数实现迭代，替代传统 ICP 的最小二乘准则：
1. 对应点搜索：对源点云中每个点，通过knnsearch（k 近邻）在目标点云中寻找最近点，形成初始点对；
2. 残差计算与核权重更新：计算每个点对的残差（欧氏距离），调用步骤 2 的核函数获取权重，若迭代中残差分布变化（如离群点比例下降），动态调整核函数参数（如 c 值）；
3. 变换矩阵估计：基于核加权最小二乘（权重矩阵为对角阵，对角元素为核权重），通过 SVD 分解求解最优旋转矩阵（R）与平移向量（t）；
4. 收敛判断：计算变换矩阵的变化量（ΔR、Δt）或核加权误差的下降率，若小于设定阈值（如 1e-6），或迭代次数达到上限，停止迭代；否则更新源点云坐标（pcapplytransform），返回步骤 1 继续迭代。
步骤 4：配准结果评估与可视化
定量评估：计算核加权 RMSE（加权残差的均方根），对比配准前后 RMSE 变化，判断精度；
定性评估：用pcshowpair叠加显示配准后的源点云与目标点云，观察重叠区域一致性：
targetCloud = pcread('target.pcd');registeredSource = pcapplytransform(denoisedSource, [R t; 0 0 0 1]);pcshowpair(registeredSource, targetCloud, 'VerticalAxis', 'Y', 'Colormap', [1 0 0; 0 1 0]);title('配准后点云（红：源点云，绿：目标点云）');三、应用领域
1. 三维重建（文物 / 场景建模）
场景需求：多视角扫描的点云存在遮挡（如文物的凹陷处）、扫描噪声（激光反射不均），离群点比例动态变化；
应用价值：可变核函数可适配不同视角点云的噪声特征（如正面扫描噪声少用 Huber 核，侧面扫描离群点多用 Cauchy 核），Matlab 的可视化工具可实时验证重建完整性，助力文物数字化保护、室内场景建模。
2. 自动驾驶环境感知
场景需求：激光雷达（LiDAR）点云受天气影响（雨天 / 雾天）产生噪声点，车辆动态行驶中需实时配准多帧点云，构建环境地图；
应用价值：Matlab 的pcregistericp可通过参数化核函数快速迭代（满足毫秒级响应），可变核函数适配不同天气下的点云质量（晴天用高斯核，雨天切换 Cauchy 核），提升障碍物检测与路径规划精度。
3. 工业检测（零件精度验证）
场景需求：工业零件扫描点云与 CAD 模型（目标点云）配准，需排除表面杂质（如油污、划痕）导致的离群点，评估零件尺寸偏差；
应用价值：通过 Matlab 自定义核函数，可针对零件不同区域（如光滑面 / 边角）调整核参数（光滑面用 Huber 核保证精度，边角离群点多用 Tukey 核），结合pcfitplane等函数计算偏差，实现自动化检测。
4. 医学影像（器官配准）
场景需求：CT/MRI 扫描的器官点云（如肝脏、骨骼）存在运动伪影（呼吸 / 心跳导致），需配准不同模态影像以定位病灶；
应用价值：Matlab 的点云工具箱支持医学影像格式（如 DICOM 转点云），可变核函数可适配不同器官的点云特征（如骨骼点云噪声少用高斯核，软组织离群点多用 Cauchy 核），提升病灶定位的准确性。
本次使用的数据是兔砸砸砸砸。
一、点云Loss配准的程序
1、最简版
clc; clear; close all;%% 1. 读取点云（原始）
[file, path] = uigetfile({'*.pcd;*.ply','点云文件 (*.pcd,*.ply)'}, ...'请选择点云（例如 bunny.pcd）');
if file==0; return; end
sourcePtCloud = pcread(fullfile(path,file));
targetPtCloud = pointCloud(sourcePtCloud.Location, ...'Color', sourcePtCloud.Color);%% 2. 对目标点云做刚性变换 + 噪声（原始）
ang = deg2rad(10);
R = [cos(ang) -sin(ang) 0;sin(ang) cos(ang) 0;0 0 1];
t = [0.01 0.015 0.02];
P = targetPtCloud.Location;
P = (R * P')'; P = P + t;
P = P + 0.0005 * randn(size(P));
targetPtCloud = pointCloud(P, 'Color', targetPtCloud.Color);
sourcePtCloud.Color = repmat(uint8([255 0 0]), size(sourcePtCloud.Location,1), 1);
targetPtCloud.Color = repmat(uint8([0 255 0]), size(P,1), 1);%% 3. 法向量（原始）
source_normals = compute_normals_kdtree(sourcePtCloud.Location, 30);
target_normals = compute_normals_kdtree(targetPtCloud.Location, 30);
sourcePtCloud = pointCloud(sourcePtCloud.Location, 'Color', sourcePtCloud.Color, 'Normal', source_normals);
targetPtCloud = pointCloud(targetPtCloud.Location, 'Color', targetPtCloud.Color, 'Normal', target_normals);%% 4. 初始可视化（原始）
figure('Name','初始位姿','Color','w'); pcshowpair(sourcePtCloud, targetPtCloud);
title('红色=source，绿色=target'); axis equal;%% 5. 复制点云（原始）
source1 = sourcePtCloud; target1 = targetPtCloud;%% 6. 配准前评估（原始）
trans_init = eye(4);
pts = sourcePtCloud.Location;
ptsHom = [pts, ones(size(pts,1),1)]; % 齐次坐标 [n,4]
transformedHom = (trans_init * ptsHom')'; % 4×4 矩阵乘法
source_init_pts = transformedHom(:,1:3); % 取回 xyz
source_init = pointCloud(source_init_pts, ...'Color', sourcePtCloud.Color, ...'Normal', sourcePtCloud.Normal); % 法向量保留distances = pdist2(source_init.Location, targetPtCloud.Location);
minDist = min(distances, [], 2);
adaptiveThreshold = 3 * mean(minDist); % 【关键修改】自适应阈值
inliers = minDist < adaptiveThreshold;
fitness0 = sum(inliers)/numel(inliers);
rmse0 = sqrt(mean(minDist(inliers).^2));
fprintf('配准前 —— 自适应阈值=%.4f，Fitness=%.6f，RMSE=%.6f\n', adaptiveThreshold, fitness0, rmse0);%% 7. ICP 配准（仅改 InlierDistance）
%% 1. 构造初始变换（只改这一行）
trans_init = rigidtform3d(eye(3), [0 0 0]); % 单位旋转 + 零平移
RobustLoss = 'Metric'; % 'Metric','Extrapolate','InlierRatio','MaxIterations','Tolerance','InitialTransform','Verbose','UseDegree','InlierDistance'
%% 8. ICP 配准（你已有的代码，不动）
[transObj, ~] = pcregistericp( ... % 返回第一个值是变换对象sourcePtCloud, targetPtCloud, ...'InlierDistance', adaptiveThreshold, ...'InitialTransform', trans_init, ...RobustLoss, 'pointToPlane', ...'MaxIterations', 50);%% 9. 把变换对象应用到源点云，得到“配准后点云”
source_registered = pctransform(sourcePtCloud, transObj); % 现在才是 pointCloud%% 10. 评估（沿用你的函数）
result = evaluateReg(source_registered, targetPtCloud, adaptiveThreshold);
fprintf('配准后 —— inlierRatio=%.2f%%，meanDist=%.6f，RMSE=%.6f\n', ...result.inlierRatio, result.meanDist, result.rmse);
%% 8. 结果可视化（原始）
figure('Name','配准后','Color','w'); pcshowpair(source_registered, target1);
title('配准后（红=配准后源点云，绿=目标点云）'); axis equal;%% 9. 法向量函数（原始，未动）
function normals = compute_normals_kdtree(points, k)[n, ~] = size(points); normals = zeros(n, 3);kd_tree = KDTreeSearcher(points);for i = 1:n[idx, ~] = knnsearch(kd_tree, points(i,:), 'K', k+1);neighbors = points(idx(2:end), :);centroid = mean(neighbors, 1);[V, ~] = eig((neighbors - centroid)' * (neighbors - centroid));normals(i,:) = V(:,1)';endnormals = normals ./ sqrt(sum(normals.^2, 2));
end%% ========== 本地函数：完全照抄你给的模板 ==========
function result = evaluateReg(regCloud, targetCloud, inlierThresh)regPoints = regCloud.Location; % M×3targetPoints = targetCloud.Location; % N×3[~, dists] = knnsearch(targetPoints, regPoints); % 最近邻距离result.inlierRatio = mean(dists < inlierThresh) * 100; % 内点比例(%)result.meanDist = mean(dists); % 平均距离result.rmse = sqrt(mean(dists.^2)); % RMSE
end2、GUI版本
function ICP_Register_GUI_RobustLoss
%ICP_Register_GUI_RobustLoss 基于ICP算法的点云配准GUI（支持多种RobustLoss参数调整）
clc; clear; close all;%% ---------------- 主窗口初始化 ----------------
fig = figure('Name','ICP配准工具（鲁棒损失可调）', 'NumberTitle','off', ...'MenuBar','none', 'ToolBar','none', ...'Position',[100 100 1366 768], 'Color','w');%% ---------------- 布局常量定义 ----------------
imgWidth = 0.78; % 三栏显示区总宽度（归一化）
panelW = imgWidth/3 - 0.005; % 单个显示面板宽度
gap = 0.005; % 面板间距
ctrlW = 1 - imgWidth; % 右侧控制面板宽度%% ---------------- 三栏显示面板（原始/目标/配准后） ----------------
% 1. 原始点云面板（红色）
pnlSrc = uipanel('Parent',fig, 'Units','normalized', ...'FontSize',14, 'Position',[0.01 0.01 panelW 0.98], ...'Title','原始点云（红）');
drawnow;
titleObj = findobj(pnlSrc, 'Type', 'text');
if ~isempty(titleObj)set(titleObj, 'FontWeight', 'bold');
end
axSrc = axes('Parent',pnlSrc, 'Units','normalized', 'Position',[0.05 0.05 0.9 0.9]);% 2. 目标点云面板（绿色）
pnlTgt = uipanel('Parent',fig, 'Units','normalized', ...'FontSize',14, 'Position',[0.01+panelW+gap 0.01 panelW 0.98], ...'Title','目标点云（绿，含变换+噪声）');
drawnow;
titleObj = findobj(pnlTgt, 'Type', 'text');
if ~isempty(titleObj)set(titleObj, 'FontWeight', 'bold');
end
axTgt = axes('Parent',pnlTgt, 'Units','normalized', 'Position',[0.05 0.05 0.9 0.9]);% 3. 配准后点云面板
pnlReg = uipanel('Parent',fig, 'Units','normalized', ...'FontSize',14, 'Position',[0.01+2*(panelW+gap) 0.01 panelW 0.98], ...'Title','ICP配准后（红→绿）');
drawnow;
titleObj = findobj(pnlReg, 'Type', 'text');
if ~isempty(titleObj)set(titleObj, 'FontWeight', 'bold');
end
axReg = axes('Parent',pnlReg, 'Units','normalized', 'Position',[0.05 0.05 0.9 0.9]);%% ---------------- 右侧控制面板 ----------------
pnlCtrl = uipanel('Parent',fig, 'Units','normalized', ...'FontSize',14, 'Position',[imgWidth 0 ctrlW 1], ...'Title','控制与参数');
drawnow;
titleObj = findobj(pnlCtrl, 'Type', 'text');
if ~isempty(titleObj)set(titleObj, 'FontWeight', 'bold');
end% 控制面板位置参数
txtH = 0.03; btnH = 0.05; sliderH = 0.03;
yPos = 0.95; margin = 0.02;%% ---------------- 控制面板组件：1. 点云加载 ----------------
uicontrol('Parent',pnlCtrl, 'Style','pushbutton', 'String','浏览加载点云', ...'FontSize',12, 'Units','normalized', ...'Position',[0.05 yPos-btnH 0.9 0.06], ...'Callback',@loadPointCloud, 'FontWeight','bold');
yPos = yPos - btnH - margin;% 状态提示
lblStatus = uicontrol('Parent',pnlCtrl, 'Style','text', ...'String','状态：未加载点云', 'FontSize',10, ...'Units','normalized', 'ForegroundColor','red', ...'Position',[0.05 yPos-txtH 0.9 txtH], ...'HorizontalAlignment','left');
yPos = yPos - txtH - margin/2;%% ---------------- 控制面板组件：2. 目标点云参数 ----------------
% 2.1 绕Z轴旋转
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','1. 绕Z轴旋转 (度)', ...'FontSize',11, 'FontWeight','bold', 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.9 txtH], 'HorizontalAlignment','left');
yPos = yPos - txtH - margin/2;sliderRot = uicontrol('Parent',pnlCtrl, 'Style','slider', ...'Min',0, 'Max',180, 'Value',10, 'SliderStep',[1/180,5/180], ...'Units','normalized', 'Position',[0.05 yPos-sliderH 0.65 sliderH], ...'Callback',@refreshTargetCloud);
txtRot = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','10', ...'FontSize',11, 'Units','normalized', ...'Position',[0.75 yPos-sliderH 0.2 sliderH], ...'Callback',@editRotation);
yPos = yPos - sliderH - margin/2;% 2.2 平移偏移
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','2. 平移偏移 (毫米)', ...'FontSize',11, 'FontWeight','bold', 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.9 txtH], 'HorizontalAlignment','left');
yPos = yPos - txtH - margin/2;% X轴平移
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','X：', ...'FontSize',10, 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.1 txtH]);
editTx = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','10', ...'FontSize',11, 'Units','normalized', ...'Position',[0.15 yPos-txtH 0.25 txtH], 'Callback',@refreshTargetCloud);% Y轴平移
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','Y：', ...'FontSize',10, 'Units','normalized', ...'Position',[0.45 yPos-txtH 0.1 txtH]);
editTy = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','15', ...'FontSize',11, 'Units','normalized', ...'Position',[0.55 yPos-txtH 0.25 txtH], 'Callback',@refreshTargetCloud);
yPos = yPos - txtH - margin/2;% Z轴平移
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','Z：', ...'FontSize',10, 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.1 txtH]);
editTz = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','20', ...'FontSize',11, 'Units','normalized', ...'Position',[0.15 yPos-txtH 0.25 txtH], 'Callback',@refreshTargetCloud);
yPos = yPos - txtH - margin/2;% 2.3 噪声强度
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','3. 高斯噪声 (毫米)', ...'FontSize',11, 'FontWeight','bold', 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.9 txtH], 'HorizontalAlignment','left');
yPos = yPos - txtH - margin/2;sliderNoise = uicontrol('Parent',pnlCtrl, 'Style','slider', ...'Min',0, 'Max',5, 'Value',0.5, 'SliderStep',[0.1/5,0.5/5], ...'Units','normalized', 'Position',[0.05 yPos-sliderH 0.65 sliderH], ...'Callback',@refreshTargetCloud);
txtNoise = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','0.5', ...'FontSize',11, 'Units','normalized', ...'Position',[0.75 yPos-sliderH 0.2 sliderH], ...'Callback',@editNoise);
yPos = yPos - sliderH - margin;%% ---------------- 控制面板组件：3. ICP配准参数 ----------------
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','4. ICP配准参数', ...'FontSize',11, 'FontWeight','bold', 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.9 txtH], 'HorizontalAlignment','left');
yPos = yPos - txtH - margin/2;% 3.1 RobustLoss选择（对应Python中的鲁棒核函数）
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','RobustLoss类型：', ...'FontSize',10, 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.4 txtH]);
popupRobustLoss = uicontrol('Parent',pnlCtrl, 'Style','popupmenu', ...'String',{'L2Loss (平方损失)'; 'L1Loss (绝对值损失)'; ...'HuberLoss (Huber损失)'; 'CauchyLoss (Cauchy损失)'; ...'pointToPlane (点到面)'}, ...'FontSize',10, 'Units','normalized', ...'Position',[0.5 yPos-txtH 0.45 txtH], ...'Value',1, 'Callback',@updateLossParameters); % 默认L2损失
yPos = yPos - txtH - margin/2;% 3.2 鲁棒损失参数（根据选择的损失函数显示不同参数）
pnlLossParams = uipanel('Parent',pnlCtrl, 'Units','normalized', ...'Title','损失函数参数', 'FontSize',10, ...'Position',[0.05 yPos-0.1 0.9 0.1]);
yPos = yPos - 0.1 - margin/2;% 损失函数参数 - delta值（Huber和Cauchy需要）
uicontrol('Parent',pnlLossParams, 'Style','text', 'String','delta值：', ...'FontSize',10, 'Units','normalized', ...'Position',[0.1 0.6 0.3 0.3]);
editDelta = uicontrol('Parent',pnlLossParams, 'Style','edit', 'String','0.1', ...'FontSize',11, 'Units','normalized', ...'Position',[0.4 0.6 0.4 0.3]);% 3.3 最大迭代次数
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','最大迭代次数：', ...'FontSize',10, 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.4 txtH]);
editMaxIter = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','50', ...'FontSize',11, 'Units','normalized', ...'Position',[0.5 yPos-txtH 0.45 txtH]);
yPos = yPos - txtH - margin/2;% 3.4 内点距离阈值倍数
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','内点阈值倍数：', ...'FontSize',10, 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.4 txtH]);
sliderThresh = uicontrol('Parent',pnlCtrl, 'Style','slider', ...'Min',1, 'Max',10, 'Value',3, 'SliderStep',[0.1/9,1/9], ...'Units','normalized', 'Position',[0.5 yPos-sliderH 0.45 sliderH], ...'Callback',@updateThreshText);
txtThresh = uicontrol('Parent',pnlCtrl, 'Style','text', 'String','3.0', ...'FontSize',10, 'Units','normalized', ...'Position',[0.8 0.45 0.15 txtH], 'HorizontalAlignment','center');
yPos = yPos - sliderH - margin;%% ---------------- 控制面板组件：4. 配准执行 ----------------
uicontrol('Parent',pnlCtrl, 'Style','pushbutton', 'String','运行ICP配准', ...'FontSize',12, 'Units','normalized', 'BackgroundColor',[0.2 0.8 0.2], ...'Position',[0.05 yPos-btnH 0.9 0.06], 'FontWeight','bold', ...'Callback',@runICPRegistration);
yPos = yPos - btnH - margin;%% ---------------- 控制面板组件：5. 结果评价 ----------------
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','配准结果评价', ...'FontSize',11, 'FontWeight','bold', 'Units','normalized', ...'Position',[0.05 yPos-txtH 0.9 txtH], 'HorizontalAlignment','left');
yPos = yPos - txtH - margin/2;% 评价指标显示
lblFitnessBefore = uicontrol('Parent',pnlCtrl, 'Style','text', ...'String','配准前Fitness：--', 'FontSize',10, ...'Units','normalized', 'Position',[0.05 yPos-txtH 0.9 txtH]);
yPos = yPos - txtH - margin/4;lblRMSBefore = uicontrol('Parent',pnlCtrl, 'Style','text', ...'String','配准前RMSE：-- mm', 'FontSize',10, ...'Units','normalized', 'Position',[0.05 yPos-txtH 0.9 txtH]);
yPos = yPos - txtH - margin/4;lblInlierAfter = uicontrol('Parent',pnlCtrl, 'Style','text', ...'String','配准后内点率：-- %', 'FontSize',10, ...'Units','normalized', 'Position',[0.05 yPos-txtH 0.9 txtH]);
yPos = yPos - txtH - margin/4;lblRMSAfter = uicontrol('Parent',pnlCtrl, 'Style','text', ...'String','配准后RMSE：-- mm', 'FontSize',10, ...'Units','normalized', 'Position',[0.05 yPos-txtH 0.9 txtH]);
yPos = yPos - txtH - margin;%% ---------------- 控制面板组件：6. 结果保存 ----------------
uicontrol('Parent',pnlCtrl, 'Style','pushbutton', 'String','保存配准后点云', ...'FontSize',12, 'Units','normalized', 'BackgroundColor',[0.8 0.8 0.2], ...'Position',[0.05 yPos-btnH 0.9 0.06], 'FontWeight','bold', ...'Callback',@saveRegisteredCloud);
yPos = yPos - btnH - margin;%% ---------------- 数据容器 ----------------
sourcePtCloud = pointCloud.empty; % 原始点云
targetPtCloud = pointCloud.empty; % 目标点云
sourceRegPtCloud = pointCloud.empty;% 配准后点云
transObj = rigidtform3d(eye(3), [0 0 0]); % 变换对象
fitnessBefore = 0; rmseBefore = 0; % 配准前指标
inlierRatioAfter = 0; rmseAfter = 0;% 配准后指标
lastRot = 10; lastNoise = 0.5; % 用于控制刷新频率
adaptiveThreshold = 0; % 自适应阈值
robustLossParams = struct('type', 'L2Loss', 'delta', 0.1); % 鲁棒损失参数%% =========================================================================
%% ---------------- 回调函数：更新损失函数参数显示 ----------------
%% =========================================================================function updateLossParameters(~,~)lossType = get(popupRobustLoss, 'Value');% 根据选择的损失函数类型设置参数可见性if lossType == 3 || lossType == 4 % Huber或Cauchy需要delta参数set(editDelta, 'Enable', 'on');else % L1、L2和点到面不需要delta参数set(editDelta, 'Enable', 'off');end% 更新当前损失函数类型lossTypes = get(popupRobustLoss, 'String');robustLossParams.type = lossTypes{lossType};robustLossParams.delta = str2double(get(editDelta, 'String'));end%% =========================================================================
%% ---------------- 回调函数：1. 加载点云 ----------------
%% =========================================================================function loadPointCloud(~,~)[file, path] = uigetfile({'*.pcd;*.ply','点云文件 (*.pcd,*.ply)'}, ...'选择原始点云文件');if isequal(file,0); return; endtrysourcePtCloud = pcread(fullfile(path,file));% 降采样优化（大型点云提速）if sourcePtCloud.Count > 10000sourcePtCloud = pcdownsample(sourcePtCloud, 'gridAverage', 0.005);end% 检查点云数量是否足够（至少100个点）if sourcePtCloud.Count < 100error('点云数量过少（需至少100个点）');endset(lblStatus, 'String', sprintf('状态：已加载 %s（%d个点）', ...file, sourcePtCloud.Count), 'ForegroundColor','green');refreshTargetCloud();catch MEerrordlg(ME.message, '读取失败');set(lblStatus, 'String','状态：读取失败，请重新选择', 'ForegroundColor','red');sourcePtCloud = pointCloud.empty;endend%% =========================================================================
%% ---------------- 回调函数：2. 刷新目标点云 ----------------
%% =========================================================================function refreshTargetCloud(~,~)if isempty(sourcePtCloud); return; end% 控制刷新频率（微小变化不刷新）currentRot = get(sliderRot, 'Value');currentNoise = get(sliderNoise, 'Value');if abs(currentRot - lastRot) < 0.5 && abs(currentNoise - lastNoise) < 0.1return;endlastRot = currentRot;lastNoise = currentNoise;% 读取参数rotAngle = currentRot;tx = str2double(get(editTx, 'String')) / 1000;ty = str2double(get(editTy, 'String')) / 1000;tz = str2double(get(editTz, 'String')) / 1000;sigma = currentNoise / 1000;% 参数校验if any(isnan([tx, ty, tz]))[tx, ty, tz] = deal(0.01, 0.015, 0.02);set(editTx, 'String','10'); set(editTy, 'String','15'); set(editTz, 'String','20');endsigma = max(0, sigma);% 生成目标点云srcLoc = sourcePtCloud.Location;angRad = deg2rad(rotAngle);R = [cos(angRad) -sin(angRad) 0;sin(angRad) cos(angRad) 0;0 0 1];tVec = [tx, ty, tz];tgtLoc = (R * srcLoc')' + repmat(tVec, size(srcLoc,1), 1);tgtLoc = tgtLoc + sigma * randn(size(tgtLoc));% 计算法向量source_normals = compute_normals_kdtree(srcLoc, 30);target_normals = compute_normals_kdtree(tgtLoc, 30);% 上色并显示sourcePtCloud = pointCloud(srcLoc, ...'Normal', source_normals, ...'Color', repmat(uint8([255 0 0]), size(srcLoc,1), 1));targetPtCloud = pointCloud(tgtLoc, ...'Normal', target_normals, ...'Color', repmat(uint8([0 255 0]), size(tgtLoc,1), 1));showPointCloud(axSrc, sourcePtCloud, '原始点云（红）');showPointCloud(axTgt, targetPtCloud, '目标点云（绿）');% 清空配准结果sourceRegPtCloud = pointCloud.empty;cla(axReg); title(axReg, 'ICP配准后（红→绿）');% 计算配准前评估指标computePreRegistrationMetrics();% 同步显示set(txtRot, 'String', sprintf('%.0f', rotAngle));set(txtNoise, 'String', sprintf('%.1f', sigma*1000));end%% =========================================================================
%% ---------------- 回调函数：3. 编辑旋转角度 ----------------
%% =========================================================================function editRotation(~,~)rotVal = str2double(get(txtRot, 'String'));rotVal = max(0, min(180, rotVal));if isnan(rotVal); rotVal = 10; endset(txtRot, 'String', sprintf('%.0f', rotVal));set(sliderRot, 'Value', rotVal);refreshTargetCloud();end%% =========================================================================
%% ---------------- 回调函数：4. 编辑噪声强度 ----------------
%% =========================================================================function editNoise(~,~)noiseVal = str2double(get(txtNoise, 'String'));noiseVal = max(0, min(5, noiseVal));if isnan(noiseVal); noiseVal = 0.5; endset(txtNoise, 'String', sprintf('%.1f', noiseVal));set(sliderNoise, 'Value', noiseVal);refreshTargetCloud();end%% =========================================================================
%% ---------------- 回调函数：5. 更新阈值文本 ----------------
%% =========================================================================function updateThreshText(~,~)threshVal = get(sliderThresh, 'Value');set(txtThresh, 'String', sprintf('%.1f', threshVal));end%% =========================================================================
%% ---------------- 回调函数：6. 运行ICP配准 ----------------
%% =========================================================================function runICPRegistration(~,~)if isempty(sourcePtCloud) || isempty(targetPtCloud)errordlg('请先加载原始点云并生成目标点云', '提示');return;endset(lblStatus, 'String','状态：ICP配准中...', 'ForegroundColor','#FFA500');drawnow;try% 获取ICP参数lossIdx = get(popupRobustLoss, 'Value');lossTypes = get(popupRobustLoss, 'String');robustLossType = lossTypes{lossIdx};maxIter = str2double(get(editMaxIter, 'String'));threshMultiplier = get(sliderThresh, 'Value');delta = str2double(get(editDelta, 'String'));% 参数校验if isnan(maxIter) || maxIter < 10maxIter = 50;set(editMaxIter, 'String', '50');endif isnan(delta) || delta <= 0delta = 0.1;set(editDelta, 'String', '0.1');end% 构造初始变换trans_init = rigidtform3d(eye(3), [0 0 0]); % 单位旋转 + 零平移% 根据选择的鲁棒损失函数设置ICP参数if strcmp(robustLossType, 'pointToPlane (点到面)')% 点到面配准[transObj, ~] = pcregistericp( ... sourcePtCloud, targetPtCloud, ...'InlierDistance', adaptiveThreshold, ...'InitialTransform', trans_init, ...'Metric', 'pointToPlane', ...'MaxIterations', maxIter);else% 点到点配准，使用不同的鲁棒损失函数% 先进行基本ICP配准获取初始变换[transObjBasic, ~] = pcregistericp( ...sourcePtCloud, targetPtCloud, ...'InlierDistance', adaptiveThreshold, ...'InitialTransform', trans_init, ...'Metric', 'pointToPoint', ...'MaxIterations', maxIter/2);% 应用鲁棒损失函数进行精配准sourceTransformed = pctransform(sourcePtCloud, transObjBasic);[transObj, ~] = robustICPPointToPoint( ...sourceTransformed.Location, targetPtCloud.Location, ...robustLossType, delta, adaptiveThreshold, maxIter/2);end% 应用最终变换得到配准后点云sourceRegPtCloud = pctransform(sourcePtCloud, transObj);% 显示结果showPointCloud(axReg, sourceRegPtCloud, '配准后点云（红）');hold(axReg, 'on');pcshow(targetPtCloud, 'Parent', axReg, 'MarkerSize',30);hold(axReg, 'off');title(axReg, 'ICP配准结果（红=配准后，绿=目标）');% 计算评价指标result = evaluateReg(sourceRegPtCloud, targetPtCloud, adaptiveThreshold);inlierRatioAfter = result.inlierRatio;rmseAfter = result.rmse * 1000; % 转换为毫米% 更新显示set(lblInlierAfter, 'String', sprintf('配准后内点率：%.1f %%', inlierRatioAfter));set(lblRMSAfter, 'String', sprintf('配准后RMSE：%.3f mm', rmseAfter));set(lblStatus, 'String','状态：ICP配准完成', 'ForegroundColor','green');catch MEerrordlg(ME.message, '配准失败');set(lblStatus, 'String','状态：配准失败，请检查点云', 'ForegroundColor','red');sourceRegPtCloud = pointCloud.empty;endend%% =========================================================================
%% ---------------- 回调函数：7. 保存配准结果 ----------------
%% =========================================================================function saveRegisteredCloud(~,~)if isempty(sourceRegPtCloud)errordlg('请先运行ICP配准生成结果', '提示');return;end[file, path] = uiputfile({'*.pcd;*.ply;*.xyz','点云文件'},'保存配准点云');if isequal(file,0); return; endtrypcwrite(sourceRegPtCloud, fullfile(path,file), 'Precision','double');msgbox('保存成功！','提示');catch MEerrordlg(ME.message, '保存失败');endend%% =========================================================================
%% ---------------- 辅助函数：计算配准前评估指标 ----------------
%% =========================================================================function computePreRegistrationMetrics()if isempty(sourcePtCloud) || isempty(targetPtCloud)return;endtrans_init = eye(4);pts = sourcePtCloud.Location;ptsHom = [pts, ones(size(pts,1),1)]; % 齐次坐标 [n,4]transformedHom = (trans_init * ptsHom')'; % 4×4 矩阵乘法source_init_pts = transformedHom(:,1:3); % 取回 xyzdistances = pdist2(source_init_pts, targetPtCloud.Location);minDist = min(distances, [], 2);% 计算自适应阈值threshMultiplier = get(sliderThresh, 'Value');adaptiveThreshold = threshMultiplier * mean(minDist);inliers = minDist < adaptiveThreshold;fitnessBefore = sum(inliers)/numel(inliers);rmseBefore = sqrt(mean(minDist(inliers).^2)) * 1000; % 转换为毫米% 更新显示set(lblFitnessBefore, 'String', sprintf('配准前Fitness：%.6f', fitnessBefore));set(lblRMSBefore, 'String', sprintf('配准前RMSE：%.3f mm', rmseBefore));set(lblInlierAfter, 'String','配准后内点率：-- %');set(lblRMSAfter, 'String','配准后RMSE：-- mm');end%% =========================================================================
%% ---------------- 辅助函数：显示点云 ----------------
%% =========================================================================function showPointCloud(ax, pc, titleStr)cla(ax); set(ax, 'Color','white');pcshow(pc, 'Parent', ax, 'MarkerSize',30);axis(ax, 'tight'); grid(ax, 'on'); view(ax, 3);title(ax, titleStr);xlabel(ax, 'X (m)'); ylabel(ax, 'Y (m)'); zlabel(ax, 'Z (m)');drawnow;end%% =========================================================================
%% ---------------- 辅助函数：计算法向量 ----------------
%% =========================================================================function normals = compute_normals_kdtree(points, k)[n, ~] = size(points); normals = zeros(n, 3);if n < k+1k = n-1; % 确保不超过点的数量endkd_tree = KDTreeSearcher(points);for i = 1:n[idx, ~] = knnsearch(kd_tree, points(i,:), 'K', k+1);neighbors = points(idx(2:end), :);centroid = mean(neighbors, 1);[V, ~] = eig((neighbors - centroid)' * (neighbors - centroid));normals(i,:) = V(:,1)';end% 归一化法向量normals = normals ./ sqrt(sum(normals.^2, 2));end%% =========================================================================
%% ---------------- 辅助函数：评估配准结果 ----------------
%% =========================================================================function result = evaluateReg(regCloud, targetCloud, inlierThresh)regPoints = regCloud.Location; % M×3targetPoints = targetCloud.Location; % N×3[~, dists] = knnsearch(targetPoints, regPoints); % 最近邻距离result.inlierRatio = mean(dists < inlierThresh) * 100; % 内点比例(%)result.meanDist = mean(dists); % 平均距离result.rmse = sqrt(mean(dists.^2)); % RMSEend%% =========================================================================
%% ---------------- 核心函数：带鲁棒损失函数的ICP点到点配准 ----------------
%% =========================================================================function [tform, metrics] = robustICPPointToPoint(source, target, lossType, delta, inlierThresh, maxIter)% 初始化变换矩阵tform = rigidtform3d(eye(3), [0 0 0]);currentSource = source;metrics = struct('rmse', [], 'inlierRatio', []);% 创建KD树加速最近邻搜索kdTree = KDTreeSearcher(target);for iter = 1:maxIter% 找到最近邻点[idx, dists] = knnsearch(kdTree, currentSource);correspondences = target(idx,:);% 根据鲁棒损失函数计算权重weights = computeRobustWeights(dists, lossType, delta);% 计算加权的点集中心点sourceCentroid = mean(currentSource .* weights, 1);targetCentroid = mean(correspondences .* weights, 1);% 去中心sourceCentered = currentSource - sourceCentroid;targetCentered = correspondences - targetCentroid;% 计算加权协方差矩阵W = diag(weights);H = sourceCentered' * W * targetCentered;% SVD分解计算旋转矩阵[U, ~, V] = svd(H);R = V * U';if det(R) < 0V(:,3) = -V(:,3);R = V * U';end% 计算平移向量t = targetCentroid' - R * sourceCentroid';% 更新变换tform = rigidtform3d(R, t');currentSource = (R * currentSource')' + t';% 计算当前迭代的误差inliers = dists < inlierThresh;metrics.rmse(iter) = sqrt(mean(dists(inliers).^2));metrics.inlierRatio(iter) = mean(inliers) * 100;% 检查收敛if iter > 1 && abs(metrics.rmse(iter) - metrics.rmse(iter-1)) < 1e-8break;endendend%% =========================================================================
%% ---------------- 辅助函数：计算鲁棒损失函数权重 ----------------
%% =========================================================================function weights = computeRobustWeights(residuals, lossType, delta)% 计算不同鲁棒损失函数的权重residuals = max(residuals, 1e-12); % 避免除零switch lossTypecase 'L2Loss (平方损失)'% L2损失：权重为1（不鲁棒）weights = ones(size(residuals));case 'L1Loss (绝对值损失)'% L1损失：权重与残差成反比weights = 1 ./ residuals;weights = weights / max(weights); % 归一化case 'HuberLoss (Huber损失)'% Huber损失：小残差用L2，大残差用L1weights = ones(size(residuals));largeResiduals = residuals > delta;weights(largeResiduals) = delta ./ residuals(largeResiduals);case 'CauchyLoss (Cauchy损失)'% Cauchy损失：对大残差有很好的鲁棒性weights = 1 ./ (1 + (residuals ./ delta).^2);otherwiseweights = ones(size(residuals));endendend二、点云Loss配准的结果
本次的GUI增加了鲁棒核函数的选择功能，各种鲁棒核函数都有自己的优缺点，具体可以看看前面和python版本部分。童鞋们，纸上得来终觉浅，绝知此事要躬行啊，动动小爪，试试吧。
就酱，下次见^-6