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

文章目录

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

    package.xml

      point_cloud_registration  0.0.0  The point_cloud_registration package  xiaqiuslam  TODO  catkin    pcl_conversions  pcl_ros  roscpp  sensor_msgs  pcl_conversions  pcl_ros  roscpp  sensor_msgs    pcl_conversions  pcl_ros  roscpp  sensor_msgs    

    CMakeLists.txt

    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

    #include #include #include #include #include #include #include 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::Ptr current_pointcloud_; pcl::PointCloud::Ptr last_pointcloud_; pcl::IterativeClosestPoint 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::Ptr cloud_msg = boost::shared_ptr(new pcl::PointCloud());    cloud_msg->points.resize(scan_msg->ranges.size());    for (unsigned int i = 0; i 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 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 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     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