ROS环境下采用PCL点云库对PCD格式点云进行滤波、旋转和平移等处理，并用RVIZ实时显示

沐春风吖

已于 2022-05-19 16:32:09 修改

阅读量4.8k

点赞数 24

文章标签： c++ ubuntu

于 2021-12-29 16:40:11 首次发布

本文链接：https://blog.csdn.net/weixin_42174523/article/details/122217637

版权

一、环境配置
操作系统: Ubuntu 18.04
ROS版本: ROS Melodic Morenia

说明: 我仅仅在上述环境下进行了验证调试，其它版本应该也是可以用的。

二、安装教程
1. 安装好ubuntu系统和ROS环境
2. 打开终端，并在终端命令行输入以下指令:
3. git clone https://gitee.com/wccworld/read_pcd.git
4. cd read_pcd/catkin_ws
5. catkin_make
6. source devel/setup.bash
7. roslaunch read_pcd read_pcd.launch

三、核心代码

1. 读取.pcd格式点云文件，并转化为ROS可发布点云格式PointCloud2

/********************************PCD点云获取**********************************************/
    pcl::PointCloud<PointType>::Ptr pcd_cloud_in (new pcl::PointCloud<PointType>);
    if (pcl::io::loadPCDFile<PointType> (pcd_doc_path, *pcd_cloud_in) == -1)
    {
    PCL_ERROR ("Couldn't read file: %s \n", pcd_doc_path.c_str());
    return (-1);
    }

    sensor_msgs::PointCloud2 input_cloud;
    pcl::toROSMsg(*pcd_cloud_in,input_cloud);
    input_cloud.header.frame_id = output_frame_id;
    // read_pcd_pub.publish(input_cloud);
/****************************************************************************************/

2. 直通滤波器对X轴进行滤波（ PCL点云滤波的主要内容、原理和详细使用说明可参考此篇文章 https://zhuanlan.zhihu.com/p/102748557 ）

 //  直通滤波器 X 轴滤波 
    pcl::PointCloud<PointType>::Ptr filter_x (new pcl::PointCloud<PointType>);
    pcl::PassThrough<PointType> ptx;
    ptx.setInputCloud(pcd_cloud_in);                 //输入点云
    ptx.setFilterFieldName("x");                     //对x轴进行操作
    ptx.setFilterLimits(pass_x_min, pass_x_max);     //设置直通滤波器操作范围
    // ptx.setFilterLimitsNegative(true);              //设置保留范围内，还是过滤掉范围内
    ptx.filter(*filter_x);                           //执行滤波，过滤结果保存在filter_x

3. VoxelGrid滤波器对点云进行下采样

//  体素降采样
    pcl::PointCloud<PointType>::Ptr vox_cloud(new pcl::PointCloud<PointType>);
    pcl::VoxelGrid<PointType> vox_grid;
    vox_grid.setInputCloud(filter_z);
    vox_grid.setLeafSize(voxel_size, voxel_size, voxel_size); //设置滤波时创建的体素立方体(m)
    vox_grid.filter(*vox_cloud);

4. statisticalOutlierRemoval滤波器移除离群点

//  statisticalOutlierRemoval滤波器移除离群点
    pcl::PointCloud<PointType>::Ptr filtered_cloud(new pcl::PointCloud<PointType>);
	pcl::StatisticalOutlierRemoval<PointType> sor;   
	sor.setInputCloud(vox_cloud);                                                 
	sor.setMeanK(sor_nearby_number);                            //设置在进行统计时考虑查询点临近点数                                              
	sor.setStddevMulThresh(sor_thresh_value);                   //设置判断是否为离群点的阀值                                           
	sor.filter(*filtered_cloud);

5. 点云旋转和平移操作

//  点云旋转和平移操作
    for(int i = 0; i < filtered_cloud->points.size(); i++) 
    {
        PointType new_point;
        double rx_x,rx_y,rx_z,ry_x,ry_y,ry_z,rz_x,rz_y,rz_z;

        double px = filtered_cloud->points[i].x;
        double py = filtered_cloud->points[i].y;
        double pz = filtered_cloud->points[i].z;
        double pi = filtered_cloud->points[i].intensity;
        
        //  点云绕 X 旋转
        rx_x = px;
        rx_y = cos(rad(x_rotate_value))*py + (-sin(rad(x_rotate_value)))*pz;
        rx_z = sin(rad(x_rotate_value))*py + cos(rad(x_rotate_value))*pz;

        //  点云绕 Y 旋转
        ry_x = cos(rad(y_rotate_value))*rx_x + (-sin(rad(y_rotate_value)))*rx_z;
        ry_y = rx_y;
        ry_z = sin(rad(y_rotate_value))*rx_x + cos(rad(y_rotate_value))*rx_z;

        //  点云绕 Z 旋转
        rz_x = cos(rad(z_rotate_value))*ry_x + (-sin(rad(z_rotate_value)))*ry_y;
        rz_y = sin(rad(z_rotate_value))*ry_x + cos(rad(z_rotate_value))*ry_y;
        rz_z = ry_z;

        //  点云整体平移
        new_point.x = rz_x + x_trans_value;
        new_point.y = rz_y + y_trans_value;
        new_point.z = rz_z + z_trans_value;
        
        new_point.intensity = pi;
        handle_cloud->points.push_back(new_point);
    }

四、使用说明
找到所下载源码里的read_pcd.launch文件，只需对相应参数进行修改，便可实现PCL点云的滤波、体素降采样、移除离群点、旋转以及平移等操作，具体参数代表的内容如下所示:

<launch>
  <param name="output_frame_id" value = "map" />                   # 设置frame_id
  <param name="pointCloud_pubTopic" value = "/handle_point" />     # 设置处理后点云的输出话题名
  <param name="pcd_doc_path" value = "/home/wcc/SLAM/map_result/lio_sam/GlobalMap.pcd" />       # 设置所要加载的PCD文件路径，此处需要用自己的PCD文件路径替换此路径
  <param name="output_pcd_path" value = "/home/wcc/csdn/read_pcd/catkin_ws/pcd/output.pcd" />   # 设置处理完之后点云输出的PCD文件路径，此处需根据自己的路径进行修改

  <param name="pass_x_min" value = "-100.0" />      # 设置直通滤波器进行X轴滤波时X的最小值
  <param name="pass_x_max" value = "100.0" />       # 设置直通滤波器进行X轴滤波时X的最大值
  <param name="pass_y_min" value = "-100.0" />      # 设置直通滤波器进行Y轴滤波时Y的最小值
  <param name="pass_y_max" value = "100.0" />       # 设置直通滤波器进行Y轴滤波时Y的最大值
  <param name="pass_z_min" value = "-1.0" />        # 设置直通滤波器进行Z轴滤波时Z的最小值
  <param name="pass_z_max" value = "30.0" />        # 设置直通滤波器进行Z轴滤波时Z的最大值
  <param name="voxel_size" value = "0.1" />         # 设置滤波时创建的体素体积值，单位(M)
  <param name="sor_nearby_number" value = "10" />   # 设置在进行统计时考虑查询点临近点数
  <param name="sor_thresh_value" value = "5.0" />   # 设置判断是否为离群点的阀值
  <param name="x_rotate_value" value = "0.0" />     # 设置整体点云绕X轴旋转的角度值
  <param name="y_rotate_value" value = "0.0" />     # 设置整体点云绕y轴旋转的角度值
  <param name="z_rotate_value" value = "0.0" />     # 设置整体点云绕z轴旋转的角度值
  <param name="x_trans_value" value = "0.0" />      # 设置整体点云绕x轴平移的距离值
  <param name="y_trans_value" value = "20.0" />     # 设置整体点云绕y轴平移的距离值
  <param name="z_trans_value" value = "0.0" />      # 设置整体点云绕z轴平移的距离值

  <node pkg="read_pcd" name="read_pcd" type="read_pcd" output="screen" />
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find read_pcd)/rviz/read_pcd.rviz" />
</launch>

五、结果展示