Matlab通过GUI实现点云的快速全局配准(FGR)
本节分享关于点云的FGR。FGR（Fast Global Registration，快速全局配准）是三维点云配准领域的关键算法，专门解决传统 ICP（迭代最近点）算法的三大痛点：
1、依赖初始变换矩阵，若初始对齐偏差大则易陷入局部最优；
2、处理大规模点云时迭代次数多，效率低下；
3、对噪声点与重叠区域缺失敏感，鲁棒性不足。
其核心逻辑是通过鲁棒特征匹配建立点云对应关系，结合全局优化模型直接求解最优刚性变换（旋转 + 平移），无需提前估计初始对齐状态，实现 “快速 + 全局最优” 的配准效果。
一. MATLAB 中的 FGR 技术实现
MATLAB 2020a 及以上版本在Computer Vision Toolbox中集成pcregisterfgr函数，为 FGR 配准提供便捷工具，核心优势包括：
零初始依赖：无需手动或通过其他算法（如 SAC-IA）估计初始变换矩阵，直接启动全局配准；
高效处理：采用特征降维与稀疏匹配策略，10 万级点云配准时间可控制在 1-5 秒，满足快速处理需求；
强鲁棒性：内置 RANSAC 外点剔除机制，支持 30% 以上重叠区域的点云配准，可容忍一定噪声（如激光扫描噪声）；
灵活可调：支持自定义特征类型（FPFH/SHOT）、匹配阈值（MatchThreshold）、内点比例（InlierRatio）等参数，适配不同场景。
二、MATLAB 点云 FGR 配准完整操作流程
基于pcregisterfgr函数，点云 FGR 配准可分为 5 个关键步骤，每步均附 MATLAB 代码与参数说明：
步骤 1：点云数据加载与预处理
（1）数据加载
通过pcread函数读取主流点云文件（PLY/PCD/XYZ 等），或对接传感器（Kinect、激光雷达）实时获取数据：
% 加载源点云（待配准）与目标点云（参考基准）ptCloudSource = pcread('source_cloud.ply'); % 待配准点云ptCloudTarget = pcread('target_cloud.ply'); % 参考点云（2）预处理操作
目的：降低噪声干扰、减少点云数量（平衡效率与精度），核心操作包括：
去噪：用pcdenoise去除孤立噪声点（基于局部点密度判断）；
下采样：用pcdownsample通过体素网格或随机采样减少点数，通常保留 30%-50% 原点数；
法向量估计：若提取 FPFH/SHOT 特征，需用pcnormals估计点云法向量（特征计算依赖法向量方向）。
示例代码：(2020a版本以后)
% 1. 去噪（默认参数，可通过'Sigma'调整噪声敏感度）ptCloudSource = pcdenoise(ptCloudSource);ptCloudTarget = pcdenoise(ptCloudTarget);% 2. 体素下采样（体素大小=0.02m，需根据点云实际尺度调整，如小零件设为0.005m）ptCloudSource = pcdownsample(ptCloudSource, 'gridAverage', 0.02);ptCloudTarget = pcdownsample(ptCloudTarget, 'gridAverage', 0.02);% 3. 估计法向量（为特征提取做准备，邻域点数默认10，可通过'NumNeighbors'调整）ptCloudSource = pcnormals(ptCloudSource);ptCloudTarget = pcnormals(ptCloudTarget);步骤 2：点云特征提取
FGR 配准依赖局部特征描述子建立点云对应关系，MATLAB 支持两种主流特征：
特征类型
特点
适用场景
FPFH（默认）
计算速度快，对旋转 / 尺度鲁棒，适用于无序点云
通用场景（如建筑建模、文物重建）
SHOT
对噪声更鲁棒，特征区分度高
纹理丰富点云（如工业零件、医疗影像）
通过pcreatefeature函数手动提取特征（pcregisterfgr内部会自动提取，手动提取可优化特征参数）：
% 提取FPFH特征（邻域半径=0.1m，需与点云尺度匹配）featureSource = pcreatefeature(ptCloudSource, 'FeatureType', 'fpfh', 'Radius', 0.1);featureTarget = pcreatefeature(ptCloudTarget, 'FeatureType', 'fpfh', 'Radius', 0.1);% 若提取SHOT特征，仅需修改'FeatureType'参数% featureSource = pcreatefeature(ptCloudSource, 'FeatureType', 'shot', 'Radius', 0.1);步骤 3：特征匹配与外点剔除
该步骤由pcregisterfgr自动完成，核心逻辑包括：
1、特征匹配：采用 K 近邻（K=2）匹配策略，对源点云每个特征，在目标点云中寻找 Top-2 相似特征；
2、外点剔除：通过 RANSAC 算法筛选内点 —— 随机抽取 3 对匹配点估计变换矩阵，计算其他匹配点的重投影误差，保留误差小于阈值的内点，迭代优化直至内点集稳定。
可通过参数控制剔除效果：
InlierRatio：预期内点比例（如 0.5 表示预期 50% 匹配对为内点，需根据点云重叠度调整）；
MatchThreshold：特征匹配阈值（值越小匹配越严格，默认 0.9）。
步骤 4：全局变换矩阵估计与配准执行
基于筛选后的内点集，pcregisterfgr通过最小二乘全局优化求解刚性变换矩阵（rigidtform3d对象），目标函数为最小化内点间欧氏距离平方和：
\(\min_{R,t} \sum_{i=1}^{N} \| R \cdot p_i + t - q_i \|^2\)
其中，\(p_i\)为源点云内点，\(q_i\)为目标点云对应内点，\(R\)为 3×3 旋转矩阵，\(t\)为 3×1 平移向量。
执行配准代码：
% 调用FGR配准函数，获取配准后点云与变换矩阵[ptCloudRegistered, tform] = pcregisterfgr(ptCloudSource, ptCloudTarget, ...'FeatureType', 'fpfh', % 特征类型'InlierRatio', 0.5, % 预期内点比例'MatchThreshold', 0.9, % 匹配阈值'Radius', 0.1); % 特征计算邻域半径% 提取变换矩阵中的旋转矩阵R与平移向量tR = tform.Rotation; % 3×3旋转矩阵t = tform.Translation; % 3×1平移向量（单位：m）步骤 5：配准结果评估与可视化
（1）定量评估
通过关键指标判断配准精度：
平均距离误差：配准后点云与目标点云的平均欧氏距离（值越小精度越高，通常要求 < 0.01m）；
内点比例：实际内点数量 / 总匹配对数量（反映匹配质量，接近InlierRatio说明参数设置合理）。
评估代码：
% 计算平均距离误差distances = pcdist(ptCloudRegistered, ptCloudTarget); % 逐点距离meanError = mean(distances); % 平均误差fprintf('配准平均距离误差：%.4f m\n', meanError);% 计算内点比例（需先获取匹配对信息，通过辅助函数实现）function inlierRatio = calcInlierRatio(ptCloudSource, ptCloudTarget, tform)% 转换源点云到目标点云坐标系ptCloudTransformed = pctransform(ptCloudSource, tform);% 计算匹配对并筛选内点（误差<0.005m为内点）[~, distances] = pcmatchpair(ptCloudTransformed, ptCloudTarget, 0.005);inlierRatio = length(distances)/size(ptCloudTransformed.Location, 1);endinlierRatio = calcInlierRatio(ptCloudSource, ptCloudTarget, tform);fprintf('实际内点比例：%.2f\n', inlierRatio);（2）可视化展示
通过pcshowpair函数对比配准前后效果，用不同颜色区分点云：
figure('Position', [100, 100, 1200, 500]); % 设置窗口大小% 配准前对比（源点云：红，目标点云：绿）subplot(1, 2, 1);pcshowpair(ptCloudSource, ptCloudTarget, 'VerticalAxis', 'Y', 'HorizontalAxis', 'X');title('配准前：源点云（红） vs 目标点云（绿）', 'FontSize', 12);xlabel('X轴（m）'), ylabel('Y轴（m）'), zlabel('Z轴（m）');% 配准后对比（配准点云：红，目标点云：绿）subplot(1, 2, 2);pcshowpair(ptCloudRegistered, ptCloudTarget, 'VerticalAxis', 'Y', 'HorizontalAxis', 'X');title('配准后：配准点云（红） vs 目标点云（绿）', 'FontSize', 12);xlabel('X轴（m）'), ylabel('Y轴（m）'), zlabel('Z轴（m）');三、点云 FGR 配准的典型应用领域
基于 “无需初始变换、速度快、鲁棒性强” 的特点，FGR 配准在多领域落地应用：
1. 三维重建领域
核心需求：将多视角点云拼接为完整三维模型，避免逐帧迭代对齐的繁琐流程。
文物数字化：通过激光扫描获取文物不同角度点云（如青铜器、石雕），FGR 直接实现全局拼接，保留毫米级细节特征，避免 ICP 因初始偏差导致的拼接错位；
建筑建模：无人机激光雷达获取建筑外立面多帧点云，FGR 快速对齐各帧数据，生成完整建筑三维模型，支撑 BIM（建筑信息模型）构建。
2. 工业检测领域
核心需求：将实际零件点云与 CAD 设计模型配准，计算尺寸偏差，实现质量检测。
汽车零件检测：扫描发动机缸体、变速箱壳体等零件，FGR 无需手动对齐零件与 CAD 模型，直接全局配准，检测关键孔位、平面的位置偏差（精度要求 < 0.005m）；
3C 产品组装验证：手机外壳、笔记本机身的扫描点云与设计模型配准，通过 FGR 外点剔除机制排除扫描噪声，精准识别组装间隙超差区域。
3. 自动驾驶与机器人领域
核心需求：实时将传感器点云与地图配准，实现定位与导航。
自动驾驶定位：激光雷达（LiDAR）获取当前帧点云，与高精度预建地图（点云格式）通过 FGR 快速配准（耗时 < 0.5 秒），获取车辆实时位置（定位精度 < 0.1m），辅助路径规划；
机器人导航：仓储机器人通过深度相机获取环境点云，FGR 将实时点云与预建仓库地图配准，实现自主避障与货架定位，提升导航效率。
4. 医疗影像领域
核心需求：不同模态医疗点云配准，辅助手术规划与治疗评估。
骨科手术规划：患者骨骼 CT 点云（高精度）与标准骨骼模型（参考）通过 FGR 配准，定位骨折、畸形部位，设计个性化内固定方案；
牙科修复：口腔扫描仪获取患者牙床点云，FGR 将其与假牙设计模型配准，确保修复体与牙床贴合度（误差 < 0.002m），减少后续调整次数。
本次使用的数据。。。。。。兔砸呗
一、点云FGR的程序
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 = copy(sourcePtCloud); % 先复制一份%% 2. 对目标点云做刚性变换 + 噪声
% 2.1 绕 Z 轴旋转 30°
ang = deg2rad(10);
R = [cos(ang) -sin(ang) 0;sin(ang) cos(ang) 0;0 0 1];% 2.2 平移向量
t = [0.01 0.015 0.02];% 2.3 手工变换
P = targetPtCloud.Location; % N×3
P = (R * P')'; % 旋转
P = P + t; % 平移% 2.4 加高斯噪声（降低噪声水平）
sigma = 0.0005; % 从0.001降低，减少特征干扰
P = P + sigma * randn(size(P));% 2.5 重新封装成 pointCloud
targetPtCloud = pointCloud(P, ...'Normal', targetPtCloud.Normal, ...'Color', targetPtCloud.Color);% 2.6 上色可视化
sourcePtCloud.Color = repmat(uint8([255 0 0]), size(sourcePtCloud.Location,1), 1);
targetPtCloud = pointCloud(P, ...'Normal', targetPtCloud.Normal, ...'Color', repmat(uint8([0 255 0]), size(P,1), 1));%% 3. 可视化初始位姿
figure('Name','初始位姿','Color','w');
pcshowpair(sourcePtCloud, targetPtCloud);
title('红色=source，绿色=target');%% 4. 改进的 Fast Global Registration（平衡匹配质量与数量）
% 4.1 计算更高质量的 FPFH 特征（增加区分度）
sourceFPFH = computeFPFH_improved(sourcePtCloud);
targetFPFH = computeFPFH_improved(targetPtCloud);% 4.2 更合理的距离阈值估算（动态调整）
dist = sqrt(sum( (sourcePtCloud.Location - targetPtCloud.Location).^2 ,2 ));
avgDist = mean(dist);
stdDist = std(dist);
maxCorrDist = max(avgDist + 1.2*stdDist, 0.01); % 限制最小阈值
fprintf('平均距离 %.4f，标准差 %.4f → 阈值 %.4f\n', avgDist, stdDist, maxCorrDist);% 4.3 稳健版 FGR 主流程（抗噪能力增强）
T = fastGlobalRegistration_improved( ...sourcePtCloud.Location, targetPtCloud.Location, ...sourceFPFH, targetFPFH, maxCorrDist);fprintf('估计的变换矩阵:\n'); disp(T);%% 5. 应用变换矩阵
R = T(1:3,1:3);
% 确保旋转矩阵正交性（修正数值误差）
[U,~,V] = svd(R);
R = U * V'; % 正交化处理
t_vec = T(1:3,4).';sourceLoc = sourcePtCloud.Location;
sourceRegLoc = (R * sourceLoc')' + t_vec;sourceRegPtCloud = pointCloud(sourceRegLoc, ...'Color', sourcePtCloud.Color, ...'Normal', sourcePtCloud.Normal);%% 6. 可视化配准结果
figure('Name','改进的 FGR 配准结果','Color','w');
pcshowpair(sourceRegPtCloud, targetPtCloud);
title('红色=registered，绿色=target');%% 7. 数值评价
rmsBefore = sqrt(mean(sum( (sourcePtCloud.Location - targetPtCloud.Location).^2 ,2)));
rmsAfter = sqrt(mean(sum( (sourceRegLoc - targetPtCloud.Location).^2 ,2)));
fprintf('RMS 误差 配准前: %.4f 配准后: %.4f 改善: %.1f%%\n', ...rmsBefore, rmsAfter, (rmsBefore-rmsAfter)/rmsBefore*100);% 计算配准后重叠率（调整阈值）
inlierDist = sqrt(sum( (sourceRegLoc - targetPtCloud.Location).^2 ,2));
overlapRate = mean(inlierDist < 1.2*maxCorrDist);
fprintf('配准后重叠率: %.1f%%\n', overlapRate*100);%% -------------------------------------------------------------------------
%% 子函数 1：改进的 FPFH 计算（增强特征区分度）
function fpfh = computeFPFH_improved(ptCloud)
P = ptCloud.Location;
N = size(P,1);
[k, d2] = knnsearch(P,P,'K',2);
density = mean(sqrt(d2(:,2)));
radNormal = 2.0 * density; % 增大半径获取更稳定法向
radFPFH = 4.0 * density; % 扩大特征支持域%% 2. 法向计算（更稳健）
K = 40; % 更多邻域点提高法向可靠性
[nnIndices, dists] = knnsearch(P, P, 'K', K+1);
nnIndices = nnIndices(:, 2:end); % 去除自身点
dists = dists(:, 2:end); % 保存距离用于加权normal = zeros(N,3);
curv = zeros(N,1);for i = 1:Nidx = nnIndices(i, :);if numel(idx) < 8 % 要求更多邻域点normal(i,:) = [0 0 1];continue;end% 基于距离的加权法向计算（修复维度问题）% 将权重转换为列向量，确保与点坐标矩阵维度匹配weights = exp(-dists(i,:).^2 / (2*(radNormal/2)^2));weights = weights'; % 关键修复：将行向量转为列向量pts = P(idx,:) - P(i,:); % M×3矩阵（M为邻域点数量）% 现在weights是M×1列向量，repmat后为M×3，与pts维度匹配weighted_pts = pts .* repmat(weights, 1, 3); % 距离近的点权重高[~,S,V] = svd(weighted_pts,'econ');n = V(:,3)';% 法向一致性增强if n*[0 0 1]' < 0, n = -n; endnormal(i,:) = n;curv(i) = S(3,3)/sum(diag(S));
end%% 3. 建立邻接关系（自适应密度）
K_fpfh = min(60, max(30, round(N/50))); % 自适应邻域点数
[adjIndices, adjDists] = knnsearch(P, P, 'K', K_fpfh+1);
adjIndices = adjIndices(:, 2:end);
adjDists = adjDists(:, 2:end);
adj = num2cell(adjIndices, 2);
adjWeights = num2cell(exp(-adjDists.^2 / (2*(radFPFH/2)^2)), 2); % 邻域权重%% 4. 计算 SPFH（增强区分度）
spfh = zeros(N,44);
for i = 1:Nnei = adj{i};if isempty(nei), continue; endu = normal(i,:);p = P(i,:);weights = adjWeights{i}; % 预计算的权重v = P(nei,:) - p;v = v ./ sqrt(sum(v.^2, 2));n2 = normal(nei,:);% 特征计算f1 = sum(v .* repmat(u, size(v,1), 1), 2);f2 = sum(v .* n2, 2);f3 = sum(repmat(u, size(n2,1), 1) .* n2, 2) + 1;% 更精细的分箱b1 = min(max(floor((f1+1)/2 * 11) + 1, 1), 11);b2 = min(max(floor(f2+1) + 1, 1), 2);b3 = min(max(floor(f3/2 * 2) + 1, 1), 2);% 加权直方图hist = zeros(1,44);for j = 1:length(b1)bin = sub2ind([11 2 2], b1(j), b2(j), b3(j));hist(bin) = hist(bin) + weights(j);end% 防止除以零histSum = sum(hist);if histSum > 0spfh(i,:) = hist ./ histSum;end
end%% 5. FPFH 计算（增强鲁棒性）
fpfh = zeros(N,44);
for i = 1:Nnei = [i; adj{i}'];dists = sqrt(sum((P(nei,:) - P(i,:)).^2, 2));w = 1 ./ (1 + dists.^2); % 距离越近权重越高fpfh(i,:) = sum(spfh(nei,:) .* w, 1) / sum(w);
endfpfh = fpfh';
end%% -------------------------------------------------------------------------
%% 子函数 2：改进的 Fast Global Registration（抗错误匹配）
function T = fastGlobalRegistration_improved(src, tgt, srcFPFH, tgtFPFH, maxCorrDist)% 1. 多阶段特征匹配（先严后宽）[idxS, idxT, ratios] = featureMatching_improved(srcFPFH, tgtFPFH);% 2. 建立分级对应集matchS = src(idxS,:);matchT = tgt(idxT,:);% 3. 基于比率的自适应过滤initDists = sqrt(sum((matchS - matchT).^2, 2));% 比率越小，匹配越可靠，可适当放宽距离限制adaptiveTh = maxCorrDist .* (1 + ratios(idxS));goodMatchIdx = initDists < adaptiveTh;matchS = matchS(goodMatchIdx,:);matchT = matchT(goodMatchIdx,:);numMatches = size(matchS,1);% 确保最低匹配点数量minRequired = 15;if numMatches < minRequiredwarning(['匹配点数量不足，使用低阈值过滤 (', num2str(numMatches), ')']);% 逐步降低阈值直到获得足够匹配点for factor = 1.1:0.1:2.0goodMatchIdx = initDists < factor * maxCorrDist;matchS = matchS(goodMatchIdx,:);matchT = matchT(goodMatchIdx,:);numMatches = size(matchS,1);if numMatches >= minRequiredbreak;endendif numMatches < 3error('匹配点数量不足3个，无法计算刚体变换');endend% 4. 增强版 RANSAC（抗噪优化）numSamples = 3;maxIter = 20000; % 增加迭代次数提高找到正确模型的概率inlierTh = 0.6 * maxCorrDist; % 更严格的初始内点阈值bestInl = 0; bestT = eye(4);matchS_T = matchS'; % 预转置加速计算% 带概率终止的RANSACp = 0.99; % 期望成功率outlierRatio = 0.5; % 初始异常值比例估计iter = 0;while iter < maxIter% 动态调整迭代次数neededIter = log(1-p)/log(1-(1-outlierRatio)^numSamples);maxIter = max(maxIter, ceil(neededIter));rnd = randi(numMatches, numSamples,1);A = matchS(rnd,:); B = matchT(rnd,:);% 计算变换并验证[R, t] = kabsch(A, B);transformed = (R * matchS_T)';transformed = transformed + repmat(t, numMatches, 1);dist = sqrt(sum( (transformed - matchT).^2 ,2));inlierCount = sum(dist <= inlierTh);% 更新最佳模型if inlierCount > bestInlbestInl = inlierCount;bestT = [R t'; 0 0 0 1];% 更新异常值比例估计outlierRatio = 1 - bestInl/numMatches;if outlierRatio < 0.1 % 内点足够多则提前退出break;endenditer = iter + 1;end% 5. 两阶段精修% 第一阶段：用宽松阈值提取内点transformed = (bestT(1:3,1:3) * matchS_T)';transformed = transformed + repmat(bestT(1:3,4)', numMatches, 1);dist = sqrt(sum( (transformed - matchT).^2 ,2));inlIdx = dist <= 1.2*inlierTh;if sum(inlIdx) < 5warning('内点太少，使用初始变换');T = bestT;return;end% 第二阶段：用内点重新估计并精修[R_refine, t_refine] = kabsch(matchS(inlIdx,:), matchT(inlIdx,:));% 对精修结果进行正交化（确保是刚体变换）[U,~,V] = svd(R_refine);R_refine = U * V';if det(R_refine) < 0U(:,3) = -U(:,3);R_refine = U * V';endT = [R_refine t_refine'; 0 0 0 1];
end%% -------------------------------------------------------------------------
%% 子函数 3：改进的特征匹配（平衡质量与数量）
function [idxS, idxT, ratios] = featureMatching_improved(featS, featT)T = featT'; % N2×44S = featS'; % N1×44[idx, dist] = knnsearch(T, S, 'NSMethod', 'kdtree', 'K', 2);% 计算比率并保存ratios = dist(:,1) ./ dist(:,2);% 动态阈值：根据比率分布自动调整ratioTh = min(0.9, max(0.7, prctile(ratios, 30))); % 取30%分位值goodMatch = ratios < ratioTh;fprintf('使用比率阈值 %.2f，保留 %d 个匹配点\n', ratioTh, sum(goodMatch));idxS = find(goodMatch);idxT = idx(goodMatch,1).';
end%% -------------------------------------------------------------------------
%% 子函数 4：加权 Kabsch 算法
function [R, t] = weightedKabsch(A, B, weights)weights = weights / sum(weights);W = diag(weights);muA = sum(A .* repmat(weights, 1, 3), 1);muB = sum(B .* repmat(weights, 1, 3), 1);A0 = A - repmat(muA, size(A,1), 1);B0 = B - repmat(muB, size(B,1), 1);H = A0' * W * B0;[U,~,V] = svd(H);d = sign(det(V' * U));if d < 0V(:,3) = -V(:,3);endR = V * U';t = muB - muA * R;
end%% -------------------------------------------------------------------------
%% 子函数 5：标准 Kabsch 算法
function [R, t] = kabsch(A, B)muA = mean(A,1); muB = mean(B,1);A0 = A - muA; B0 = B - muB;H = A0' * B0;[U,~,V] = svd(H);d = sign(det(V' * U));S = eye(3); S(3,3) = d;R = V * S * U';t = muB - muA * R;
end2、GUI版本
function FGR_Register_GUI
%FGR_Register_GUI 基于改进FGR算法的点云配准GUI（修复索引超出问题）
clc; clear; close all;%% ---------------- 主窗口初始化 ----------------
fig = figure('Name','改进FGR配准工具', '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','FGR配准后（红→绿）');
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. 配准执行 ----------------
uicontrol('Parent',pnlCtrl, 'Style','pushbutton', 'String','运行FGR配准', ...'FontSize',12, 'Units','normalized', 'BackgroundColor',[0.2 0.8 0.2], ...'Position',[0.05 yPos-btnH 0.9 0.06], 'FontWeight','bold', ...'Callback',@runFGRRegistration);
yPos = yPos - btnH - margin;%% ---------------- 控制面板组件：4. 结果评价 ----------------
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;% 评价指标显示
lblRMSBefore = uicontrol('Parent',pnlCtrl, 'Style','text', ...'String','配准前RMS：-- mm', 'FontSize',10, ...'Units','normalized', 'Position',[0.05 yPos-txtH 0.9 txtH]);
yPos = yPos - txtH - margin/4;lblRMSAfter = uicontrol('Parent',pnlCtrl, 'Style','text', ...'String','配准后RMS：-- mm', 'FontSize',10, ...'Units','normalized', 'Position',[0.05 yPos-txtH 0.9 txtH]);
yPos = yPos - txtH - margin/4;lblImprove = uicontrol('Parent',pnlCtrl, 'Style','text', ...'String','误差改善：-- %', 'FontSize',10, ...'Units','normalized', 'Position',[0.05 yPos-txtH 0.9 txtH]);
yPos = yPos - txtH - margin/4;lblOverlap = uicontrol('Parent',pnlCtrl, 'Style','text', ...'String','配准重叠率：-- %', 'FontSize',10, ...'Units','normalized', 'Position',[0.05 yPos-txtH 0.9 txtH]);
yPos = yPos - txtH - margin;%% ---------------- 控制面板组件：5. 结果保存 ----------------
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;% 配准后点云
T = eye(4); % 变换矩阵
rmsBefore = 0; rmsAfter = 0; % 误差指标
overlapRate = 0;
lastRot = 10; lastNoise = 0.5; % 用于控制刷新频率%% =========================================================================
%% ---------------- 回调函数：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));% 上色并显示targetPtCloud = pointCloud(tgtLoc, ...'Normal', sourcePtCloud.Normal, ...'Color', repmat(uint8([0 255 0]), size(tgtLoc,1), 1));sourcePtCloud.Color = repmat(uint8([255 0 0]), size(srcLoc,1), 1);showPointCloud(axSrc, sourcePtCloud, '原始点云（红）');showPointCloud(axTgt, targetPtCloud, '目标点云（绿）');% 清空配准结果sourceRegPtCloud = pointCloud.empty;cla(axReg); title(axReg, 'FGR配准后（红→绿）');set(lblRMSBefore, 'String', sprintf('配准前RMS：%.3f mm', ...computeRMS(srcLoc, tgtLoc)*1000));set(lblRMSAfter, 'String','配准后RMS：-- mm');set(lblImprove, 'String','误差改善：-- %');set(lblOverlap, 'String','配准重叠率：-- %');% 同步显示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. 运行FGR配准 ----------------
%% =========================================================================function runFGRRegistration(~,~)if isempty(sourcePtCloud) || isempty(targetPtCloud)errordlg('请先加载原始点云并生成目标点云', '提示');return;endset(lblStatus, 'String','状态：FGR配准中...', 'ForegroundColor','#FFA500');drawnow;trysrcLoc = sourcePtCloud.Location;tgtLoc = targetPtCloud.Location;% 检查点云是否为空if isempty(srcLoc) || isempty(tgtLoc)error('点云数据为空，无法配准');end% 计算FPFH特征sourceFPFH = computeFPFH_improved(sourcePtCloud);targetFPFH = computeFPFH_improved(targetPtCloud);% 计算匹配阈值dist = sqrt(sum((srcLoc - tgtLoc).^2, 2));avgDist = mean(dist);stdDist = std(dist);maxCorrDist = max(avgDist + 1.2*stdDist, 0.01);% 执行FGR配准T = fastGlobalRegistration_improved(srcLoc, tgtLoc, ...sourceFPFH, targetFPFH, maxCorrDist);% 应用变换R = T(1:3,1:3);[U,~,V] = svd(R);R = U * V';tVec = T(1:3,4).';srcRegLoc = (R * srcLoc')' + repmat(tVec, size(srcLoc,1), 1);% 封装配准结果sourceRegPtCloud = pointCloud(srcRegLoc, ...'Color', sourcePtCloud.Color, ...'Normal', sourcePtCloud.Normal);% 显示结果showPointCloud(axReg, sourceRegPtCloud, '配准后点云（红）');hold(axReg, 'on');pcshow(targetPtCloud, 'Parent', axReg, 'MarkerSize',30);hold(axReg, 'off');title(axReg, 'FGR配准结果（红=配准后，绿=目标）');% 计算评价指标rmsBefore = computeRMS(srcLoc, tgtLoc) * 1000;rmsAfter = computeRMS(srcRegLoc, tgtLoc) * 1000;improveRate = (rmsBefore - rmsAfter) / rmsBefore * 100;inlierDist = sqrt(sum((srcRegLoc - tgtLoc).^2, 2));overlapRate = mean(inlierDist < 1.2*maxCorrDist) * 100;% 更新显示set(lblRMSBefore, 'String', sprintf('配准前RMS：%.3f mm', rmsBefore));set(lblRMSAfter, 'String', sprintf('配准后RMS：%.3f mm', rmsAfter));set(lblImprove, 'String', sprintf('误差改善：%.1f %%', improveRate));set(lblOverlap, 'String', sprintf('配准重叠率：%.1f %%', overlapRate));set(lblStatus, 'String','状态：FGR配准完成', 'ForegroundColor','green');catch MEerrordlg(ME.message, '配准失败');set(lblStatus, 'String','状态：配准失败，请检查点云', 'ForegroundColor','red');sourceRegPtCloud = pointCloud.empty;endend%% =========================================================================
%% ---------------- 回调函数：6. 保存配准结果 ----------------
%% =========================================================================function saveRegisteredCloud(~,~)if isempty(sourceRegPtCloud)errordlg('请先运行FGR配准生成结果', '提示');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 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%% =========================================================================
%% ---------------- 辅助函数：计算RMS误差 ----------------
%% =========================================================================function rms = computeRMS(p1, p2)% 确保输入数组大小一致if size(p1,1) ~= size(p2,1)error('输入点云数量不一致，无法计算RMS');enddistSq = sum((p1 - p2).^2, 2);rms = sqrt(mean(distSq));end%% =========================================================================
%% ---------------- 核心子函数：改进版FPFH计算（添加索引检查） ----------------
%% =========================================================================function fpfh = computeFPFH_improved(ptCloud)P = ptCloud.Location;N = size(P,1);% 检查点云数量是否足够if N < 10error('点云数量过少，无法计算特征');end[k, d2] = knnsearch(P,P,'K',2);density = mean(sqrt(d2(:,2)));radNormal = 1.5 * density;radFPFH = 3.0 * density;% 法向计算（减少邻域点）K = min(20, N-1); % 确保不超过点云总数[nnIndices, dists] = knnsearch(P, P, 'K', K+1);nnIndices = nnIndices(:, 2:end); % 去除自身dists = dists(:, 2:end);normal = zeros(N,3);curv = zeros(N,1);for i = 1:Nidx = nnIndices(i, :);% 过滤无效索引（确保不超出范围）idx = idx(idx > 0 & idx <= N);if numel(idx) < 3 % 降低要求，避免索引不足normal(i,:) = [0 0 1];continue;endweights = exp(-dists(i,1:numel(idx)).^2 / (2*(radNormal/2)^2));weights = weights';pts = P(idx,:) - P(i,:);weighted_pts = pts .* repmat(weights, 1, 3);[~,S,V] = svd(weighted_pts,'econ');n = V(:,3)';if n*[0 0 1]' < 0, n = -n; endnormal(i,:) = n;curv(i) = S(3,3)/sum(diag(S));end% 简化SPFH计算K_fpfh = min(40, max(5, round(N/80))); % 确保至少5个邻域点[adjIndices, adjDists] = knnsearch(P, P, 'K', K_fpfh+1);adjIndices = adjIndices(:, 2:end);adjDists = adjDists(:, 2:end);adj = num2cell(adjIndices, 2);adjWeights = num2cell(exp(-adjDists.^2 / (2*(radFPFH/2)^2)), 2);spfh = zeros(N,33);for i = 1:Nnei = adj{i};% 过滤无效邻域索引if isempty(nei)continue;endnei = nei(nei > 0 & nei <= N); % 确保索引在有效范围内if numel(nei) < 3continue;endu = normal(i,:);p = P(i,:);weights = adjWeights{i}(1:numel(nei)); % 截断权重数组v = P(nei,:) - p;v = v ./ sqrt(sum(v.^2, 2));n2 = normal(nei,:);f1 = sum(v .* repmat(u, size(v,1), 1), 2);f2 = sum(v .* n2, 2);f3 = sum(repmat(u, size(n2,1), 1) .* n2, 2) + 1;b1 = min(max(floor((f1+1)/2 * 9) + 1, 1), 9);b2 = min(max(floor(f2+1) + 1, 1), 2);b3 = min(max(floor(f3/2 * 2) + 1, 1), 2);hist = zeros(1,33);for j = 1:length(b1)bin = sub2ind([9 2 2], b1(j), b2(j), b3(j));if bin >= 1 && bin <= 33 % 检查分箱索引有效性hist(bin) = hist(bin) + weights(j);endendhistSum = sum(hist);if histSum > 0spfh(i,:) = hist ./ histSum;endend% FPFH计算（简化权重）fpfh = zeros(N,33);for i = 1:Nnei = [i; adj{i}'];nei = nei(nei > 0 & nei <= N); % 过滤无效索引if numel(nei) < 1fpfh(i,:) = zeros(1,33);continue;endfpfh(i,:) = mean(spfh(nei,:), 1);endfpfh = fpfh';end%% =========================================================================
%% ---------------- 核心子函数：改进版FGR配准（添加索引检查） ----------------
%% =========================================================================function T = fastGlobalRegistration_improved(src, tgt, srcFPFH, tgtFPFH, maxCorrDist)% 检查输入特征维度是否匹配if size(srcFPFH,2) ~= size(tgtFPFH,2)error('特征维度不匹配，无法进行匹配');end[idxS, idxT, ratios] = featureMatching_improved(srcFPFH, tgtFPFH);% 过滤无效索引idxS = idxS(idxS > 0 & idxS <= size(src,1));idxT = idxT(idxT > 0 & idxT <= size(tgt,1));% 确保索引数量一致minLen = min(length(idxS), length(idxT));idxS = idxS(1:minLen);idxT = idxT(1:minLen);if isempty(idxS) || isempty(idxT)error('未找到有效匹配点，无法配准');endmatchS = src(idxS,:);matchT = tgt(idxT,:);initDists = sqrt(sum((matchS - matchT).^2, 2));adaptiveTh = maxCorrDist .* (1 + ratios(1:minLen)); % 确保 ratios 索引有效goodMatchIdx = initDists < adaptiveTh;matchS = matchS(goodMatchIdx,:);matchT = matchT(goodMatchIdx,:);numMatches = size(matchS,1);minRequired = 10;if numMatches < minRequiredwarning(['匹配点数量不足，使用低阈值过滤 (', num2str(numMatches), ')']);for factor = 1.1:0.2:2.0goodMatchIdx = initDists < factor * maxCorrDist;matchS = matchS(goodMatchIdx,:);matchT = matchT(goodMatchIdx,:);numMatches = size(matchS,1);if numMatches >= minRequiredbreak;endendif numMatches < 3error('匹配点数量不足3个，无法计算刚体变换');endend% 优化RANSAC参数numSamples = min(2, numMatches); % 确保采样数不超过匹配点数量maxIter = 5000;inlierTh = 0.8 * maxCorrDist;bestInl = 0; bestT = eye(4);matchS_T = matchS';p = 0.95;outlierRatio = 0.5;iter = 0;while iter < maxIter && numMatches >= numSamplesneededIter = log(1-p)/log(1-(1-outlierRatio)^numSamples);maxIter = max(maxIter, ceil(neededIter));rnd = randi(numMatches, numSamples,1);A = matchS(rnd,:); B = matchT(rnd,:);[R, t] = kabsch(A, B);transformed = (R * matchS_T)';transformed = transformed + repmat(t, numMatches, 1);dist = sqrt(sum( (transformed - matchT).^2 ,2));inlierCount = sum(dist <= inlierTh);if inlierCount > bestInlbestInl = inlierCount;bestT = [R t'; 0 0 0 1];outlierRatio = 1 - bestInl/numMatches;if outlierRatio < 0.05break;endenditer = iter + 1;end% 精修步骤if numMatches >= 1transformed = (bestT(1:3,1:3) * matchS_T)';transformed = transformed + repmat(bestT(1:3,4)', numMatches, 1);dist = sqrt(sum( (transformed - matchT).^2 ,2));inlIdx = dist <= 1.5*inlierTh;if sum(inlIdx) >= 3[R_refine, t_refine] = kabsch(matchS(inlIdx,:), matchT(inlIdx,:));T = [R_refine t_refine'; 0 0 0 1];return;endend% 若精修失败，返回最佳变换T = bestT;end%% =========================================================================
%% ---------------- 核心子函数：改进版特征匹配（添加索引检查） ----------------
%% =========================================================================function [idxS, idxT, ratios] = featureMatching_improved(featS, featT)% 确保特征非空if isempty(featS) || isempty(featT)error('输入特征为空，无法匹配');endT = featT';S = featS';[idx, dist] = knnsearch(T, S, 'NSMethod', 'kdtree', 'K', 2);% 过滤无效距离（避免除以零）valid = dist(:,2) > 1e-9;idx = idx(valid,:);dist = dist(valid,:);if isempty(dist)idxS = [];idxT = [];ratios = [];return;endratios = dist(:,1) ./ dist(:,2);ratioTh = min(0.9, max(0.7, prctile(ratios, 30)));goodMatch = ratios < ratioTh;fprintf('特征匹配：比率阈值 %.2f，保留 %d 个匹配点\n', ratioTh, sum(goodMatch));idxS = find(valid);idxS = idxS(goodMatch);idxT = idx(goodMatch,1).';end%% =========================================================================
%% ---------------- 核心子函数：标准Kabsch算法 ----------------
%% =========================================================================function [R, t] = kabsch(A, B)% 确保输入点数量一致if size(A,1) ~= size(B,1)error('输入点数量不一致，无法计算变换');endmuA = mean(A,1); muB = mean(B,1);A0 = A - muA; B0 = B - muB;H = A0' * B0;[U,~,V] = svd(H);d = sign(det(V' * U));S = eye(3); S(3,3) = d;R = V * S * U';t = muB - muA * R;endend二、点云FGR的结果
可以看到，点云大致能配准上，谁让它是粗配准呢。不过这不是最要紧的，最要紧的是，本期的FGR使用的是matlab2020a版本，没使用pcregisterfgr函数，这样就意味着它不依赖于matlab版本，为FGR多元化做出了一定的贡献。
就酱，下次见^-6