计算两帧雷达数据之间的变换矩阵

文章目录

    • package.xml
    • CMakeLists.txt
    • point_cloud_registration.cc
    • 运行结果

package.xml

<?xml version="1.0"?>
<package format="2"><name>point_cloud_registration</name><version>0.0.0</version><description>The point_cloud_registration package</description><maintainer email="xiaoqiuslam@qq.com">xiaqiuslam</maintainer><license>TODO</license><buildtool_depend>catkin</buildtool_depend><build_depend>pcl_conversions</build_depend><build_depend>pcl_ros</build_depend><build_depend>roscpp</build_depend><build_depend>sensor_msgs</build_depend><build_export_depend>pcl_conversions</build_export_depend><build_export_depend>pcl_ros</build_export_depend><build_export_depend>roscpp</build_export_depend><build_export_depend>sensor_msgs</build_export_depend><exec_depend>pcl_conversions</exec_depend><exec_depend>pcl_ros</exec_depend><exec_depend>roscpp</exec_depend><exec_depend>sensor_msgs</exec_depend><export></export>
</package>

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)project(point_cloud_registration)add_compile_options(-std=c++11)find_package(catkin REQUIRED COMPONENTSpcl_conversionspcl_rosroscppsensor_msgs
)find_package(PCL REQUIRED QUIET)catkin_package()include_directories(
include${catkin_INCLUDE_DIRS}${PCL_INCLUDE_DIRS}
)add_executable(point_cloud_registration src/point_cloud_registration.cc)target_link_libraries(point_cloud_registration ${catkin_LIBRARIES})

point_cloud_registration.cc

#include <chrono>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
void ConvertScan2PointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
void ScanMatchWithICP(const sensor_msgs::LaserScan::ConstPtr &scan_msg);bool is_first_scan_ = true;
pcl::PointCloud<pcl::PointXYZ>::Ptr current_pointcloud_; 
pcl::PointCloud<pcl::PointXYZ>::Ptr last_pointcloud_; 
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp_;void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{// 第一帧雷达数据if (is_first_scan_ == true){// 转换数据类型 并保存到current_pointcloud_ConvertScan2PointCloud(scan_msg);is_first_scan_ = false;}// 第二帧雷达数据else{// 数据类型转换std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();// 将current_pointcloud_赋值到last_pointcloud_进行保存*last_pointcloud_ = *current_pointcloud_;   // 数据类型转换 ConvertScan2PointCloud(scan_msg);// 调用ICP进行计算 雷达前后两帧间的坐标变换ScanMatchWithICP(scan_msg);}
}void ConvertScan2PointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_msg = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>());cloud_msg->points.resize(scan_msg->ranges.size());for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i){pcl::PointXYZ &point_tmp = cloud_msg->points[i];float range = scan_msg->ranges[i];if (!std::isfinite(range))continue;if (range > scan_msg->range_min && range < scan_msg->range_max){float angle = scan_msg->angle_min + i * scan_msg->angle_increment;point_tmp.x = range * cos(angle);point_tmp.y = range * sin(angle);point_tmp.z = 0.0;}}cloud_msg->width = scan_msg->ranges.size();cloud_msg->height = 1;cloud_msg->is_dense = true;pcl_conversions::toPCL(scan_msg->header, cloud_msg->header);*current_pointcloud_ = *cloud_msg;
}void ScanMatchWithICP(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{icp_.setInputSource(last_pointcloud_);icp_.setInputTarget(current_pointcloud_);pcl::PointCloud<pcl::PointXYZ> unused_result;icp_.align(unused_result);if (icp_.hasConverged() == false){return;}else{Eigen::Affine3f transfrom;transfrom = icp_.getFinalTransformation();float x, y, z, roll, pitch, yaw;pcl::getTranslationAndEulerAngles(transfrom, x, y, z, roll, pitch, yaw);std::cout << "transfrom: (x: " << x << ", y: " << y << ", yaw: " << yaw * 180 / M_PI << ")" << std::endl;}
}int main(int argc, char **argv)
{ros::init(argc, argv, "point_cloud_registration");ros::NodeHandle node_handle_; ros::Subscriber laser_scan_subscriber_;laser_scan_subscriber_ = node_handle_.subscribe("laser_scan", 1, &ScanCallback);current_pointcloud_ = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>());last_pointcloud_ = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>());ros::spin();return 0;
}

运行结果

roscore 
source devel/setup.bash && rosrun point_cloud_registration point_cloud_registration
rosbag play 1.bag 

在这里插入图片描述

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

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

相关文章

九州金榜|孩子厌学的因素及解决办法

孩子在学习的过程中&#xff0c;遇到厌学这种情况非常容易见到&#xff0c;这也是孩子在成长的过程中经常遇到的烦恼。面对孩子的厌学&#xff0c;作为家长这时候不要慌乱&#xff0c;要做到分析孩子产生厌学的原因&#xff0c;在去寻找解决孩子厌学的办法。下面九州金榜家庭教…

【漏洞复现】大华ICC智能物联综合管理平台任意文件读取漏洞

Nx01 产品简介 大华智能物联综合管理平台 iConnection Center&#xff08;以下简称&#xff1a;ICC平台&#xff09;&#xff0c;是一套基于智能物联的综合业务管理平台软件&#xff0c;具备强大的后台服务能力&#xff0c;配套了B/S管理员端、C/S客户端、移动APP终端、小程序等…

mmlspark.lightgbm.LightGBMClassifier参数说明

mmlspark.lightgbm.LightGBMClassifier 是一个用于二元分类和多类分类的机器学习模型&#xff0c;它是基于 Microsoft ML for Apache Spark (MMLSpark) 库的。这个类是为了在Spark环境中使用LightGBM实现&#xff0c;提供了大量的参数用于调整模型。下面是一些主要参数的详细中…

数据分析之Excel的使用

数据分析之Excel的使用 SUM()求和SUMIF()单条件求和SUMIFS()多条件求和日期函数YEAR()提取年份MONTH()提取月份DAY()提取日 SUBTOTAL()聚合函数IF()函数IF嵌套 VLOOKUP()搜索取值MATCH()返回行值或列值INDEX()定位取值 SUM()求和 SUMIF()单条件求和 SUMIFS()多条件求和 日期…

python的类修饰器

类修饰器有 staticmethodclassmethod staticmethod 函数传参不会传实例 classmethod 函数传参的第一个参数是类&#xff0c;不是实例

nacos闪退无法正常启动bug

配置java的时候要配置JAVA_HOME 不能直接在path中用jdk的bin路径&#xff0c;不然会报错&#xff0c;虽然你能够正常使用java&#xff0c;但还是会报错 这是报错信息 //bin/java -Djava.ext.dirs//jre/lib/ext://lib/ext -server -Xms2g -Xmx2g -Xmn1g -XX:MetaspaceSize128…

vue中性能优化

目录 1. 编码优化 2. 源码优化 3. 打包优化 4. 利用 Vue Devtools 总结 Vue.js 作为一个强大的前端框架&#xff0c;提供了丰富的功能和工具来帮助开发者构建高效的 Web 应用。然而&#xff0c;在开发过程中&#xff0c;性能优化仍然是一个需要关注的问题。以下是对 Vue.j…

华为OD机考-C卷

文章目录 攀登者问题停车场最短路径 攀登者问题 24/03/09 20:50~23:10 攀登者喜欢寻找各种地图&#xff0c;并且尝试攀登到最高的山峰。地图表示为一维数组&#xff0c;数组的索引代表水平位置&#xff0c;数组的元素代表相对海拔高度。其中数组元素0代表地面。一个山脉可能有多…

GTH手册学习注解

CPLL的动态配置 终于看到有这个复位功能了 QPLL SWITCHing需要复位 器件级RESET没发现有管脚引出来 两种复位方式&#xff0c;对应全复位和器件级复位 对应的复位功能管脚 改那个2分频的寄存器说明段&#xff0c;复位是自动发生的&#xff1f;说明可能起效了&#xff0c;但是分…

Linux 之七:Linux 防火墙 和进程管理

防火墙 查看防火墙 查看 Centos7 的防火墙的状态 sudo systemctl status firewalld。 查看后&#xff0c;看到active(running)就意味着防火墙打开了。 关闭防火墙&#xff0c;命令为&#xff1a; sudo systemctl stop firewalld。 关闭后查看是否关闭成功&#xff0c;如果…

python 判断操作系统是windows 还是linux

下载脚本之后需要先判断是windows还是linux&#xff0c;如果是windows的话&#xff0c;下载完成之后需要等待用户操作才能进行安装&#xff0c;安装之后需要先在windows上安装openssh 在Python中&#xff0c;你可以使用platform模块来判断当前操作系统是Windows还是Linux。以下…

leetcode必刷题 96.不同的二叉搜索树

一、问题描述&#xff1a; 给你一个整数 n &#xff0c;求恰由 n 个节点组成且节点值从 1 到 n 互不相同的 二叉搜索树 有多少种&#xff1f;返回满足题意的二叉搜索树的种数。 二、解题思路&#xff1a; 二叉树是由根节点&#xff0c;左右子树组成的&#xff0c;二叉搜索树要…

提高驾驶安全性 | 基于ACM32 MCU的胎压监测仪方案

概述 胎压监测系统 作为车辆的基础部件&#xff0c;轮胎是影响行车安全不可忽视的因素之一。据统计&#xff0c;中国每年由胎压问题引起轮胎爆炸的交通事故约占 30%&#xff0c;其中 50%的高速交通事故是由车辆胎压异常引起。因此&#xff0c;准确实时地监测车辆在行驶过程中…

Java虚拟机(JVM)元数据区存放的内容

类元数据 元数据区&#xff08;在HotSpot虚拟机中也称为Metaspace&#xff09;主要存放了类的元数据信息&#xff0c;如类的名称、访问修饰符、常量池、字段描述、方法描述等。 运行时常量池 运行时常量池是每个类或接口的常量池表的运行时表示形式&#xff0c;包含了若干种不…

Curriculum Manager for Source Selection in Multi-Source Domain Adaptation

GRL: gradient reversal layer&#xff0c;CM: Curriculum Manager 辅助信息 作者未提供代码

PokéLLMon 源码解析(四)

.\PokeLLMon\poke_env\exceptions.py """ This module contains exceptions. """# 定义一个自定义异常类 ShowdownException&#xff0c;继承自内置异常类 Exception class ShowdownException(Exception):"""This exception is …

openssl3.2 - exp - generate prime

文章目录 openssl3.2 - exp - generate prime概述笔记END openssl3.2 - exp - generate prime 概述 openssl3.2.命令行可以生成质数 openssl prime -generate -bits 256 -hex -safe 单步调试openssl.exe工程, 整理了一个函数, 用openssl API来产生质数. openssl命令行是将结果…

2024/3/10打卡借教室——二分+差分

题目 在大学期间&#xff0c;经常需要租借教室。 大到院系举办活动&#xff0c;小到学习小组自习讨论&#xff0c;都需要向学校申请借教室。 教室的大小功能不同&#xff0c;借教室人的身份不同&#xff0c;借教室的手续也不一样。  面对海量租借教室的信息&#xff0c;我们自…

IDEA打开项目文件目录不见了

偶尔发生新拉下来的代码&#xff0c;或者旧代码修改了包名&#xff0c;项目名称等&#xff0c;idea左侧project一栏不显示代码的文件目录。例如下面此时不要慌张&#xff0c;不用删除项目重新拉取&#xff0c;通过以下方式解决&#xff1a; 本人尝试能够解决&#xff0c;如果无…

c# 二分查找(迭代与递归)

二分搜索被定义为一种在排序数组中使用的搜索算法&#xff0c;通过重复将搜索间隔一分为二。二分查找的思想是利用数组已排序的信息&#xff0c;将时间复杂度降低到O(log N)。 二分查找算法示例 何时在数据结构中应用二分查找的条件&#xff1a; 应用二分查找算法&#xff1a…