使用PCL进行ICP点云配准

  下面代码的功能是：把一个文件夹中所有的pcd文件进行ICP点云配准，并且把每帧结果使用PCL的cloud_viewer进行显示。因为是在ROS下使用，所以还有一个ROS的发布操作(可忽略)。
  源码如下：

#include <iostream>
#include <fstream>
#include <sys/types.h>
#include <dirent.h>
#include <vector>
#include <cstring>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include "position/Position.h"using namespace std;#define NUMBER 50000
#define ITERATIONS 5
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;// function：获取路径下所有文件名，存在filenames中
void getFiles(string path, vector<string>& filenames)
{DIR *pDir;struct dirent* ptr;if(!(pDir = opendir(path.c_str()))){cout<<"Folder doesn't Exist!"<<endl;return;}while((ptr = readdir(pDir))!=0) {if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0){filenames.push_back(path + "/" + ptr->d_name);}}closedir(pDir);
}// function：读取整个文件夹下pcd文件，通过PCL将其中的坐标存储在position_x和position_y中
void Get_position_data(float *position_x, float *position_y, int &position_num,int &file_count) {string filePath = "/home/lyn/pcd";   // 待读取文件夹路径string pwd = get_current_dir_name();string logPath = pwd + "/pcd.log";vector<string> files;//获取该路径下的所有文件getFiles(filePath, files);float bckgr_gray_level = 0.0;  // Blackfloat txt_gray_lvl = 1.0 - bckgr_gray_level;PointCloudT::Ptr first_cloud (new PointCloudT);PointCloudT::Ptr second_cloud (new PointCloudT);PointCloudT::Ptr cloud_icp(new PointCloudT);pcl::visualization::PCLVisualizer viewer("viewer");fstream log;log.open(logPath,ios::app);if(log.fail()){cout<<"open fail"<<endl;}else{log<<"file size is "<<(int)files.size()<<endl;log<<"begin reading,file count is "<<file_count<<endl;}if(file_count==(int)files.size()){log<<"stop reading"<<endl;return;}if(pcl::io::loadPCDFile<PointT>(files[file_count],*first_cloud)==-1){PCL_ERROR("Couldn't read file test_pcd.pcd\n");return ;}else{log<<"file 0 is "<<files[file_count]<<endl;}if(pcl::io::loadPCDFile<PointT>(files[file_count+1],*second_cloud)==-1){PCL_ERROR("Couldn't read file test_pcd.pcd\n");return ;}else{log<<"file 1 is "<<files[file_count+1]<<endl;}file_count += 2;pcl::IterativeClosestPoint<PointT,PointT> icp;icp.setMaximumIterations(ITERATIONS);icp.setInputSource(first_cloud);icp.setInputTarget(second_cloud);icp.align(*cloud_icp);if(icp.hasConverged()){std::cout<<"\nICP has converged" << icp.getFitnessScore()<<std::endl;}else{PCL_ERROR ("\nICP has not converged.\n");}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);pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_color(first_cloud,(int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,(int) 255 * txt_gray_lvl);viewer.addPointCloud(first_cloud,cloud_color,"cloud_in_v1",v1);viewer.addPointCloud(first_cloud,cloud_color,"cloud_in_v2",v2);pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color(second_cloud, 20, 180, 20);viewer.addPointCloud(second_cloud,cloud_tr_color,"cloud_tr",v1);pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color(cloud_icp, 180, 20, 20);viewer.addPointCloud(cloud_icp,cloud_icp_color,"cloud_icp",v2);viewer.setCameraPosition (-3.68332, 2.94092, 12.71266, 0.289847, 0.921947, -0.256907, 0);viewer.setSize (1280, 1960);  // Visualiser window sizefor (int i=2; i<(int)files.size(); i++) { PointCloudT::Ptr cloud_in (new PointCloudT);PointCloudT::Ptr cloud_temp (new PointCloudT);if(pcl::io::loadPCDFile<PointT>(files[i], *cloud_in)==-1) {  //*打开点云文件 PCL_ERROR("Couldn't read file test_pcd.pcd\n");return ;}else{log<<"target file is "<<files[i]<<endl;    }// The Iterative Closest Point algorithm*cloud_temp = *cloud_icp;pcl::IterativeClosestPoint<PointT,PointT> icp;icp.setMaximumIterations(ITERATIONS);icp.setInputSource(cloud_icp);icp.setInputTarget(cloud_in);icp.align(*cloud_icp);if(icp.hasConverged()){std::cout<<"\nICP has converged" << icp.getFitnessScore()<<std::endl;            }else{PCL_ERROR ("\nICP has not converged.\n");}viewer.updatePointCloud(cloud_in,cloud_color,"cloud_in_v1");viewer.updatePointCloud(cloud_in,cloud_color,"cloud_in_v2");viewer.updatePointCloud(cloud_temp,cloud_tr_color,"cloud_tr");viewer.updatePointCloud(cloud_icp,cloud_icp_color,"cloud_icp");for(size_t t=0; t<cloud_icp->points.size(); ++t) {position_x[position_num] = cloud_icp->points[t].x;position_y[position_num] = cloud_icp->points[t].y;position_num++;}file_count++;} log<<"end reading,file count is"<<file_count<<endl;log.close();return ;
}int main(int argc, char** argv) {float *position_x = (float *)malloc(sizeof(float) * NUMBER);float *position_y = (float *)malloc(sizeof(float) * NUMBER);// ROS节点初始化ros::init(argc, argv, "position_publisher");// 创建节点句柄ros::NodeHandle n;// 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10ros::Publisher position_info_pub = n.advertise<position::Position>("/position_info", 10);//设置循环的频率ros::Rate loop_rate(1);int count = 0;int file_count = 0;while (ros::ok()) {int position_num = 0;Get_position_data(position_x, position_y, position_num,file_count);// 初始化learning_topic::Person类型的消息position::Position position_msg;for (int i=0; i<position_num; i++) {position_msg.position_x[i] = position_x[i];position_msg.position_y[i] = position_y[i];}position_msg.position_num = position_num;// 发布消息position_info_pub.publish(posirtion_msg);ROS_INFO("Publish Successfully!");cout << "point num is:" << position_num << endl;// 按照循环频率延时loop_rate.sleep();sleep(5);}return 0;
}

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

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

相关文章

最新AI创作系统+ChatGPT网站源码+支持GPT4.0+支持ai绘画+支持国内全AI模型

最新AI创作系统+ChatGPT网站源码+支持GPT4.0+支持ai绘画+支持国内全AI模型

一、AI创作系统 SparkAi系统是基于很火的GPT提问进行开发的Ai智能问答系统。本期针对源码系统整体测试下来非常完美&#xff0c;可以说SparkAi是目前国内一款的ChatGPT对接OpenAI软件系统。那么如何搭建部署AI创作ChatGPT系统&#xff1f;小编这里写一个详细图文教程吧&#x…
阅读更多...
Unity引擎更新收费模式：从收入分成转向游戏安装量，将会有哪些影响呢

Unity引擎更新收费模式：从收入分成转向游戏安装量，将会有哪些影响呢

一、前言 Unity 引擎宣布自 2024 年 1 月 1 日起&#xff0c;将根据游戏安装量对开发者进行收费。官网通知如下 收费模式如图 这张图的大致意思就是&#xff0c; 从2024年1月1日开始&#xff0c;Unity将对所有达标的用户&#xff08;开发者&#xff09;根据游戏安装量征收“安…
阅读更多...
ES6-扩展运算符“...“

ES6-扩展运算符“...“

三个连续的句点 … 称为 “展开运算符” (spread operator)&#xff0c;它可以将数组或对象展开成一个列表或一组键值对,常用于组合两个或多个阵列。 组合数组 const arr1 [1, 2, 3]; const arr2 [...arr1, 4, 5, 6]; // [1, 2, 3, 4, 5, 6] 组合对象 const obj1 {a: 1, …
阅读更多...
ARM Linux DIY（十三）Qt5 移植

ARM Linux DIY（十三）Qt5 移植

前言 板子带有屏幕&#xff0c;那当然要设计一下 GUI&#xff0c;对 Qt5 比较熟悉&#xff0c;那就移植它吧。 移植 Qt5 buildroot 使能 Qt5&#xff0c;这里我们只开启核心功能 gui module --> widgets module 编译 $ make ODIY_V3S/ qt5base编译报错&#xff1a;找不…
阅读更多...
Windows11系统C盘用户文件夹下用户文件夹为中文，解决方案

Windows11系统C盘用户文件夹下用户文件夹为中文，解决方案

说明&#xff1a; 1. 博主电脑为Windows11操作系统&#xff0c;亲测有效&#xff0c;修改后无任何影响&#xff0c;软件都可以正常运行&#xff01; 2. Windows10系统还不知道可不可行&#xff0c;因为Windows11的计算机管理中没有本地用户和组&#xff0c;博主在csdn上看到很…
阅读更多...
【斗罗2】霍雨浩实力被否定，超级斗罗眼光被嘲，魂导院成功捡漏

【斗罗2】霍雨浩实力被否定，超级斗罗眼光被嘲，魂导院成功捡漏

Hello,小伙伴们&#xff0c;我是小郑继续为大家深度解析斗罗大陆2绝世唐门。 最新一集《绝世唐门》已经更新&#xff0c;相信不少小伙伴和小郑一样都已经先睹为快&#xff0c;本集虽然是过渡剧情&#xff0c;但本身还是有不少有意思的剧情&#xff0c;作为过渡文戏也算是可圈可…
阅读更多...
网工基础知识——以太网

网工基础知识——以太网

1972年Bob Metcalfe“以太网之父”被Xerox雇佣为网络专家&#xff0c;Bob Metcalfe 来到Xerox公司的Palo Alto研究中心&#xff08;PARC&#xff09;的第一个任务是把Palo Alto的计算机连接到ARPANET&#xff08;Internet的前身&#xff09;上。1972年底Bob Metcalfe以ALOHA系统…
阅读更多...
修改配置maven镜像仓库位置，将maven镜像更换成阿里镜像

修改配置maven镜像仓库位置，将maven镜像更换成阿里镜像

大家都知道Maven默认连接的仓库位置https://repo .maven.apache.org/maven2访问国外镜像下载东西时相对较慢&#xff0c;所以多数朋友想切换到国内镜像&#xff0c;国内阿里已经给大家提供了一套完整的镜像供大家使用。 Maven默认连接的仓库位置 <repositories><repos…
阅读更多...
【1++的Linux】之进程（三）

【1++的Linux】之进程（三）

&#x1f44d;作者主页&#xff1a;进击的1 &#x1f929; 专栏链接&#xff1a;【1的Linux】 文章目录 一&#xff0c;什么是进程地址空间&#xff1f;二&#xff0c;进程地址空间是怎么设计的&#xff1f;三&#xff0c;为什么要有进程地址空间&#xff1f; 一&#xff0c;什…
阅读更多...
sql注入原理分析

sql注入原理分析

阅读更多...
【最新！企知道AES加密分析】使用Python实现完整解密算法

【最新！企知道AES加密分析】使用Python实现完整解密算法

文章目录 1. 写在前面2. 过debugger3. 抓包分析4. 断点分析5. Python实现解密算法1. 写在前面 最近华为各方面传递出来的消息无不体现出华为科技实力与技术处于遥遥领先的地位。所以出于好奇想要了解一下咱们国内这些互联网科技企业有哪些技术专利，于是就有了这篇文章！ 分析目…
阅读更多...
【操作系统】聊聊Linux软中断

【操作系统】聊聊Linux软中断

什么是中断 中断是系统用来响应硬件设备请求的一种机制&#xff0c;会打断进程的正常调度和执行&#xff0c;转而去执行内核中的中断处理程序。 比如你正在看书&#xff0c;你女朋友叫你出去逛街。你就需要先放下手里的事情&#xff0c;然后逛街。回来之后&#xff0c;在接着看…
阅读更多...
Parasoft Jtest 2023.1

Parasoft Jtest 2023.1

Parasoft Jtest 2023.1 2692407267qq.com&#xff0c;更多内容请见http://user.qzone.qq.com/2692407267/
阅读更多...
Codeforces Round 848 (Div. 2)C

Codeforces Round 848 (Div. 2)C

B. The Forbidden Permutation 一定要注意题目中说的是对于all i满足才算不好的&#xff0c;我们做的时候只要破坏一个i这个a就不算好的了,被这一点坑了&#xff0c;没注意到all。 #include <bits/stdc.h>using namespace std; typedef long long LL; const int N 2e5 …
阅读更多...
Linux系统100条命令：关于Ubuntu和 CentOS 7 相同功能的不同的终端操作命令

Linux系统100条命令：关于Ubuntu和 CentOS 7 相同功能的不同的终端操作命令

安装软件包&#xff1a; Ubuntu&#xff1a;apt-get install package_name CentOS 7&#xff1a;yum install package_name 更新软件包列表&#xff1a; Ubuntu&#xff1a;apt-get update CentOS 7&#xff1a;yum update 卸载软件包&#xff1a; Ubuntu&#xff1a;apt-…
阅读更多...
制作学生查询小程序

制作学生查询小程序

学生个人成绩查询小程序&#xff0c;一款助力教师实时了解学生学习情况的便捷工具。本文将为您揭秘它的制作过程&#xff0c;并提供实用的建议。然而&#xff0c;简便易用的方法莫过于选择现有的工具。 许多教师都偏爱使用易查分来快速创建查分网站。与传统的独立开发方式不同…
阅读更多...
kubeadm搭建k8s高可用集群（keepalived+nginx+3master）

kubeadm搭建k8s高可用集群（keepalived+nginx+3master）

目录 前言服务器准备架构讲解环境初始化安装keepalived软件安装nginx软件初始化k8s节点安装docker初始化master01节点的控制面板master02、master03节点加入集群node01、node02节点加入集群检查集群配置docker和kubectl命令补全创建应用验证集群功能验证master节点高可用方式二…
阅读更多...
深入解析TI毫米波雷达ROS驱动器的改进：从雷达参数配置、多普勒数据集成，到多雷达协同及传感器融合

深入解析TI毫米波雷达ROS驱动器的改进：从雷达参数配置、多普勒数据集成，到多雷达协同及传感器融合

第一部分&#xff1a;概述与与原始TI版本的主要区别 1.1 背景简介 毫米波雷达是近年来在汽车、无人机和其他应用中越来越受欢迎的传感器。其优点包括在恶劣天气条件下也能工作、可以提供速度和距离数据、以及不受环境光线影响。Texas Instruments&#xff08;TI&#xff09;是…
阅读更多...
QT-day4

QT-day4

画一个时钟 widget.h #ifndef WIDGET_H #define WIDGET_H#include <QWidget> #include <QPaintEvent> #include <QDebug> #include <QPainter> #include <QTimer> #include <QTime>QT_BEGIN_NAMESPACE namespace Ui { class Widget; } Q…
阅读更多...
Linux chmod命令——修改权限信息

Linux chmod命令——修改权限信息

我们可以使用chmod命令&#xff0c;修改文件、文件夹的权限信息。注意&#xff0c;只有文件、文件夹的所属用户或root用户可以修改。 chmod [-R] 权限 文件或文件夹 -R&#xff0c;对文件夹内的全部内容应用同样的操作 例如&#xff1a; chmod urwx,grx,ox hello.txt &…
阅读更多...
最新文章

Copyright @ 2022~2023