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

文章目录

package.xml

bash 复制代码
<?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

bash 复制代码
cmake_minimum_required(VERSION 3.0.2)

project(point_cloud_registration)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_ros
  roscpp
  sensor_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

cpp 复制代码
#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;
}

运行结果

bash 复制代码
roscore 
source devel/setup.bash && rosrun point_cloud_registration point_cloud_registration
rosbag play 1.bag 
相关推荐
淘小白_TXB21969 小时前
头条号矩阵运营经验访谈记录
线性代数·矩阵
智者知已应修善业1 天前
【矩阵找最大小所在位置】2022-11-13
c语言·c++·经验分享·笔记·算法·矩阵
semantist@语校1 天前
第二十篇|SAMU教育学院的教育数据剖析:制度阈值、能力矩阵与升学网络
大数据·数据库·人工智能·百度·语言模型·矩阵·prompt
deephub1 天前
机器人逆运动学进阶:李代数、矩阵指数与旋转流形计算
人工智能·机器学习·矩阵·机器人·李群李代数
时空无限2 天前
说说transformer 中的掩码矩阵以及为什么能掩盖住词语
人工智能·矩阵·transformer
hn小菜鸡2 天前
LeetCode 3643.垂直翻转子矩阵
算法·leetcode·矩阵
张晓~183399481212 天前
短视频矩阵源码-视频剪辑+AI智能体开发接入技术分享
c语言·c++·人工智能·矩阵·c#·php·音视频
The_Killer.2 天前
格密码--从FFT到NTT(附源码)
学习·线性代数·密码学·格密码
小李独爱秋2 天前
特征值优化:机器学习中的数学基石
人工智能·python·线性代数·机器学习·数学建模
听情歌落俗2 天前
MATLAB3-1变量-台大郭彦甫
开发语言·笔记·算法·matlab·矩阵