交互式ICP

        以下程序演示如何编写交互式ICP查看器。该程序将加载点云并对其进行刚性变换。之后,使用ICP算法将变换后的点云与原来的点云对齐。每次用户按下“空格”,进行ICP迭代,刷新可视化界面。

代码实现

资源准备 monkey.ply

#include <string>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;bool next_iteration = false;void print4x4Matrix(const Eigen::Matrix4d& matrix)
{printf("Rotation matrix :\n");printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));printf("Translation vector :\n");printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}
/*** 此函数是查看器的回调。当查看器窗口位于顶部时,只要按任意键,就会调用此函数。如果碰到“空格”,将布尔值设置为true。*/
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,void* nothing)
{if (event.getKeySym() == "space" && event.keyDown())next_iteration = true;
}int main(int argc,char* argv[])
{// 准备将使用的点云PointCloudT::Ptr cloud_in(new PointCloudT);  // Original point cloudPointCloudT::Ptr cloud_tr(new PointCloudT);  // Transformed point cloudPointCloudT::Ptr cloud_icp(new PointCloudT);  // ICP output point cloud// 检查程序的参数,设置初始ICP迭代的次数,然后尝试加载PLY文件if (argc < 2){printf("Usage :\n");printf("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);PCL_ERROR("Provide one ply file.\n");return (-1);}int iterations = 1;  // 默认的ICP迭代次数if (argc > 2){// 如果用户将迭代次数作为参数传递iterations = atoi(argv[2]);if (iterations < 1){PCL_ERROR("Number of initial iterations must be >= 1\n");return (-1);}}pcl::console::TicToc time;time.tic();if (pcl::io::loadPLYFile(argv[1], *cloud_in) < 0){PCL_ERROR("Error loading cloud %s.\n", argv[1]);return (-1);}std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size() << " points) in " << time.toc() << " ms\n" << std::endl;// 我们使用刚性矩阵变换来变换原始点云。// cloud_in包含原始点云。// cloud_tr和cloud_icp包含平移/旋转的点云。// cloud_tr是我们将用于显示的备份(绿点云)。// 定义旋转矩阵和平移向量Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();// 一个旋转矩阵 (see https://en.wikipedia.org/wiki/Rotation_matrix)double theta = M_PI / 8;  // 以弧度为单位的旋转角度transformation_matrix(0, 0) = std::cos(theta);transformation_matrix(0, 1) = -sin(theta);transformation_matrix(1, 0) = sin(theta);transformation_matrix(1, 1) = std::cos(theta);// Z轴上的平移(0.4米)transformation_matrix(2, 3) = 0.4;// 在终端中显示变换矩阵std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;print4x4Matrix(transformation_matrix);// 执行转换pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);*cloud_tr = *cloud_icp;  // 我们将cloud_icp备份到cloud_tr中以备将来使用// 这是ICP对象的创建。我们设置ICP算法的参数。// setMaximumIterations(iterations)设置要执行的初始迭代次数(默认值为1)。// 然后,我们将点云转换为cloud_icp。 第一次对齐后,我们将在下一次使用该ICP对象时(当用户按下“空格”时)将ICP最大迭代次数设置为1。// ICP算法time.tic();pcl::IterativeClosestPoint<PointT, PointT> icp;icp.setMaximumIterations(iterations);icp.setInputSource(cloud_icp);icp.setInputTarget(cloud_in);icp.align(*cloud_icp);icp.setMaximumIterations(1);  // 下次调用.align()函数时,我们将此变量设置为1std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;// 检查ICP算法是否收敛;否则退出程序。如果返回true,我们将转换矩阵存储在4x4矩阵中,然后打印刚性矩阵转换。if (icp.hasConverged()){std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;transformation_matrix = icp.getFinalTransformation().cast<double>();print4x4Matrix(transformation_matrix);}else{PCL_ERROR("\nICP has not converged.\n");return (-1);}// 可视化pcl::visualization::PCLVisualizer viewer("ICP demo");// 创建两个垂直分隔的窗口int v1(0);int v2(1);viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);// 颜色float bckgr_gray_level = 0.0;  // 黑色float txt_gray_lvl = 1.0 - bckgr_gray_level;// 原始点云为白色pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,(int)255 * txt_gray_lvl);viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);// 变换后的点云为绿色pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_tr, 20, 180, 20);viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);// ICP对齐点云为红色pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_icp, 180, 20, 20);viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);// 在每个视口中添加文本描述viewer.addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);viewer.addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);std::stringstream ss;ss << iterations;std::string iterations_cnt = "ICP iterations = " + ss.str();viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);// 设置背景颜色viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);// 设置相机位置和方向viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);viewer.setSize(1280, 1024);  // 可视化窗口的尺寸// 注册键盘回调:viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);//可视化while (!viewer.wasStopped()){viewer.spinOnce();// 如果用户按下空格:if (next_iteration){// ICP算法time.tic();// 如果用户按下键盘上的任意键,则会调用keyboardEventOccurred函数。 此功能检查键是否为“空格”。// 如果是,则全局布尔值next_iteration设置为true,从而允许查看器循环输入代码的下一部分:调用ICP对象以进行对齐。// 记住,我们已经配置了该对象输入/输出云,并且之前通过setMaximumIterations将最大迭代次数设置为1。icp.align(*cloud_icp);std::cout << "Applied 1 ICP iteration in " << time.toc() << " ms" << std::endl;// 和以前一样,我们检查ICP是否收敛,如果不收敛,则退出程序。if (icp.hasConverged()){// printf(“ 033 [11A”); 在终端增加11行以覆盖显示的最后一个矩阵是一个小技巧。// 简而言之,它允许替换文本而不是编写新行; 使输出更具可读性。 我们增加迭代次数以更新可视化器中的文本值。printf("\033[11A");  // Go up 11 lines in terminal output.printf("\nICP has converged, score is %+.0e\n", icp.getFitnessScore());// 这意味着,如果您已经完成了10次迭代,则此函数返回矩阵以将点云从迭代10转换为11。std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;// 函数getFinalTransformation()返回在迭代过程中完成的刚性矩阵转换(此处为1次迭代)。transformation_matrix *= icp.getFinalTransformation().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!print4x4Matrix(transformation_matrix);  // 打印原始姿态和当前姿态之间的转换ss.str("");ss << iterations;std::string iterations_cnt = "ICP iterations = " + ss.str();viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");}else{PCL_ERROR("\nICP has not converged.\n");return (-1);}//这不是我们想要的。如果我们将最后一个矩阵与新矩阵相乘,那么结果就是从开始到当前迭代的转换矩阵。}next_iteration = false;}return (0);
}

使用方式

windows下执行以下命令:

.\xxx.exe ...\monkey.ply 1

随着迭代次数增加,效果如下:

迭代次数=1

迭代次数=19

迭代次数=41

官方文档:https://pcl-tutorials.readthedocs.io/en/latest/interactive_icp.html 

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/98947.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

I/O多路复用【Linux/网络】(C++实现select、poll和epoll服务器)

阅读前导&#xff1a; “I/O 多路复用”处于知识树中网络和操作系统的最后&#xff0c;因此本文默认读者有计算机网络和操作系统的基础。 1. 引入&#xff1a;C10K 问题 c10k 问题是指如何让一个服务器同时处理超过 10000 个客户端的连接&#xff0c;这是一个网络编程中的经…

STM32+USB3300复位枚举异常的问题

关键字&#xff1a;STM32F4&#xff0c;STM32H7&#xff0c;USB3300&#xff0c;USBHS&#xff0c;Reset复位 F4和H7用的都是DWC2的USBIP&#xff0c;我的板子上3300单片机工作的很好&#xff0c;插入枚举一切正常&#xff0c;但是设备收到上位机的复位命令后&#xff0c;单片…

ubuntu安装ssh

安装 OpenSSH 服务器&#xff08;如果尚未安装&#xff09;&#xff1a; apt-get update && apt-get upgrade -y sudo apt-get install -y openssh-server 检查 SSH 服务是否正在运行&#xff1a; sudo service ssh status 如果 SSH 服务未运行&#xff0c;请通过以…

Redis之主从复制,哨兵模式,集群

Redis之主从复制&#xff0c;哨兵模式&#xff0c;集群 1、主从复制1.1主从复制概述1.2Redis主从复制作用1.3Redis主从复制流程1.4部署Redis 主从复制 2、哨兵模式2.1哨兵模式原理2.2哨兵模式的作用2.3哨兵模式的结构2.4故障转移机制2.5搭建Redis 哨兵模式 3、Redis集群模式3.1…

Deep learning of free boundary and Stefan problems论文阅读复现

Deep learning of free boundary and Stefan problems论文阅读复现 摘要1. 一维一相Stefan问题1.1 Direct Stefan problem1.2 Inverse Type I1.3 Inverse Type II 2. 一维二相Stefan问题2.1 Direct Stefan problem2.2 Inverse Type I2.3 Inverse Type II 3. 二维一相Stefan问题…

数据结构 | (四) Queue

队列 &#xff1a;只允许在一端进行插入数据操作&#xff0c;在另一端进行删除数据操作的特殊线性表&#xff0c;队列具有先进先出 FIFO(First In First Out) 入队列&#xff1a;进行插入操作的一端称为 队尾&#xff08; Tail/Rear &#xff09; 出队列&#xff1a;进行删除操…

如何在mac a1系统下将mysql加入环境变量

mac系统若使用二进制软件包直接安装&#xff0c;默认路径为/usr/local/mysql 故而需要将/usr/local/mysql/bin加入到环境变量中即可&#xff0c;具体操作过程如下&#xff1a; 打开终端open -e .zprofile回车在TextEdit中追加如下内容&#xff0c;并保存 PATH"/usr/loc…

代码随想录 单调栈part2

503. 下一个更大元素 II 给定一个循环数组 nums &#xff08; nums[nums.length - 1] 的下一个元素是 nums[0] &#xff09;&#xff0c;返回 nums 中每个元素的 下一个更大元素 。 数字 x 的 下一个更大的元素 是按数组遍历顺序&#xff0c;这个数字之后的第一个比它更大的数…

iOS App上架全流程及相关处理

iOS app上架总体流程&#xff1a; 一、IOS上架整个流程 1、申请开发者账号 2、创建APP ID及申请证书 3、itunes connect 创建APP 4、打包 上传APP 5、提交APP&#xff0c;上线成功 1、申请开发者账号 苹果开发者账号主要分为三种&#xff1a;个人账号、公司账号、企业账…

Linux安装单机PostgreSQL15.4

1. 联网rpm安装 1.1.关闭服务 ## 关闭防火墙 systemctl stop firewalld.service systemctl disable firewalld.service ## 关闭 selinux cat /etc/selinux/config SELINUXdisabled1.2.安装yum源 yum install -y https://download.postgresql.org/pub/repos/yum/reporpms/EL-…

深度学习基础知识 register_buffer 与 register_parameter用法分析

深度学习基础知识 register_buffer 与 register_parameter用法分析 1、问题引入2、register_parameter()2.1 作用2.2 用法 3、register_buffer()3.1 作用3.2 用法 1、问题引入 思考问题&#xff1a;定义的weight与bias是否会被保存到网络的参数中&#xff0c;可否在优化器的作用…

使用webclient出现Exceeded limit on max bytes to buffer : 262144

使用webclient出现Exceeded limit on max bytes to buffer : 262144 由于公司技术升级&#xff0c;要求我用webflux的webclient替换原来的restTemplate&#xff0c;代码看起来很好改动&#xff0c;但是在改完后测试出现了这样的报错 org.springframework.core.io.buffer.Data…

《深入理解计算机系统》(2):虚拟内存

虚拟内存是一种对主存的抽象概念。 &#xff08;1&#xff09;将主存看作一个存储在磁盘上的地址空间的高速缓存&#xff0c;在主存中只保存活动区域&#xff0c;并根据需要在磁盘和主存之间来回传送数据&#xff0c;通过这种方式高效地使用内存 &#xff08;2&#xff09;为每…

MyBatis注解开发的多表操作

1.一对一 环境准备 数据表为之前多表操作db1的数据表 bean.Card&#xff1a; package Mybatis5.bean;public class Card {private Integer id; //主键idprivate String number; //身份证号private Person p;//所属人的对象public Card() {}public Card(Integer id, String …

GitHub要求开启2FA,否则不让用了。

背景 其实大概在一个多月前&#xff0c;在 GitHub 网页端以及邮箱里都被提示&#xff1a;要求开启 2FA &#xff0c;即双因子认证&#xff1b;但是当时由于拖延症和侥幸心理作祟&#xff0c;直接忽略了相关信息&#xff0c;毕竟“又不是不能用”。。 只到今天发现 GitHub 直接…

Linux软硬链接和动静态库

本文已收录至《Linux知识与编程》专栏&#xff01; 作者&#xff1a;ARMCSKGT 演示环境&#xff1a;CentOS 7 软硬链接和动静态库 前言正文软硬链接原理使用 文件时间动静态库库介绍静态库静态库制作静态库的使用关于静态链接 动态库动态库制作动态库的使用关于动态链接 补充 最…

Java练习题-用冒泡排序法实现数组排序

✅作者简介&#xff1a;CSDN内容合伙人、阿里云专家博主、51CTO专家博主、新星计划第三季python赛道Top1&#x1f3c6; &#x1f4c3;个人主页&#xff1a;hacker707的csdn博客 &#x1f525;系列专栏&#xff1a;Java练习题 &#x1f4ac;个人格言&#xff1a;不断的翻越一座又…

Computer Architecture Subtitle:Engineering And Technology

原文链接&#xff1a;https://www.cs.umd.edu/~meesh/411/CA-online/index.html

基于Springboot实现疫情网课管理系统项目【项目源码+论文说明】

基于Springboot实现疫情网课管理系统演示 摘要 随着科学技术的飞速发展&#xff0c;各行各业都在努力与现代先进技术接轨&#xff0c;通过科技手段提高自身的优势&#xff1b;对于疫情网课管理系统当然也不能排除在外&#xff0c;随着网络技术的不断成熟&#xff0c;带动了疫情…