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函数去除孤立噪声点，基于局部点密度或统计距离（如标准差阈值）筛选有效点；
cpp 复制代码
% 示例：对源点云去噪

sourceCloud = pcread('source.pcd');

[denoisedSource, ~] = pcdenoise(sourceCloud, 'NumNeighbors', 10, 'Threshold', 1.5);
降采样：通过pcdownsample（体素网格 / 随机采样）减少点云数量，提升迭代效率；

初始配准：若源点云与目标点云初始偏差大，先用pcalign（基于特征 / 手动对齐）获取粗略变换矩阵，避免 ICP 陷入局部最优。

步骤 2：鲁棒核函数的选择与构建

根据点云特征动态确定核函数类型，核心是定义核加权误差函数：

核函数选择逻辑：

离群点比例 < 10%：Huber 核（小残差用平方核，大残差用线性核，平衡精度与鲁棒性）；

离群点比例 10%-30%：Cauchy 核（对大残差权重衰减更显著，抗差性更强）；

噪声呈高斯分布：高斯核（通过 σ 值控制权重衰减速度，适配噪声强度）；

Matlab 核函数实现：定义自定义函数计算核权重，例如 Cauchy 核：
cpp 复制代码
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叠加显示配准后的源点云与目标点云，观察重叠区域一致性：
cpp 复制代码
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('配准后点云（红：源点云，绿：目标点云）');
三、应用领域

三维重建（文物 / 场景建模）

场景需求：多视角扫描的点云存在遮挡（如文物的凹陷处）、扫描噪声（激光反射不均），离群点比例动态变化；

应用价值：可变核函数可适配不同视角点云的噪声特征（如正面扫描噪声少用 Huber 核，侧面扫描离群点多用 Cauchy 核），Matlab 的可视化工具可实时验证重建完整性，助力文物数字化保护、室内场景建模。

自动驾驶环境感知

场景需求：激光雷达（LiDAR）点云受天气影响（雨天 / 雾天）产生噪声点，车辆动态行驶中需实时配准多帧点云，构建环境地图；

应用价值：Matlab 的pcregistericp可通过参数化核函数快速迭代（满足毫秒级响应），可变核函数适配不同天气下的点云质量（晴天用高斯核，雨天切换 Cauchy 核），提升障碍物检测与路径规划精度。

工业检测（零件精度验证）

场景需求：工业零件扫描点云与 CAD 模型（目标点云）配准，需排除表面杂质（如油污、划痕）导致的离群点，评估零件尺寸偏差；

应用价值：通过 Matlab 自定义核函数，可针对零件不同区域（如光滑面 / 边角）调整核参数（光滑面用 Huber 核保证精度，边角离群点多用 Tukey 核），结合pcfitplane等函数计算偏差，实现自动化检测。

医学影像（器官配准）

场景需求：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)';
    end
    normals = normals ./ sqrt(sum(normals.^2, 2));
end

%% ========== 本地函数：完全照抄你给的模板 ==========
function result = evaluateReg(regCloud, targetCloud, inlierThresh)
    regPoints   = regCloud.Location;      % M×3
    targetPoints = 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
end

2、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; end
        
        try
            sourcePtCloud = pcread(fullfile(path,file));
            % 降采样优化（大型点云提速）
            if sourcePtCloud.Count > 10000
                sourcePtCloud = pcdownsample(sourcePtCloud, 'gridAverage', 0.005);
            end
            % 检查点云数量是否足够（至少100个点）
            if sourcePtCloud.Count < 100
                error('点云数量过少（需至少100个点）');
            end
            set(lblStatus, 'String', sprintf('状态：已加载 %s（%d个点）', ...
                file, sourcePtCloud.Count), 'ForegroundColor','green');
            refreshTargetCloud();
        catch ME
            errordlg(ME.message, '读取失败');
            set(lblStatus, 'String','状态：读取失败，请重新选择', 'ForegroundColor','red');
            sourcePtCloud = pointCloud.empty;
        end
    end

%% =========================================================================
%% ---------------- 回调函数：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.1
            return;
        end
        lastRot = 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');
        end
        sigma = 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; end
        set(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; end
        set(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;
        end
        
        set(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 < 10
                maxIter = 50;
                set(editMaxIter, 'String', '50');
            end
            if isnan(delta) || delta <= 0
                delta = 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 ME
            errordlg(ME.message, '配准失败');
            set(lblStatus, 'String','状态：配准失败，请检查点云', 'ForegroundColor','red');
            sourceRegPtCloud = pointCloud.empty;
        end
    end

%% =========================================================================
%% ---------------- 回调函数：7. 保存配准结果 ----------------
%% =========================================================================
    function saveRegisteredCloud(~,~)
        if isempty(sourceRegPtCloud)
            errordlg('请先运行ICP配准生成结果', '提示');
            return;
        end
        
        [file, path] = uiputfile({'*.pcd;*.ply;*.xyz','点云文件'},'保存配准点云');
        if isequal(file,0); return; end
        
        try
            pcwrite(sourceRegPtCloud, fullfile(path,file), 'Precision','double');
            msgbox('保存成功！','提示');
        catch ME
            errordlg(ME.message, '保存失败');
        end
    end

%% =========================================================================
%% ---------------- 辅助函数：计算配准前评估指标 ----------------
%% =========================================================================
    function computePreRegistrationMetrics()
        if isempty(sourcePtCloud) || isempty(targetPtCloud)
            return;
        end
        
        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
        
        distances = 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+1
            k = n-1;  % 确保不超过点的数量
        end
        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)';
        end
        % 归一化法向量
        normals = normals ./ sqrt(sum(normals.^2, 2));
    end

%% =========================================================================
%% ---------------- 辅助函数：评估配准结果 ----------------
%% =========================================================================
    function result = evaluateReg(regCloud, targetCloud, inlierThresh)
        regPoints   = regCloud.Location;      % M×3
        targetPoints = 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
    end

%% =========================================================================
%% ---------------- 核心函数：带鲁棒损失函数的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) < 0
                V(:,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-8
                break;
            end
        end
    end

%% =========================================================================
%% ---------------- 辅助函数：计算鲁棒损失函数权重 ----------------
%% =========================================================================
    function weights = computeRobustWeights(residuals, lossType, delta)
        % 计算不同鲁棒损失函数的权重
        residuals = max(residuals, 1e-12);  % 避免除零
        
        switch lossType
            case 'L2Loss (平方损失)'
                % L2损失：权重为1（不鲁棒）
                weights = ones(size(residuals));
                
            case 'L1Loss (绝对值损失)'
                % L1损失：权重与残差成反比
                weights = 1 ./ residuals;
                weights = weights / max(weights);  % 归一化
                
            case 'HuberLoss (Huber损失)'
                % Huber损失：小残差用L2，大残差用L1
                weights = ones(size(residuals));
                largeResiduals = residuals > delta;
                weights(largeResiduals) = delta ./ residuals(largeResiduals);
                
            case 'CauchyLoss (Cauchy损失)'
                % Cauchy损失：对大残差有很好的鲁棒性
                weights = 1 ./ (1 + (residuals ./ delta).^2);
                
            otherwise
                weights = ones(size(residuals));
        end
    end

end

二、点云Loss配准的结果

本次的GUI增加了鲁棒核函数的选择功能，各种鲁棒核函数都有自己的优缺点，具体可以看看前面和python版本部分。童鞋们，纸上得来终觉浅，绝知此事要躬行啊，动动小爪，试试吧。

就酱，下次见^-6