文章目录
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