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

2024-02-11 23:52

本文主要是介绍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结果

ndt结果

cpd结果

三种方法实验下来,两个点云基本都是z轴有45度的相对转角

三、工具箱安装和示例文件

点云工具箱:链接:https://pan.baidu.com/s/1zNo03fIxP63-lOSjePCcLg 
提取码:wstc 

cpd工具箱:链接:https://pan.baidu.com/s/1-Um4pRcYJOAKLWjeuL-zlA 
提取码:wstc 
示例文件:链接:https://pan.baidu.com/s/1ql_q4jnUZjlZL3l3fRo8vQ 
提取码:wstc 

完整代码:matlab点云配准,包括ICP/NDT/CPD算法资源-CSDN文库

这篇关于Matlab使用点云工具箱进行点云配准ICP\NDT\CPD的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/701153

相关文章

中文分词jieba库的使用与实景应用(一)

知识星球:https://articles.zsxq.com/id_fxvgc803qmr2.html 目录 一.定义: 精确模式(默认模式): 全模式: 搜索引擎模式: paddle 模式(基于深度学习的分词模式): 二 自定义词典 三.文本解析   调整词出现的频率 四. 关键词提取 A. 基于TF-IDF算法的关键词提取 B. 基于TextRank算法的关键词提取

使用SecondaryNameNode恢复NameNode的数据

1)需求: NameNode进程挂了并且存储的数据也丢失了,如何恢复NameNode 此种方式恢复的数据可能存在小部分数据的丢失。 2)故障模拟 (1)kill -9 NameNode进程 [lytfly@hadoop102 current]$ kill -9 19886 (2)删除NameNode存储的数据(/opt/module/hadoop-3.1.4/data/tmp/dfs/na

Hadoop数据压缩使用介绍

一、压缩原则 (1)运算密集型的Job,少用压缩 (2)IO密集型的Job,多用压缩 二、压缩算法比较 三、压缩位置选择 四、压缩参数配置 1)为了支持多种压缩/解压缩算法,Hadoop引入了编码/解码器 2)要在Hadoop中启用压缩,可以配置如下参数

Makefile简明使用教程

文章目录 规则makefile文件的基本语法:加在命令前的特殊符号:.PHONY伪目标: Makefilev1 直观写法v2 加上中间过程v3 伪目标v4 变量 make 选项-f-n-C Make 是一种流行的构建工具,常用于将源代码转换成可执行文件或者其他形式的输出文件(如库文件、文档等)。Make 可以自动化地执行编译、链接等一系列操作。 规则 makefile文件

使用opencv优化图片(画面变清晰)

文章目录 需求影响照片清晰度的因素 实现降噪测试代码 锐化空间锐化Unsharp Masking频率域锐化对比测试 对比度增强常用算法对比测试 需求 对图像进行优化,使其看起来更清晰,同时保持尺寸不变,通常涉及到图像处理技术如锐化、降噪、对比度增强等 影响照片清晰度的因素 影响照片清晰度的因素有很多,主要可以从以下几个方面来分析 1. 拍摄设备 相机传感器:相机传

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯,先赞后看哦~🎈🎈 🏆 作者简介:景天科技苑 🏆《头衔》:大厂架构师,华为云开发者社区专家博主,阿里云开发者社区专家博主,CSDN全栈领域优质创作者,掘金优秀博主,51CTO博客专家等。 🏆《博客》:Python全栈,前后端开发,小程序开发,人工智能,js逆向,App逆向,网络系统安全,数据分析,Django,fastapi

pdfmake生成pdf的使用

实际项目中有时会有根据填写的表单数据或者其他格式的数据,将数据自动填充到pdf文件中根据固定模板生成pdf文件的需求 文章目录 利用pdfmake生成pdf文件1.下载安装pdfmake第三方包2.封装生成pdf文件的共用配置3.生成pdf文件的文件模板内容4.调用方法生成pdf 利用pdfmake生成pdf文件 1.下载安装pdfmake第三方包 npm i pdfma

零基础学习Redis(10) -- zset类型命令使用

zset是有序集合,内部除了存储元素外,还会存储一个score,存储在zset中的元素会按照score的大小升序排列,不同元素的score可以重复,score相同的元素会按照元素的字典序排列。 1. zset常用命令 1.1 zadd  zadd key [NX | XX] [GT | LT]   [CH] [INCR] score member [score member ...]

业务中14个需要进行A/B测试的时刻[信息图]

在本指南中,我们将全面了解有关 A/B测试 的所有内容。 我们将介绍不同类型的A/B测试,如何有效地规划和启动测试,如何评估测试是否成功,您应该关注哪些指标,多年来我们发现的常见错误等等。 什么是A/B测试? A/B测试(有时称为“分割测试”)是一种实验类型,其中您创建两种或多种内容变体——如登录页面、电子邮件或广告——并将它们显示给不同的受众群体,以查看哪一种效果最好。 本质上,A/B测

git使用的说明总结

Git使用说明 下载安装(下载地址) macOS: Git - Downloading macOS Windows: Git - Downloading Windows Linux/Unix: Git (git-scm.com) 创建新仓库 本地创建新仓库:创建新文件夹,进入文件夹目录,执行指令 git init ,用以创建新的git 克隆仓库 执行指令用以创建一个本地仓库的