本次分享Matlab实现点云的最小二乘法配准。点云配准是计算机视觉、三维重建等领域的核心任务，其目标是寻找两个点云（源点云与目标点云）之间的空间变换关系（旋转矩阵R和平移向量t），使两者在几何位置上实现最优对齐。Kabsch 算法是一种基于最小二乘法的解析解法，专门用于在已知点对对应关系的情况下，求解最优旋转矩阵，具有计算高效、精度高的特点，在 Matlab 中可通过矩阵运算和点云处理工具快速实现。
一、Kabsch 算法的核心原理
Kabsch 算法的核心是通过最小化源点云与目标点云对应点对之间的均方误差（MSE），求解最优旋转矩阵R。其数学基础是：对于两组对应点集\( P = \{p_1, p_2, ..., p_n\} \)（源点云）和\( Q = \{q_1, q_2, ..., q_n\} \)（目标点云），假设对应点满足\( q_i = R p_i + t \)，则通过以下步骤可求解R和t：
1. 消除平移影响：将两组点云分别平移至各自的质心（中心化），转化为求解旋转矩阵的问题；
2. 计算中心化后点集的协方差矩阵；
3. 通过奇异值分解（SVD）求解协方差矩阵，得到最优旋转矩阵；
4. 根据质心和旋转矩阵计算平移向量t。
二、Matlab 中 Kabsch 算法点云配准的主要流程
在 Matlab 中实现基于 Kabsch 算法的点云配准，需结合点云处理工具箱（Point Cloud Processing Toolbox）和矩阵运算函数，具体流程如下：
1. 数据准备与对应点对确定
读取点云：使用pcread函数读取源点云（sourcePts）和目标点云（targetPts），格式为\( n \times 3 \)矩阵（\( n \)为点数量，3 个维度对应 x、y、z 坐标）。
确定对应点对：Kabsch 算法要求已知点对的对应关系（即明确\( p_i \)与\( q_i \)的匹配），可通过特征匹配（如 SIFT、FPFH 特征）或手动标记获取。
2. 点云中心化
计算源点云和目标点云的质心：
centroid_source = mean(sourcePts, 1); % 源点云质心（1×3向量） centroid_target = mean(targetPts, 1); % 目标点云质心（1×3向量）
对两组点云进行中心化（减去各自质心），消除平移分量影响：
source_centered = sourcePts - repmat(centroid_source, size(sourcePts, 1), 1); target_centered = targetPts - repmat(centroid_target, size(targetPts, 1), 1);3. 计算协方差矩阵并求解旋转矩阵
计算中心化后点集的协方差矩阵\( H \)：
H = source_centered' * target_centered; % 3×3协方差矩阵
对协方差矩阵进行奇异值分解（SVD）：\( H = U S V^T \)
[U, ~, V] = svd(H);
求解最优旋转矩阵R，并修正特殊情况（确保旋转矩阵行列式为 1，避免镜像变换）：
R = V * U'; if det(R) < 0 % 若行列式为-1，修正旋转矩阵 V(:, 3) = -V(:, 3); R = V * U'; end4. 计算平移向量并完成配准
根据质心和旋转矩阵计算平移向量t：
t = centroid_target' - R * centroid_source'; % 平移向量（3×1）
应用变换矩阵对源点云进行配准：
% 源点云配准后坐标 = R × 源点云坐标 + t source_registered = (R * sourcePts')' + repmat(t', size(sourcePts, 1), 1);5. 配准效果评估
计算配准后对应点对的均方根误差（RMSE），评估对齐精度：
errors = sqrt(sum((source_registered - targetPts).^2, 2)); rmse = mean(errors); % RMSE越小，配准效果越好
可视化配准结果：使用pcshow函数对比配准前后的点云重叠度：
figure; pcshow(sourcePts, 'r'); hold on; % 源点云（红色） pcshow(targetPts, 'b'); % 目标点云（蓝色） title('配准前'); figure; pcshow(source_registered, 'r'); hold on; % 配准后源点云（红色） pcshow(targetPts, 'b'); % 目标点云（蓝色） title('配准后');三、Kabsch 算法点云配准的应用领域
Kabsch 算法因解析解特性（无需迭代）和高精度，在多个领域得到广泛应用：
1. 计算机视觉与三维重建
用于多视角点云的拼接（如双目相机或激光雷达的多帧点云对齐），构建完整的三维场景模型。
2. 机器人技术
在机器人抓取和导航中，通过配准传感器（如深度相机）获取的物体点云与预存模型，确定物体姿态，实现精准抓取或避障。
3. 逆向工程与工业检测
将实物扫描点云与 CAD 模型配准，检测零件加工误差（如尺寸偏差、变形），或用于零件的逆向建模。
4. 医学影像
配准患者的 CT/MRI 点云与解剖学标准模型，辅助病灶定位、手术规划（如骨科手术中骨骼点云与假体模型的对齐）。
5. 文化遗产保护
对文物扫描点云进行多视角配准，构建高精度数字模型，用于文物修复或虚拟展示。
四、总结
Kabsch 算法是点云配准中求解最优旋转矩阵的经典方法，尤其适用于已知对应点对的场景。在 Matlab 中，借助矩阵运算函数（如svd）和点云工具箱，可快速实现从数据预处理到配准评估的全流程。其高效性和精度使其在三维重建、工业检测、机器人等领域具有不可替代的应用价值。
我们这次使用的数据，依然是，不说啦，自己猜
一、点云实现Kabsch配准的程序
1、最简版
%% Kabsch_Register_Bunny.m —— MATLAB 2020a 兼容
clc; clear; close all;
%% 1. 读取点云
[file, path] = uigetfile({'*.pcd;*.ply;*.xyz','点云文件 (*.pcd,*.ply,*.xyz)'},...
'请选择点云（例如 bunny.pcd）');
if file==0; return; end
fname = fullfile(path,file);
ptCloudSrc = pcread(fname);
src = ptCloudSrc.Location; % N×3 坐标
%% 2. 加噪声
noise = 0.001 * randn(size(src)); % N(0,0.001^2)
tgt = src + noise; % 先噪声
%% 3. 平移
t = [0.1 0.15 0.2]; % x,y,z 偏移
tgt = tgt + t;
ptCloudTgt = pointCloud(tgt, ...
'Normal', ptCloudSrc.Normal, ...
'Color', ptCloudSrc.Color); % 保留属性
%% 4. 可视化原始
figure('Name','原始点云','Color','w');
pcshowpair(ptCloudSrc, ptCloudTgt);
title('红色=source，绿色=target');
%% 5. Kabsch 配准
registered = kabschRegister(src, tgt); % 核心函数
ptCloudReg = pointCloud(registered);
%% 6. 可视化结果
figure('Name','Kabsch配准结果','Color','w');
pcshowpair(ptCloudReg, ptCloudTgt);
title('红色=registered，绿色=target');
%% 7. 核心 Kabsch 配准函数（纯 MATLAB 内置）
function out = kabschRegister(src, tgt)
% src, tgt : N×3 double
muS = mean(src,1); % 质心
muT = mean(tgt,1);
% 去质心
A = src - muS;
B = tgt - muT;
% 协方差矩阵
H = A' * B;
[U,~,V] = svd(H);
% 防止反射
d = sign(det(V' * U));
S = eye(3); S(3,3) = d;
R = V * S * U';
t = muT - muS * R; % 1×3
out = src * R + t; % 变换后坐标
end2、GUI版本
function Kabsch_Register_GUI
%Kabsch_Register_GUI 基于Kabsch算法的点云配准GUI（三栏视图）
% 兼容MATLAB 2020a，无需额外工具箱
clc; clear; close all;
%% ---------------- 主窗口 ----------------
fig = figure('Name','Kabsch 配准工具', 'NumberTitle','off', ...
'MenuBar','none', 'ToolBar','none', ...
'Position',[100 100 1280 720], 'Color','w');
%% ---------------- 布局常量 ----------------
imgWidth = 0.78; % 三栏总宽
panelW = imgWidth/3 - 0.005; % 单栏宽
gap = 0.005;
%% ---------------- 三栏 axes ----------------
pnlSrc = uipanel('Parent',fig, 'Units','normalized', ...
'FontSize',16, 'Position',[0.02 0.02 panelW 0.96], ...
'Title','原始点云（红）');
pnlTgt = uipanel('Parent',fig, 'Units','normalized', ...
'FontSize',16, ...
'Position',[0.02+panelW+gap 0.02 panelW 0.96], ...
'Title','目标点云（绿）');
pnlReg = uipanel('Parent',fig, 'Units','normalized', ...
'FontSize',16, ...
'Position',[0.02+2*(panelW+gap) 0.02 panelW 0.96], ...
'Title','Kabsch 配准后（红→绿）');
axSrc = axes('Parent',pnlSrc, 'Units','normalized', 'Position',[0.05 0.05 0.9 0.9]);
axTgt = axes('Parent',pnlTgt, 'Units','normalized', 'Position',[0.05 0.05 0.9 0.9]);
axReg = axes('Parent',pnlReg, 'Units','normalized', 'Position',[0.05 0.05 0.9 0.9]);
%% ---------------- 控制面板 ----------------
pnlCtrl = uipanel('Parent',fig, 'Units','normalized', ...
'FontSize',16, 'Position',[0.78 0 0.22 1], ...
'Title','控制');
txtH = 0.04; btnH = 0.06; yTop = 0.94;
% ① 加载
uicontrol('Parent',pnlCtrl, 'Style','pushbutton', 'String','浏览…', ...
'FontSize',16, 'Units','normalized', ...
'Position',[0.05 yTop-btnH 0.9 btnH], ...
'Callback',@loadCloud);
yTop = yTop - btnH - 0.02;
% 状态提示
lblInfo = uicontrol('Parent',pnlCtrl, 'Style','text', ...
'String','未加载点云', 'FontSize',10, ...
'Units','normalized', ...
'Position',[0.05 yTop-txtH 0.9 txtH], ...
'HorizontalAlignment','left');
yTop = yTop - txtH - 0.02;
% ② 噪声强度 / mm
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','噪声强度 / mm', ...
'FontSize',12, 'FontWeight','bold', 'Units','normalized', ...
'Position',[0.05 yTop-txtH 0.9 txtH], 'HorizontalAlignment','left');
yTop = yTop - txtH - 0.01;
sliderNoise = uicontrol('Parent',pnlCtrl, 'Style','slider', ...
'Min',0, 'Max',10, 'Value',1, ...
'Units','normalized', ...
'Position',[0.05 yTop-btnH 0.65 btnH], ...
'Callback',@refreshTarget);
txtNoise = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','1', ...
'FontSize',14, 'Units','normalized', ...
'Position',[0.75 yTop-btnH 0.2 btnH], ...
'Callback',@editNoiseCB);
yTop = yTop - btnH - 0.02;
% ③ 平移偏移 / mm
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','平移偏移 / mm', ...
'FontSize',12, 'FontWeight','bold', 'Units','normalized', ...
'Position',[0.05 yTop-txtH 0.9 txtH], 'HorizontalAlignment','left');
yTop = yTop - txtH - 0.01;
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','X', ...
'FontSize',10, 'Units','normalized', ...
'Position',[0.05 yTop-txtH 0.15 txtH]);
editX = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','100', ...
'FontSize',14, 'Units','normalized', ...
'Position',[0.20 yTop-txtH 0.20 txtH], ...
'Callback',@refreshTarget);
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','Y', ...
'FontSize',10, 'Units','normalized', ...
'Position',[0.45 yTop-txtH 0.15 txtH]);
editY = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','150', ...
'FontSize',14, 'Units','normalized', ...
'Position',[0.60 yTop-txtH 0.20 txtH], ...
'Callback',@refreshTarget);
uicontrol('Parent',pnlCtrl, 'Style','text', 'String','Z', ...
'FontSize',10, 'Units','normalized', ...
'Position',[0.05 yTop-2*txtH 0.15 txtH]);
editZ = uicontrol('Parent',pnlCtrl, 'Style','edit', 'String','200', ...
'FontSize',14, 'Units','normalized', ...
'Position',[0.20 yTop-2*txtH 0.20 txtH], ...
'Callback',@refreshTarget);
yTop = yTop - 2*txtH - 0.02;
% ④ 运行 Kabsch
uicontrol('Parent',pnlCtrl, 'Style','pushbutton', 'String','运行 Kabsch 配准', ...
'FontSize',16, 'Units','normalized', ...
'Position',[0.05 yTop-btnH 0.9 btnH], ...
'Callback',@runKabsch);
yTop = yTop - btnH - 0.02;
% ⑤ 保存
uicontrol('Parent',pnlCtrl, 'Style','pushbutton', 'String','保存配准结果', ...
'FontSize',16, 'Units','normalized', ...
'Position',[0.05 yTop-btnH 0.9 btnH], ...
'Callback',@(s,e)saveCloud(ptCloudReg));
%% ---------------- 数据容器 ----------------
ptCloudSrc = pointCloud.empty;
ptCloudTgt = pointCloud.empty;
ptCloudReg = pointCloud.empty;
%% ---------------- 回调函数 ----------------
function loadCloud(~,~)
[file,path] = uigetfile({'*.ply;*.pcd;*.xyz','点云文件'},'选择点云');
if isequal(file,0); return; end
try
ptCloudSrc = pcread(fullfile(path,file));
catch ME
errordlg(ME.message,'读取失败'); return;
end
set(lblInfo,'String',sprintf('已加载：%s (%d 点)',file,ptCloudSrc.Count));
refreshTarget();
end
function refreshTarget(~,~)
if isempty(ptCloudSrc); return; end
% 1. 读取界面参数
sigma = get(sliderNoise,'Value')/1000; % 转换为米
t = [str2double(get(editX,'String'));
str2double(get(editY,'String'));
str2double(get(editZ,'String'))]/1000; % 3×1，转换为米
if any(isnan(t)); t = [0.1; 0.15; 0.2]; end
t = t.'; % 1×3，方便批量操作
% 2. 生成目标点云（添加噪声和平移）
srcLoc = ptCloudSrc.Location; % N×3 坐标
noise = sigma*randn(size(srcLoc)); % 高斯噪声
tgt = srcLoc + noise + repmat(t,size(srcLoc,1),1);% 加噪声和平移
% 3. 组装 pointCloud 对象
ptCloudTgt = pointCloud(tgt,'Normal',ptCloudSrc.Normal,'Color',ptCloudSrc.Color);
% 4. 上色区分
ptCloudSrc.Color = repmat(uint8([255 0 0]),size(srcLoc,1),1); % 红色
ptCloudTgt.Color = repmat(uint8([0 255 0]),size(srcLoc,1),1); % 绿色
% 5. 显示点云
showPointCloud(axSrc,ptCloudSrc);
showPointCloud(axTgt,ptCloudTgt);
% 6. 清空上次结果
ptCloudReg = pointCloud.empty;
cla(axReg); title(axReg,'Kabsch 配准后（红→绿）');
% 7. 同步滑竿文本
set(txtNoise,'String',num2str(get(sliderNoise,'Value')));
end
function editNoiseCB(src,~)
v = str2double(get(src,'String'));
if isnan(v); v=1; end
v = max(0,min(10,v)); % 限制在0-10范围内
set(sliderNoise,'Value',v);
refreshTarget();
end
function runKabsch(~,~)
if isempty(ptCloudSrc) || isempty(ptCloudTgt)
errordlg('请先加载点云','提示'); return;
end
% Kabsch 配准
src = ptCloudSrc.Location;
tgt = ptCloudTgt.Location;
registered = kabschRegister(src,tgt); % 调用核心算法
ptCloudReg = pointCloud(registered,'Color',ptCloudSrc.Color,'Normal',ptCloudSrc.Normal);
% 可视化配准结果
showPointCloud(axReg,ptCloudReg); hold(axReg,'on');
pcshow(ptCloudTgt,'Parent',axReg); hold(axReg,'off');
title(axReg,'Kabsch 配准结果：红→绿');
end
function saveCloud(cloud)
if isempty(cloud)
errordlg('请先运行 Kabsch 配准','提示'); return;
end
[file,path] = uiputfile({'*.pcd','PCD';'*.ply','PLY';'*.xyz','XYZ'},'保存配准点云');
if isequal(file,0); return; end
try
pcwrite(cloud,fullfile(path,file),'Precision','double');
msgbox('保存成功！','提示');
catch ME
errordlg(ME.message,'保存失败');
end
end
function showPointCloud(ax,pc)
cla(ax); set(ax,'Color','w');
pcshow(pc,'Parent',ax,'MarkerSize',35);
axis(ax,'tight'); grid(ax,'on'); view(ax,3); drawnow;
end
%% ---------------- 核心：Kabsch 配准算法 ----------------
function out = kabschRegister(src, tgt)
% src, tgt : N×3 double，点云坐标
muS = mean(src,1); % 计算原始点云质心
muT = mean(tgt,1); % 计算目标点云质心
% 去质心处理
A = src - muS;
B = tgt - muT;
% 计算协方差矩阵并SVD分解
H = A' * B;
[U,~,V] = svd(H);
% 防止反射（保持右手坐标系）
d = sign(det(V' * U));
S = eye(3); S(3,3) = d;
R = V * S * U'; % 计算旋转矩阵
t = muT - muS * R; % 计算平移向量
out = src * R + t; % 应用变换得到配准结果
end
end二、点云进行Kabsch配准的结果
从结果中可以看到，最小二乘法还是很厉害的，相距那么远还能很好的配准上。
多嘴说一句，题主的很多程序确实借用了AI，不过不要以为用AI就很好实现了，个中艰辛，唯有亲自动手才知道呀。此次结果和GUI不重要，重要的调试过程。还是那句话，纸上得来终觉浅，绝知此事要躬行！
就酱，下次见^_6
浙公网安备 33010602011771号