ROS入门——PCL激光雷达点云处理（1）

前言：

参考书：《ros机器人高效编程》

源码地址：http://www.hzcourse.com/web/refbook/detail/7182/226

配置：ubuntu 16.04 、ROS-kinetic

零、创建ros工作空间

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:

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

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

rospack profile
roscd chapter10_tutorials
mkdir src

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

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_create");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
    pcl::PointCloud<pcl::PointXYZ> 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;
}

5、修改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})

6、进入工作空间编译包

cd ~/catkin_ws
catkin_make --pkg chapter10_tutorials

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

roscore
rosrun chapter10_tutorials pcl_create

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

rviz

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

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

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_read");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);

    sensor_msgs::PointCloud2 output;
    pcl::PointCloud<pcl::PointXYZ> 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 <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
    pcl::PointCloud<pcl::PointXYZ> 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

6、可视化同上

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

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

#include <iostream>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud<pcl::PointXYZ> 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 <iostream>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>

class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
     pcl::io::loadPCDFile ("0.pcd", cloud);

     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> 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<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
 pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;

     pcl::io::loadPCDFile ("0.pcd", cloud);

//剔除离群值
      pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);
//缩减采样
 	pcl::VoxelGrid<pcl::PointXYZ> 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 <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
    ros::NodeHandle nh;
    //初始化
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
     pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
     pcl::PointCloud<pcl::PointXYZ> cloud_segmented;
    ros::Publisher pcl_pub0 = nh.advertise<sensor_msgs::PointCloud2> ("pcl_cloud", 1);
    ros::Publisher pcl_pub1 = nh.advertise<sensor_msgs::PointCloud2> ("pcl_segment", 1);
    ros::Publisher ind_pub = nh.advertise<pcl_msgs::PointIndices>("point_indices", 1);
    ros::Publisher coef_pub = nh.advertise<pcl_msgs::ModelCoefficients>("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<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);
//体素栅格法下采样
 	pcl::VoxelGrid<pcl::PointXYZ> 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<pcl::PointXYZ> 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<pcl::PointXYZ> 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 <iostream>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>


class cloudHandler
{
public:
    cloudHandler()
     : viewer("Cloud Viewer")
    {
//初始化
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
     pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
     pcl::PointCloud<pcl::PointXYZ> cloud_segmented;
     pcl_pubb = nh.advertise<sensor_msgs::PointCloud2>("pcl_cloud", 1);
     pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_segmented", 1);
     ind_pub = nh.advertise<pcl_msgs::PointIndices>("point_indices", 1);
     coef_pub = nh.advertise<pcl_msgs::ModelCoefficients>("planar_coef", 1);
        
     pcl::io::loadPCDFile ("test_pcd.pcd", cloud);
//剔除离群值
      pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);
//体素栅格法下采样
 	pcl::VoxelGrid<pcl::PointXYZ> 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<pcl::PointXYZ> 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<pcl::PointXYZ> 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里修改了，图像和点云都可以显示了。

ROS入门——PCL激光雷达点云处理（1）

前言：

零、创建ros工作空间

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

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

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

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

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

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

2020年上半年数据库系统工程师考试

數學建模：無車承運人平臺線路定價

圖像處理——皮膚分割方法彙總（最全最有用的）

win10+ ubuntu16.04 雙系統及無線、輸入法、deepin-wineQQ微信等配置(親測)

ROS入門——ubuntu16.04 安裝和使用ROS 報錯

tensorflow學習（9）風格遷移中Gram矩陣的理解（最生動最直觀）

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結