11.1 pcl_ros的点云学习

本文是看了两个博主的内容，整理在这里是为了以后用时方便查找，更容易理解。引用的博文路径如下（本人也是刚开始看PCL的运用，本文是完全抄下面博主的内容，觉得这位博主写的很详细很清楚，并且自己运行了一遍有效）：

ROS入门——PCL激光雷达点云处理（1）_pcl::torosmsg-CSDN博客

以下功能的实现是我在ubuntu20.04的环境下，搭建好了ros环境后进行的。ros环境的搭建可以参考以下博文：

3.ubuntu20.04环境的ros搭建-CSDN博客    或者

4.livox hap(大疆激光雷达）环境搭建_livox hap激光雷达-CSDN博客

下面进行pcl的实现

1、在home下新建文件夹

mkdir -p ~/catkin_ws/src

2、进入src文件夹并初始化

cd ~/catkin_ws/src
catkin_init_workspace

执行完该命令后，src目录下会多出一个 CMakeLists.txt 文件，这个文件一般不需要我们修改。

3、 返回到catkin_ws下，进行编译(注意：每次编译必须在这个ws工作空间下才能编译成功！)

cd ~/catkin_ws/
catkin_make

执行完该命令后，发现工作空间catkin_ws中有三个目录： build  devel  src。可以从5、看到它们的作用。

4、source一下将工作空间加入环境变量

source devel/setup.bash

注意： 这一步只重新加载了setup.bash文件。如果关闭并打开一个新的命令行窗口，也需要再输入该命令将得到同样的效果。

所以建议采用一劳永逸的方法：.

5、.bashrc文件在用户的home文件夹(/home/USERNAME/.bashrc)下。每次用户打开终端，这个文件会加载命令行窗口或终端的配置。所以可以添加命令或进行配置以方便用户使用。在bashrc文件添加source命令：
 

echo "source ~/catkin_ws/devel/setup.bash">> ~/.bashrc

或者也可以打开.bashrc文件采用手动修改的方式添加source ~/catkin_ws/devel/setup.bash：

gedit ~/.bashrc

添加完毕，你的bashrc文件应该有两句source:

添加完后，在 .bashrc的同目录下运行

~/.bashrc

 5、理解工作空间

工作空间结构：

源空间(src文件夹)，放置了功能包、项目、复制的包等。在这个空间中,，最重要的一个文件是CMakeLists.txt。当在工作空间中配置包时,src文件夹中有CMakeLists.txt因为cmake调用它。这个文件是通过catkin_init_workspace命令创建的。

编译空间(build space):在build文件夹里,cmake和catkin为功能包和项目保存缓存信息、配置和其他中间文件。

开发空间(Development(devel)space):devel文件夹用来保存编译后的程序,这些是无须安装就能用来测试的程序。一旦项目通过测试,就可以安装或导出功能包从而与其他开发人员分享。
 

一、创建节点发布点云数据并可视化

1、在ros工作空间的src目录下新建包（包含依赖项）

catkin_create_pkg chapter10_tutorials pcl_conversions pcl_ros pcl_msgs sensor_msgs

2、在软件包中新建src目录

rospack profile
roscd chapter10_tutorials
mkdir src

3、在src目录下新建pcl_create.cpp，该程序创建了100个随机坐标的点云并以1Hz的频率，topic为“pcl_output"发布。frame设为odom。

#include 
#include 
#include 
#include 
 
main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_create");
 
    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise ("pcl_output", 1);
    pcl::PointCloud cloud;
    sensor_msgs::PointCloud2 output;
 
    // Fill in the cloud data
    cloud.width  = 100;
    cloud.height = 1;
    cloud.points.resize(cloud.width * cloud.height);
 
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
        cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }
 
    //Convert the cloud to ROS message
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";
 
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
 
    return 0;
}

4、修改cmakelist.txt内容

cmake_minimum_required(VERSION 2.8.3)
project(chapter10_tutorials)
find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_ros
  roscpp
  sensor_msgs
  rospy
)
 
find_package(PCL REQUIRED)
catkin_package()
 
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)
 
link_directories(${PCL_LIBRARY_DIRS})
 
 
add_executable(pcl_create src/pcl_create.cpp)
target_link_libraries(pcl_create ${catkin_LIBRARIES} ${PCL_LIBRARIES})
 

5、进入工作空间编译包

cd ~/catkin_ws
catkin_make --pkg chapter10_tutorials

6、若编译成功，新窗口打开ros，新窗口运行pcl_create节点

roscore
rosrun chapter10_tutorials pcl_create

7、新窗口打开rviz，add topic"pcl_output"，Global options 设置Frame为odom

rviz

二、加载pcd文件、保存点云为pcd文件

1、加载pcd文件并发布为pcl_output点云：在src下新建pcl_read.cpp，内容为：

#include 
#include 
#include 
#include 
#include 
 
main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_read");
 
    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise ("pcl_output", 1);
 
    sensor_msgs::PointCloud2 output;
    pcl::PointCloud cloud;
 
    pcl::io::loadPCDFile ("test_pcd.pcd", cloud);
 
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";
 
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
 
    return 0;
}

2、保存topic发布的点云为pcd文件，在src下新建pcl_write.cpp内容为：

#include 
#include 
#include 
#include 
#include 
 
void cloudCB(const sensor_msgs::PointCloud2 &input)
{
    pcl::PointCloud cloud;
    pcl::fromROSMsg(input, cloud);
    pcl::io::savePCDFileASCII ("write_pcd_test.pcd", cloud);
}
 
main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_write");
    ros::NodeHandle nh;
    ros::Subscriber bat_sub = nh.subscribe("pcl_output", 10, cloudCB);
    ros::spin();
 
    return 0;
}

 3、添加内容到cmakelist.txt

add_executable(pcl_read src/pcl_read.cpp)
add_executable(pcl_write src/pcl_write.cpp)
 
target_link_libraries(pcl_read ${catkin_LIBRARIES} ${PCL_LIBRARIES})
target_link_libraries(pcl_write ${catkin_LIBRARIES} ${PCL_LIBRARIES})

4、在catkin_ws空间下编译包（同上）

5、打开不同的窗口，在pcd文件夹下分别运行节点（因为pcl_read要读取pcd文件）

roscore
roscd chapter10_tutorials/data && rosrun chapter10_tutorials pcl_read
roscd chapter10_tutorials/data && rosrun chapter10_tutorials pcl_write

    注意：以上3个命令要打开3个新的终端窗口，依次运行即可。

6、可视化同上

三、cloud_viewer可视化pcd文件的点云

新建cpp文件，所有步骤同上。

#include 
#include 
#include 
#include 
#include 
 
class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud cloud;
     pcl::io::loadPCDFile ("0.pcd", cloud);
     viewer.showCloud(cloud.makeShared());
     viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }
 
    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }
 
protected:
    ros::NodeHandle nh;
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
};
 
main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
    cloudHandler handler;
    ros::spin();
    return 0;
}

编译并在pcd数据文件夹下运行节点，可得下图。可以按住ctrl键滑轮放大缩小。

四、点云预处理——滤波和缩减采样

1、滤波删除离群值pcl_filter.cpp，处理流程同上

#include 
#include 
#include 
#include 
#include 
#include 
 
class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud cloud;
     pcl::PointCloud cloud_filtered;
     pcl::io::loadPCDFile ("0.pcd", cloud);
 
     pcl::StatisticalOutlierRemoval statFilter;//统计离群值算法
        statFilter.setInputCloud(cloud.makeShared());//输入点云
        statFilter.setMeanK(10);//均值滤波
        statFilter.setStddevMulThresh(0.4);//方差0.4
        statFilter.filter(cloud_filtered);//输出结果到点云
 
     viewer.showCloud(cloud_filtered.makeShared());
     viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }
 
    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }
 
protected:
    ros::NodeHandle nh;
    
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
};
 
main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
    cloudHandler handler;
    ros::spin();
    return 0;
}

滤波结果：

2、滤波以后缩减采样pcl_dawnSample.cpp，采用体素栅格的方法，将点云分割为若干的小立方体（体素），以体素重心的点代表这个体素中所有的点。

public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud cloud;
pcl::PointCloud cloud_filtered;
 pcl::PointCloud cloud_downsampled;
 
     pcl::io::loadPCDFile ("0.pcd", cloud);
 
//剔除离群值
      pcl::StatisticalOutlierRemoval statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);
//缩减采样
 	pcl::VoxelGrid voxelSampler;//初始化 体素栅格滤波器
        voxelSampler.setInputCloud(cloud_filtered.makeShared());//输入点云
        voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);//每个体素的长宽高0.01m
        voxelSampler.filter(cloud_downsampled);//输出点云结果
 
     viewer.showCloud(cloud_downsampled.makeShared());
 
     viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }

缩减采样结果：

五、点云预处理——点云分割

1、pcl_segmentation.cpp采用RANSAC算法提取点云的plane平面。处理步骤同一

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
 
main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
    ros::NodeHandle nh;
    //初始化
     pcl::PointCloud cloud;
     pcl::PointCloud cloud_filtered;
     pcl::PointCloud cloud_downsampled;
     pcl::PointCloud cloud_segmented;
    ros::Publisher pcl_pub0 = nh.advertise ("pcl_cloud", 1);
    ros::Publisher pcl_pub1 = nh.advertise ("pcl_segment", 1);
    ros::Publisher ind_pub = nh.advertise("point_indices", 1);
    ros::Publisher coef_pub = nh.advertise("planar_coef", 1);
        
    sensor_msgs::PointCloud2 output0;
    sensor_msgs::PointCloud2 output1;
    pcl::io::loadPCDFile ("0.pcd", cloud);
    pcl::toROSMsg(cloud, output0);
    output0.header.frame_id = "odom";
//剔除离群值
      pcl::StatisticalOutlierRemoval statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);
//体素栅格法下采样
 	pcl::VoxelGrid voxelSampler;
        voxelSampler.setInputCloud(cloud_filtered.makeShared());
        voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
        voxelSampler.filter(cloud_downsampled);
//RANSAC算法 分割
	pcl::ModelCoefficients coefficients;//初始化模型系数
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices());//初始化索引参数
        pcl::SACSegmentation segmentation;//创建算法
        segmentation.setModelType(pcl::SACMODEL_PLANE);//设置分割模型为平面模型
        segmentation.setMethodType(pcl::SAC_RANSAC);//设置迭代算法
        segmentation.setMaxIterations(1000);//设置最大迭代次数
        segmentation.setDistanceThreshold(0.01);//设置到模型的最大距离
        segmentation.setInputCloud(cloud_downsampled.makeShared());//输入点云
        segmentation.segment(*inliers, coefficients);//输出点云结果  ×inliers是结果点云的索引，coe是模型系数
//发布模型系数
        pcl_msgs::ModelCoefficients ros_coefficients;
        pcl_conversions::fromPCL(coefficients, ros_coefficients);//pcl->msg
        
//发布抽样的内点索引
        pcl_msgs::PointIndices ros_inliers;
        pcl_conversions::fromPCL(*inliers, ros_inliers);
        
 
//创建分割点云，从点云中提取内点
        pcl::ExtractIndices extract;
        extract.setInputCloud(cloud_downsampled.makeShared());
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(cloud_segmented);
        pcl::toROSMsg(cloud_segmented, output1);
        output1.header.frame_id = "odom";
 
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub0.publish(output0);
        pcl_pub1.publish(output1);
        
        ind_pub.publish(ros_inliers);
        coef_pub.publish(ros_coefficients);//发布
        ros::spinOnce();
        loop_rate.sleep();
    }
 
    return 0;
}
 

出现警告：

[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.

滤波缩减采样后的结果：

平面滤波结果：

滤波缩减采样结果：

平面滤波结果：

2、（这是一个错误的程序，等以后学明白了再来解释为什么错误。viewer可以显示分割出的点云，不知道为啥没有发布成功，在rviz中不能显示，rostopic echo topic 也不能显示消息。）

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
 
 
class cloudHandler
{
public:
    cloudHandler()
     : viewer("Cloud Viewer")
    {
//初始化
     pcl::PointCloud cloud;
     pcl::PointCloud cloud_filtered;
     pcl::PointCloud cloud_downsampled;
     pcl::PointCloud cloud_segmented;
     pcl_pubb = nh.advertise("pcl_cloud", 1);
     pcl_pub = nh.advertise("pcl_segmented", 1);
     ind_pub = nh.advertise("point_indices", 1);
     coef_pub = nh.advertise("planar_coef", 1);
        
     pcl::io::loadPCDFile ("test_pcd.pcd", cloud);
//剔除离群值
      pcl::StatisticalOutlierRemoval statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);
//体素栅格法下采样
 	pcl::VoxelGrid voxelSampler;
        voxelSampler.setInputCloud(cloud_filtered.makeShared());
        voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
        voxelSampler.filter(cloud_downsampled);
 
//RANSAC算法 分割
	pcl::ModelCoefficients coefficients;//初始化模型系数
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices());//初始化索引参数
        pcl::SACSegmentation segmentation;//创建算法
        segmentation.setModelType(pcl::SACMODEL_PLANE);//设置分割模型为平面模型
        segmentation.setMethodType(pcl::SAC_RANSAC);//设置迭代算法
        segmentation.setMaxIterations(1000);//设置最大迭代次数
        segmentation.setDistanceThreshold(0.01);//设置到模型的最大距离
        segmentation.setInputCloud(cloud_downsampled.makeShared());//输入点云
        segmentation.segment(*inliers, coefficients);//输出点云结果  ×inliers是结果点云的索引，coe是模型系数
//发布模型系数
        pcl_msgs::ModelCoefficients ros_coefficients;
        pcl_conversions::fromPCL(coefficients, ros_coefficients);//pcl->msg
        coef_pub.publish(ros_coefficients);//发布
//发布抽样的内点索引
        pcl_msgs::PointIndices ros_inliers;
        pcl_conversions::fromPCL(*inliers, ros_inliers);
        ind_pub.publish(ros_inliers);
 
//创建分割点云，从点云中提取内点
        pcl::ExtractIndices extract;
        extract.setInputCloud(cloud_downsampled.makeShared());
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(cloud_segmented);
 
//发布点云
        sensor_msgs::PointCloud2 output;
        pcl::toROSMsg(cloud_segmented, output);
        pcl_pub.publish(output);
        sensor_msgs::PointCloud2 outputt;
	pcl::toROSMsg(cloud,outputt);
        pcl_pubb.publish(outputt);
 
//可视化
      viewer.showCloud(cloud_segmented.makeShared());
 
      viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
      }
 
 
 
    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }
 
protected:
    ros::NodeHandle nh;
    
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
    ros::Publisher pcl_pubb,pcl_pub, ind_pub, coef_pub;
};
 
main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
 
    cloudHandler handler;
 
    ros::spin();
 
    return 0;
}

六、播放bag并可视化激光雷达点云

1、查看无人艇的视频和雷达数据20200116.bag信息：

rosbag info 20200116.bag

发现时长约3分钟，topic有fix(卫星导航数据)、heading(姿态四元数)、points_raw（激光雷达点云）、rosout(节点图)、camera_info(摄像机信息)、image_raw(摄像机图片)、time_reference(时间)、vel(速度？角速度？)

2、回放bag文件：

rosbag play 20200116.bag

3、打开rviz:

rosrun rviz rviz

4、点击add--->add topic--->Pointcloud2(或 image)

5、Global Options ：Fixed Frame 改成pandar

（刚开始没有改fixed frame，add topic以后总是显示错误，后来在src下的雷达的包里看到readme文件里面写着：4. Change fixed frame to `pandar`。所以在vriz里修改了，图像和点云都可以显示了。

七、其他

1、将bag文件中的点云直接转成pcd文件，每帧一个pcd文件。

 rosrun pcl_ros bag_to_pcd data.bag /points_raw /media/xiaoxiong/小狗熊/无人艇/GKSdata

原文链接：https://blog.csdn.net/weixin_40820983/article/details/105803811