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

文章目录

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="[email protected]">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