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

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="[e-mail]">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