Matlab使用点云工具箱进行点云配准ICP\NDT\CPD

一、代码

主代码main.m，三种配准方法任选其一

% 读取点云文件
source_pc = pcread('bun_zipper.ply');
target_pc = pcread('bun_zipper2.ply');

% 下采样
ptCloudA = point_downsample(source_pc);
ptCloudB = point_downsample(target_pc);

% 配准参数设置
opt = param_set("icp");
% opt = param_set("ndt");
% opt = param_set("cpd");
% 执行点云配准
[tform,translation,rotation,registered_pc] = icp_r(ptCloudA,ptCloudB,source_pc,opt);
% [tform,translation,rotation,registered_pc] = ndt_r(ptCloudA,ptCloudB,source_pc,opt);
% [tform,translation,rotation,registered_pc] = cpd_r(ptCloudA,ptCloudB,opt);
cal_and_print_data(tform,translation,rotation);


% 可视化
pc_visualization(ptCloudA, ptCloudB, target_pc, registered_pc);

配准参数设置

function[opt] = param_set(name, varargin)
p = inputParser;
addParameter(p,'Metric','pointToPoint');
addParameter(p,'Extrapolate',true);
addParameter(p,'InlierRatio',0.9);
addParameter(p,'Tolerance',[0.01, 0.01]);
addParameter(p,'MaxIterations',100);
addParameter(p,'Verbose',true);
addParameter(p,'method','rigid');
addParameter(p,'viz',0);
addParameter(p,'max_it',100);
addParameter(p,'tol',1e-6);
parse(p,varargin{:});
Metric = p.Results.Metric;
Extrapolate = p.Results.Extrapolate;
InlierRatio = p.Results.InlierRatio;
Tolerance = p.Results.Tolerance;
MaxIterations = p.Results.MaxIterations;
Verbose = p.Results.Verbose;
method = p.Results.method;
viz = p.Results.viz;
max_it = p.Results.max_it;
tol = p.Results.tol;
opt = containers.Map();
if name=="icp" || name == "ndt"
    opt('Metric') = Metric;
    opt('Extrapolate') = Extrapolate;
    opt('InlierRatio') = InlierRatio;
    opt('Tolerance') = Tolerance;
    opt('MaxIterations') = MaxIterations;
    opt('Verbose') = Verbose;
elseif name == "cpd"
    opt('method') = method;
    opt('viz') = viz;
    opt('max_it') = max_it;
    opt('tol') = tol;
end

icp函数代码icp_r.m

function [tform,translation,rotation,registered_pc] = icp_r(ptCloudA, ptCloudB, source_pc, opt)

% tform 是一个 rigid3d 类型的对象，包含了配准后的转换矩阵。
% 参数说明:
% 'Metric' - 配准的度量类型，可以是 'pointToPoint'（默认值）或 'pointToPlane'，
%            'pointToPoint' 直接最小化点之间的距离，
%            'pointToPlane' 最小化点到面的距离，通常更快收敛但需要法线信息。
% 'Extrapolate' - 用于加速算法，如果设置为 true，算法会用前两次迭代的变换来预测下一步的变换。
% 'InlierRatio' - 预期的内点比例，范围从 0 到 1。内点是最有可能对应于固定点云中点的移动点云中的点。
% 'MaxIterations' - ICP算法的最大迭代次数。
% 'Tolerance' - 一个包含两个元素的向量，第一个元素是均方根变化容忍度，第二个元素是最小迭代改变容忍度。
% 'Verbose' - 如果设置为 true，将在命令窗口中显示算法的进度信息。
tform = pcregistericp(ptCloudA,ptCloudB, 'Metric', opt('Metric'), ...
                       'Extrapolate', opt('Extrapolate'), ...
                       'InlierRatio', opt('InlierRatio'), ...
                       'Tolerance', opt('Tolerance'), ...
                       'MaxIterations', opt('MaxIterations'), ...
                       'Verbose', opt('Verbose'));
% 提取平移向量
translation = tform.T(4, 1:3);
% 提取旋转矩阵
rotation = tform.T(1:3, 1:3);
% 应用配准变换到源点云
registered_pc = pctransform(source_pc, tform);

end

ndt函数代码ndt_r.m，由于matlab点云工具箱没有提供相关的特征提取函数，所以采用icp粗配准获得初始变换矩阵，再进行ndt精配准

function[tform,translation,rotation,registered_pc] = ndt_r(ptCloudA, ptCloudB, source_pc,opt)
% 使用 ICP 算法进行粗略配准，获取初始变换矩阵
tform = pcregistericp(ptCloudA,ptCloudB, 'Metric', opt('Metric'), ...
                       'Extrapolate', opt('Extrapolate'), ...
                       'InlierRatio', opt('InlierRatio'), ...
                       'Tolerance', opt('Tolerance'), ...
                       'MaxIterations', opt('MaxIterations'), ...
                       'Verbose', opt('Verbose'));
% 使用 NDT 算法进行精确配准
% 参数说明:
% gridSize - 用于创建用于 NDT 算法的体素网格的大小。较小的值可能会提高精度，但会增加计算成本。
% 'MaxIterations' - NDT算法的最大迭代次数。
% 'Tolerance' - 一个包含两个元素的向量：
%               tolerance1 - 迭代之间变换的最大容忍度。
%               tolerance2 - 均方根误差的最大容忍度。
% 'InitialTransform' - 配准之前的初始变换，这是一个 rigid3d 类型的对象。
% 'Verbose' - 如果设置为 true，将在命令窗口中显示算法的进度信息。

% tform 是一个 rigid3d 类型的对象，包含了配准后的变换矩阵。
gridStep =0.1; % 网格大小
tform = pcregisterndt(ptCloudA, ptCloudB, gridStep, ...
                       'MaxIterations', opt('MaxIterations'), ...
                       'Tolerance', opt('Tolerance'), ...
                       'InitialTransform', tform, ... % 使用单位矩阵作为初始变换
                       'Verbose', opt('Verbose'));
% 提取平移向量
translation = tform.T(4, 1:3);
% 提取旋转矩阵
rotation = tform.T(1:3, 1:3);
% 应用配准变换到源点云
registered_pc = pctransform(source_pc, tform);
end

cpd函数代码cpd_r.m，这个cpd配准还需要额外的cpd工具箱

function[tform,translation,rotation,registered_pc] = cpd_r(ptCloudA,ptCloudB, opt)
% 转换为双精度的坐标矩阵
X = double(ptCloudA.Location);
Y = double(ptCloudB.Location);
% 设置CPD选项，根据需要调整参数
op.method = opt('method'); % 使用非刚性变换，也可以选择 'rigid' 或 'affine'
op.viz = opt('viz');             % 显示配准过程
op.max_it = opt('max_it');        % 最大迭代次数
op.tol = opt('tol');          % 收敛容忍度

% 执行CPD配准
[tform, C] = cpd_register(Y, X, op);
% 提取平移向量
translation = tform.t;

% 提取旋转矩阵
rotation = tform.R;
registered_pc = pointCloud(tform.Y);
end

点云下采样

function[ptCloud] = point_downsample(pc)
gridStep = 0.005;
ptCloud = pcdownsample(pc,'gridAverage',gridStep);
end

计算并打印相关位姿信息

function[] = cal_and_print_data(tform,translation,rotation)

% 将旋转矩阵转换为欧拉角\四元数
eulerAngles = rotm2eul(rotation);
quat = rotm2quat(rotation);
%打印信息
fprintf('变换矩阵:')
disp(tform)
fprintf('平移量 (x, y, z): %.4f, %.4f, %.4f\n', translation(1), translation(2), translation(3));
fprintf('欧拉角 (rx, ry, rz): %.4f, %.4f, %.4f\n', rad2deg(eulerAngles(3)), rad2deg(eulerAngles(2)), rad2deg(eulerAngles(1)));
fprintf('四元数 (w, x, y, z): %.4f, %.4f, %.4f, %.4f\n', quat(1), quat(2), quat(3), quat(4));
end

可视化

function[] = pc_visualization(ptCloudA, ptCloudB, target_pc, registered_source_pc)
figure("Name", "原图像与配准后的图像");
set(gcf,'position',[150 80 1000 800])
subplot(2,1,1)
pcshowpair(ptCloudA, ptCloudB, 'MarkerSize', 20,'BackgroundColor',"white");
title('原图像');
xlabel('X-axis');
ylabel('Y-axis');
zlabel('Z-axis');
view(2)
legend('Target Point Cloud', 'Source Point Cloud');
%figure("Name", "配准后的图像");
subplot(2,1,2)
pcshowpair(target_pc, registered_source_pc, 'MarkerSize', 20,'BackgroundColor',"white");
title('配准后的图像');
xlabel('X-axis');
ylabel('Y-axis');
zlabel('Z-axis');
view(2)
legend('Target Point Cloud', 'Registered Source Point Cloud');

% 调整子图之间的距离
h = gcf; % 获取当前图形的句柄
h.Children(1).Position(2) = h.Children(1).Position(2) + 0.05; % 调整第一个子图的位置
h.Children(2).Position(2) = h.Children(2).Position(2) - 0.05; % 调整第二个子图的位置
end

二、结果

icp结果